PID 的研究筆記 : Picopter

MS5611  -> 氣壓感測計

HMC5883L ->  電子羅盤

void PIDClass::initialise(float KP, float KI, float KD, float ILIM, float LIM, int DFILLEN, double* ALT_DERIV_SOURCE) { kp = KP;  ki = KI; kd = KD; ilim = ILIM; lim = LIM;
dFilLen = DFILLEN; altDerivativeSource = ALT_DERIV_SOURCE; }

設初始值

void PIDClass::setPID(float KP, float KI, float KD) { kp = KP; ki = KI; kd = KD;	}

卡上下限

inline void PIDClass::constrain_(float* value, float range) { if(*value > range) *value = range; else if(*value < -range) *value = -range; }

核心function:  output = error * kp + integral * ki + derivative*kd; (PID)  error = *setpoint - *position; (P值) integral += error * *dt;  (積分) void PIDClass::calculate(double* position, float* setpoint, float* dt) {    prevError = error;    error = (*setpoint) - (*position);    integral += error * (*dt);    if(altDerivativeSource == NULL) {         //Derivative low pass filter         //Store current derivative value into history table         dHist[dFilK] = (error - prevError) / *dt;         dFilK++;         if(dFilK == dFilLen) {               dFilK = 0;         }        //Average history table       derivative = 0;       for(int k = 0; k < dFilLen; k++) {              derivative += dHist[k];       }       derivative /= dFilLen;      }else{         derivative = -*altDerivativeSource;      }     //Anti-windup      constrain_(&integral, ilim);      output = error * kp + integral * ki + derivative*kd;     //Anti-saturation     constrain_(&output, lim); } 電子羅盤: 重點就是   :getField()....  得到         rawData->mag_x ,  rawData->mag_y,  rawData->mag_z void HMC5883LClass::getField(s_rawData* rawData){ uint8_t buf[6]; I2CInterface.readRegister(HMC5883L_ADDRESS, HMC5883L_RA_X_H, buf, 6); rawData->mag_x = static_cast<int16_t>((buf[0] << 8) | buf[1]); rawData->mag_z = static_cast<int16_t>((buf[2] << 8) | buf[3]); rawData->mag_y = static_cast<int16_t>((buf[4] << 8) | buf[5]);} void HMC5883LClass::initialise(){     checkCommunication_();     setConfigA_(0b00011000); //No averaging, 75Hz update rate, no bias     setConfigB_(0b00100000); //+-1.3 gauss range, 1090 LSB/gauss setMode_(0); //Continuous             measurement mode} bool HMC5883LClass::setConfigA_(uint8_t value){ return I2CInterface.writeRegister(HMC5883L_ADDRESS, HMC5883L_RA_CONFIG_A, &value, 1);} bool HMC5883LClass::setConfigB_(uint8_t value){   return I2CInterface.writeRegister(HMC5883L_ADDRESS, HMC5883L_RA_CONFIG_B, &value, 1);} bool HMC5883LClass::setMode_(uint8_t value){   return I2CInterface.writeRegister(HMC5883L_ADDRESS, HMC5883L_RA_MODE, &value, 1);} bool HMC5883LClass::checkCommunication_() { uint8_t buf[3]; I2CInterface.readRegister(HMC5883L_ADDRESS, HMC5883L_RA_ID_A, buf, 3); if (buf[0] != 'H' | buf[1] != '4' | buf[2] != '3') { std::cout << "HMC5883L communication failed, recieved " << buf[0] << ", " << buf[1] << ", " << buf[2] << std::endl; return false; } return true; } 接下來看主程式 ->  main.cpp HCM5883L 是電子羅盤.... ==================================== int main(int argc, char** argv){  MPU6050.initialise();   HMC5883L.initialise(); Timer.start(); CLI.open();   while (1) { sleep(1000); }}

void TimerClass::start() {    timeValue_.tv_sec = 0;    timeValue_.tv_nsec = PERIOD;    timeToSet_.it_value = timeValue_;    timer_settime(timerId, 0, &timeToSet_, NULL);    started = true; } TimerClass::TimerClass(){ /* Intialize sigaction struct */        signalAction.sa_handler = &sig_handler_; /* Connect a signal handler routine to the SIGALRM signal */        sigaction(SIGALRM, &signalAction, NULL); /* Allocate a timer */       timer_create(CLOCK_REALTIME, NULL, &timerId);         started = false;} void TimerClass::sig_handler_(int signum){     pthread_mutex_lock(&TimerMutex_);     PICInterface.getRX();     Timer.calcdt_();     AHRS.update();     Control.update();     LogMan.update();     Timer.compensate_();      pthread_mutex_unlock(&TimerMutex_); } 每次都timer 啟動時...會執行上述的function 1.     pthread_mutex_lock(&TimerMutex_);  打開  mutex  ->  不讓中斷切走 2.    PICInterface.getRX(); 3.    Timer.calcdt_(); 4.    AHRS.update(); 5.   Control.update(); 6.  LogMan.update(); 7.    Timer.compensate_(); 8.       pthread_mutex_unlock(&TimerMutex_);   , 關閉 mutex...讓中斷可以啟動 void PICInterfaceClass::getRX() {     uint8_t widthsChar[12] = {0}; //Appears to generate read errors above ~5 bytes, returning only 1's // I2CInterface.readRegister(PIC_ADDRESS, REG_RX1H, widthsChar, sizeof(widthsChar));    I2CInterface.readRegister(PIC_ADDRESS, REG_RX1H, &widthsChar[0], 2);    I2CInterface.readRegister(PIC_ADDRESS, REG_RX2H, &widthsChar[2], 2);    I2CInterface.readRegister(PIC_ADDRESS, REG_RX3H, &widthsChar[4], 2);    I2CInterface.readRegister(PIC_ADDRESS, REG_RX4H, &widthsChar[6], 2);    I2CInterface.readRegister(PIC_ADDRESS, REG_RX5H, &widthsChar[8], 2);    I2CInterface.readRegister(PIC_ADDRESS, REG_RX6H, &widthsChar[10], 2);    rxWidths.roll = make16_(widthsChar[0], widthsChar[1]);    rxWidths.pitch = make16_(widthsChar[2], widthsChar[3]);    rxWidths.throttle = make16_(widthsChar[4], widthsChar[5]);    rxWidths.yaw = make16_(widthsChar[6], widthsChar[7]);    rxWidths.sw1 = make16_(widthsChar[8], widthsChar[9]);    rxWidths.sw2 = make16_(widthsChar[10], widthsChar[11]);    static int i = 0;       rxWidthsHist[i] = rxWidths;    i++;     if(i == FILTER_LEN) {        i = 0;     }     rxWidths = averageRX_(rxWidthsHist, FILTER_LEN, i);     calibrateRX_(); } inline void PICInterfaceClass::calibrateRX_() {     rx.pitchDem = PITCH_RANGE * (static_cast<float> (rxWidths.pitch - ((RX_MAX - RX_MIN) / 2) - RX_MIN) / (RX_MAX - RX_MIN));    rx.rollDem = ROLL_RANGE * (static_cast<float> (rxWidths.roll - ((RX_MAX - RX_MIN) / 2) - RX_MIN) / (RX_MAX - RX_MIN));    rx.yawRateDem = YAW_RATE_RANGE * (static_cast<float> (rxWidths.yaw - ((RX_MAX - RX_MIN) / 2) - RX_MIN) / (RX_MAX - RX_MIN));    rx.throttleDem = static_cast<float> (rxWidths.throttle - RX_MIN) / (RX_MAX - RX_MIN);    rx.sw1 = (rxWidths.sw1 > 15000);    rx.sw2 = (rxWidths.sw2 > 15000);    rx.pitchRateDem = PITCH_RATE_RANGE * (static_cast<float> (rxWidths.pitch - ((RX_MAX - RX_MIN) / 2) - RX_MIN) / (RX_MAX - RX_MIN));;    rx.rollRateDem = ROLL_RATE_RANGE * (static_cast<float> (rxWidths.roll - ((RX_MAX - RX_MIN) / 2) - RX_MIN) / (RX_MAX - RX_MIN));    rx.yawRateDem = -rx.yawRateDem; } 3. ///  計算時間的間隔 inline void TimerClass::calcdt_(){     oldtime_ = time_;       clock_gettime(CLOCK_MONOTONIC, &time_);       Timer.dt = ((static_cast<int64_t>(time_.tv_sec) * 1000000000 + static_cast<int64_t>(time_.tv_nsec)) - (static_cast<int64_t>(oldtime_.tv_sec) * 1000000000 + static_cast<int64_t>(oldtime_.tv_nsec))) / 1000000000.0;    } 4. void AHRSClass::update() {     getSensors_();     calibrateData_();     temperatureCompensate_();    fuse_(); } void AHRSClass::getSensors_() {    MPU6050.getSensors(&rawData_);    HMC5883L.getField(&rawData_);    MS5611.getPressure(&rawData_.pressure);   } bool MPU6050Class::getSensors(s_rawData* rawData) { uint8_t buf[14]; I2CInterface.readRegister(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, buf, 14); rawData->x = static_cast<int16_t>((buf[0]<<8)|buf[1]); rawData->y = static_cast<int16_t>((buf[2]<<8)|buf[3]); rawData->z = static_cast<int16_t>((buf[4]<<8)|buf[5]); rawData->temp = static_cast<int16_t>((buf[6]<<8)|buf[7]); rawData->p = static_cast<int16_t>((buf[8]<<8)|buf[9]); rawData->q = static_cast<int16_t>((buf[10]<<8)|buf[11]); rawData->r = static_cast<int16_t>((buf[12]<<8)|buf[13]); } void HMC5883LClass::getField(s_rawData* rawData) { uint8_t buf[6]; I2CInterface.readRegister(HMC5883L_ADDRESS, HMC5883L_RA_X_H, buf, 6); rawData->mag_x = static_cast<int16_t>((buf[0] << 8) | buf[1]); rawData->mag_z = static_cast<int16_t>((buf[2] << 8) | buf[3]); rawData->mag_y = static_cast<int16_t>((buf[4] << 8) | buf[5]); } void MS5611Class::getPressure(int32_t *pressure) {     static int i = 0;     if(i == 3)     { //Pressure is only updated every 4 cycles to keep conversion rate at 100Hz            if(lastConv_ == Pressure) {   //  兩個會change                getRawPressure_();                startTempConversion_();                calculatePressure_(pressure);                lastConv_ = Temperature;            } else if(lastConv_ == Temperature) {               getRawTemperature_();               startPressureConversion_();               *pressure = P_;               lastConv_ = Pressure;           }           i = 0;      } else {  // i= 0, 1 ,2        i++;       *pressure = P_;      } } //  g = 9.81 void AHRSClass::calibrateData_() {      calibratedData.x = (rawData_.x * (9.81 / 4096.0));      calibratedData.y = (rawData_.y * (9.81 / 4096.0));      calibratedData.z = (rawData_.z * (9.81 / 4096.0));      calibratedData.temp = (rawData_.temp + 12412) / 340.0;      calibratedData.p = (rawData_.p / 65.5);      calibratedData.q = (rawData_.q / 65.5);     calibratedData.r = (rawData_.r / 65.5);     calibratedData.magx = rawData_.mag_x / 1090.0;     calibratedData.magy = rawData_.mag_y / 1090.0;     calibratedData.magz = rawData_.mag_z / 1090.0;     calibratedData.pressure = rawData_.pressure; //Pascals     calibratedData.altitude = ((-8.31447 * 288.15) / (9.80665 * 0.0289644)) * log(calibratedData.pressure / 101325);     calibratedData.q = -calibratedData.q; //Accelerometer scale and bias correction static double acceltemp[3]; acceltemp[0] = calibratedData.x - accelZeroX; acceltemp[1] = calibratedData.y - accelZeroY; acceltemp[2] = calibratedData.z - accelZeroZ; calibratedData.x = accelEllipsoid00_ * acceltemp[0] + accelEllipsoid01_ * acceltemp[1] + accelEllipsoid02_ * acceltemp[2]; calibratedData.y = accelEllipsoid11_ * acceltemp[1] + accelEllipsoid12_ * acceltemp[2]; calibratedData.z = accelEllipsoid22_ * acceltemp[2]; //Magnetometer scale and bias correction static double magtemp[3]; magtemp[0] = calibratedData.magx - magZeroX; magtemp[1] = calibratedData.magy - magZeroY; magtemp[2] = calibratedData.magz - magZeroZ; calibratedData.magx = magEllipsoid00_ * magtemp[0] + magEllipsoid01_ * magtemp[1] + magEllipsoid02_ * magtemp[2]; calibratedData.magy = magEllipsoid11_ * magtemp[1] + magEllipsoid12_ * magtemp[2]; calibratedData.magz = magEllipsoid22_ * magtemp[2]; //Altitude LPF #define LENGTH 20 static int i = 0; static double mvAvg[LENGTH] = {0}; mvAvg[i] = calibratedData.altitude; calibratedData.altitude = 0; for(int k = 0; k < LENGTH; k++) { calibratedData.altitude += mvAvg[k]; } calibratedData.altitude /= LENGTH; i++; if(i == LENGTH) { i = 0; } //End Altitude LPF } //Values calculated from matlab script MgnCalibration const double accelZeroX = 0.2238; const double accelZeroY = 0.1543; const double accelZeroZ = -0.3633; const double accelEllipsoid00_ = 0.1007; const double accelEllipsoid01_ = -0.0007; const double accelEllipsoid02_ = 0.0002; const double accelEllipsoid11_ = 0.1020; const double accelEllipsoid12_ = 0.0005; const double accelEllipsoid22_ = 0.1003; //Values calculated from matlab script MgnCalibration const double magZeroX = 0.0576; const double magZeroY = -0.0929; const double magZeroZ = -0.0092; const double magEllipsoid00_ = 1.8925; const double magEllipsoid01_ = 0.0399; const double magEllipsoid02_ = 0.0132; const double magEllipsoid11_ = 1.8375; const double magEllipsoid12_ = 0.0474; const double magEllipsoid22_ = 2.1528; void AHRSClass::temperatureCompensate_() {    static double tempPow1 = calibratedData.temp;    static double tempPow2 = pow(calibratedData.temp, 2);    static double tempPow3 = pow(calibratedData.temp, 3);    static double tempPow4 = pow(calibratedData.temp, 4);       //Coefficients calculated from freezetest4, 4th degree polynomial     calibratedData.p -= 8.4877e-9 * tempPow4 + 6.4219e-6 * tempPow3 + 2.5782e-4 * tempPow2 -      0.0041145 * tempPow1 - 1.2974;     calibratedData.q -= 5.863e-8 * tempPow4 - 5.9746e-6 * tempPow3 + 5.1324e-5 * tempPow2 + 0.0079355 * tempPow1 + 0.59859;    calibratedData.r -= 4.4929e-8 * tempPow4 - 1.6137e-7 * tempPow3 + 4.8876e-5 * tempPow2 + 0.021246 * tempPow1 - 2.9723; //calibratedData.x -= -2.8664e-6 * tempPow2 + 4.9565e-4 * tempPow1; - 0.0011611; //calibratedData.y -= 1.2728e-6 * tempPow2 + 6.5989e-6 * tempPow1; + 0.025702; //calibratedData.z -= 1.6966e-5 * tempPow2 - 0.0035421 * tempPow1; + 0.056; //Z axis accel shows       huge temperature drift (15% over 40 degrees) } void AHRSClass::fuse_() {      if(Timer.dt < 0.03)      {         quaternion = EKF.predict(&calibratedData, Timer.dt);      }       quaternion = EKF.update(&calibratedData, Timer.dt);       quaternionToYPR_(&quaternion, &orientation); } void AHRSClass::quaternionToYPR_(QuaternionClass* q, s_euler* orientation) { orientation->pitch = -(180/pi) * atan2(2*(q->w*q->x + q->y*q->z), 1 - 2*(pow(q->x,2)+pow(q->y,2))); orientation->roll = (180/pi) * asin(2*(q->w*q->y - q->z*q->x)); orientation->yaw = (180/pi) * atan2(2*(q->w*q->z + q->x*q->y), 1 - 2*(pow(q->y,2)+pow(q->z,2))); } 5.   Control.update(); void ControlClass::update() {        if(motorTesting_ == false) {               if(PICInterface.rx.sw2 == false) {//in rate mode                                         rateControl_(&PICInterface.rx.pitchRateDem, &PICInterface.rx.rollRateDem,                                                &PICInterface.rx.yawRateDem); }               else if(PICInterface.rx.sw2 == true) { //in attitude mode                       attitudeControl_(&PICInterface.rx.pitchDem, &PICInterface.rx.rollDem,                       &PICInterface.rx.yawRateDem);               }          } else          {            incrementMotorTest_();          } // evaluateAltitudeControl_(); //Checks to see if altitude control if required, and performs             //if necessary } void ControlClass::rateControl_(float* pitchrate, float* rollrate, float* yawrate) {     ratePitchPID.calculate(&AHRS.calibratedData.p, pitchrate, &Timer.dt);     rateRollPID.calculate(&AHRS.calibratedData.q, rollrate, &Timer.dt);     rateYawPID.calculate(&AHRS.calibratedData.r, yawrate, &Timer.dt);     updatePWM_(&PICInterface.rx.throttleDem, &ratePitchPID.output, &rateRollPID.output, &rateYawPID.output); } void ControlClass::attitudeControl_(float* targetPitch, float* targetRoll, float* targetYawRate) {     attitudePitchPID.calculate(&AHRS.orientation.pitch, targetPitch, &Timer.dt);     attitudeRollPID.calculate(&AHRS.orientation.roll, targetRoll, &Timer.dt);    rateControl_(&attitudePitchPID.output, &attitudeRollPID.output, targetYawRate); } s_altitudePID altitudePID; PIDClass ratePitchPID, rateRollPID, rateYawPID; PIDClass attitudePitchPID, attitudeRollPID; inline void ControlClass::updatePWM_(float* throttle, float* pitch, float* roll, float* yaw) { 四軸 :  1 ->  frontleft...  2-> frontright...  3->  rearright....  4->  rearleft PICInterface.pwmwidths.frontleft = ((*throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) - *pitch + *roll - *yaw + altitudePID.output; PICInterface.pwmwidths.frontright = ((*throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) - *pitch - *roll + *yaw + altitudePID.output; PICInterface.pwmwidths.rearright = ((*throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) + *pitch - *roll - *yaw + altitudePID.output; PICInterface.pwmwidths.rearleft = ((*throttle * (MOTOR_MAX - MOTOR_MIN)) + MOTOR_MIN) + *pitch + *roll + *yaw + altitudePID.output; PICInterface.setPWM(); } void PICInterfaceClass::setPWM() { uint8_t widthsChar[13]; //+1 for pwm_fire bit  make8_(&pwmwidths.frontright, &widthsChar[0]); make8_(&pwmwidths.rearright, &widthsChar[2]);  make8_(&pwmwidths.rearleft, &widthsChar[4]);  make8_(&pwmwidths.frontleft, &widthsChar[6]);  make8_(&pwmwidths.aux1, &widthsChar[8]);  make8_(&pwmwidths.aux2, &widthsChar[10]);  I2CInterface.writeRegister(PIC_ADDRESS, REG_PWM1H, widthsChar, 13);} 6.  LogMan.update(); // 紀錄log void LoggerClass::update() {      if(logging) {                   sampleno++;                   log << sampleno << ", "                         << Timer.dt * 1000 << ", "                         << AHRS.calibratedData.x << ", "                         << AHRS.calibratedData.y << ", "                          << AHRS.calibratedData.z << ", "                          << AHRS.calibratedData.p << ", "                          << AHRS.calibratedData.q << ", "                          << AHRS.calibratedData.r << ", "                          << AHRS.calibratedData.temp << ", "                          << AHRS.calibratedData.magx << ", "                          << AHRS.calibratedData.magy << ", "                          << AHRS.calibratedData.magz << ", "                          << AHRS.calibratedData.pressure << ", "                          << AHRS.calibratedData.altitude << ", "                          << AHRS.orientation.pitch << ", "                          << AHRS.orientation.roll << ", "                          << AHRS.orientation.yaw << ", "                          << PICInterface.rx.pitchDem << ", "                          << PICInterface.rx.pitchRateDem << ", "                          << PICInterface.rx.rollDem << ", "                          << PICInterface.rx.rollRateDem << ", "                          << PICInterface.rx.throttleDem << ", "                          << PICInterface.rx.yawRateDem << ", "                          << PICInterface.rx.sw1 << ", "                          << PICInterface.rx.sw2 << ", "                          << PICInterface.pwmwidths.frontleft << ", "                          << PICInterface.pwmwidths.frontright << ", "                          << PICInterface.pwmwidths.rearleft << ", "                          << PICInterface.pwmwidths.rearright << ", "                          << Control.ratePitchPID.output << ", "                          << Control.rateRollPID.output << ", "                          << Control.rateYawPID.output << ", "                          << Control.attitudePitchPID.output << ", "                          << Control.attitudeRollPID.output << ", "                          << AHRS.quaternion.w << ", "                          << AHRS.quaternion.x << ", "                          << AHRS.quaternion.y << ", "                          << AHRS.quaternion.z //Add additional logs below                          << std::endl;                           if(PICInterface.rx.sw1 == false) { doWeNeedToFlush(); }                        }                     } 7.   Timer.compensate_();   //  盡量讓某 400Hz 啟動timer 一次 inline void TimerClass::compensate_(){   //Timer aims to get as close to 400Hz as possible, mostly limited by the I2C bandwidth clock_gettime(CLOCK_MONOTONIC, &iterationtime_); //      iterationtime_ -> now time //      time_             -> old time //     ((iterationtime_.tv_sec * 1000000000 + iterationtime_.tv_nsec) - (time_.tv_sec * 1000000000 + time_.tv_nsec)) // -> processing time long inttime = PERIOD - ((iterationtime_.tv_sec * 1000000000 + iterationtime_.tv_nsec) -                                    (time_.tv_sec * 1000000000 + time_.tv_nsec));         if (inttime < 0)                 Timer.timeValue_.tv_nsec = 1;         else                Timer.timeValue_.tv_nsec = inttime; Timer.timeToSet_.it_value = Timer.timeValue_;  timer_settime(timerId, 0, &timeToSet_, NULL);} 8.    pthread_mutex_unlock(&TimerMutex_);   , 關閉 mutex...讓中斷可以啟動

Reference : https://github.com/matthew-t-watson/Picopter/

MPU6050 的研究筆記

1.  不使用DMP -> Ref 1

首先用 git 將 GitHub 上的程式碼都下載下來：

git clone https://github.com/richardghirst/PiBits.git

進入 PiBits/MPU6050-Pi-Demo 目錄：

cd PiBits/MPU6050-Pi-Demo

這個目錄包含了三個範例程式以及 I2Cdev 與 MPU6050 兩個類別，基本上整個程式架構都使用物件導向的方式規劃的很清楚，所以只要稍微懂一點 C++ 的人，應該都可以立即上手。

我們來看最簡單的 demo_raw.cpp，這一個程式是單純讀取 MPU-6050 的感測資料，然後輸出：

#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro;  // 預設 I2C 位址為 0x68
//MPU6050 accelgyro(0x69);  // 設定 I2C 位址為 0x68

int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  // 初始化 I2C 設備
  printf("Initializing I2C devices...\n");
  accelgyro.initialize();

  // 測試連線是否正常
  printf("Testing device connections...\n");
  printf(accelgyro.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
}

void loop() {
  // 從 MPU-6050 讀取加速度計與陀螺儀資料
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // 其他的讀取方式
  //accelgyro.getAcceleration(&ax, &ay, &az);
  //accelgyro.getRotation(&gx, &gy, &gz);

  // 輸出
  printf("a/g: %6hd %6hd %6hd   %6hd %6hd %6hd\n",ax,ay,az,gx,gy,gz);
}

int main() {
  setup();
  for (;;)
    loop();
}

這個程式碼非常簡單，我就不多作解釋了。由於 PiBits 中的專案都已經寫好 Makefile 了，所以直接執行 make 就以自動編譯：

make

然後執行

sudo ./demo_raw

正常的話，應該就可以看到類似這樣的輸出：

Initializing I2C devices...
Testing device connections...
MPU6050 connection successful
a/g:  16804     24  -3420     -113   -154    -68
a/g:  16664    -72  -3544      -89   -177    -78
a/g:  16800    -20  -3376     -100   -144    -48
a/g:  16676    -56  -3468      -87   -174    -84
a/g:  16712    -64  -3544     -116   -162    -41
a/g:  16776     28  -3528     -107   -157    -95

如果您執行之後出現這樣的錯誤訊息：

Failed to open device: No such file or directory

那有可能是因為 I2C 的設備位址指定錯誤，可以檢查一下自己的 I2C 設備：

ls /dev/i2c*

如果您的輸出是這樣：

/dev/i2c-1

那麼請將 I2Cdev.cpp 中所有的

open("/dev/i2c-0", O_RDWR);

改為

open("/dev/i2c-1", O_RDWR);

然後再執行一次

make

這樣就可以讓一般使者直接存取了。


2.  使用DMP (減輕RPI負擔) -> REF 2

MPU-6050 加速度計與陀螺儀六軸感測器內建的 Digital Motion Processor（DMP）可以負責一些運動處理演算法（motion processing algorithm）的計算，DMP 可從加速度計（accelerometers）、陀螺儀（gyroscopes）或第三方的感測器上讀取資料，透過 DMP 的暫存器來讓使用者讀取運算的結果，或是將運算結果放進 FIFO 中。

DMP 主要的用途在於處理即時性的需求，並且分擔一些運算的工作，一般來說 DMP 會以很高的計算速度（大約是 200Hz）來進行運動處理演算法的運算，降低延遲以提供精準的數據，甚至在低取樣速度（如 5Hz）的應用上，DMP 還是會保持這樣的運算速度，以確保資料的精準性。

在前一篇文章中，我們使用了 PiBits 這個專案的 demo_raw.cpp 讀取原始的感測資料，而接下來我們繼續來看第二個使用 DMP 的範例程式 demo_dmp.cpp，因為這個程式碼比較長，所以就不全部貼上來了，我只說明比較重要的部分。

程式一開始先呼叫 setup() 進行初始化：

// 初始化 I2C 設備
printf("Initializing I2C devices...\n");
mpu.initialize();

// 測試一下連線是否正常
printf("Testing device connections...\n");
printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");

// 載入與設定 DMP
printf("Initializing DMP...\n");
devStatus = mpu.dmpInitialize();

接著啟用 DMP：

// 開啟 DMP
printf("Enabling DMP...\n");
mpu.setDMPEnabled(true);

在讀取資料之前，必須先確認 DMP 的封包大小：

// 取得 DMP 封包大小
packetSize = mpu.dmpGetFIFOPacketSize();

這樣就完成初始化的動作了，接著就開始進入主要的無窮迴圈，重複呼叫loop() 讀取資料。在 loop() 中，先取得 FIFO 目前的大小：

fifoCount = mpu.getFIFOCount();

然後檢查看看 FIFO 是否有溢位的狀況，如果 FIFO 沒有溢位，再檢查 FIFO 資料大小是否超過一個 DMP 封包大小，如果超過的話，就可以讀取一個封包的資料進來：

if (fifoCount == 1024) { // FIFO 溢位
  // 重設 FIFO
    mpu.resetFIFO();
  printf("FIFO overflow!\n");

// 檢查 FIFO 中 DMP 的資料是否已經可以讀取了
} else if (fifoCount >= packetSize) {
  // 從 FIFO 中讀取一個 DMP 封包資料
  mpu.getFIFOBytes(fifoBuffer, packetSize);

  // 解析 DMP 封包，並輸出資料 ...[略]
}

由於 MPU-6050 的 FIFO 緩衝區的大小是 1024 bytes（請參考 MPU-6050 的官方說明文件），所以這裡我們依據 fifoCount 的值是否為 1024 來判斷 FIFO 是否有溢位。

將資料從 MPU-6050 的 FIFO 讀出來之後，會儲存在 fifoBuffer 這個陣列中，而接下來就是要解析這個 DMP 封包，輸出自己想要的數值資料，在demo_dmp.cpp 中，有很多寫好範例，例如輸出去除重力（gravity）的加速度：

如果要改變 DMP FIFO 的更新速度，可以在 Makefile 中透過DMP_FIFO_RATE 調整：

CXXFLAGS = -DDMP_FIFO_RATE=9 -Wall -g -O2 `pkg-config gtkmm-3.0 --cflags --libs`

這裡 FIFO rate 的計算公式為

FIFO Rate = (200Hz / (1 + DMP_FIFO_RATE))

如果 DMP_FIFO_RATE 設為 9，則計算出來的 FIFO rate 就是 20Hz，以此類推。

這裡只是簡要的敘述程式的重點，如果想要了解整個程式的細節，還是需要從原始碼一行一行來看才有辦法，另外最好也先看過 InvenSense 官方的文件。



Reference  1: http://blog.gtwang.org/iot/raspberry-pi-read-data-from-mpu6050-using-cpp/

Reference  2: http://blog.gtwang.org/iot/raspberry-pi-mpu-6050-read-data-using-dmp/

Reference : http://gogoprivateryan.blogspot.tw/2014/07/mpu-6050-google.html

使用自製PWM量測工具....量測多軸飛行器的PWM value.....

遙控器 PWM output:  

第一通道 右手搖桿上下 :      最小值 4.99% ...中間值 : 6.81% .... 最大值(上) :  8.63% ..... 頻率  : 45HZ

第二通道 右手搖桿左右 :      最小值 4.99% ...中間值 : 6.81% .... 最大值 :  8.63% ..... 頻率  : 45HZ

第三通道 左手搖桿上下 :      最小值 4.99% ...中間值 : 6.81% .... 最大值(上) :  8.63% ..... 頻率  : 45HZ

第四通道 左手搖桿左右 :      最小值 4.99% ...中間值 : 6.81% .... 最大值(左) :  8.63% ..... 頻率  : 45HZ

第五通道 FMOD :      最小值 4.99% ...中間值 : 6.81% .... 最大值(2) :  8.63% ..... 頻率  : 45HZ

第六通道 GEAR :      最小值(0) 4.99% ... .... 最大值(1) :  8.63% ..... 頻率  : 45HZ

電變PWM input :   最小值 :  49%.....  中間值 : ˙77%   最大值 :  93%    頻率  : 490HZ

一些資料整理 : 電流電壓感應器(Power Module)

電流電壓感應器(Power Module)。Power Module 有兩個功用，第一它可以感測飛行時動力系統耗電的資訊，如電池的電壓、電流及電量；第二，Power Module 可以帶有穩壓器UBEC(市場上也有不帶UBEC的Power Module，選購時要注意)，意思是這個內置的UBEC可以給APM 提供一個5.3V的穩定電源。當然你也可以利用其他UBEC為APM供電，但Powe Module 同時有測電及供電的功能，所以比較方便。我們不建議用電調(ESC)附帶的BEC供電，因為當電調工作其間，它的BEC供給APM的電源可以非常不穩定，供電不穩對飛控的致命傷，嚴重的話可以令飛行器失控。

安裝Power Module 時要注意正負電極不可接錯及分清楚哪一邊是接分電板哪一邊是接電池。一般的Power Module 上有一個細箭咀指示電流的方向，箭咀尾是接電池端，箭咀頭是接分電板端。

我們建議把APM安裝在減震器上，因為過量的震動對飛控內的加速器產生不良的影響

最後，就是把接收機跟APM連接。連接方法有兩種，主要針對用PWM及PPM-SUM 方法連接。用PWM方法連接，即一般接收機的連接方法，把接收機的CH1 跟 APM 的input 1 連接，CH2跟APM的input 2 連接，如此類推。PPM-SUM是用一條訊號線輸出多個Channel 的訊號，但並不是所有接收機都可以用輸出PPM-SUM 訊號，連接時要清楚你的接收機用什麼方法輸出PPM-SUM 訊號，如果選擇用PPM-SUM 方式連接，請把PPM-SUM訊號輸入到INPUT 1，你還要把APM的 INPUT 2 及INPUT 3 接通，讓APM懂得辨認PPM-SUM訊號，否則APM只用PWM 方法連接。



Reference : http://www.isaacuav.com/2343335037apm-3913125511.html

apm 的中文blog

Reference : https://apmcopter.wordpress.com/

F450的組裝

左前  :  正獎

右前  :  負槳

右後 :   正槳

左後 :   負槳

Reference : https://www.youtube.com/watch?v=GZteV5ZmEic&ebc=ANyPxKoPXKJbeXiyhfoPl5aarjw_3kfWjfQhoKCGLITIUbzJOwhBREHkzl3C7n4ZYVTQfQBBjYfAzQSqmpAqqPb3TvvuHZRNZw