多軸飛行的研究筆記: 3月 2016

2016年3月29日星期二

3.3 v to 5.0 v buffer ic

1. 74LCX244

https://www.fairchildsemi.com/datasheets/74/74LCX244.pdf

2. MC74VHC1GT275

http://www.onsemi.com/pub_link/Collateral/MC74VHC1GT125-D.PDF

arduino + RPI + spi

Arduino 線路 : D10, D11, D12,D13
wire :

SS – digital 10. You can use other digital pins, but 10 is generally the default as it is next to the other SPI pins;
MOSI – digital 11;
MISO – digital 12;
SCK – digital 13;

RPI pin 腳

Rabpberry Pi Signal	Serial 7-seg Signal
GND	GND
3.3V	VCC
CE1	SS (Shift Select)
SCK	SCK
MOSI	SDI
MISO	SDO

pin number

MOSI    P1-19
MISO    P1-21
SCLK    P1-23   P1-24    CE0
GND     P1-25   P1-26    CE1

如果是用 /dev/spi0.0 就是接 CE0 到 SS
如果是用 /dev/spi0.1 就是接 CE1 到 SS

P1-24 接到 D10
P1 -19 接到 D11
P1-21 串一個1K 電阻接到 D12
P1-23 接到 D13

SPI序列周邊介面匯流排
SPI (Serial Peripheral Interface Bus)，類似I²C，是一種4線同步序列資料協定，適用於可攜式裝置平臺系統，但使用率較 I²C少。SPI匯流排定義四組 logic signals：
‧ SCLK—Serial Clock（自master輸出）
‧ MOSI/SIMO—Master Output, Slave Input（自master輸出）
‧ MISO/SOMI—Master Input, Slave Output（自slave輸出）
‧ SS—Slave Select（active low;自master輸出）

RX1002 + 遙控器 + 馬達....

新增

pin 2, 3, 8, 9 去驅動馬達 .....

接收器接腳如下

左手上下 : 油門 (第一通道) pin 7

左右 : YAW (第四通道) pin 4

右手上下 : PITCH (第三通道) pin 5

左右 ROLL (第二通道) pin 6

code 如下:

#include <Servo.h>
#include <PinChangeInt.h>

#define THR_PIN 7
#define ROLL_PIN 6
#define PITCH_PIN 5
#define YAW_PIN 4 // PITH
#define SERIAL_BAND 9600

#define FL_PIN 2
#define FR_PIN 3
#define BL_PIN 8
#define BR_PIN 9

#define thr_min 1100
#define thr_max 1900

#define SERVO_NUM 4

unsigned int thr_start_time=0, thr_time=0;
unsigned int yaw_start_time=0, yaw_time=0;
unsigned int roll_start_time=0, roll_time=0;
unsigned int pitch_start_time=0, pitch_time=0;
unsigned int thr_val;

Servo MOTOR[SERVO_NUM];

void setup()
{

Serial.begin(SERIAL_BAND);
// while (!Serial) {}

Serial.println("Init done");
pinMode(THR_PIN, INPUT);
pinMode(YAW_PIN, INPUT);
pinMode(PITCH_PIN, INPUT);
pinMode(ROLL_PIN, INPUT);
//digitalWrite(UPPER_LIMIT_PIN, HIGH);
PCintPort::attachInterrupt(THR_PIN, THR_Function, CHANGE );
PCintPort::attachInterrupt(YAW_PIN, YAW_Function, CHANGE );
PCintPort::attachInterrupt(PITCH_PIN, PITCH_Function, CHANGE );
PCintPort::attachInterrupt(ROLL_PIN, ROLL_Function, CHANGE );

MOTOR[0].attach(FL_PIN);
MOTOR[1].attach(FR_PIN);
MOTOR[2].attach(BL_PIN);
MOTOR[3].attach(BR_PIN);

for (int i=0;i<SERVO_NUM;i++)
MOTOR[i].writeMicroseconds(thr_min);

}

void loop()
{
while(Serial.available() > 0)
{
char c = Serial.read(); // get it
Serial.println(thr_time);
//Serial.println(yaw_time);
//Serial.println(pitch_time);
//Serial.println(roll_time);
Serial.println(thr_val);
}
/*
thr_val = (( long)(thr_time - thr_min)) * 100 / (thr_max - thr_min) + 137;

// 1100/2040 *255 ~ 1900/2040*255 -> 137 ~ 237

if(thr_val >=255)
thr_val = 255;
else if(thr_val <=0)
thr_val = 0;

analogWrite(FL_PIN,thr_val);
analogWrite(FR_PIN,thr_val);
analogWrite(BL_PIN,thr_val);
analogWrite(BR_PIN,thr_val);
*/

for (int i=0;i<SERVO_NUM;i++)
MOTOR[i].writeMicroseconds(thr_time);

}

void THR_Function()
{
if(digitalRead(THR_PIN)==HIGH) // rising
thr_start_time = micros();
else
thr_time = micros() - thr_start_time;
}

void YAW_Function()
{
if(digitalRead(YAW_PIN)==HIGH) // rising
yaw_start_time = micros();
else
yaw_time = micros() - yaw_start_time;
}

void PITCH_Function()
{
if(digitalRead(PITCH_PIN)==HIGH) // rising
pitch_start_time = micros();
else
pitch_time = micros() - pitch_start_time;
}

void ROLL_Function()
{
if(digitalRead(ROLL_PIN)==HIGH) // rising
roll_start_time = micros();
else
roll_time = micros() - roll_start_time;
}

quad-copter v2 研究筆記2

main.cpp 的流程

//setting up IMU
imu.set_com();
imu.initialize();

//setting up SPI
ArduSPI.initialize();

Set_default_PID_config();

//Say I am ready

Beep(3);

Blink_led(10);

//Starting Timer

Timer.start();

