Quadrotor stabi
Dependencies: ESC HMC5883L MPU6050 MadgwickAHRS MemLog Servo mbed ms5611
Diff: main.cpp
- Revision:
- 0:05cb0f94324f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon May 30 08:11:32 2016 +0000 @@ -0,0 +1,610 @@ +#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(); + } +} +