HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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?

UserRevisionLine numberNew 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 }