Quadrotor stabi
Dependencies: ESC HMC5883L MPU6050 MadgwickAHRS MemLog Servo mbed ms5611
main.cpp
- Committer:
- Decimus
- Date:
- 2016-05-30
- Revision:
- 1:4a8469635592
- Parent:
- 0:05cb0f94324f
File content as of revision 1:4a8469635592:
#include "mbed.h" #include "HMC5883L.h" #include "MPU6050.h" #include "ms5611.h" #include "MadgwickAHRS.h" #include "ESC.h" #include "MemLog.h" #include "defines.h" #include "utils.h" #define DEBUG // Debug to COM //#define DEBUG_F // Debug to flash #define FLY_MODE_X #ifdef DEBUG Serial pc(USBTX, USBRX); #define DBG_DELAY 0.1 #endif #ifdef DEBUG_F MemLog flog(2, 2000); #endif // x1, x2, y1, y2 ESC esc( p21, p22, p23, p24, LED4 ); // sda, scl HMC5883L mag_src(p28, p27); ms5611 bar_src(p28, p27); MPU6050 mpu_src(p28, p27); I2CSlave i2c(p9, p10); DigitalOut led_1(LED1); DigitalOut led_2(LED2); DigitalOut led_3(LED3); DigitalOut buzz(p17); DigitalIn cal_mode(p16); Timer tmr; typedef float TVector3[3]; typedef float TVector4[4]; struct TThrust { float x1,x2,y1,y2; }; struct TPID{ short p,i,d; }; int mode = 0; int16_t mag_raw[3], acc_raw[3]; //gyr_raw[3] float spd_yaw, spd_alt, thr_base, thr_avg, thr_to, altz, acc_1g; float bar_table[BAR_TABLE_SIZE], bar_alt, bar_lpf, bar_base, bar_table_sum; uint8_t bar_table_idx; TVector3 gyr, acc_lpf, mag, grv, grv_old, grv_err; TVector4 pos, pos_base, pos_offset, imu, pid, pid_ist, pid_dst, pid_err, quat; TThrust thr; TPID pidk[4]; //DEBUG float debug_data_1; // hs battop const float mag_cal_def[3][3] = MAG_CAL; const float mag_off_def[3] = MAG_OFF; const float pidk_def[4][3] = MAG_CAL; void get_mag(); // Calibrate accel offsets and set it up to adxl345 registers void calibrate_acc_offsets(){ float biasa[3], biasg[3]; mpu_src.calibrate(biasa,biasg); } void calibrate_acc_grv(){ int16_t data[3]; int32_t avg[3] = {0,0,0}; for(uint16_t i=0; i<100; i++ ){ mpu_src.getAccelRaw(data); avg[X] += data[X]; avg[Y] += data[Y]; avg[Z] += data[Z]; wait_ms(10); } // calc averages and set default values for lpf for(uint8_t i=0; i<3; i++ ){ avg[i] = avg[i]/100; acc_lpf[i] = float(avg[i]); } acc_1g = 1.0f/invSqrt(avg[X]*avg[X] + avg[Y]*avg[Y] + avg[Z]*avg[Z]); grv_err[2] = avg[Z] - acc_1g; } // Calibrate barometer level void calibrate_bar(){ float bar_sum = 0.0f; blink(2000, &led_1); // 1 measure per 3 iterations for(uint16_t i=0; i<300; i++){ if( bar_src.calcPT() ){ bar_sum += bar_src.getAltitude(); } wait_ms(10); } bar_base = bar_sum/100.0f; //fill default bar table bar_table_sum = 0.0f; for(uint8_t i=0; i<BAR_TABLE_SIZE; i++){ bar_table[i] = bar_base; bar_table_sum += bar_base; } } // Get compass heading. Add current magnetic declination magnitude and correct angle of copter nose. int get_heading(){ float yaw = imu[YAW] * TO_DEG - MAG_DECL + 90; if( yaw > 180.0 ) yaw -= 360.0; else if( yaw < -180.0 ) yaw += 360.0; return yaw; } // Calc Z-acceleration vector without gravitational part void get_grv(float g[3]){ g[X] = 2 * (quat[1] * quat[3] - quat[0] * quat[2]); g[Y] = 2 * (quat[0] * quat[1] + quat[2] * quat[3]); g[Z] = (quat[0] * quat[0] - quat[1] * quat[1] - quat[2] * quat[2] + quat[3] * quat[3]); } // Calc barometer altitude void get_bar(){ float bar_raw; if( bar_src.calcPT() ){ bar_raw = bar_src.getAltitude(); // meters // LPF over baro values uint8_t bar_table_idx1 = (bar_table_idx + 1); if (bar_table_idx1 == BAR_TABLE_SIZE) bar_table_idx1 = 0; bar_table[bar_table_idx] = bar_raw; bar_table_sum += bar_table[bar_table_idx]; bar_table_sum -= bar_table[bar_table_idx1]; bar_table_idx = bar_table_idx1; bar_lpf = bar_lpf*(1-LPFC_BAR) + bar_table_sum*(LPFC_BAR)/21.0f; // relative bar //bar_alt = clamp_bot(bar_base - bar_lpf, 0); bar_alt = bar_lpf - bar_base; } } // Calc Z-displacement after 2-stage integration void get_alt( float tdelta ){ float vdelta; float g[3]; float alt_err = 0.0f; float tmp = 1.0f; // don't fuse baro in takeoff mode if( !(mode & MODE_TAKEOFF) && !(mode & MODE_ON) ){ alt_err = clamp((imu[ALT] - bar_alt), -0.5, 0.5) * ALT_P; tmp = alt_err*ALT_ACC_I; grv_err[Z]+= acc_lpf[Z] * tmp; // don't take into account X and Y if UAV position is almost vertical if(abs(imu[ROLL]) > GRAD5 || abs(imu[PITCH]) > GRAD5) { grv_err[X]+= acc_lpf[X] * 2 * tmp; grv_err[Y]+= acc_lpf[Y] * 2 * tmp; } } // fetch gravitational vector from position quaternion get_grv(&g[0]); tmp = (acc_lpf[X]-grv_err[0]) * g[X] + (acc_lpf[Y]-grv_err[1]) * g[Y]; grv_old[0] = grv[0]; grv[0] = ((tmp + (acc_lpf[Z] - grv_err[2]) * g[Z]) * invSqrt(g[X]*g[X] + g[Y]*g[Y] + g[Z]*g[Z]) - acc_1g)* ACC_GS; // calc velocity in meters per seconds by median integration grv_old[1] = grv[1]; grv[1] = grv[1] + deadband((grv[0]+grv_old[0])/2.0f, ACC_DEADBAND) * tdelta; // calc displacement in meters by median integration vdelta = (grv[1]+grv_old[1]) * tdelta / 2.0f; if( !(mode & MODE_TAKEOFF) && !(mode & MODE_ON) ){ vdelta -= alt_err*ALT_VEL_P; } imu[ALT] += vdelta - alt_err; // pressure and gravitational altitude fusing //imu[ALT] = imu[ALT]*(1-LPFC_ALT) + bar_alt*LPFC_ALT; } // Calc accel void get_acc(){ mpu_src.getAccelRaw(acc_raw); // LPF over accel values for(uint8_t i=0; i<3; i++ ){ acc_lpf[i] = acc_lpf[i]*(1-LPFC_ACC) + acc_raw[i]*LPFC_ACC; } } // Calc gyro void get_gyr(){ mpu_src.getGyro(gyr); for(uint8_t i=0; i<3; i++ ){ gyr[i] = TO_RAD*gyr[i]; } } // Calc magnitude void get_mag(){ mag_src.getXYZ(mag_raw); for(uint8_t i=0; i<3; i++ ){ mag[i] = mag_raw[i] - mag_off_def[i]; } mag[X] = mag_cal_def[0][0]*mag[X] + mag_cal_def[0][1]*mag[Y] + mag_cal_def[0][2]*mag[Z]; mag[Y] = mag_cal_def[1][0]*mag[X] + mag_cal_def[1][1]*mag[Y] + mag_cal_def[1][2]*mag[Z]; mag[Z] = mag_cal_def[2][0]*mag[X] + mag_cal_def[2][1]*mag[Y] + mag_cal_def[2][2]*mag[Z]; } // Calc pitch, roll, yaw angles void get_imu( float tdelta ) { MadgwickAHRSupdate( tdelta, gyr[X], gyr[Y], gyr[Z], float(acc_raw[X]), float(acc_raw[Y]), float(acc_raw[Z]), mag[X], mag[Z], mag[Y]); quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3; quat_2_euler(&quat[0], &imu[0]); // check imu size (4 actually) } void takeoff_init(){ // Setup base yaw pos_base[YAW] = pos[YAW] = imu[YAW]; // Setup base alt pos_base[ALT] = pos[ALT] = imu[ALT]; // reset velocity grv[1] = 0.0f; } void takeoff_process(){ thr_to += TAKEOFF_SPEED; if( imu[ALT] > TAKEOFF_LEV_ALT ){ // remove takeoff flag, add altitude stab flag and set target altitude mode = mode ^ MODE_TAKEOFF; mode = mode | MODE_STAB_ALT; pos[ALT] += TAKEOFF_CLIMB_ALT; } } void calc_thrust( float tdelta ){ float p, i, d, error; float thr_a, thr_base; float mpos; TThrust thr_pry; // x (roll) #ifdef FLY_MODE_X mpos = rotx( imu[ROLL], imu[PITCH] ); error = rotx(pos[ROLL] + pos_offset[ROLL], pos[PITCH]) - mpos; #else mpos = imu[ROLL]; // moment pos error = pos[ROLL] + pos_offset[ROLL] - mpos; #endif p = pidk[ROLL].p * error; pid_ist[ROLL] = clamp( pid_ist[ROLL] + error * tdelta, -1, 1 ); i = pidk[ROLL].i * pid_ist[ROLL]; d = pidk[ROLL].d * (pid_dst[ROLL] - mpos); pid_dst[ROLL] = mpos; pid[ROLL] = p + i + d; // y (pitch) mpos = roty( imu[ROLL], imu[PITCH] ); #ifdef FLY_MODE_X error = roty(pos[ROLL], pos[PITCH] + pos_offset[PITCH]) - mpos; #else error = pos[PITCH] + pos_offset[PITCH] - mpos; #endif p = pidk[PITCH].p * error; pid_ist[PITCH] = clamp( pid_ist[PITCH] + error * tdelta, -1, 1 ); i = pidk[PITCH].i * pid_ist[PITCH]; d = pidk[PITCH].d * (pid_dst[PITCH] - mpos); pid_dst[PITCH] = mpos; pid[PITCH] = p + i + d; pos[YAW] = pos[YAW]+spd_yaw*tdelta; // z (yaw) mpos = imu[YAW]; error = pos[YAW] - mpos; if( error > 180.0*TO_RAD ) error -= 360.0*TO_RAD; else if( error < -180.0*TO_RAD ) error += 360.0*TO_RAD; p = pidk[YAW].p * error; pid_ist[YAW] = clamp( pid_ist[YAW] + error * tdelta, -3, 3 ); i = pidk[YAW].i * pid_ist[YAW]; d = pidk[YAW].d * (pid_dst[YAW] - mpos); pid_dst[YAW] = mpos; pid[YAW] = p + i + d; // Thrust only for PRY stabilization thr_pry.x1 = -pid[ROLL] * ROLL_SCALE - YAW_PID_DIR * pid[YAW] * YAW_SCALE; thr_pry.x2 = +pid[ROLL] * ROLL_SCALE - YAW_PID_DIR * pid[YAW] * YAW_SCALE; thr_pry.y1 = +pid[PITCH] * PITCH_SCALE + YAW_PID_DIR * pid[YAW] * YAW_SCALE; thr_pry.y2 = -pid[PITCH] * PITCH_SCALE + YAW_PID_DIR * pid[YAW] * YAW_SCALE; // do alt stabilization only if automatic miode is on if( mode & MODE_STAB_ALT && !(mode & MODE_MANUAL ) ){ // increase target altitude according to current speed pos[ALT] = pos[ALT] + spd_alt*tdelta; mpos = imu[ALT]; error = pos[ALT] - mpos; p = pidk[ALT].p * error; pid_ist[ALT] = clamp( pid_ist[ALT] + error * tdelta, -1, 1 ); i = pidk[ALT].i * pid_ist[ALT]; d = pidk[ALT].d * (pid_dst[ALT] - mpos); pid_dst[ALT] = mpos; pid[ALT] = p + i + d; thr_a = pid[ALT] * ALT_SCALE; } else { pos[ALT] = imu[ALT]; thr_a = 0.0; } if( mode & MODE_MANUAL ) thr_base = clamp(spd_alt*4, 0.0, 1.0); else thr_base = thr_to; // Summary thrust for PRY + ALT stabilization thr.x1 = clamp( thr_base + thr_pry.x1 + thr_a, 0.0, 1.0 ); thr.x2 = clamp( thr_base + thr_pry.x2 + thr_a, 0.0, 1.0 ); thr.y1 = clamp( thr_base + thr_pry.y1 + thr_a, 0.0, 1.0 ); thr.y2 = clamp( thr_base + thr_pry.y2 + thr_a, 0.0, 1.0 ); thr_avg = (thr.x1 + thr.x2 + thr.y1 + thr.y2) / 4.0f; } void init(){ // reset pid values, pid coeffs, and pos offsets for(uint8_t i=0; i<4; i++){ pos_offset[i] = 0.0; pid_ist[i] = 0.0; pid_dst[i] = 0.0; pidk[i].p = pidk_def[i][0]; pidk[i].i = pidk_def[i][1]; pidk[i].d = pidk_def[i][2]; pos[i] = 0.0; } // desire xyz imu[ALT] = 0.0; spd_alt = 0; thr_to = 0.0f; thr_base = 0.0f; thr_avg = 0.0f; for(uint8_t i=0; i<3; i++){ grv[i] = 0.0f; grv_err[i] = 0.0f; } altz = 0; bar_lpf = 0.0f; bar_table_idx = 0; acc_1g = 0.0f; } void stop(){ esc.stop(); #ifdef DEBUG_F flog.save(); #endif } void checkMode(){ if( mode & MODE_READY ){ mode = mode ^ MODE_READY; mode = mode | MODE_STAB_PRY; if( !(mode & MODE_MANUAL) ){ mode = mode | MODE_TAKEOFF; } takeoff_init(); } else if ( mode & MODE_MANUAL){ //pass } else { } } void obey(){ char cbuf[32]; short sbuf[16]; char cbufn = 15; int i = i2c.receive(); switch (i) { case I2CSlave::ReadAddressed: sbuf[0] = (short)(debug_data_1*10000);//(thr_avg*100); sbuf[1] = (short)(imu[ALT]*100); sbuf[2] = (short)(pos[ALT]*100); sbuf[3] = (short)(get_heading()); sbuf[4] = (short)(imu[ROLL]*TO_DEG); sbuf[5] = (short)(imu[PITCH]*TO_DEG); sbuf[6] = (short)(mode); short2char( &sbuf[0], &cbuf[0], 0, 7 ); i2c.write( cbuf, 14 ); break; case I2CSlave::WriteAddressed: //receive on/off command i2c.read(cbuf, 32); if( cbuf[0]==CMD_I2C_OFF ){ if( mode & MODE_ON ){ stop(); mode = 0; // remove MODE_ON flag beep(5, 50, &buzz); beep(1, 200, &buzz); } } else if( cbuf[0]==CMD_I2C_ON ){ if( !(mode & MODE_ON) ){ beep(1, 200, &buzz); beep(5, 50, &buzz); mode = mode | MODE_READY | MODE_ON; // set default flags } } // receive manual/auto command else if( cbuf[0]==CMD_I2C_AUTO ){ if( mode & MODE_MANUAL ){ beep(6, 50, &buzz); beep(1, 200, &buzz); mode = mode ^ MODE_MANUAL; // remove MODE_MANUAL flag } } else if( cbuf[0]==CMD_I2C_MANUAL ){ if( !(mode & MODE_MANUAL) ){ beep(1, 200, &buzz); beep(6, 50, &buzz); mode = mode | MODE_MANUAL; // add MODE_MANUAL flag } } // receive control values else if( cbuf[0]==CMD_I2C_CTRL ){ char2short( &cbuf[0], &sbuf[0], 1, cbufn*2 ); spd_alt = sbuf[0]*ALT_MAX/CTRL_DIA; pos[PITCH] = (sbuf[1]*PITCH_MAX/CTRL_DIA)*TO_RAD; pos[ROLL] = (sbuf[2]*ROLL_MAX/CTRL_DIA)*TO_RAD; spd_yaw = (sbuf[3]*YAW_MAX/CTRL_DIA)*TO_RAD; pidk[ROLL].p = sbuf[4]; pidk[PITCH].p = sbuf[4]; pidk[ROLL].i = sbuf[5]; pidk[PITCH].i = sbuf[5]; pidk[ROLL].d = sbuf[6]; pidk[PITCH].d = sbuf[6]; pidk[YAW].p = sbuf[7]; pidk[YAW].i = sbuf[8]; pidk[YAW].d = sbuf[9]; pidk[ALT].p = sbuf[10]; pidk[ALT].i = sbuf[11]; pidk[ALT].d = sbuf[12]; pos_offset[ROLL] = sbuf[13]*TO_RAD/10.0; pos_offset[PITCH] = sbuf[14]*TO_RAD/10.0; } break; } } int main() { double tc, td, incl_time, ctrl_time, mag_time, bar_time; incl_time = ctrl_time = mag_time = bar_time = 0; // Variables init init(); blink(1000, &led_1); // set i2c address i2c.address(0xA0); #ifdef DEBUG float dbg_time = 0; pc.baud(19200); #endif // ESC init esc.initialize_(); // Accel and Gyro init mpu_src.init(); mpu_src.getAres(); mpu_src.getGres(); // Barometer init bar_src.cmd_reset(); calibrate_bar(); beep(2, 100, &buzz); beep(1, 500, &buzz); // Accel optional calibration if( cal_mode ){ calibrate_acc_offsets(); beep(3, 100, &buzz); beep(1, 500, &buzz); }; calibrate_acc_grv(); beep(4, 100, &buzz); beep(1, 500, &buzz); // Magnitometer init mag_src.init(); blink(1000, &led_1); beep(1, 1000, &buzz); tmr.start(); while(1){ obey(); tc = tmr.read(); if( mag_time + MAG_DELAY < tc ){ mag_time = tmr.read(); get_mag(); } tc = tmr.read(); if( bar_time + BAR_DELAY < tc ){ bar_time = tc; get_bar(); } tc = tmr.read(); if( incl_time + INCL_DELAY < tc ){ td = tc - incl_time; incl_time = tc; get_acc(); // collect accel data get_gyr(); // collect gyro data get_imu(td); // calc angles, after acc & gyr calcs get_alt(td); // calc gravity altitude, after grv calc debug_data_1 = td; } tc = tmr.read(); if( ctrl_time + CTRL_DELAY < tc ){ td = tc - ctrl_time; ctrl_time = tc; #ifdef DEBUG_F flog.log(imu[X]*TO_DEG); flog.log(gyr[X]); #endif if( mode & MODE_TAKEOFF ){ takeoff_process(); } if( mode & MODE_ON ){ calc_thrust( td ); esc.set(thr.x1, thr.x2, thr.y1, thr.y2); } } #ifdef DEBUG tc = tmr.read(); if( dbg_time + DBG_DELAY < tc ){ dbg_time = tc; pc.printf("imu %+7.2f %+7.2f %+7.2f %+7.2f %+7.2f \r\n", TO_DEG*imu[X], TO_DEG*imu[Y], TO_DEG*imu[Z], bar_alt, imu[ALT]); //pc.printf("imu %d %d %d %+7.2f %+7.2f %+7.2f \r\n", acc_raw[0], acc_raw[1], acc_raw[2], gyr[0], gyr[1], gyr[2]); } #endif if( mode & MODE_ON ) checkMode(); } }