HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

setup.cpp@56:888379912f81, 2021-05-31 (annotated)

Committer:
cocorlow
Date:
Mon May 31 18:59:36 2021 +0000
Revision:
56:888379912f81
Child:
58:a7947322db87
file divided

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 {
cocorlow 56:888379912f81 5 pitchPID.setSetPoint(0.0);
cocorlow 56:888379912f81 6 pitchratePID.setSetPoint(0.0);
cocorlow 56:888379912f81 7 pitchPID.setBias(0.0);
cocorlow 56:888379912f81 8 pitchratePID.setBias(0.0);
cocorlow 56:888379912f81 9 pitchPID.setOutputLimits(-1.0,1.0);
cocorlow 56:888379912f81 10 pitchratePID.setOutputLimits(-1.0,1.0);
cocorlow 56:888379912f81 11 pitchPID.setInputLimits(-M_PI, M_PI);
cocorlow 56:888379912f81 12 pitchratePID.setInputLimits(-M_PI, M_PI);
cocorlow 56:888379912f81 13
cocorlow 56:888379912f81 14 servo.period_us(15000.0);
cocorlow 56:888379912f81 15 servo.pulsewidth_us(1500.0);
cocorlow 56:888379912f81 16
cocorlow 56:888379912f81 17 accelgyro.initialize();
cocorlow 56:888379912f81 18 //加速度計のフルスケールレンジを設定
cocorlow 56:888379912f81 19 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
cocorlow 56:888379912f81 20 //角速度計のフルスケールレンジを設定
cocorlow 56:888379912f81 21 accelgyro.setFullScaleGyroRange(GYRO_FSR);
cocorlow 56:888379912f81 22 //MPU6050のLPFを設定
cocorlow 56:888379912f81 23 accelgyro.setDLPFMode(MPU6050_LPF);
cocorlow 56:888379912f81 24 //地磁気
cocorlow 56:888379912f81 25 mag_sensor.enable();
cocorlow 56:888379912f81 26 }
cocorlow 56:888379912f81 27
cocorlow 56:888379912f81 28 void calibrate()
cocorlow 56:888379912f81 29 {
cocorlow 56:888379912f81 30 pc.printf("\r\nEnter to Calibration Mode\r\n");
cocorlow 56:888379912f81 31 wait(5);
cocorlow 56:888379912f81 32 pc.printf("Acc and Gyro Calibration Start\r\n");
cocorlow 56:888379912f81 33
cocorlow 56:888379912f81 34 int gxs = 0;
cocorlow 56:888379912f81 35 int gys = 0;
cocorlow 56:888379912f81 36 int gzs = 0;
cocorlow 56:888379912f81 37 int iter_n = 10000;
cocorlow 56:888379912f81 38
cocorlow 56:888379912f81 39 for(int i = 0;i<iter_n ;i++)
cocorlow 56:888379912f81 40 {
cocorlow 56:888379912f81 41 accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
cocorlow 56:888379912f81 42 gxs += gx;
cocorlow 56:888379912f81 43 gys += gy;
cocorlow 56:888379912f81 44 gzs -= gz;
cocorlow 56:888379912f81 45 //wait(0.01);
cocorlow 56:888379912f81 46 }
cocorlow 56:888379912f81 47 gxs = gxs /iter_n;
cocorlow 56:888379912f81 48 gys = gys /iter_n;
cocorlow 56:888379912f81 49 gzs = gzs /iter_n;
cocorlow 56:888379912f81 50 agoffset[3] = gxs;
cocorlow 56:888379912f81 51 agoffset[4] = gys;
cocorlow 56:888379912f81 52 agoffset[5] = gzs;
cocorlow 56:888379912f81 53 pc.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
cocorlow 56:888379912f81 54 pc.printf("Mag Calibration Start\r\n");
cocorlow 56:888379912f81 55
cocorlow 56:888379912f81 56 float f = 0;
cocorlow 56:888379912f81 57
cocorlow 56:888379912f81 58 while(1)
cocorlow 56:888379912f81 59 {
cocorlow 56:888379912f81 60 mag_sensor.getAxis(mdata); // flush the magnetmeter
cocorlow 56:888379912f81 61 magval[0] = (mdata.x - magbias[0]);
cocorlow 56:888379912f81 62 magval[1] = (mdata.y - magbias[1]);
cocorlow 56:888379912f81 63 magval[2] = (mdata.z - magbias[2]);
cocorlow 56:888379912f81 64 float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2];
cocorlow 56:888379912f81 65 float lr = 0.000001f;
cocorlow 56:888379912f81 66 f = mag_r - magbias[3]*magbias[3];
cocorlow 56:888379912f81 67 magbias[0] = magbias[0] + 4 * lr * f * magval[0];
cocorlow 56:888379912f81 68 magbias[1] = magbias[1] + 4 * lr * f * magval[1];
cocorlow 56:888379912f81 69 magbias[2] = magbias[2] + 4 * lr * f * magval[2];
cocorlow 56:888379912f81 70 magbias[3] = magbias[3] + 4 * lr * f * magbias[3];
cocorlow 56:888379912f81 71 if(userButton.read() == 1)
cocorlow 56:888379912f81 72 {
cocorlow 56:888379912f81 73 break;
cocorlow 56:888379912f81 74 }
cocorlow 56:888379912f81 75 wait(0.001);
cocorlow 56:888379912f81 76 }
cocorlow 56:888379912f81 77 pc.printf("Magbias : %f, %f, %f, %f\r\n", magbias[0], magbias[1], magbias[2], magbias[3]);
cocorlow 56:888379912f81 78 pc.printf("Determine Position of MBED\r\n");
cocorlow 56:888379912f81 79 wait(1);
cocorlow 56:888379912f81 80 pc.printf("Press the user button\r\n");
cocorlow 56:888379912f81 81
cocorlow 56:888379912f81 82 while(userButton.read() == 0)
cocorlow 56:888379912f81 83 {
cocorlow 56:888379912f81 84 wait(0.01);
cocorlow 56:888379912f81 85 }
cocorlow 56:888379912f81 86 while(1)
cocorlow 56:888379912f81 87 {
cocorlow 56:888379912f81 88 pc.printf("Left\r\n");
cocorlow 56:888379912f81 89 wait(2);
cocorlow 56:888379912f81 90 if(userButton.read() == 0)
cocorlow 56:888379912f81 91 {
cocorlow 56:888379912f81 92 pos_tail = 0;
cocorlow 56:888379912f81 93 break;
cocorlow 56:888379912f81 94 };
cocorlow 56:888379912f81 95 pc.printf("Center\r\n");
cocorlow 56:888379912f81 96 wait(2);
cocorlow 56:888379912f81 97 if(userButton.read() == 0)
cocorlow 56:888379912f81 98 {
cocorlow 56:888379912f81 99 pos_tail = 1;
cocorlow 56:888379912f81 100 break;
cocorlow 56:888379912f81 101 };
cocorlow 56:888379912f81 102 pc.printf("Right\r\n");
cocorlow 56:888379912f81 103 wait(2);
cocorlow 56:888379912f81 104 if(userButton.read() == 0)
cocorlow 56:888379912f81 105 {
cocorlow 56:888379912f81 106 pos_tail = 2;
cocorlow 56:888379912f81 107 break;
cocorlow 56:888379912f81 108 };
cocorlow 56:888379912f81 109 };
cocorlow 56:888379912f81 110 pc.printf("tail_address : %d\r\n", tail_address[pos_tail]);
cocorlow 56:888379912f81 111 switch(pos_tail)
cocorlow 56:888379912f81 112 {
cocorlow 56:888379912f81 113 case 0:
cocorlow 56:888379912f81 114 pc.printf("This MBED is Located at Left \r\n");
cocorlow 56:888379912f81 115 break;
cocorlow 56:888379912f81 116 case 1:
cocorlow 56:888379912f81 117 pc.printf("This MBED is Located at Center \r\n");
cocorlow 56:888379912f81 118 break;
cocorlow 56:888379912f81 119 case 2:
cocorlow 56:888379912f81 120 pc.printf("This MBED is Located at Right \r\n");
cocorlow 56:888379912f81 121 break;
cocorlow 56:888379912f81 122 default: // error situation
cocorlow 56:888379912f81 123 pc.printf("error\r\n");
cocorlow 56:888379912f81 124 break;
cocorlow 56:888379912f81 125 }
cocorlow 56:888379912f81 126
cocorlow 56:888379912f81 127 //姿勢オフセットを計算
cocorlow 56:888379912f81 128 rpy_align.y = 0.0f*M_PI/180.0f;
cocorlow 56:888379912f81 129 rpy_align.x = 0.0f*M_PI/180.0f;
cocorlow 56:888379912f81 130 float ave_pitch = 0.0f;
cocorlow 56:888379912f81 131 float ave_roll = 0.0f;
cocorlow 56:888379912f81 132 Timer _t;
cocorlow 56:888379912f81 133 _t.start();
cocorlow 56:888379912f81 134 for (int i = 0 ; i < 2200; i++)
cocorlow 56:888379912f81 135 {
cocorlow 56:888379912f81 136 float tstart = _t.read();
cocorlow 56:888379912f81 137 //姿勢角を更新
cocorlow 56:888379912f81 138 getIMUval();
cocorlow 56:888379912f81 139 ekf.updateBetweenMeasures(gyro, att_dt);
cocorlow 56:888379912f81 140 ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag);
cocorlow 56:888379912f81 141 ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc);
cocorlow 56:888379912f81 142 ekf.computeAngles(rpy, rpy_g, rpy_align);
cocorlow 56:888379912f81 143 if(i>199)
cocorlow 56:888379912f81 144 {
cocorlow 56:888379912f81 145 ave_pitch += rpy.y;
cocorlow 56:888379912f81 146 ave_roll += rpy.x;
cocorlow 56:888379912f81 147 }
cocorlow 56:888379912f81 148 wait(0.001);
cocorlow 56:888379912f81 149 float tend = _t.read();
cocorlow 56:888379912f81 150 att_dt = (tend-tstart);
cocorlow 56:888379912f81 151 }
cocorlow 56:888379912f81 152
cocorlow 56:888379912f81 153 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 154 pc.printf("Writing to EEPROM...\r\n");
cocorlow 56:888379912f81 155
cocorlow 56:888379912f81 156 U transfer_data;
cocorlow 56:888379912f81 157 transfer_data.i[0] = pos_tail;
cocorlow 56:888379912f81 158 for (int i = 1; i < 5; i++)
cocorlow 56:888379912f81 159 {
cocorlow 56:888379912f81 160 if (!isnan(magbias[i - 1]))
cocorlow 56:888379912f81 161 transfer_data.i[i] = int(magbias[i - 1]*1000); // intに丸めた値を送る。
cocorlow 56:888379912f81 162 else
cocorlow 56:888379912f81 163 {
cocorlow 56:888379912f81 164 pc.printf("Mag bias is NOT transferred\n");
cocorlow 56:888379912f81 165 transfer_data.i[i] = 100;
cocorlow 56:888379912f81 166 }
cocorlow 56:888379912f81 167 }
cocorlow 56:888379912f81 168 // gxs,gys,gzsを送る
cocorlow 56:888379912f81 169 int gxyzs[3] = {gxs, gys, gzs};
cocorlow 56:888379912f81 170 for (int i = 5; i < 8; i++)
cocorlow 56:888379912f81 171 {
cocorlow 56:888379912f81 172 if (!isnan(gxyzs[i - 5]))
cocorlow 56:888379912f81 173 transfer_data.i[i] = gxyzs[i - 5];
cocorlow 56:888379912f81 174 else
cocorlow 56:888379912f81 175 {
cocorlow 56:888379912f81 176 pc.printf("gxyzs is NOT transferred\n");
cocorlow 56:888379912f81 177 transfer_data.i[i] = 0;
cocorlow 56:888379912f81 178 }
cocorlow 56:888379912f81 179 }
cocorlow 56:888379912f81 180
cocorlow 56:888379912f81 181 // ave_pitch,ave_rollを送る
cocorlow 56:888379912f81 182 int ave_pr[2] = {ave_pitch*100, ave_roll*100};
cocorlow 56:888379912f81 183 for (int i = 8; i < 10; i++)
cocorlow 56:888379912f81 184 {
cocorlow 56:888379912f81 185 if (!isnan(ave_pr[i - 8]))
cocorlow 56:888379912f81 186 transfer_data.i[i] = ave_pr[i - 8];
cocorlow 56:888379912f81 187 else
cocorlow 56:888379912f81 188 {
cocorlow 56:888379912f81 189 pc.printf("alignment data is NOT transferred\n");
cocorlow 56:888379912f81 190 transfer_data.i[i] = 0;
cocorlow 56:888379912f81 191 }
cocorlow 56:888379912f81 192 }
cocorlow 56:888379912f81 193 // transfer_data.i[10] = tail_address[pos_tail];
cocorlow 56:888379912f81 194 for (int i = 0; i < N_EEPROM; i++)
cocorlow 56:888379912f81 195 {
cocorlow 56:888379912f81 196 pc.printf("transfer_data[%d]: %d\r\n", i, transfer_data.i[i]);
cocorlow 56:888379912f81 197 }
cocorlow 56:888379912f81 198 writeEEPROM(eeprom_address, eeprom_pointeraddress, transfer_data.c, N_EEPROM*4);
cocorlow 56:888379912f81 199 wait(3);
cocorlow 56:888379912f81 200
cocorlow 56:888379912f81 201 U read_test;
cocorlow 56:888379912f81 202 readEEPROM(eeprom_address, eeprom_pointeraddress, read_test.c, N_EEPROM*4);
cocorlow 56:888379912f81 203 wait(3);
cocorlow 56:888379912f81 204 for (int i = 0 ; i < N_EEPROM; i ++)
cocorlow 56:888379912f81 205 {
cocorlow 56:888379912f81 206 pc.printf("transfer_data[%d]: %d\r\n",i, read_test.i[i]);
cocorlow 56:888379912f81 207 }
cocorlow 56:888379912f81 208
cocorlow 56:888379912f81 209 while(1)
cocorlow 56:888379912f81 210 {
cocorlow 56:888379912f81 211 wait(1000);
cocorlow 56:888379912f81 212 }
cocorlow 56:888379912f81 213 }