Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
setup.cpp@61:c05353850017, 2021-06-03 (annotated)
- Committer:
- NaotoMorita
- Date:
- Thu Jun 03 11:28:13 2021 +0000
- Revision:
- 61:c05353850017
- Parent:
- 60:b89ac376fba3
- Child:
- 65:ea184054e659
modify calibrate magneto
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 56:888379912f81 | 1 | #include "global.hpp" |
cocorlow | 56:888379912f81 | 2 | |
cocorlow | 56:888379912f81 | 3 | void setup() |
cocorlow | 56:888379912f81 | 4 | { |
NaotoMorita | 58:a7947322db87 | 5 | pc.baud(57600); |
cocorlow | 56:888379912f81 | 6 | pitchPID.setSetPoint(0.0); |
cocorlow | 56:888379912f81 | 7 | pitchratePID.setSetPoint(0.0); |
cocorlow | 56:888379912f81 | 8 | pitchPID.setBias(0.0); |
cocorlow | 56:888379912f81 | 9 | pitchratePID.setBias(0.0); |
cocorlow | 56:888379912f81 | 10 | pitchPID.setOutputLimits(-1.0,1.0); |
cocorlow | 56:888379912f81 | 11 | pitchratePID.setOutputLimits(-1.0,1.0); |
cocorlow | 56:888379912f81 | 12 | pitchPID.setInputLimits(-M_PI, M_PI); |
cocorlow | 56:888379912f81 | 13 | pitchratePID.setInputLimits(-M_PI, M_PI); |
cocorlow | 56:888379912f81 | 14 | |
cocorlow | 56:888379912f81 | 15 | servo.period_us(15000.0); |
cocorlow | 56:888379912f81 | 16 | servo.pulsewidth_us(1500.0); |
cocorlow | 56:888379912f81 | 17 | |
cocorlow | 56:888379912f81 | 18 | accelgyro.initialize(); |
cocorlow | 56:888379912f81 | 19 | //加速度計のフルスケールレンジを設定 |
cocorlow | 56:888379912f81 | 20 | accelgyro.setFullScaleAccelRange(ACCEL_FSR); |
cocorlow | 56:888379912f81 | 21 | //角速度計のフルスケールレンジを設定 |
cocorlow | 56:888379912f81 | 22 | accelgyro.setFullScaleGyroRange(GYRO_FSR); |
cocorlow | 56:888379912f81 | 23 | //MPU6050のLPFを設定 |
cocorlow | 56:888379912f81 | 24 | accelgyro.setDLPFMode(MPU6050_LPF); |
cocorlow | 56:888379912f81 | 25 | //地磁気 |
cocorlow | 56:888379912f81 | 26 | mag_sensor.enable(); |
cocorlow | 56:888379912f81 | 27 | } |
cocorlow | 56:888379912f81 | 28 | |
cocorlow | 56:888379912f81 | 29 | void calibrate() |
cocorlow | 56:888379912f81 | 30 | { |
cocorlow | 56:888379912f81 | 31 | pc.printf("\r\nEnter to Calibration Mode\r\n"); |
cocorlow | 56:888379912f81 | 32 | wait(5); |
cocorlow | 56:888379912f81 | 33 | pc.printf("Acc and Gyro Calibration Start\r\n"); |
cocorlow | 56:888379912f81 | 34 | |
cocorlow | 56:888379912f81 | 35 | int gxs = 0; |
cocorlow | 56:888379912f81 | 36 | int gys = 0; |
cocorlow | 56:888379912f81 | 37 | int gzs = 0; |
cocorlow | 56:888379912f81 | 38 | int iter_n = 10000; |
cocorlow | 56:888379912f81 | 39 | |
cocorlow | 56:888379912f81 | 40 | for(int i = 0;i<iter_n ;i++) |
cocorlow | 56:888379912f81 | 41 | { |
cocorlow | 56:888379912f81 | 42 | accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz); |
cocorlow | 56:888379912f81 | 43 | gxs += gx; |
cocorlow | 56:888379912f81 | 44 | gys += gy; |
cocorlow | 56:888379912f81 | 45 | gzs -= gz; |
cocorlow | 56:888379912f81 | 46 | //wait(0.01); |
cocorlow | 56:888379912f81 | 47 | } |
cocorlow | 56:888379912f81 | 48 | gxs = gxs /iter_n; |
cocorlow | 56:888379912f81 | 49 | gys = gys /iter_n; |
cocorlow | 56:888379912f81 | 50 | gzs = gzs /iter_n; |
cocorlow | 56:888379912f81 | 51 | agoffset[3] = gxs; |
cocorlow | 56:888379912f81 | 52 | agoffset[4] = gys; |
cocorlow | 56:888379912f81 | 53 | agoffset[5] = gzs; |
cocorlow | 56:888379912f81 | 54 | pc.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs); |
NaotoMorita | 59:7b9b842efd5e | 55 | pc.printf("Press userbutton without initial Magbias value from EEPROM\r\n"); |
NaotoMorita | 59:7b9b842efd5e | 56 | int magBiasFromEEPROM = 1; |
NaotoMorita | 59:7b9b842efd5e | 57 | for(int i = 0;i<50 ;i++){ |
NaotoMorita | 59:7b9b842efd5e | 58 | wait(0.1); |
NaotoMorita | 59:7b9b842efd5e | 59 | if(userButton.read() == 1) |
NaotoMorita | 59:7b9b842efd5e | 60 | { |
NaotoMorita | 59:7b9b842efd5e | 61 | magBiasFromEEPROM = 0; |
NaotoMorita | 59:7b9b842efd5e | 62 | break; |
NaotoMorita | 59:7b9b842efd5e | 63 | } |
NaotoMorita | 59:7b9b842efd5e | 64 | }; |
NaotoMorita | 59:7b9b842efd5e | 65 | wait(1); |
NaotoMorita | 59:7b9b842efd5e | 66 | if(magBiasFromEEPROM==1){ |
NaotoMorita | 59:7b9b842efd5e | 67 | pc.printf("Read Initial Magbias from EEPROM\r\n"); |
NaotoMorita | 59:7b9b842efd5e | 68 | U read_bias; |
NaotoMorita | 59:7b9b842efd5e | 69 | readEEPROM(eeprom_address, eeprom_pointeraddress, read_bias.c, N_EEPROM*4); |
NaotoMorita | 59:7b9b842efd5e | 70 | wait(3); |
NaotoMorita | 59:7b9b842efd5e | 71 | for (int i = 0 ; i < N_EEPROM; i ++) |
NaotoMorita | 59:7b9b842efd5e | 72 | { |
NaotoMorita | 61:c05353850017 | 73 | magbiasMin[0] = float(read_bias.i[1])/1000.0f; |
NaotoMorita | 61:c05353850017 | 74 | magbiasMin[1] = float(read_bias.i[2])/1000.0f; |
NaotoMorita | 61:c05353850017 | 75 | magbiasMin[2] = float(read_bias.i[3])/1000.0f; |
NaotoMorita | 61:c05353850017 | 76 | magbiasMax[0] = float(read_bias.i[4])/1000.0f; |
NaotoMorita | 61:c05353850017 | 77 | magbiasMax[1] = float(read_bias.i[5])/1000.0f; |
NaotoMorita | 61:c05353850017 | 78 | magbiasMax[2] = float(read_bias.i[6])/1000.0f; |
NaotoMorita | 61:c05353850017 | 79 | } |
NaotoMorita | 61:c05353850017 | 80 | magCalibrator.setExtremes(magbiasMin,magbiasMax); |
NaotoMorita | 59:7b9b842efd5e | 81 | }else{ |
NaotoMorita | 59:7b9b842efd5e | 82 | pc.printf("Preset Initial Magbias \r\n"); |
NaotoMorita | 59:7b9b842efd5e | 83 | } |
NaotoMorita | 61:c05353850017 | 84 | magCalibrator.getExtremes(magbiasMin,magbiasMax); |
NaotoMorita | 61:c05353850017 | 85 | pc.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); |
NaotoMorita | 61:c05353850017 | 86 | pc.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); |
NaotoMorita | 61:c05353850017 | 87 | |
cocorlow | 56:888379912f81 | 88 | pc.printf("Mag Calibration Start\r\n"); |
cocorlow | 56:888379912f81 | 89 | |
NaotoMorita | 61:c05353850017 | 90 | float inputMag[3]; |
NaotoMorita | 61:c05353850017 | 91 | float outputMag[3]; |
cocorlow | 56:888379912f81 | 92 | while(1) |
cocorlow | 56:888379912f81 | 93 | { |
cocorlow | 56:888379912f81 | 94 | mag_sensor.getAxis(mdata); // flush the magnetmeter |
NaotoMorita | 61:c05353850017 | 95 | inputMag[0] = mdata.x; |
NaotoMorita | 61:c05353850017 | 96 | inputMag[1] = mdata.y; |
NaotoMorita | 61:c05353850017 | 97 | inputMag[2] = mdata.z; |
NaotoMorita | 61:c05353850017 | 98 | magCalibrator.run(inputMag,outputMag); |
cocorlow | 56:888379912f81 | 99 | if(userButton.read() == 1) |
cocorlow | 56:888379912f81 | 100 | { |
cocorlow | 56:888379912f81 | 101 | break; |
cocorlow | 56:888379912f81 | 102 | } |
cocorlow | 56:888379912f81 | 103 | wait(0.001); |
cocorlow | 56:888379912f81 | 104 | } |
NaotoMorita | 61:c05353850017 | 105 | magCalibrator.getExtremes(magbiasMin,magbiasMax); |
NaotoMorita | 61:c05353850017 | 106 | pc.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); |
NaotoMorita | 61:c05353850017 | 107 | pc.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); |
NaotoMorita | 61:c05353850017 | 108 | magCalibrator.getExtremes(magbiasMin,magbiasMax); |
cocorlow | 56:888379912f81 | 109 | pc.printf("Determine Position of MBED\r\n"); |
cocorlow | 56:888379912f81 | 110 | wait(1); |
cocorlow | 56:888379912f81 | 111 | pc.printf("Press the user button\r\n"); |
cocorlow | 56:888379912f81 | 112 | |
cocorlow | 56:888379912f81 | 113 | while(userButton.read() == 0) |
cocorlow | 56:888379912f81 | 114 | { |
cocorlow | 56:888379912f81 | 115 | wait(0.01); |
cocorlow | 56:888379912f81 | 116 | } |
cocorlow | 56:888379912f81 | 117 | while(1) |
cocorlow | 56:888379912f81 | 118 | { |
cocorlow | 56:888379912f81 | 119 | pc.printf("Left\r\n"); |
cocorlow | 56:888379912f81 | 120 | wait(2); |
cocorlow | 56:888379912f81 | 121 | if(userButton.read() == 0) |
cocorlow | 56:888379912f81 | 122 | { |
cocorlow | 56:888379912f81 | 123 | pos_tail = 0; |
cocorlow | 56:888379912f81 | 124 | break; |
cocorlow | 56:888379912f81 | 125 | }; |
cocorlow | 56:888379912f81 | 126 | pc.printf("Center\r\n"); |
cocorlow | 56:888379912f81 | 127 | wait(2); |
cocorlow | 56:888379912f81 | 128 | if(userButton.read() == 0) |
cocorlow | 56:888379912f81 | 129 | { |
cocorlow | 56:888379912f81 | 130 | pos_tail = 1; |
cocorlow | 56:888379912f81 | 131 | break; |
cocorlow | 56:888379912f81 | 132 | }; |
cocorlow | 56:888379912f81 | 133 | pc.printf("Right\r\n"); |
cocorlow | 56:888379912f81 | 134 | wait(2); |
cocorlow | 56:888379912f81 | 135 | if(userButton.read() == 0) |
cocorlow | 56:888379912f81 | 136 | { |
cocorlow | 56:888379912f81 | 137 | pos_tail = 2; |
cocorlow | 56:888379912f81 | 138 | break; |
cocorlow | 56:888379912f81 | 139 | }; |
cocorlow | 56:888379912f81 | 140 | }; |
cocorlow | 56:888379912f81 | 141 | pc.printf("tail_address : %d\r\n", tail_address[pos_tail]); |
cocorlow | 56:888379912f81 | 142 | switch(pos_tail) |
cocorlow | 56:888379912f81 | 143 | { |
cocorlow | 56:888379912f81 | 144 | case 0: |
cocorlow | 56:888379912f81 | 145 | pc.printf("This MBED is Located at Left \r\n"); |
cocorlow | 56:888379912f81 | 146 | break; |
cocorlow | 56:888379912f81 | 147 | case 1: |
cocorlow | 56:888379912f81 | 148 | pc.printf("This MBED is Located at Center \r\n"); |
cocorlow | 56:888379912f81 | 149 | break; |
cocorlow | 56:888379912f81 | 150 | case 2: |
cocorlow | 56:888379912f81 | 151 | pc.printf("This MBED is Located at Right \r\n"); |
cocorlow | 56:888379912f81 | 152 | break; |
cocorlow | 56:888379912f81 | 153 | default: // error situation |
cocorlow | 56:888379912f81 | 154 | pc.printf("error\r\n"); |
cocorlow | 56:888379912f81 | 155 | break; |
cocorlow | 56:888379912f81 | 156 | } |
NaotoMorita | 60:b89ac376fba3 | 157 | pc.printf("Calculating pitch/roll Offset \r\n"); |
cocorlow | 56:888379912f81 | 158 | //姿勢オフセットを計算 |
cocorlow | 56:888379912f81 | 159 | rpy_align.y = 0.0f*M_PI/180.0f; |
cocorlow | 56:888379912f81 | 160 | rpy_align.x = 0.0f*M_PI/180.0f; |
cocorlow | 56:888379912f81 | 161 | float ave_pitch = 0.0f; |
cocorlow | 56:888379912f81 | 162 | float ave_roll = 0.0f; |
cocorlow | 56:888379912f81 | 163 | Timer _t; |
cocorlow | 56:888379912f81 | 164 | _t.start(); |
cocorlow | 56:888379912f81 | 165 | for (int i = 0 ; i < 2200; i++) |
cocorlow | 56:888379912f81 | 166 | { |
cocorlow | 56:888379912f81 | 167 | float tstart = _t.read(); |
cocorlow | 56:888379912f81 | 168 | //姿勢角を更新 |
cocorlow | 56:888379912f81 | 169 | getIMUval(); |
cocorlow | 56:888379912f81 | 170 | ekf.updateBetweenMeasures(gyro, att_dt); |
cocorlow | 56:888379912f81 | 171 | ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag); |
cocorlow | 56:888379912f81 | 172 | ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc); |
cocorlow | 56:888379912f81 | 173 | ekf.computeAngles(rpy, rpy_g, rpy_align); |
cocorlow | 56:888379912f81 | 174 | if(i>199) |
cocorlow | 56:888379912f81 | 175 | { |
cocorlow | 56:888379912f81 | 176 | ave_pitch += rpy.y; |
cocorlow | 56:888379912f81 | 177 | ave_roll += rpy.x; |
cocorlow | 56:888379912f81 | 178 | } |
cocorlow | 56:888379912f81 | 179 | wait(0.001); |
cocorlow | 56:888379912f81 | 180 | float tend = _t.read(); |
cocorlow | 56:888379912f81 | 181 | att_dt = (tend-tstart); |
cocorlow | 56:888379912f81 | 182 | } |
cocorlow | 56:888379912f81 | 183 | |
cocorlow | 56:888379912f81 | 184 | pc.printf("aliginment data : %f(pitch deg) %f(roll deg)\r\n",ave_pitch/2000.0f*180.0f/M_PI,ave_roll/2000.0f*180.0f/M_PI); |
cocorlow | 56:888379912f81 | 185 | pc.printf("Writing to EEPROM...\r\n"); |
cocorlow | 56:888379912f81 | 186 | |
cocorlow | 56:888379912f81 | 187 | U transfer_data; |
cocorlow | 56:888379912f81 | 188 | transfer_data.i[0] = pos_tail; |
NaotoMorita | 61:c05353850017 | 189 | for (int i = 0; i < 3; i++) |
cocorlow | 56:888379912f81 | 190 | { |
NaotoMorita | 61:c05353850017 | 191 | if (!isnan(magbiasMax[i])) |
NaotoMorita | 61:c05353850017 | 192 | transfer_data.i[i+1] = int(magbiasMin[i]*1000); // intに丸めた値を送る。 |
cocorlow | 56:888379912f81 | 193 | else |
cocorlow | 56:888379912f81 | 194 | { |
cocorlow | 56:888379912f81 | 195 | pc.printf("Mag bias is NOT transferred\n"); |
NaotoMorita | 61:c05353850017 | 196 | transfer_data.i[i+1] = 100; |
NaotoMorita | 61:c05353850017 | 197 | } |
NaotoMorita | 61:c05353850017 | 198 | } |
NaotoMorita | 61:c05353850017 | 199 | for (int i = 0; i < 3; i++) |
NaotoMorita | 61:c05353850017 | 200 | { |
NaotoMorita | 61:c05353850017 | 201 | if (!isnan(magbiasMax[i])) |
NaotoMorita | 61:c05353850017 | 202 | transfer_data.i[i+4] = int(magbiasMax[i]*1000); // intに丸めた値を送る。 |
NaotoMorita | 61:c05353850017 | 203 | else |
NaotoMorita | 61:c05353850017 | 204 | { |
NaotoMorita | 61:c05353850017 | 205 | pc.printf("Mag bias is NOT transferred\n"); |
NaotoMorita | 61:c05353850017 | 206 | transfer_data.i[i+4] = 100; |
cocorlow | 56:888379912f81 | 207 | } |
cocorlow | 56:888379912f81 | 208 | } |
cocorlow | 56:888379912f81 | 209 | // gxs,gys,gzsを送る |
cocorlow | 56:888379912f81 | 210 | int gxyzs[3] = {gxs, gys, gzs}; |
NaotoMorita | 61:c05353850017 | 211 | for (int i = 0; i < 3; i++) |
cocorlow | 56:888379912f81 | 212 | { |
NaotoMorita | 61:c05353850017 | 213 | if (!isnan(gxyzs[i])) |
NaotoMorita | 61:c05353850017 | 214 | transfer_data.i[i+7] = gxyzs[i]; |
cocorlow | 56:888379912f81 | 215 | else |
cocorlow | 56:888379912f81 | 216 | { |
cocorlow | 56:888379912f81 | 217 | pc.printf("gxyzs is NOT transferred\n"); |
NaotoMorita | 61:c05353850017 | 218 | transfer_data.i[i+7] = 0; |
cocorlow | 56:888379912f81 | 219 | } |
cocorlow | 56:888379912f81 | 220 | } |
cocorlow | 56:888379912f81 | 221 | |
cocorlow | 56:888379912f81 | 222 | // ave_pitch,ave_rollを送る |
cocorlow | 56:888379912f81 | 223 | int ave_pr[2] = {ave_pitch*100, ave_roll*100}; |
NaotoMorita | 61:c05353850017 | 224 | for (int i = 0; i < 2; i++) |
cocorlow | 56:888379912f81 | 225 | { |
NaotoMorita | 61:c05353850017 | 226 | if (!isnan(ave_pr[i])) |
NaotoMorita | 61:c05353850017 | 227 | transfer_data.i[i+10] = ave_pr[i]; |
cocorlow | 56:888379912f81 | 228 | else |
cocorlow | 56:888379912f81 | 229 | { |
cocorlow | 56:888379912f81 | 230 | pc.printf("alignment data is NOT transferred\n"); |
NaotoMorita | 61:c05353850017 | 231 | transfer_data.i[i+10] = 0; |
cocorlow | 56:888379912f81 | 232 | } |
cocorlow | 56:888379912f81 | 233 | } |
cocorlow | 56:888379912f81 | 234 | // transfer_data.i[10] = tail_address[pos_tail]; |
cocorlow | 56:888379912f81 | 235 | for (int i = 0; i < N_EEPROM; i++) |
cocorlow | 56:888379912f81 | 236 | { |
cocorlow | 56:888379912f81 | 237 | pc.printf("transfer_data[%d]: %d\r\n", i, transfer_data.i[i]); |
cocorlow | 56:888379912f81 | 238 | } |
cocorlow | 56:888379912f81 | 239 | writeEEPROM(eeprom_address, eeprom_pointeraddress, transfer_data.c, N_EEPROM*4); |
cocorlow | 56:888379912f81 | 240 | wait(3); |
cocorlow | 56:888379912f81 | 241 | |
cocorlow | 56:888379912f81 | 242 | U read_test; |
cocorlow | 56:888379912f81 | 243 | readEEPROM(eeprom_address, eeprom_pointeraddress, read_test.c, N_EEPROM*4); |
cocorlow | 56:888379912f81 | 244 | wait(3); |
cocorlow | 56:888379912f81 | 245 | for (int i = 0 ; i < N_EEPROM; i ++) |
cocorlow | 56:888379912f81 | 246 | { |
cocorlow | 56:888379912f81 | 247 | pc.printf("transfer_data[%d]: %d\r\n",i, read_test.i[i]); |
cocorlow | 56:888379912f81 | 248 | } |
cocorlow | 56:888379912f81 | 249 | |
cocorlow | 56:888379912f81 | 250 | while(1) |
cocorlow | 56:888379912f81 | 251 | { |
cocorlow | 56:888379912f81 | 252 | wait(1000); |
cocorlow | 56:888379912f81 | 253 | } |
cocorlow | 56:888379912f81 | 254 | } |