UAVRO
Quadrotor stabi
Dependencies: ESC HMC5883L MPU6050 MadgwickAHRS MemLog Servo mbed ms5611
Diff: main.cpp
- Revision:
- 0:05cb0f94324f
diff -r 000000000000 -r 05cb0f94324f main.cpp
--- /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();
+ }
+}
+