Quadrotor stabi
Dependencies: ESC HMC5883L MPU6050 MadgwickAHRS MemLog Servo mbed ms5611
main.cpp@0:05cb0f94324f, 2016-05-30 (annotated)
- Committer:
- Decimus
- Date:
- Mon May 30 08:11:32 2016 +0000
- Revision:
- 0:05cb0f94324f
[+]
Who changed what in which revision?
User | Revision | Line number | New 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 |