while(1){

sleep(20000);

}

1. imu.set_com

void DMP::set_com() {

// initialize device

mpu.initialize();

// verify connection

printf("Testing device connections...\n");

printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");

// load and configure the DMP

printf("Initializing DMP...\n");

devStatus = mpu.dmpInitialize();

// make sure it worked (returns 0 if so)

if (devStatus == 0) {

// turn on the DMP, now that it's ready

printf("Enabling DMP...\n");

mpu.setDMPEnabled(true);

mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it's okay to use it

printf("DMP ready!\n");

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

} else {

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it's going to break, usually the code will be 1)

printf("DMP Initialization failed (code %d)\n", devStatus);

}

2. initialize()

void DMP::initialize(){

//This routine waits for the yaw angle to stop

//drifting

if (!dmpReady) return;

printf("Initializing IMU...\n");

//float gyr_old = 10;

int n=0;

do {

// wait for FIFO count > 42 bits

do {

fifoCount = mpu.getFIFOCount();

}while (fifoCount<42);

if (fifoCount >= 1024) {

// reset so we can continue cleanly

mpu.resetFIFO();

printf("FIFO overflow!\n");

// otherwise, check for DMP data ready interrupt

//(this should happen frequently)

} else {

//read packet from fifo

mpu.getFIFOBytes(fifoBuffer, packetSize);

mpu.dmpGetGyro(g, fifoBuffer);

//0=gyroX, 1=gyroY, 2=gyroZ

//swapped to match Yaw,Pitch,Roll

//Scaled from deg/s to get tr/s

for (int i=0;i<DIM;i++){

gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;

}

n++;

}while (fabs(gyro[ROLL]) + fabs(gyro[PITCH]) > 0.03 && n<5000);

// 發覺已經沒有振動了

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

for (int i=0;i<DIM;i++) m_ypr_off[i] = ypr[i];

printf("IMU init done; offset values are :\n");

printf("yaw = %f, pitch = %f, roll = %f, n= %d\n\n",

ypr[YAW]*180/M_PI, ypr[PITCH]*180/M_PI,

ypr[ROLL]*180/M_PI,n);

initialized = true;

}

3. ArduSPI.initilize()

SPI::SPI()

{

_devName = "/dev/spidev0.0";

_speed = 1000000;

_bits = 8;

_delay = 0;

_mode = 0;

}

int SPI::initialize(){

int ret = 0;

_fd = open(_devName, O_RDWR); // _devName = "/dev/spidev0.0";

if (_fd < 0)

printf("SPI initialize : can't open device\n");

* spi mode

ret = ioctl(_fd, SPI_IOC_WR_MODE, &_mode); // _mode =0

if (ret == -1)

printf("SPI initialize : can't set spi mode\n");

* bits per word

ret = ioctl(_fd, SPI_IOC_WR_BITS_PER_WORD, &_bits); // bit = 8

if (ret == -1)

printf("SPI initialize : can't set bits per word\n");

* max speed hz

ret = ioctl(_fd, SPI_IOC_WR_MAX_SPEED_HZ, &_speed); // _speed = 1000000;

if (ret == -1)

printf("SPI initialize : can't set max speed hz\n");

printf("SPI initialization finished\n");

return ret;

}

4. Set_default_PID_config()

void Set_default_PID_config(){

//manual initialization of PID constants

yprRATE[YAW].set_Kpid(6.0, 0.0, 0.0);

for (int i=1;i<3;i++){ // only roll and pitch

yprSTAB[i].set_Kpid(2.0, 0.001, 0.1);

yprRATE[i].set_Kpid(2.0, 0.0, 0.0);

}

5. Timer.start()

void TimerClass::sig_handler_(int signum)

{

pthread_mutex_lock(&TimerMutex_);

//output to a log file

//open log file

fstream logfile;

logfile.open("quadpilot.log", ios::out|ios::app);

if (logfile.fail()) // Check for file creation and return error.

{

cout << "Error opening output.\n";

}

float RCinput[N_RC_CHAN],PIDout[3];

uint16_t ESC[N_SERVO];

//------------------------------------------------------

//1-Get Remote values using SPI

union bytes{

uint8_t u8[2];

uint16_t u16;

} rc_union;

uint8_t checksum=0,recv_checksum=1;

while (checksum != recv_checksum) {

checksum=0;

for (int i=0;i<4;i++){

ArduSPI.writeByte((uint8_t) (i+1)*10);

rc_union.u8[0] = ArduSPI.rwByte((uint8_t) (i+1)*10+1);

rc_union.u8[1] = ArduSPI.rwByte('S');

//transaction ended

RCinput[i] = (float) rc_union.u16;

checksum+=rc_union.u8[0];

checksum+=rc_union.u8[1];

}

//Control checksum

ArduSPI.writeByte('C');

recv_checksum = ArduSPI.rwByte('S');

}

//Bounding RC values to avoid division by zeros fex.

for (int i=1;i<4;i++){

if ( RCinput[i] > RC_MAX) RCinput[i] = RC_MAX;

if ( RCinput[i] < RC_MIN) RCinput[i] = RC_MIN;

}

//outputing values to logfile

logfile << RCinput[0] << " " << RCinput[1] << " "

<< RCinput[2] << " " << RCinput[3] << " ";

// //convert into PID usable values

RCinput[0] = (RCinput[0] - THR_MIN)/(THR_MAX-THR_MIN) * 100.0;

RCinput[1] = -(RCinput[1] -(RC_MAX+RC_MIN)/2.) /

(RC_MAX-RC_MIN) * K_YAW;

RCinput[2] = (RCinput[2] -(RC_MAX+RC_MIN)/2.)/

(RC_MAX-RC_MIN) * K_PITCH;

RCinput[3] = (RCinput[3] -(RC_MAX+RC_MIN)/2.)/

(RC_MAX-RC_MIN) * K_ROLL;

//outputing values to logfile

logfile << RCinput[0] << " " << RCinput[1] << " "

<< RCinput[2] << " " << RCinput[3] << " ";

#ifdef XMODE

//Switch to Xmode instead of +mode

//orders are given in a ref frame rotated by 90deg.

float cs45 = sqrt(2.)/2.;

float Px = RCinput[2]*cs45 + RCinput[3]*cs45;

float Rx = - RCinput[2]*cs45 + RCinput[3]*cs45;

RCinput[2] = Px;

RCinput[3] = Rx;

#endif

// printf("Received : %6.3f %6.3f %6.3f %6.3f\n", RCinput[0],

// RCinput[1], RCinput[2], RCinput[3]);

//------------------------------------------------------

//2- Get attitude of the drone

while (imu.getAttitude() < 0){

};

//Compensate lost of Thrust due to angle of drone

RCinput[0] = RCinput[0]/cos(imu.ypr[ROLL]/180*M_PI)

/cos(imu.ypr[PITCH]/180*M_PI);

//output to logfile

// logfile << imu.ypr[YAW] << " " << imu.ypr[PITCH] << " "

// << imu.ypr[ROLL] << " "

// << imu.gyro[YAW] << " " << imu.gyro[PITCH] << " "

// << imu.gyro[ROLL] << " ";

// printf("ATTITUDE: %7.2f %7.2f %7.2f\n",imu.ypr[YAW],

// imu.ypr[PITCH],

// imu.ypr[ROLL]);

// printf(" %7.2f %7.2f %7.2f\n",imu.gyro[YAW],

// imu.gyro[PITCH],

// imu.gyro[ROLL]);

//------------------------------------------------------

//3- Timer dt

Timer.calcdt_();

//printf("dt : %f \n",Timer.dt);

//------------------------------------------------------

//4-1 Calculate PID on attitude

#ifdef PID_STAB

for (int i=1;i<DIM;i++){

//yprSTAB[i].updateKpKi(RCinput[i+1],imu.ypr[i]);

PIDout[i] =

yprSTAB[i].update_pid_std(RCinput[i+1],

imu.ypr[i],

Timer.dt);

}

//yaw is rate PID only

PIDout[YAW] = RCinput[YAW+1];

// printf("PITCH: %7.2f %7.2f %7.2f\n",RCinput[PITCH+1],

// imu.ypr[PITCH],

// PIDout[PITCH]);

// printf("ROLL: %7.2f %7.2f %7.2f\n",RCinput[ROLL+1],

// imu.ypr[ROLL],

// PIDout[ROLL]);

for (int i=0;i<DIM;i++){

PIDout[i] =

yprRATE[i].update_pid_std(PIDout[i],

imu.gyro[i],

Timer.dt);

}

// printf("YAW: %7.2f %7.2f %7.2f\n",RCinput[YAW+1],

// imu.gyro[YAW],

// PIDout[YAW]);

// printf("PITCH: %7.2f %7.2f %7.2f\n",RCinput[PITCH+1],

// imu.gyro[PITCH],

// PIDout[PITCH]);

// printf("ROLL: %7.2f %7.2f %7.2f\n",RCinput[ROLL+1],

// imu.gyro[ROLL],

// PIDout[ROLL]);

#endif

//4-2 Calculate PID on rotational rate

#ifdef PID_RATE

for (int i=0;i<DIM;i++){

PIDout[i] =

yprRATE[i].update_pid_std(RCinput[i+1],

imu.gyro[i],

Timer.dt);

}

//printf("%7.2f %7.2f\n",imu.gyro[PITCH],Timer.PIDout[PITCH]);

#endif

// logfile << PIDout[YAW] << " " << PIDout[PITCH] << " "

// << PIDout[ROLL] << " ";

//------------------------------------------------------

//5- Send ESC update via SPI

//compute each new ESC value

//if THR is low disable PID and be sure that ESC receive Zero

//printf("%f \n",RCinput[0]);

if (RCinput[0] < 7.0) {

for (int i=0;i<4;i++){

ESC[i] = (uint16_t)0;

}

} else {

ESC[1] = (uint16_t)(RCinput[0]*10+1000

+ PIDout[ROLL] + PIDout[YAW]);

ESC[3] = (uint16_t)(RCinput[0]*10+1000

- PIDout[ROLL] + PIDout[YAW]);

ESC[0] = (uint16_t)(RCinput[0]*10+1000

+ PIDout[PITCH] - PIDout[YAW]);

ESC[2] = (uint16_t)(RCinput[0]*10+1000

- PIDout[PITCH] - PIDout[YAW]);

}

checksum = 0;

for (int iesc=0;iesc < N_SERVO; iesc++) {

ArduSPI.writeByte(ESC[iesc] & 0xff);

checksum+=ESC[iesc] & 0xff;

ArduSPI.writeByte((ESC[iesc] >> 8) & 0xff);

checksum+=(ESC[iesc] >> 8) & 0xff;

}

ArduSPI.writeByte(checksum);

//sending Proccess it

ArduSPI.writeByte('P');

// printf(" Sent : %4d %4d %4d %4d\n", ESC[0],

// ESC[1], ESC[2], ESC[3]);

//------------------------------------------------------

//6-compensate dt

Timer.compensate_();

//ouputting ESC values to logfile

logfile << ESC[0] << " " << ESC[1] << " "

<< ESC[2] << " " << ESC[3] << " " << endl;

//closing logfile

logfile.close();

pthread_mutex_unlock(&TimerMutex_);

//end of interrupt

}

2016年3月28日星期一

RX1002 搭配arduino

左手上下 : 油門 (第一通道) pin 7

左右 : YAW (第四通道) pin 4

右手上下 : PITCH (第三通道) pin 5

左右 ROLL (第二通道) pin 6

接到 arduino

sample code 如下 :

#include <PinChangeInt.h>
#define THR_PIN 7
#define ROLL_PIN 6
#define PITCH_PIN 5
#define YAW_PIN 4 // PITH
#define SERIAL_BAND 9600
unsigned int thr_start_time=0, thr_time=0;
unsigned int yaw_start_time=0, yaw_time=0;
unsigned int roll_start_time=0, roll_time=0;
unsigned int pitch_start_time=0, pitch_time=0;
void setup()
{

Serial.begin(SERIAL_BAND);
// while (!Serial) {}

Serial.println("Init done");
pinMode(THR_PIN, INPUT);
pinMode(YAW_PIN, INPUT);
pinMode(PITCH_PIN, INPUT);
pinMode(ROLL_PIN, INPUT);
//digitalWrite(UPPER_LIMIT_PIN, HIGH);
PCintPort::attachInterrupt(THR_PIN, THR_Function, CHANGE );
PCintPort::attachInterrupt(YAW_PIN, YAW_Function, CHANGE );
PCintPort::attachInterrupt(PITCH_PIN, PITCH_Function, CHANGE );
PCintPort::attachInterrupt(ROLL_PIN, ROLL_Function, CHANGE );
}

void loop()
{
while(Serial.available() > 0)
{
char c = Serial.read(); // get it
Serial.println(thr_time);
Serial.println(yaw_time);
Serial.println(pitch_time);
Serial.println(roll_time);
}
}

void THR_Function()
{
if(digitalRead(THR_PIN)==HIGH) // rising
thr_start_time = micros();
else
thr_time = micros() - thr_start_time;
}

void YAW_Function()
{
if(digitalRead(YAW_PIN)==HIGH) // rising
yaw_start_time = micros();
else
yaw_time = micros() - yaw_start_time;
}

void PITCH_Function()
{
if(digitalRead(PITCH_PIN)==HIGH) // rising
pitch_start_time = micros();
else
pitch_time = micros() - pitch_start_time;
}

void ROLL_Function()
{
if(digitalRead(ROLL_PIN)==HIGH) // rising
roll_start_time = micros();
else
roll_time = micros() - roll_start_time;
}

2016年3月22日星期二

V2 針腳

arduino 的輸出腳位....

#define FL_MOTOR_OUT_PIN 4
#define FR_MOTOR_OUT_PIN 5
#define BL_MOTOR_OUT_PIN 6
#define BR_MOTOR_OUT_PIN 7

右上的排針....

1. SCL
2. SDA
3. BL -> 黃色
4. FL -> 橘色
5. BR -> 藍色
6. FR -> 綠色

2016年3月18日星期五

cleanflight

Reference : https://github.com/cleanflight/cleanflight

2016年3月17日星期四

電子變速器校正

個別校正電變(尚未在PX4上測試)

將其中一個電變的三線陀機線連接到您搖控接收的油門通道(通常是通道3)。
打開搖控器並將油門拉到最高位。
連接鋰聚合物電池。
您將會聽到一段音響接著會聽到兩聲嗶聲。
在兩聲嗶聲後請將油門拉到最低位。
您將會聽到幾聲嗶聲(您使用幾個cell的電池就會聽到幾聲嗶聲)，最後會聽到一聲長嗶聲代表電變已經接收到最高及最低位的PWM值，同時意味著電變的校正已經完成。
斷開電池的連接，並每個電變執行上面的步驟。
如果您沒辦法正常校正電變，可能代表您必須將油門通道反向。
如果您以上面的步驟沒辦法正常校正電變(例如持續聽到嗶聲)，您可以試著將油門的最低位再往更低位微調50%。
您也可以試著在連接電池前先使用USB將APM上電開機。

Reference : http://copter.ardupilot.tw/initial-setup/esc-motor/

2016年3月16日星期三

quad-copter pi 的研究筆記 3

Q1 : 在 net.cpp 中設的 setpoint...

yaw setpoint : 0.000000
pitch setpoint : 0.100000
roll setpoint : -0.300000

為什麼不是零? (建議把 ypr_off 打開)

這會導致算PID的時候..sum 的值變很大 ....(sum += ki * error * dt) error = input - setpoint

Q2 : 量測 YPR 的初始值怪怪的..... Roll 的值會一直變大.... 這裡需要再 double-confirm

Q3 : 最後servo update 的地方...可能需要先改回來....感覺比較好抓bug...

=================================================================

今日觀察.....

YAW 的值會隨著時間的增加 ...其值一直在累加.......

我先把 setpoint 的地方改掉....改成

I highly recommend the FlameWheel 450, it flies quite well with standard PIDs but I have changed mine as follows: Angular Rate P = .145, Angular Rate I = .030 and Stabilize P = 4.0

腳位排針

1. SCL 黃色
2. SDA 橘色
3. BL -> 黃色
4. FL -> 橘色
5. BR -> 藍色
6. FR -> 綠色

[0] - GPIO4 綠色 -> FR
[1] - GPIO17 : 藍色 -> BR
[2] - GPIO18 : 黃色 -> BL
[3] - GPIO27 : 橘色 -> FL

遙控器

通道一 : TXD 0 -> GPIO14 -> WiringPi 6
通道二 : RXD 0 -> GPIO15 -> WiringPi 16
通道三 : GPIO4 -> GPIO4 -> WiringPi 4
通道四 : GPIO5 -> GPIO5 -> WiringPi 5

servo blaster :
servoval[0] =(int)(throttle - PIDoutput[ROLL] + PIDoutput[PITCH] + PIDoutput[YAW]); // FR
servoval[1] =(int)(throttle - PIDoutput[ROLL] - PIDoutput[PITCH] - PIDoutput[YAW]); // BR
servoval[2] =(int)(throttle + PIDoutput[ROLL] - PIDoutput[PITCH] + PIDoutput[YAW]); // BL
servoval[3] =(int)(throttle + PIDoutput[ROLL] + PIDoutput[PITCH] - PIDoutput[YAW]); // FL

PITCH : 正值.... 機身前面比較高..... 所以應該要把後面的轉速提高

ROLL : 正值..... 機身左側比較高...... 所以應該要把右邊的轉速提高

2016年3月15日星期二

四旋翼飞行器Quadrotor飞控之 PID调节（参考APM程序）

Reference : http://blog.csdn.net/super_mice/article/details/38436723

2016年3月12日星期六

自製PWM 的小工具

先安裝wiringPi

使用 GPIO 4 , GPIO 17, GPIO 18, GPIO24 (not pin number..)

使用四條杜邦線....從上面四個 GPIO .... 聯到你要量的四個點....

然後執行

g++ -Wall -o PWM pwm.cpp -lwiringPi

sudo ./PWM 即可

pwm.cpp 程式碼如下:

#include <math.h>
#include <wiringPi.h>
#include <stdio.h>
#include <stdlib.h>
#include <wiringPiI2C.h>
#include <string.h>

int PWM_PIN0 = 7; // GPIO 4
int PWM_PIN1 = 0; // GPIO 17
int PWM_PIN2 = 1; // GPIO 18
int PWM_PIN3 = 5; // GPIO 24

void measure(int PWM_PIN);

PI_THREAD (myThread0)
{
for (;;)
{
measure(PWM_PIN0);
delay (500) ;
}
}

PI_THREAD (myThread1)
{
for (;;)
{
measure(PWM_PIN1);
delay (500) ;
}
}

PI_THREAD (myThread2)

{

for (;;)

{

measure(PWM_PIN2);

delay (500) ;

}

PI_THREAD (myThread3)

{

for (;;)

{

measure(PWM_PIN3);

delay (500) ;

}

int main (void)
{
int x;
wiringPiSetup () ;
x = piThreadCreate (myThread0) ;
if (x != 0)
printf ("it didn't start\n");

x = piThreadCreate (myThread1) ;
if (x != 0)
printf ("it didn't start\n");

x = piThreadCreate (myThread2) ;
if (x != 0)
printf ("it didn't start\n");

x = piThreadCreate (myThread3) ;
if (x != 0)
printf ("it didn't start\n");

for (;;)
{
delay (500) ;
}
return 0 ;
}

void measure(int PWM_PIN)
{
unsigned int start,mid,end,temp,diff;
while(digitalRead(PWM_PIN)==HIGH)
{

}
while(digitalRead(PWM_PIN)==LOW)
{
}

start = micros();

while(digitalRead(PWM_PIN)==HIGH)
{
}

mid = micros();

while(digitalRead(PWM_PIN)==LOW)
{
}

end = micros();

diff = mid - start;
temp = (diff * 100) / (end-start);
printf("pin : %d, pulse : %d us, PWM : %d, Freq : %d \n",PWM_PIN,diff,temp,1000000/(end-start));
}

2016年3月11日星期五

Pixhawk 源碼解析：

Pixhawk 源碼解析：
http://goo.gl/PyU01A

cc3d servo 的連線

servo

channel 1 -> FL

channel 2 -> FR

channel 3 -> BR

channel 4 -> BL

2016年3月10日星期四

servoBlaster 研究筆記

他有兩個版本

1. kernel space 版本

2. user space 版本

kernel 版本 compile 起來有問題....然後作者也推薦使用 user space 版本

原文如下:

There are two implementations of ServoBlaster; a kernel module based one, and a
user space daemon.  The kernel module based one is the original, but is more of
a pain to build because you need a matching kernel build.  The user space
daemon implementation is much more convenient to use and now has rather more
features than the kernel based one.  I would strongly recommend you use the
user space implementation.

進到 user folder....

編譯指令:

sudo make

或是

sudo make install

其實兩個只差在如果你打 install... 就是開機的時候就會去load 去執行

執行指令:

$ sudo ./servod

結果如下:

Board revision:                  1
Using hardware:                PWM
Using DMA channel:              14
Idle timeout:             Disabled
Number of servos:                8
Servo cycle time:            20000us
Pulse increment step size:      10us
Minimum width value:            50 (500us)
Maximum width value:           250 (2500us)
Output levels:              Normal

Using P1 pins:               7,11,12,13,15,16,18,22

Servo mapping:
     0 on P1-7           GPIO-4
     1 on P1-11          GPIO-17
     2 on P1-12          GPIO-18
     3 on P1-13          GPIO-27
     4 on P1-15          GPIO-22
     5 on P1-16          GPIO-23
     6 on P1-18          GPIO-24
     7 on P1-22          GPIO-25

它總共有八個PWM 輸出....

If you want
to stop servod, the easiest way is to run:

$ sudo killall servod

The
default option is the equivalent of specifying

   --p1pins=7,11,12,13,15,16,18,22

可以修改的地方

Q1 : 如果只要四個輸出...要怎改?

A1 :
修改 servod.c

把static char *default_p1_pins = "7,11,12,13,15,16,18,22";

改成static char *default_p1_pins = "7,11,12,13";

就可以了

Q2 : 如何改pulse width 的最小值（1000us）...最大值(2000us)

A2 :

#define DEFAULT_SERVO_MIN_US 500
#define DEFAULT_SERVO_MAX_US 2500

改成

單位是 1 us

#define DEFAULT_SERVO_MIN_US 1000

#define DEFAULT_SERVO_MAX_US 2000

Q3 : 如何更改 cycle time, 改成 490Hz -> (1000000/490 = 2040):

#define DEFAULT_CYCLE_TIME_US 20000

改成#define DEFAULT_CYCLE_TIME_US 2040

Q4 : 如何更改 step_size , 從 10us 改成 2us...

#define DEFAULT_STEP_TIME_US 10

改成#define DEFAULT_STEP_TIME_US 2

Q5 : 如何動態更改pulse寬度

指定us 的方式

echo P1-7=1500us > /dev/servoblaster

指定 step 的方式

echo P1-7=150 > /dev/servoblaster # Using P1 pin number rather

Reference : https://github.com/richardghirst/PiBits/tree/master/ServoBlaster

mini osd update firmware

用FTDI232 和 mini osd 連接....新版的tool 要接五條線

燒入工具的路徑 https://code.google.com/archive/p/minimosd-extra/downloads

*.hex 是 driver 的16進制的輸出檔

Reference : http://www.rcgroups.com/forums/showthread.php?t=2026936

2016年3月8日星期二

quadcopter v2 的研究筆記

先來看arduino 部分

//For ease of programming
	#define THR 0
	#define YAW 1
	#define PITCH 2
	#define ROLL 3

	// Assign your channel in pins
	#define THROTTLE_IN_PIN 8
	#define YAW_IN_PIN 11
	#define PITCH_IN_PIN 10
	#define ROLL_IN_PIN 9

// Assign your channel out pins
	#define FL_MOTOR_OUT_PIN 4
	#define FR_MOTOR_OUT_PIN 5
	#define BL_MOTOR_OUT_PIN 6
	#define BR_MOTOR_OUT_PIN 7

ESC 接線
FL -> PIN 4
FR -> PIN 5
BL -> PIN 6
BR -> PIN 7

RC 接收器接線

THROTTLE -> PIN 8
YAW -> PIN 11
PITCH -> PIN 10
ROLL -> PIN 9

// 宣告五個 interrupt...... 四個rc input 和一個 spi...

PCintPort::attachInterrupt(THROTTLE_IN_PIN, calcThrottle,CHANGE);

PCintPort::attachInterrupt(YAW_IN_PIN, calcYaw,CHANGE);

PCintPort::attachInterrupt(PITCH_IN_PIN, calcPitch,CHANGE);

PCintPort::attachInterrupt(ROLL_IN_PIN, calcRoll,CHANGE);

後面的CHANGE 是代表rising edge 的時候才會觸發.....

// if the pin is high, its a rising edge of the signal pulse, so lets record its value
 if(digitalRead(THROTTLE_IN_PIN) == HIGH)

{

ulThrottleStart = micros();

}

else

{

 // else it must be a falling edge, so lets get the time and subtract the time of the rising edge

// this gives use the time between the rising and falling edges i.e. the pulse duration.

unThrottleInShared = (uint16_t)(micros() - ulThrottleStart);

}

unThrottleInShared 就是代表為level 1 的時間....單位為us(10^-6)

void setup()
	{
	Serial.begin(9600); //for debugging...

	pinMode(LED_PIN, OUTPUT);

	/*
	PWM measurement settings
	*/

	// using the PinChangeInt library, attach the interrupts
	// used to read the channels // 宣告成 interrupt
	PCintPort::attachInterrupt(THROTTLE_IN_PIN, calcThrottle,CHANGE);
	PCintPort::attachInterrupt(YAW_IN_PIN, calcYaw,CHANGE);
	PCintPort::attachInterrupt(PITCH_IN_PIN, calcPitch,CHANGE);
	PCintPort::attachInterrupt(ROLL_IN_PIN, calcRoll,CHANGE);

	// attach servo objects, these will generate the correct
	// pulses for driving Electronic speed controllers, servos or other devices
	// designed to interface directly with RC Receivers
	MOTOR[0].attach(FL_MOTOR_OUT_PIN);
	MOTOR[1].attach(FR_MOTOR_OUT_PIN);
	MOTOR[2].attach(BL_MOTOR_OUT_PIN);
	MOTOR[3].attach(BR_MOTOR_OUT_PIN);

	//Set servo values to min
	for (int i=0;i<SERVO_NUM;i++)
	{
	MOTOR[i].writeMicroseconds(RC_MIN);
	}

	/*
	SPI settings
	*/

	// Declare MISO as output : have to send on
	//master in, slave out
	pinMode(MISO, OUTPUT);

	// turn on SPI in slave mode
	SPCR \|= _BV(SPE);

	// now turn on interrupts
	SPI.attachInterrupt();


	}

void loop()
	{
	//Constantly update rc_data
	rc_data[THR].u16 = unThrottleInShared;
	rc_data[YAW].u16 = unYawInShared;
	rc_data[PITCH].u16 = unPitchInShared;
	rc_data[ROLL].u16 = unRollInShared;

	if (unThrottleInShared < 1000 &
	unYawInShared < 1200 &
	unPitchInShared < 1200 &
	unRollInShared > 1200 ) {

	uint32_t t_old = millis();
	while (millis()-t_old < 500 ){
	//wait to be sure that sticks are still in position
	}

	// if so change current status
	if (unThrottleInShared < 1000 &
	unYawInShared < 1200 &
	unPitchInShared < 1200 &
	unRollInShared > 1200 ) {

	start = !start;
	//change LED status
	digitalWrite(13, start);
	}
	}

	//Update servo (ESC) if necessary and started
	if (update_servo & start){
	uint8_t ipos=0;
	byte checksum2=0;
	for (int i=0;i<SERVO_NUM;i++)
	{
	//process buffer values
	for (int ibyte = 0;ibyte<2;ibyte++){
	esc_data[i].u8[ibyte] = rx_buf[ipos];
	checksum2+=rx_buf[ipos];
	ipos++;
	}
	}

	if (rx_buf[ipos] == checksum2) {
	//write ESC output
	for (int i=0;i<SERVO_NUM;i++)
	MOTOR[i].writeMicroseconds(esc_data[i].u16);
	}

	update_servo = false;

	} else if (!start) {
	for (int i=0;i<SERVO_NUM;i++)
	{
	MOTOR[i].writeMicroseconds(RC_MIN);
	}
	}

	}

void calcThrottle()
	{
	// if the pin is high, its a rising edge of the signal pulse, so lets record its value
	if(digitalRead(THROTTLE_IN_PIN) == HIGH)
	{
	ulThrottleStart = micros();
	}
	else
	{
	// else it must be a falling edge, so lets get the time and subtract the time of the rising edge
	// this gives use the time between the rising and falling edges i.e. the pulse duration.
	unThrottleInShared = (uint16_t)(micros() - ulThrottleStart);

	}
	}

/*-----------------------
	SPI interrupt routine
	------------------------*/
	ISR (SPI_STC_vect)
	{

	//grab a new command and process it
	byte cmd = SPDR;

	if (cmd == 'S') {
	//STOP do nothing : end of sending RC data
	pos=0;
	return;
	}

	if (cmd == 'P') {
	//process it !
	update_servo = true;
	pos=0;
	return;
	}

	if (cmd == 'C') {
	//push Cheksum into the register
	SPDR = checksum;
	checksum = 0;
	pos=0;
	return;
	}


	//push data into a byte buffer
	rx_buf[pos++] = cmd;

	// 10-11 -> send 2bytes channel 1 value
	// ...
	// 40-41 -> send 2bytes channel 4 value
	// ...
	// 60-61 -> send 2bytes channel 6 value
	switch (cmd){
	case 10:
	SPDR = rc_data[THR].u8[0];
	checksum += rc_data[THR].u8[0];
	break;

	case 11:
	SPDR = rc_data[THR].u8[1];
	checksum += rc_data[THR].u8[1];
	break;

	case 20:
	SPDR = rc_data[YAW].u8[0];
	checksum += rc_data[YAW].u8[0];
	break;

	case 21:
	SPDR = rc_data[YAW].u8[1];
	checksum += rc_data[YAW].u8[1];
	break;

	case 30:
	SPDR = rc_data[PITCH].u8[0];
	checksum += rc_data[PITCH].u8[0];
	break;

	case 31:
	SPDR = rc_data[PITCH].u8[1];
	checksum += rc_data[PITCH].u8[1];
	break;

	case 40:
	SPDR = rc_data[ROLL].u8[0];
	checksum += rc_data[ROLL].u8[0];
	break;

	case 41:
	SPDR = rc_data[ROLL].u8[1];
	checksum += rc_data[ROLL].u8[1];
	break;
	}

	}

再來看 RPI 的部分

void TimerClass::sig_handler_(int signum)
	{
	pthread_mutex_lock(&TimerMutex_);

	//output to a log file
	//open log file
	fstream logfile;
	logfile.open("quadpilot.log", ios::out\|ios::app);
	if (logfile.fail()) // Check for file creation and return error.
	{
	cout << "Error opening output.\n";
	}


	float RCinput[N_RC_CHAN],PIDout[3];
	uint16_t ESC[N_SERVO];

	//------------------------------------------------------
	//1-Get Remote values using SPI
	union bytes{
	uint8_t u8[2];
	uint16_t u16;
	} rc_union;
	uint8_t checksum=0,recv_checksum=1;

	while (checksum != recv_checksum) {

	checksum=0;

	for (int i=0;i<4;i++){
	ArduSPI.writeByte((uint8_t) (i+1)*10);
	rc_union.u8[0] = ArduSPI.rwByte((uint8_t) (i+1)*10+1);
	rc_union.u8[1] = ArduSPI.rwByte('S');
	//transaction ended
	RCinput[i] = (float) rc_union.u16;

	checksum+=rc_union.u8[0];
	checksum+=rc_union.u8[1];
	}

	//Control checksum
	ArduSPI.writeByte('C');
	recv_checksum = ArduSPI.rwByte('S');
	}

	//Bounding RC values to avoid division by zeros fex.
	for (int i=1;i<4;i++){
	if ( RCinput[i] > RC_MAX) RCinput[i] = RC_MAX;
	if ( RCinput[i] < RC_MIN) RCinput[i] = RC_MIN;
	}

	//outputing values to logfile
	logfile << RCinput[0] << " " << RCinput[1] << " "
	<< RCinput[2] << " " << RCinput[3] << " ";


	// //convert into PID usable values
	RCinput[0] = (RCinput[0] - THR_MIN)/(THR_MAX-THR_MIN) * 100.0;
	RCinput[1] = -(RCinput[1] -(RC_MAX+RC_MIN)/2.) /
	(RC_MAX-RC_MIN) * K_YAW;
	RCinput[2] = (RCinput[2] -(RC_MAX+RC_MIN)/2.)/
	(RC_MAX-RC_MIN) * K_PITCH;
	RCinput[3] = (RCinput[3] -(RC_MAX+RC_MIN)/2.)/
	(RC_MAX-RC_MIN) * K_ROLL;

	//outputing values to logfile
	logfile << RCinput[0] << " " << RCinput[1] << " "
	<< RCinput[2] << " " << RCinput[3] << " ";


	#ifdef XMODE
	//Switch to Xmode instead of +mode
	//orders are given in a ref frame rotated by 90deg.
	float cs45 = sqrt(2.)/2.;
	float Px = RCinput[2]cs45 + RCinput[3]cs45;
	float Rx = - RCinput[2]cs45 + RCinput[3]cs45;

	RCinput[2] = Px;
	RCinput[3] = Rx;

	#endif

	// printf("Received : %6.3f %6.3f %6.3f %6.3f\n", RCinput[0],
	// RCinput[1], RCinput[2], RCinput[3]);

	//------------------------------------------------------
	//2- Get attitude of the drone
	while (imu.getAttitude() < 0){
	};


	//Compensate lost of Thrust due to angle of drone
	RCinput[0] = RCinput[0]/cos(imu.ypr[ROLL]/180*M_PI)
	/cos(imu.ypr[PITCH]/180*M_PI);

	//output to logfile
	// logfile << imu.ypr[YAW] << " " << imu.ypr[PITCH] << " "
	// << imu.ypr[ROLL] << " "
	// << imu.gyro[YAW] << " " << imu.gyro[PITCH] << " "
	// << imu.gyro[ROLL] << " ";



	// printf("ATTITUDE: %7.2f %7.2f %7.2f\n",imu.ypr[YAW],
	// imu.ypr[PITCH],
	// imu.ypr[ROLL]);

	// printf(" %7.2f %7.2f %7.2f\n",imu.gyro[YAW],
	// imu.gyro[PITCH],
	// imu.gyro[ROLL]);


	//------------------------------------------------------
	//3- Timer dt
	Timer.calcdt_();
	//printf("dt : %f \n",Timer.dt);

	//------------------------------------------------------
	//4-1 Calculate PID on attitude
	#ifdef PID_STAB

	for (int i=1;i<DIM;i++){

	//yprSTAB[i].updateKpKi(RCinput[i+1],imu.ypr[i]);

	PIDout[i] =
	yprSTAB[i].update_pid_std(RCinput[i+1],
	imu.ypr[i],
	Timer.dt);
	}

	//yaw is rate PID only
	PIDout[YAW] = RCinput[YAW+1];

	// printf("PITCH: %7.2f %7.2f %7.2f\n",RCinput[PITCH+1],
	// imu.ypr[PITCH],
	// PIDout[PITCH]);

	// printf("ROLL: %7.2f %7.2f %7.2f\n",RCinput[ROLL+1],
	// imu.ypr[ROLL],
	// PIDout[ROLL]);

	for (int i=0;i<DIM;i++){
	PIDout[i] =
	yprRATE[i].update_pid_std(PIDout[i],
	imu.gyro[i],
	Timer.dt);
	}

	// printf("YAW: %7.2f %7.2f %7.2f\n",RCinput[YAW+1],
	// imu.gyro[YAW],
	// PIDout[YAW]);

	// printf("PITCH: %7.2f %7.2f %7.2f\n",RCinput[PITCH+1],
	// imu.gyro[PITCH],
	// PIDout[PITCH]);

	// printf("ROLL: %7.2f %7.2f %7.2f\n",RCinput[ROLL+1],
	// imu.gyro[ROLL],
	// PIDout[ROLL]);


	#endif

	//4-2 Calculate PID on rotational rate
	#ifdef PID_RATE
	for (int i=0;i<DIM;i++){
	PIDout[i] =
	yprRATE[i].update_pid_std(RCinput[i+1],
	imu.gyro[i],
	Timer.dt);
	}
	//printf("%7.2f %7.2f\n",imu.gyro[PITCH],Timer.PIDout[PITCH]);
	#endif

	// logfile << PIDout[YAW] << " " << PIDout[PITCH] << " "
	// << PIDout[ROLL] << " ";


	//------------------------------------------------------
	//5- Send ESC update via SPI
	//compute each new ESC value

	//if THR is low disable PID and be sure that ESC receive Zero
	//printf("%f \n",RCinput[0]);

	if (RCinput[0] < 7.0) {
	for (int i=0;i<4;i++){
	ESC[i] = (uint16_t)0;
	}
	} else {

	ESC[1] = (uint16_t)(RCinput[0]*10+1000
	+ PIDout[ROLL] + PIDout[YAW]);
	ESC[3] = (uint16_t)(RCinput[0]*10+1000
	- PIDout[ROLL] + PIDout[YAW]);
	ESC[0] = (uint16_t)(RCinput[0]*10+1000
	+ PIDout[PITCH] - PIDout[YAW]);
	ESC[2] = (uint16_t)(RCinput[0]*10+1000
	- PIDout[PITCH] - PIDout[YAW]);
	}

	checksum = 0;
	for (int iesc=0;iesc < N_SERVO; iesc++) {
	ArduSPI.writeByte(ESC[iesc] & 0xff);
	checksum+=ESC[iesc] & 0xff;
	ArduSPI.writeByte((ESC[iesc] >> 8) & 0xff);
	checksum+=(ESC[iesc] >> 8) & 0xff;
	}
	ArduSPI.writeByte(checksum);
	//sending Proccess it
	ArduSPI.writeByte('P');

	// printf(" Sent : %4d %4d %4d %4d\n", ESC[0],
	// ESC[1], ESC[2], ESC[3]);

	//------------------------------------------------------
	//6-compensate dt
	Timer.compensate_();

	//ouputting ESC values to logfile
	logfile << ESC[0] << " " << ESC[1] << " "
	<< ESC[2] << " " << ESC[3] << " " << endl;

	//closing logfile
	logfile.close();

	pthread_mutex_unlock(&TimerMutex_);
	//end of interrupt

	}

Ref : https://github.com/vjaunet/QUADCOPTER_V2

訂閱：文章 (Atom)

2016年3月29日 星期二

2016年3月28日 星期一

2016年3月22日 星期二

2016年3月18日 星期五

2016年3月17日 星期四

個別校正電變(尚未在PX4上測試)

2016年3月16日 星期三

2016年3月15日 星期二

2016年3月12日 星期六

2016年3月11日 星期五

2016年3月10日 星期四

2016年3月8日 星期二

2016年3月29日星期二

2016年3月28日星期一

2016年3月22日星期二

2016年3月18日星期五

2016年3月17日星期四

2016年3月16日星期三

2016年3月15日星期二

2016年3月12日星期六

2016年3月11日星期五

2016年3月10日星期四

2016年3月8日星期二