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/