UAVRO / Mbed 2 deprecated Q2_Stabi

Quadrotor stabi

Dependencies:   ESC HMC5883L MPU6050 MadgwickAHRS MemLog Servo mbed ms5611

main.cpp@1:4a8469635592, 2016-05-30 (annotated)

Committer:
Decimus
Date:
Mon May 30 08:16:04 2016 +0000
Revision:
1:4a8469635592
Parent:
0:05cb0f94324f 
[+]

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Decimus 0:05cb0f94324f 1 #include "mbed.h"
Decimus 0:05cb0f94324f 2 #include "HMC5883L.h"
Decimus 0:05cb0f94324f 3 #include "MPU6050.h"
Decimus 0:05cb0f94324f 4 #include "ms5611.h"
Decimus 0:05cb0f94324f 5 #include "MadgwickAHRS.h"
Decimus 0:05cb0f94324f 6 #include "ESC.h"
Decimus 0:05cb0f94324f 7 #include "MemLog.h"
Decimus 0:05cb0f94324f 8 #include "defines.h"
Decimus 0:05cb0f94324f 9 #include "utils.h"
Decimus 0:05cb0f94324f 10
Decimus 0:05cb0f94324f 11 #define DEBUG // Debug to COM
Decimus 0:05cb0f94324f 12 //#define DEBUG_F // Debug to flash
Decimus 0:05cb0f94324f 13
Decimus 0:05cb0f94324f 14 #define FLY_MODE_X
Decimus 0:05cb0f94324f 15
Decimus 0:05cb0f94324f 16 #ifdef DEBUG
Decimus 0:05cb0f94324f 17 Serial pc(USBTX, USBRX);
Decimus 0:05cb0f94324f 18 #define DBG_DELAY 0.1
Decimus 0:05cb0f94324f 19 #endif
Decimus 0:05cb0f94324f 20
Decimus 0:05cb0f94324f 21 #ifdef DEBUG_F
Decimus 0:05cb0f94324f 22 MemLog flog(2, 2000);
Decimus 0:05cb0f94324f 23 #endif
Decimus 0:05cb0f94324f 24
Decimus 0:05cb0f94324f 25 // x1, x2, y1, y2
Decimus 0:05cb0f94324f 26 ESC esc( p21, p22, p23, p24, LED4 );
Decimus 0:05cb0f94324f 27
Decimus 0:05cb0f94324f 28 // sda, scl
Decimus 0:05cb0f94324f 29 HMC5883L mag_src(p28, p27);
Decimus 0:05cb0f94324f 30 ms5611 bar_src(p28, p27);
Decimus 0:05cb0f94324f 31 MPU6050 mpu_src(p28, p27);
Decimus 0:05cb0f94324f 32 I2CSlave i2c(p9, p10);
Decimus 0:05cb0f94324f 33
Decimus 0:05cb0f94324f 34 DigitalOut led_1(LED1);
Decimus 0:05cb0f94324f 35 DigitalOut led_2(LED2);
Decimus 0:05cb0f94324f 36 DigitalOut led_3(LED3);
Decimus 0:05cb0f94324f 37 DigitalOut buzz(p17);
Decimus 0:05cb0f94324f 38
Decimus 0:05cb0f94324f 39 DigitalIn cal_mode(p16);
Decimus 0:05cb0f94324f 40
Decimus 0:05cb0f94324f 41 Timer tmr;
Decimus 0:05cb0f94324f 42
Decimus 0:05cb0f94324f 43 typedef float TVector3[3];
Decimus 0:05cb0f94324f 44 typedef float TVector4[4];
Decimus 0:05cb0f94324f 45
Decimus 0:05cb0f94324f 46 struct TThrust {
Decimus 0:05cb0f94324f 47 float x1,x2,y1,y2;
Decimus 0:05cb0f94324f 48 };
Decimus 0:05cb0f94324f 49
Decimus 0:05cb0f94324f 50 struct TPID{
Decimus 0:05cb0f94324f 51 short p,i,d;
Decimus 0:05cb0f94324f 52 };
Decimus 0:05cb0f94324f 53
Decimus 0:05cb0f94324f 54 int mode = 0;
Decimus 0:05cb0f94324f 55
Decimus 0:05cb0f94324f 56 int16_t mag_raw[3], acc_raw[3];
Decimus 0:05cb0f94324f 57 //gyr_raw[3]
Decimus 0:05cb0f94324f 58 float spd_yaw, spd_alt, thr_base, thr_avg, thr_to, altz, acc_1g;
Decimus 0:05cb0f94324f 59 float bar_table[BAR_TABLE_SIZE], bar_alt, bar_lpf, bar_base, bar_table_sum;
Decimus 0:05cb0f94324f 60 uint8_t bar_table_idx;
Decimus 0:05cb0f94324f 61 TVector3 gyr, acc_lpf, mag, grv, grv_old, grv_err;
Decimus 0:05cb0f94324f 62 TVector4 pos, pos_base, pos_offset, imu, pid, pid_ist, pid_dst, pid_err, quat;
Decimus 0:05cb0f94324f 63 TThrust thr;
Decimus 0:05cb0f94324f 64 TPID pidk[4];
Decimus 0:05cb0f94324f 65
Decimus 0:05cb0f94324f 66 //DEBUG
Decimus 0:05cb0f94324f 67 float debug_data_1;
Decimus 0:05cb0f94324f 68
Decimus 0:05cb0f94324f 69 // hs battop
Decimus 0:05cb0f94324f 70 const float mag_cal_def[3][3] = MAG_CAL;
Decimus 0:05cb0f94324f 71 const float mag_off_def[3] = MAG_OFF;
Decimus 0:05cb0f94324f 72 const float pidk_def[4][3] = MAG_CAL;
Decimus 0:05cb0f94324f 73
Decimus 0:05cb0f94324f 74 void get_mag();
Decimus 0:05cb0f94324f 75
Decimus 0:05cb0f94324f 76 // Calibrate accel offsets and set it up to adxl345 registers
Decimus 0:05cb0f94324f 77 void calibrate_acc_offsets(){
Decimus 0:05cb0f94324f 78 float biasa[3], biasg[3];
Decimus 0:05cb0f94324f 79 mpu_src.calibrate(biasa,biasg);
Decimus 0:05cb0f94324f 80 }
Decimus 0:05cb0f94324f 81
Decimus 0:05cb0f94324f 82 void calibrate_acc_grv(){
Decimus 0:05cb0f94324f 83 int16_t data[3];
Decimus 0:05cb0f94324f 84 int32_t avg[3] = {0,0,0};
Decimus 0:05cb0f94324f 85 for(uint16_t i=0; i<100; i++ ){
Decimus 0:05cb0f94324f 86 mpu_src.getAccelRaw(data);
Decimus 0:05cb0f94324f 87 avg[X] += data[X];
Decimus 0:05cb0f94324f 88 avg[Y] += data[Y];
Decimus 0:05cb0f94324f 89 avg[Z] += data[Z];
Decimus 0:05cb0f94324f 90 wait_ms(10);
Decimus 0:05cb0f94324f 91 }
Decimus 0:05cb0f94324f 92
Decimus 0:05cb0f94324f 93 // calc averages and set default values for lpf
Decimus 0:05cb0f94324f 94 for(uint8_t i=0; i<3; i++ ){
Decimus 0:05cb0f94324f 95 avg[i] = avg[i]/100;
Decimus 0:05cb0f94324f 96 acc_lpf[i] = float(avg[i]);
Decimus 0:05cb0f94324f 97 }
Decimus 0:05cb0f94324f 98 acc_1g = 1.0f/invSqrt(avg[X]*avg[X] + avg[Y]*avg[Y] + avg[Z]*avg[Z]);
Decimus 0:05cb0f94324f 99
Decimus 0:05cb0f94324f 100 grv_err[2] = avg[Z] - acc_1g;
Decimus 0:05cb0f94324f 101 }
Decimus 0:05cb0f94324f 102
Decimus 0:05cb0f94324f 103 // Calibrate barometer level
Decimus 0:05cb0f94324f 104 void calibrate_bar(){
Decimus 0:05cb0f94324f 105 float bar_sum = 0.0f;
Decimus 0:05cb0f94324f 106 blink(2000, &led_1);
Decimus 0:05cb0f94324f 107
Decimus 0:05cb0f94324f 108 // 1 measure per 3 iterations
Decimus 0:05cb0f94324f 109 for(uint16_t i=0; i<300; i++){
Decimus 0:05cb0f94324f 110 if( bar_src.calcPT() ){
Decimus 0:05cb0f94324f 111 bar_sum += bar_src.getAltitude();
Decimus 0:05cb0f94324f 112 }
Decimus 0:05cb0f94324f 113 wait_ms(10);
Decimus 0:05cb0f94324f 114 }
Decimus 0:05cb0f94324f 115
Decimus 0:05cb0f94324f 116 bar_base = bar_sum/100.0f;
Decimus 0:05cb0f94324f 117
Decimus 0:05cb0f94324f 118 //fill default bar table
Decimus 0:05cb0f94324f 119 bar_table_sum = 0.0f;
Decimus 0:05cb0f94324f 120 for(uint8_t i=0; i<BAR_TABLE_SIZE; i++){
Decimus 0:05cb0f94324f 121 bar_table[i] = bar_base;
Decimus 0:05cb0f94324f 122 bar_table_sum += bar_base;
Decimus 0:05cb0f94324f 123 }
Decimus 0:05cb0f94324f 124 }
Decimus 0:05cb0f94324f 125
Decimus 0:05cb0f94324f 126 // Get compass heading. Add current magnetic declination magnitude and correct angle of copter nose.
Decimus 0:05cb0f94324f 127 int get_heading(){
Decimus 0:05cb0f94324f 128 float yaw = imu[YAW] * TO_DEG - MAG_DECL + 90;
Decimus 0:05cb0f94324f 129 if( yaw > 180.0 )
Decimus 0:05cb0f94324f 130 yaw -= 360.0;
Decimus 0:05cb0f94324f 131 else if( yaw < -180.0 )
Decimus 0:05cb0f94324f 132 yaw += 360.0;
Decimus 0:05cb0f94324f 133 return yaw;
Decimus 0:05cb0f94324f 134 }
Decimus 0:05cb0f94324f 135
Decimus 0:05cb0f94324f 136 // Calc Z-acceleration vector without gravitational part
Decimus 0:05cb0f94324f 137 void get_grv(float g[3]){
Decimus 0:05cb0f94324f 138 g[X] = 2 * (quat[1] * quat[3] - quat[0] * quat[2]);
Decimus 0:05cb0f94324f 139 g[Y] = 2 * (quat[0] * quat[1] + quat[2] * quat[3]);
Decimus 0:05cb0f94324f 140 g[Z] = (quat[0] * quat[0] - quat[1] * quat[1] - quat[2] * quat[2] + quat[3] * quat[3]);
Decimus 0:05cb0f94324f 141 }
Decimus 0:05cb0f94324f 142
Decimus 0:05cb0f94324f 143 // Calc barometer altitude
Decimus 0:05cb0f94324f 144 void get_bar(){
Decimus 0:05cb0f94324f 145 float bar_raw;
Decimus 0:05cb0f94324f 146 if( bar_src.calcPT() ){
Decimus 0:05cb0f94324f 147 bar_raw = bar_src.getAltitude(); // meters
Decimus 0:05cb0f94324f 148 // LPF over baro values
Decimus 0:05cb0f94324f 149 uint8_t bar_table_idx1 = (bar_table_idx + 1);
Decimus 0:05cb0f94324f 150 if (bar_table_idx1 == BAR_TABLE_SIZE)
Decimus 0:05cb0f94324f 151 bar_table_idx1 = 0;
Decimus 0:05cb0f94324f 152 bar_table[bar_table_idx] = bar_raw;
Decimus 0:05cb0f94324f 153 bar_table_sum += bar_table[bar_table_idx];
Decimus 0:05cb0f94324f 154 bar_table_sum -= bar_table[bar_table_idx1];
Decimus 0:05cb0f94324f 155 bar_table_idx = bar_table_idx1;
Decimus 0:05cb0f94324f 156
Decimus 0:05cb0f94324f 157 bar_lpf = bar_lpf*(1-LPFC_BAR) + bar_table_sum*(LPFC_BAR)/21.0f;
Decimus 0:05cb0f94324f 158
Decimus 0:05cb0f94324f 159 // relative bar
Decimus 0:05cb0f94324f 160 //bar_alt = clamp_bot(bar_base - bar_lpf, 0);
Decimus 0:05cb0f94324f 161 bar_alt = bar_lpf - bar_base;
Decimus 0:05cb0f94324f 162 }
Decimus 0:05cb0f94324f 163 }
Decimus 0:05cb0f94324f 164
Decimus 0:05cb0f94324f 165 // Calc Z-displacement after 2-stage integration
Decimus 0:05cb0f94324f 166 void get_alt( float tdelta ){
Decimus 0:05cb0f94324f 167 float vdelta;
Decimus 0:05cb0f94324f 168 float g[3];
Decimus 0:05cb0f94324f 169 float alt_err = 0.0f;
Decimus 0:05cb0f94324f 170 float tmp = 1.0f;
Decimus 0:05cb0f94324f 171
Decimus 0:05cb0f94324f 172 // don't fuse baro in takeoff mode
Decimus 0:05cb0f94324f 173 if( !(mode & MODE_TAKEOFF) && !(mode & MODE_ON) ){
Decimus 0:05cb0f94324f 174 alt_err = clamp((imu[ALT] - bar_alt), -0.5, 0.5) * ALT_P;
Decimus 0:05cb0f94324f 175 tmp = alt_err*ALT_ACC_I;
Decimus 0:05cb0f94324f 176 grv_err[Z]+= acc_lpf[Z] * tmp;
Decimus 0:05cb0f94324f 177
Decimus 0:05cb0f94324f 178 // don't take into account X and Y if UAV position is almost vertical
Decimus 0:05cb0f94324f 179 if(abs(imu[ROLL]) > GRAD5 || abs(imu[PITCH]) > GRAD5) {
Decimus 0:05cb0f94324f 180 grv_err[X]+= acc_lpf[X] * 2 * tmp;
Decimus 0:05cb0f94324f 181 grv_err[Y]+= acc_lpf[Y] * 2 * tmp;
Decimus 0:05cb0f94324f 182 }
Decimus 0:05cb0f94324f 183 }
Decimus 0:05cb0f94324f 184
Decimus 0:05cb0f94324f 185 // fetch gravitational vector from position quaternion
Decimus 0:05cb0f94324f 186 get_grv(&g[0]);
Decimus 0:05cb0f94324f 187
Decimus 0:05cb0f94324f 188 tmp = (acc_lpf[X]-grv_err[0]) * g[X] + (acc_lpf[Y]-grv_err[1]) * g[Y];
Decimus 0:05cb0f94324f 189
Decimus 0:05cb0f94324f 190 grv_old[0] = grv[0];
Decimus 0:05cb0f94324f 191 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;
Decimus 0:05cb0f94324f 192
Decimus 0:05cb0f94324f 193 // calc velocity in meters per seconds by median integration
Decimus 0:05cb0f94324f 194 grv_old[1] = grv[1];
Decimus 0:05cb0f94324f 195 grv[1] = grv[1] + deadband((grv[0]+grv_old[0])/2.0f, ACC_DEADBAND) * tdelta;
Decimus 0:05cb0f94324f 196
Decimus 0:05cb0f94324f 197 // calc displacement in meters by median integration
Decimus 0:05cb0f94324f 198 vdelta = (grv[1]+grv_old[1]) * tdelta / 2.0f;
Decimus 0:05cb0f94324f 199
Decimus 0:05cb0f94324f 200 if( !(mode & MODE_TAKEOFF) && !(mode & MODE_ON) ){
Decimus 0:05cb0f94324f 201 vdelta -= alt_err*ALT_VEL_P;
Decimus 0:05cb0f94324f 202 }
Decimus 0:05cb0f94324f 203 imu[ALT] += vdelta - alt_err;
Decimus 0:05cb0f94324f 204
Decimus 0:05cb0f94324f 205 // pressure and gravitational altitude fusing
Decimus 0:05cb0f94324f 206 //imu[ALT] = imu[ALT]*(1-LPFC_ALT) + bar_alt*LPFC_ALT;
Decimus 0:05cb0f94324f 207 }
Decimus 0:05cb0f94324f 208
Decimus 0:05cb0f94324f 209 // Calc accel
Decimus 0:05cb0f94324f 210 void get_acc(){
Decimus 0:05cb0f94324f 211 mpu_src.getAccelRaw(acc_raw);
Decimus 0:05cb0f94324f 212
Decimus 0:05cb0f94324f 213 // LPF over accel values
Decimus 0:05cb0f94324f 214 for(uint8_t i=0; i<3; i++ ){
Decimus 0:05cb0f94324f 215 acc_lpf[i] = acc_lpf[i]*(1-LPFC_ACC) + acc_raw[i]*LPFC_ACC;
Decimus 0:05cb0f94324f 216 }
Decimus 0:05cb0f94324f 217 }
Decimus 0:05cb0f94324f 218
Decimus 0:05cb0f94324f 219 // Calc gyro
Decimus 0:05cb0f94324f 220 void get_gyr(){
Decimus 0:05cb0f94324f 221 mpu_src.getGyro(gyr);
Decimus 0:05cb0f94324f 222 for(uint8_t i=0; i<3; i++ ){
Decimus 0:05cb0f94324f 223 gyr[i] = TO_RAD*gyr[i];
Decimus 0:05cb0f94324f 224 }
Decimus 0:05cb0f94324f 225 }
Decimus 0:05cb0f94324f 226
Decimus 0:05cb0f94324f 227 // Calc magnitude
Decimus 0:05cb0f94324f 228 void get_mag(){
Decimus 0:05cb0f94324f 229 mag_src.getXYZ(mag_raw);
Decimus 0:05cb0f94324f 230
Decimus 0:05cb0f94324f 231 for(uint8_t i=0; i<3; i++ ){
Decimus 0:05cb0f94324f 232 mag[i] = mag_raw[i] - mag_off_def[i];
Decimus 0:05cb0f94324f 233 }
Decimus 0:05cb0f94324f 234
Decimus 0:05cb0f94324f 235 mag[X] = mag_cal_def[0][0]*mag[X] + mag_cal_def[0][1]*mag[Y] + mag_cal_def[0][2]*mag[Z];
Decimus 0:05cb0f94324f 236 mag[Y] = mag_cal_def[1][0]*mag[X] + mag_cal_def[1][1]*mag[Y] + mag_cal_def[1][2]*mag[Z];
Decimus 0:05cb0f94324f 237 mag[Z] = mag_cal_def[2][0]*mag[X] + mag_cal_def[2][1]*mag[Y] + mag_cal_def[2][2]*mag[Z];
Decimus 0:05cb0f94324f 238 }
Decimus 0:05cb0f94324f 239
Decimus 0:05cb0f94324f 240 // Calc pitch, roll, yaw angles
Decimus 0:05cb0f94324f 241 void get_imu( float tdelta ) {
Decimus 0:05cb0f94324f 242 MadgwickAHRSupdate( tdelta, gyr[X], gyr[Y], gyr[Z],
Decimus 0:05cb0f94324f 243 float(acc_raw[X]), float(acc_raw[Y]), float(acc_raw[Z]),
Decimus 0:05cb0f94324f 244 mag[X], mag[Z], mag[Y]);
Decimus 0:05cb0f94324f 245
Decimus 0:05cb0f94324f 246 quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
Decimus 0:05cb0f94324f 247 quat_2_euler(&quat[0], &imu[0]); // check imu size (4 actually)
Decimus 0:05cb0f94324f 248 }
Decimus 0:05cb0f94324f 249
Decimus 0:05cb0f94324f 250 void takeoff_init(){
Decimus 0:05cb0f94324f 251 // Setup base yaw
Decimus 0:05cb0f94324f 252 pos_base[YAW] = pos[YAW] = imu[YAW];
Decimus 0:05cb0f94324f 253
Decimus 0:05cb0f94324f 254 // Setup base alt
Decimus 0:05cb0f94324f 255 pos_base[ALT] = pos[ALT] = imu[ALT];
Decimus 0:05cb0f94324f 256
Decimus 0:05cb0f94324f 257 // reset velocity
Decimus 0:05cb0f94324f 258 grv[1] = 0.0f;
Decimus 0:05cb0f94324f 259 }
Decimus 0:05cb0f94324f 260
Decimus 0:05cb0f94324f 261 void takeoff_process(){
Decimus 0:05cb0f94324f 262 thr_to += TAKEOFF_SPEED;
Decimus 0:05cb0f94324f 263 if( imu[ALT] > TAKEOFF_LEV_ALT ){
Decimus 0:05cb0f94324f 264 // remove takeoff flag, add altitude stab flag and set target altitude
Decimus 0:05cb0f94324f 265 mode = mode ^ MODE_TAKEOFF;
Decimus 0:05cb0f94324f 266 mode = mode | MODE_STAB_ALT;
Decimus 0:05cb0f94324f 267 pos[ALT] += TAKEOFF_CLIMB_ALT;
Decimus 0:05cb0f94324f 268 }
Decimus 0:05cb0f94324f 269 }
Decimus 0:05cb0f94324f 270
Decimus 0:05cb0f94324f 271 void calc_thrust( float tdelta ){
Decimus 0:05cb0f94324f 272 float p, i, d, error;
Decimus 0:05cb0f94324f 273 float thr_a, thr_base;
Decimus 0:05cb0f94324f 274 float mpos;
Decimus 0:05cb0f94324f 275 TThrust thr_pry;
Decimus 0:05cb0f94324f 276
Decimus 0:05cb0f94324f 277 // x (roll)
Decimus 0:05cb0f94324f 278 #ifdef FLY_MODE_X
Decimus 0:05cb0f94324f 279 mpos = rotx( imu[ROLL], imu[PITCH] );
Decimus 0:05cb0f94324f 280 error = rotx(pos[ROLL] + pos_offset[ROLL], pos[PITCH]) - mpos;
Decimus 0:05cb0f94324f 281 #else
Decimus 0:05cb0f94324f 282 mpos = imu[ROLL]; // moment pos
Decimus 0:05cb0f94324f 283 error = pos[ROLL] + pos_offset[ROLL] - mpos;
Decimus 0:05cb0f94324f 284 #endif
Decimus 0:05cb0f94324f 285 p = pidk[ROLL].p * error;
Decimus 0:05cb0f94324f 286 pid_ist[ROLL] = clamp( pid_ist[ROLL] + error * tdelta, -1, 1 );
Decimus 0:05cb0f94324f 287 i = pidk[ROLL].i * pid_ist[ROLL];
Decimus 0:05cb0f94324f 288 d = pidk[ROLL].d * (pid_dst[ROLL] - mpos);
Decimus 0:05cb0f94324f 289 pid_dst[ROLL] = mpos;
Decimus 0:05cb0f94324f 290
Decimus 0:05cb0f94324f 291 pid[ROLL] = p + i + d;
Decimus 0:05cb0f94324f 292
Decimus 0:05cb0f94324f 293 // y (pitch)
Decimus 0:05cb0f94324f 294 mpos = roty( imu[ROLL], imu[PITCH] );
Decimus 0:05cb0f94324f 295 #ifdef FLY_MODE_X
Decimus 0:05cb0f94324f 296 error = roty(pos[ROLL], pos[PITCH] + pos_offset[PITCH]) - mpos;
Decimus 0:05cb0f94324f 297 #else
Decimus 0:05cb0f94324f 298 error = pos[PITCH] + pos_offset[PITCH] - mpos;
Decimus 0:05cb0f94324f 299 #endif
Decimus 0:05cb0f94324f 300 p = pidk[PITCH].p * error;
Decimus 0:05cb0f94324f 301 pid_ist[PITCH] = clamp( pid_ist[PITCH] + error * tdelta, -1, 1 );
Decimus 0:05cb0f94324f 302 i = pidk[PITCH].i * pid_ist[PITCH];
Decimus 0:05cb0f94324f 303 d = pidk[PITCH].d * (pid_dst[PITCH] - mpos);
Decimus 0:05cb0f94324f 304 pid_dst[PITCH] = mpos;
Decimus 0:05cb0f94324f 305
Decimus 0:05cb0f94324f 306 pid[PITCH] = p + i + d;
Decimus 0:05cb0f94324f 307
Decimus 0:05cb0f94324f 308 pos[YAW] = pos[YAW]+spd_yaw*tdelta;
Decimus 0:05cb0f94324f 309 // z (yaw)
Decimus 0:05cb0f94324f 310 mpos = imu[YAW];
Decimus 0:05cb0f94324f 311 error = pos[YAW] - mpos;
Decimus 0:05cb0f94324f 312 if( error > 180.0*TO_RAD )
Decimus 0:05cb0f94324f 313 error -= 360.0*TO_RAD;
Decimus 0:05cb0f94324f 314 else if( error < -180.0*TO_RAD )
Decimus 0:05cb0f94324f 315 error += 360.0*TO_RAD;
Decimus 0:05cb0f94324f 316
Decimus 0:05cb0f94324f 317 p = pidk[YAW].p * error;
Decimus 0:05cb0f94324f 318 pid_ist[YAW] = clamp( pid_ist[YAW] + error * tdelta, -3, 3 );
Decimus 0:05cb0f94324f 319 i = pidk[YAW].i * pid_ist[YAW];
Decimus 0:05cb0f94324f 320 d = pidk[YAW].d * (pid_dst[YAW] - mpos);
Decimus 0:05cb0f94324f 321 pid_dst[YAW] = mpos;
Decimus 0:05cb0f94324f 322
Decimus 0:05cb0f94324f 323 pid[YAW] = p + i + d;
Decimus 0:05cb0f94324f 324
Decimus 0:05cb0f94324f 325 // Thrust only for PRY stabilization
Decimus 0:05cb0f94324f 326 thr_pry.x1 = -pid[ROLL] * ROLL_SCALE - YAW_PID_DIR * pid[YAW] * YAW_SCALE;
Decimus 0:05cb0f94324f 327 thr_pry.x2 = +pid[ROLL] * ROLL_SCALE - YAW_PID_DIR * pid[YAW] * YAW_SCALE;
Decimus 0:05cb0f94324f 328 thr_pry.y1 = +pid[PITCH] * PITCH_SCALE + YAW_PID_DIR * pid[YAW] * YAW_SCALE;
Decimus 0:05cb0f94324f 329 thr_pry.y2 = -pid[PITCH] * PITCH_SCALE + YAW_PID_DIR * pid[YAW] * YAW_SCALE;
Decimus 0:05cb0f94324f 330
Decimus 0:05cb0f94324f 331 // do alt stabilization only if automatic miode is on
Decimus 0:05cb0f94324f 332 if( mode & MODE_STAB_ALT && !(mode & MODE_MANUAL ) ){
Decimus 0:05cb0f94324f 333 // increase target altitude according to current speed
Decimus 0:05cb0f94324f 334 pos[ALT] = pos[ALT] + spd_alt*tdelta;
Decimus 0:05cb0f94324f 335 mpos = imu[ALT];
Decimus 0:05cb0f94324f 336 error = pos[ALT] - mpos;
Decimus 0:05cb0f94324f 337 p = pidk[ALT].p * error;
Decimus 0:05cb0f94324f 338 pid_ist[ALT] = clamp( pid_ist[ALT] + error * tdelta, -1, 1 );
Decimus 0:05cb0f94324f 339 i = pidk[ALT].i * pid_ist[ALT];
Decimus 0:05cb0f94324f 340 d = pidk[ALT].d * (pid_dst[ALT] - mpos);
Decimus 0:05cb0f94324f 341 pid_dst[ALT] = mpos;
Decimus 0:05cb0f94324f 342
Decimus 0:05cb0f94324f 343 pid[ALT] = p + i + d;
Decimus 0:05cb0f94324f 344
Decimus 0:05cb0f94324f 345 thr_a = pid[ALT] * ALT_SCALE;
Decimus 0:05cb0f94324f 346 } else {
Decimus 0:05cb0f94324f 347 pos[ALT] = imu[ALT];
Decimus 0:05cb0f94324f 348 thr_a = 0.0;
Decimus 0:05cb0f94324f 349 }
Decimus 0:05cb0f94324f 350
Decimus 0:05cb0f94324f 351 if( mode & MODE_MANUAL )
Decimus 0:05cb0f94324f 352 thr_base = clamp(spd_alt*4, 0.0, 1.0);
Decimus 0:05cb0f94324f 353 else
Decimus 0:05cb0f94324f 354 thr_base = thr_to;
Decimus 0:05cb0f94324f 355
Decimus 0:05cb0f94324f 356 // Summary thrust for PRY + ALT stabilization
Decimus 0:05cb0f94324f 357 thr.x1 = clamp( thr_base + thr_pry.x1 + thr_a, 0.0, 1.0 );
Decimus 0:05cb0f94324f 358 thr.x2 = clamp( thr_base + thr_pry.x2 + thr_a, 0.0, 1.0 );
Decimus 0:05cb0f94324f 359 thr.y1 = clamp( thr_base + thr_pry.y1 + thr_a, 0.0, 1.0 );
Decimus 0:05cb0f94324f 360 thr.y2 = clamp( thr_base + thr_pry.y2 + thr_a, 0.0, 1.0 );
Decimus 0:05cb0f94324f 361 thr_avg = (thr.x1 + thr.x2 + thr.y1 + thr.y2) / 4.0f;
Decimus 0:05cb0f94324f 362 }
Decimus 0:05cb0f94324f 363
Decimus 0:05cb0f94324f 364 void init(){
Decimus 0:05cb0f94324f 365 // reset pid values, pid coeffs, and pos offsets
Decimus 0:05cb0f94324f 366 for(uint8_t i=0; i<4; i++){
Decimus 0:05cb0f94324f 367 pos_offset[i] = 0.0;
Decimus 0:05cb0f94324f 368
Decimus 0:05cb0f94324f 369 pid_ist[i] = 0.0;
Decimus 0:05cb0f94324f 370 pid_dst[i] = 0.0;
Decimus 0:05cb0f94324f 371
Decimus 0:05cb0f94324f 372 pidk[i].p = pidk_def[i][0];
Decimus 0:05cb0f94324f 373 pidk[i].i = pidk_def[i][1];
Decimus 0:05cb0f94324f 374 pidk[i].d = pidk_def[i][2];
Decimus 0:05cb0f94324f 375
Decimus 0:05cb0f94324f 376 pos[i] = 0.0;
Decimus 0:05cb0f94324f 377 }
Decimus 0:05cb0f94324f 378
Decimus 0:05cb0f94324f 379 // desire xyz
Decimus 0:05cb0f94324f 380 imu[ALT] = 0.0;
Decimus 0:05cb0f94324f 381
Decimus 0:05cb0f94324f 382 spd_alt = 0;
Decimus 0:05cb0f94324f 383 thr_to = 0.0f;
Decimus 0:05cb0f94324f 384 thr_base = 0.0f;
Decimus 0:05cb0f94324f 385 thr_avg = 0.0f;
Decimus 0:05cb0f94324f 386
Decimus 0:05cb0f94324f 387 for(uint8_t i=0; i<3; i++){
Decimus 0:05cb0f94324f 388 grv[i] = 0.0f;
Decimus 0:05cb0f94324f 389 grv_err[i] = 0.0f;
Decimus 0:05cb0f94324f 390 }
Decimus 0:05cb0f94324f 391 altz = 0;
Decimus 0:05cb0f94324f 392
Decimus 0:05cb0f94324f 393 bar_lpf = 0.0f;
Decimus 0:05cb0f94324f 394 bar_table_idx = 0;
Decimus 0:05cb0f94324f 395
Decimus 0:05cb0f94324f 396 acc_1g = 0.0f;
Decimus 0:05cb0f94324f 397
Decimus 0:05cb0f94324f 398 }
Decimus 0:05cb0f94324f 399
Decimus 0:05cb0f94324f 400 void stop(){
Decimus 0:05cb0f94324f 401 esc.stop();
Decimus 0:05cb0f94324f 402 #ifdef DEBUG_F
Decimus 0:05cb0f94324f 403 flog.save();
Decimus 0:05cb0f94324f 404 #endif
Decimus 0:05cb0f94324f 405 }
Decimus 0:05cb0f94324f 406
Decimus 0:05cb0f94324f 407 void checkMode(){
Decimus 0:05cb0f94324f 408 if( mode & MODE_READY ){
Decimus 0:05cb0f94324f 409 mode = mode ^ MODE_READY;
Decimus 0:05cb0f94324f 410 mode = mode | MODE_STAB_PRY;
Decimus 0:05cb0f94324f 411 if( !(mode & MODE_MANUAL) ){
Decimus 0:05cb0f94324f 412 mode = mode | MODE_TAKEOFF;
Decimus 0:05cb0f94324f 413 }
Decimus 0:05cb0f94324f 414 takeoff_init();
Decimus 0:05cb0f94324f 415 }
Decimus 0:05cb0f94324f 416 else if ( mode & MODE_MANUAL){
Decimus 0:05cb0f94324f 417 //pass
Decimus 0:05cb0f94324f 418 }
Decimus 0:05cb0f94324f 419 else {
Decimus 0:05cb0f94324f 420 }
Decimus 0:05cb0f94324f 421 }
Decimus 0:05cb0f94324f 422
Decimus 0:05cb0f94324f 423 void obey(){
Decimus 0:05cb0f94324f 424 char cbuf[32];
Decimus 0:05cb0f94324f 425 short sbuf[16];
Decimus 0:05cb0f94324f 426 char cbufn = 15;
Decimus 0:05cb0f94324f 427
Decimus 0:05cb0f94324f 428 int i = i2c.receive();
Decimus 0:05cb0f94324f 429 switch (i) {
Decimus 0:05cb0f94324f 430 case I2CSlave::ReadAddressed:
Decimus 0:05cb0f94324f 431 sbuf[0] = (short)(debug_data_1*10000);//(thr_avg*100);
Decimus 0:05cb0f94324f 432 sbuf[1] = (short)(imu[ALT]*100);
Decimus 0:05cb0f94324f 433 sbuf[2] = (short)(pos[ALT]*100);
Decimus 0:05cb0f94324f 434 sbuf[3] = (short)(get_heading());
Decimus 0:05cb0f94324f 435 sbuf[4] = (short)(imu[ROLL]*TO_DEG);
Decimus 0:05cb0f94324f 436 sbuf[5] = (short)(imu[PITCH]*TO_DEG);
Decimus 0:05cb0f94324f 437 sbuf[6] = (short)(mode);
Decimus 0:05cb0f94324f 438 short2char( &sbuf[0], &cbuf[0], 0, 7 );
Decimus 0:05cb0f94324f 439 i2c.write( cbuf, 14 );
Decimus 0:05cb0f94324f 440 break;
Decimus 0:05cb0f94324f 441
Decimus 0:05cb0f94324f 442 case I2CSlave::WriteAddressed:
Decimus 0:05cb0f94324f 443 //receive on/off command
Decimus 0:05cb0f94324f 444 i2c.read(cbuf, 32);
Decimus 0:05cb0f94324f 445 if( cbuf[0]==CMD_I2C_OFF ){
Decimus 0:05cb0f94324f 446 if( mode & MODE_ON ){
Decimus 0:05cb0f94324f 447 stop();
Decimus 0:05cb0f94324f 448 mode = 0; // remove MODE_ON flag
Decimus 0:05cb0f94324f 449 beep(5, 50, &buzz);
Decimus 0:05cb0f94324f 450 beep(1, 200, &buzz);
Decimus 0:05cb0f94324f 451 }
Decimus 0:05cb0f94324f 452 }
Decimus 0:05cb0f94324f 453 else if( cbuf[0]==CMD_I2C_ON ){
Decimus 0:05cb0f94324f 454 if( !(mode & MODE_ON) ){
Decimus 0:05cb0f94324f 455 beep(1, 200, &buzz);
Decimus 0:05cb0f94324f 456 beep(5, 50, &buzz);
Decimus 0:05cb0f94324f 457 mode = mode | MODE_READY | MODE_ON; // set default flags
Decimus 0:05cb0f94324f 458 }
Decimus 0:05cb0f94324f 459 }
Decimus 0:05cb0f94324f 460 // receive manual/auto command
Decimus 0:05cb0f94324f 461 else if( cbuf[0]==CMD_I2C_AUTO ){
Decimus 0:05cb0f94324f 462 if( mode & MODE_MANUAL ){
Decimus 0:05cb0f94324f 463 beep(6, 50, &buzz);
Decimus 0:05cb0f94324f 464 beep(1, 200, &buzz);
Decimus 0:05cb0f94324f 465 mode = mode ^ MODE_MANUAL; // remove MODE_MANUAL flag
Decimus 0:05cb0f94324f 466 }
Decimus 0:05cb0f94324f 467 }
Decimus 0:05cb0f94324f 468 else if( cbuf[0]==CMD_I2C_MANUAL ){
Decimus 0:05cb0f94324f 469 if( !(mode & MODE_MANUAL) ){
Decimus 0:05cb0f94324f 470 beep(1, 200, &buzz);
Decimus 0:05cb0f94324f 471 beep(6, 50, &buzz);
Decimus 0:05cb0f94324f 472 mode = mode | MODE_MANUAL; // add MODE_MANUAL flag
Decimus 0:05cb0f94324f 473 }
Decimus 0:05cb0f94324f 474 }
Decimus 0:05cb0f94324f 475 // receive control values
Decimus 0:05cb0f94324f 476 else if( cbuf[0]==CMD_I2C_CTRL ){
Decimus 0:05cb0f94324f 477 char2short( &cbuf[0], &sbuf[0], 1, cbufn*2 );
Decimus 0:05cb0f94324f 478 spd_alt = sbuf[0]*ALT_MAX/CTRL_DIA;
Decimus 0:05cb0f94324f 479 pos[PITCH] = (sbuf[1]*PITCH_MAX/CTRL_DIA)*TO_RAD;
Decimus 0:05cb0f94324f 480 pos[ROLL] = (sbuf[2]*ROLL_MAX/CTRL_DIA)*TO_RAD;
Decimus 0:05cb0f94324f 481 spd_yaw = (sbuf[3]*YAW_MAX/CTRL_DIA)*TO_RAD;
Decimus 0:05cb0f94324f 482 pidk[ROLL].p = sbuf[4];
Decimus 0:05cb0f94324f 483 pidk[PITCH].p = sbuf[4];
Decimus 0:05cb0f94324f 484 pidk[ROLL].i = sbuf[5];
Decimus 0:05cb0f94324f 485 pidk[PITCH].i = sbuf[5];
Decimus 0:05cb0f94324f 486 pidk[ROLL].d = sbuf[6];
Decimus 0:05cb0f94324f 487 pidk[PITCH].d = sbuf[6];
Decimus 0:05cb0f94324f 488 pidk[YAW].p = sbuf[7];
Decimus 0:05cb0f94324f 489 pidk[YAW].i = sbuf[8];
Decimus 0:05cb0f94324f 490 pidk[YAW].d = sbuf[9];
Decimus 0:05cb0f94324f 491 pidk[ALT].p = sbuf[10];
Decimus 0:05cb0f94324f 492 pidk[ALT].i = sbuf[11];
Decimus 0:05cb0f94324f 493 pidk[ALT].d = sbuf[12];
Decimus 0:05cb0f94324f 494 pos_offset[ROLL] = sbuf[13]*TO_RAD/10.0;
Decimus 0:05cb0f94324f 495 pos_offset[PITCH] = sbuf[14]*TO_RAD/10.0;
Decimus 0:05cb0f94324f 496 }
Decimus 0:05cb0f94324f 497 break;
Decimus 0:05cb0f94324f 498 }
Decimus 0:05cb0f94324f 499 }
Decimus 0:05cb0f94324f 500
Decimus 0:05cb0f94324f 501 int main() {
Decimus 0:05cb0f94324f 502 double tc, td, incl_time, ctrl_time, mag_time, bar_time;
Decimus 0:05cb0f94324f 503 incl_time = ctrl_time = mag_time = bar_time = 0;
Decimus 0:05cb0f94324f 504
Decimus 0:05cb0f94324f 505 // Variables init
Decimus 0:05cb0f94324f 506 init();
Decimus 0:05cb0f94324f 507
Decimus 0:05cb0f94324f 508 blink(1000, &led_1);
Decimus 0:05cb0f94324f 509
Decimus 0:05cb0f94324f 510 // set i2c address
Decimus 0:05cb0f94324f 511 i2c.address(0xA0);
Decimus 0:05cb0f94324f 512
Decimus 0:05cb0f94324f 513 #ifdef DEBUG
Decimus 0:05cb0f94324f 514 float dbg_time = 0;
Decimus 0:05cb0f94324f 515 pc.baud(19200);
Decimus 0:05cb0f94324f 516 #endif
Decimus 0:05cb0f94324f 517
Decimus 0:05cb0f94324f 518 // ESC init
Decimus 0:05cb0f94324f 519 esc.initialize_();
Decimus 0:05cb0f94324f 520
Decimus 0:05cb0f94324f 521 // Accel and Gyro init
Decimus 0:05cb0f94324f 522 mpu_src.init();
Decimus 0:05cb0f94324f 523 mpu_src.getAres();
Decimus 0:05cb0f94324f 524 mpu_src.getGres();
Decimus 0:05cb0f94324f 525
Decimus 0:05cb0f94324f 526 // Barometer init
Decimus 0:05cb0f94324f 527 bar_src.cmd_reset();
Decimus 0:05cb0f94324f 528 calibrate_bar();
Decimus 0:05cb0f94324f 529
Decimus 0:05cb0f94324f 530 beep(2, 100, &buzz);
Decimus 0:05cb0f94324f 531 beep(1, 500, &buzz);
Decimus 0:05cb0f94324f 532
Decimus 0:05cb0f94324f 533 // Accel optional calibration
Decimus 0:05cb0f94324f 534 if( cal_mode ){
Decimus 0:05cb0f94324f 535 calibrate_acc_offsets();
Decimus 0:05cb0f94324f 536 beep(3, 100, &buzz);
Decimus 0:05cb0f94324f 537 beep(1, 500, &buzz);
Decimus 0:05cb0f94324f 538 };
Decimus 0:05cb0f94324f 539 calibrate_acc_grv();
Decimus 0:05cb0f94324f 540 beep(4, 100, &buzz);
Decimus 0:05cb0f94324f 541 beep(1, 500, &buzz);
Decimus 0:05cb0f94324f 542
Decimus 0:05cb0f94324f 543 // Magnitometer init
Decimus 0:05cb0f94324f 544 mag_src.init();
Decimus 0:05cb0f94324f 545
Decimus 0:05cb0f94324f 546 blink(1000, &led_1);
Decimus 0:05cb0f94324f 547 beep(1, 1000, &buzz);
Decimus 0:05cb0f94324f 548
Decimus 0:05cb0f94324f 549 tmr.start();
Decimus 0:05cb0f94324f 550 while(1){
Decimus 0:05cb0f94324f 551 obey();
Decimus 0:05cb0f94324f 552
Decimus 0:05cb0f94324f 553 tc = tmr.read();
Decimus 0:05cb0f94324f 554 if( mag_time + MAG_DELAY < tc ){
Decimus 0:05cb0f94324f 555 mag_time = tmr.read();
Decimus 0:05cb0f94324f 556 get_mag();
Decimus 0:05cb0f94324f 557 }
Decimus 0:05cb0f94324f 558
Decimus 0:05cb0f94324f 559 tc = tmr.read();
Decimus 0:05cb0f94324f 560 if( bar_time + BAR_DELAY < tc ){
Decimus 0:05cb0f94324f 561 bar_time = tc;
Decimus 0:05cb0f94324f 562 get_bar();
Decimus 0:05cb0f94324f 563 }
Decimus 0:05cb0f94324f 564
Decimus 0:05cb0f94324f 565 tc = tmr.read();
Decimus 0:05cb0f94324f 566 if( incl_time + INCL_DELAY < tc ){
Decimus 0:05cb0f94324f 567 td = tc - incl_time;
Decimus 0:05cb0f94324f 568 incl_time = tc;
Decimus 0:05cb0f94324f 569
Decimus 0:05cb0f94324f 570 get_acc(); // collect accel data
Decimus 0:05cb0f94324f 571 get_gyr(); // collect gyro data
Decimus 0:05cb0f94324f 572 get_imu(td); // calc angles, after acc & gyr calcs
Decimus 0:05cb0f94324f 573 get_alt(td); // calc gravity altitude, after grv calc
Decimus 0:05cb0f94324f 574 debug_data_1 = td;
Decimus 0:05cb0f94324f 575 }
Decimus 0:05cb0f94324f 576
Decimus 0:05cb0f94324f 577 tc = tmr.read();
Decimus 0:05cb0f94324f 578 if( ctrl_time + CTRL_DELAY < tc ){
Decimus 0:05cb0f94324f 579 td = tc - ctrl_time;
Decimus 0:05cb0f94324f 580 ctrl_time = tc;
Decimus 0:05cb0f94324f 581
Decimus 0:05cb0f94324f 582 #ifdef DEBUG_F
Decimus 0:05cb0f94324f 583 flog.log(imu[X]*TO_DEG);
Decimus 0:05cb0f94324f 584 flog.log(gyr[X]);
Decimus 0:05cb0f94324f 585 #endif
Decimus 0:05cb0f94324f 586
Decimus 0:05cb0f94324f 587 if( mode & MODE_TAKEOFF ){
Decimus 0:05cb0f94324f 588 takeoff_process();
Decimus 0:05cb0f94324f 589 }
Decimus 0:05cb0f94324f 590
Decimus 0:05cb0f94324f 591 if( mode & MODE_ON ){
Decimus 0:05cb0f94324f 592 calc_thrust( td );
Decimus 0:05cb0f94324f 593 esc.set(thr.x1, thr.x2, thr.y1, thr.y2);
Decimus 0:05cb0f94324f 594 }
Decimus 0:05cb0f94324f 595 }
Decimus 0:05cb0f94324f 596
Decimus 0:05cb0f94324f 597 #ifdef DEBUG
Decimus 0:05cb0f94324f 598 tc = tmr.read();
Decimus 0:05cb0f94324f 599 if( dbg_time + DBG_DELAY < tc ){
Decimus 0:05cb0f94324f 600 dbg_time = tc;
Decimus 0:05cb0f94324f 601 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]);
Decimus 0:05cb0f94324f 602 //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]);
Decimus 0:05cb0f94324f 603 }
Decimus 0:05cb0f94324f 604 #endif
Decimus 0:05cb0f94324f 605
Decimus 0:05cb0f94324f 606 if( mode & MODE_ON )
Decimus 0:05cb0f94324f 607 checkMode();
Decimus 0:05cb0f94324f 608 }
Decimus 0:05cb0f94324f 609 }
Decimus 0:05cb0f94324f 610