HAPSRG / Mbed 2 deprecated HAPStail

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MatrixMath LPS25HB_I2C LSM9DS1 Matrix2 PIDcontroller LoopTicker SBUS_without_mainfile UsaPack solaESKF_wind Vector3 CalibrateMagneto FastPWM

run.cpp@89:316c20d3184c, 2022-01-12 (annotated)

Committer:
NaotoMorita
Date:
Wed Jan 12 09:54:55 2022 +0000
Revision:
89:316c20d3184c
Parent:
88:0fc5df2fddcb
Child:
90:0b1f62f7a83a
test run

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 run()
cocorlow 56:888379912f81 4 {
osaka 85:0b14a2a600fc 5
cocorlow 56:888379912f81 6 pc.printf("reading calibration value\r\n");
cocorlow 56:888379912f81 7 //キャリブレーション値を取得
cocorlow 56:888379912f81 8 U read_calib;
cocorlow 56:888379912f81 9 readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4);
cocorlow 56:888379912f81 10 wait(3);
cocorlow 56:888379912f81 11 pos_tail = (int)read_calib.i[0];
osaka 88:0fc5df2fddcb 12 roll_offset = (float)read_calib.i[10] / 200000.0f;
osaka 88:0fc5df2fddcb 13 pitch_offset = (float)read_calib.i[11] / 200000.0f;
osaka 85:0b14a2a600fc 14 /*
NaotoMorita 61:c05353850017 15 agoffset[3] = float(read_calib.i[7]);
NaotoMorita 61:c05353850017 16 agoffset[4] = float(read_calib.i[8]);
NaotoMorita 61:c05353850017 17 agoffset[5] = float(read_calib.i[9]);
NaotoMorita 61:c05353850017 18 magbiasMin[0] = float(read_calib.i[1])/1000.0f;
NaotoMorita 61:c05353850017 19 magbiasMin[1] = float(read_calib.i[2])/1000.0f;
NaotoMorita 61:c05353850017 20 magbiasMin[2] = float(read_calib.i[3])/1000.0f;
NaotoMorita 61:c05353850017 21 magbiasMax[0] = float(read_calib.i[4])/1000.0f;
NaotoMorita 61:c05353850017 22 magbiasMax[1] = float(read_calib.i[5])/1000.0f;
NaotoMorita 61:c05353850017 23 magbiasMax[2] = float(read_calib.i[6])/1000.0f;
NaotoMorita 71:62eb45ecffe9 24 rpy_align.x = float(read_calib.i[10])/200000.0f;
NaotoMorita 71:62eb45ecffe9 25 rpy_align.y = float(read_calib.i[11])/200000.0f;
NaotoMorita 61:c05353850017 26 magCalibrator.setExtremes(magbiasMin,magbiasMax);
cocorlow 56:888379912f81 27 // tail_address[pos_tail] = (int)read_calib.i[10];
osaka 84:028bd650e8bc 28 */
osaka 85:0b14a2a600fc 29 /*
osaka 84:028bd650e8bc 30 //センサの初期化・ジャイロバイアス・加速度スケールの取得
osaka 84:028bd650e8bc 31 int n_init = 1000;
osaka 84:028bd650e8bc 32 for(int i = 0;i<n_init;i++){
osaka 84:028bd650e8bc 33 lsm.readAccel();
osaka 84:028bd650e8bc 34 lsm.readMag();
osaka 84:028bd650e8bc 35 lsm.readGyro();
osaka 84:028bd650e8bc 36 agoffset[0] += lsm.ax * 9.8f;
osaka 84:028bd650e8bc 37 agoffset[1] += lsm.ay * 9.8f;
osaka 84:028bd650e8bc 38 agoffset[2] += lsm.az * 9.8f-9.8f;
osaka 84:028bd650e8bc 39 agoffset[3] +=(lsm.gx * M_PI / 180.0f);
osaka 84:028bd650e8bc 40 agoffset[4] +=(lsm.gy * M_PI / 180.0f);
osaka 84:028bd650e8bc 41 agoffset[5] +=(lsm.gz * M_PI / 180.0f);
osaka 84:028bd650e8bc 42 palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
osaka 84:028bd650e8bc 43 magref.x += lsm.mx;
osaka 84:028bd650e8bc 44 magref.y += lsm.my;
osaka 84:028bd650e8bc 45 magref.z += lsm.mz;
osaka 84:028bd650e8bc 46 }
osaka 84:028bd650e8bc 47 for(int i = 0;i<6;i++){
osaka 84:028bd650e8bc 48 agoffset[i] /= float(n_init);
osaka 84:028bd650e8bc 49 }
osaka 84:028bd650e8bc 50 magref.x /= float(n_init);
osaka 84:028bd650e8bc 51 magref.y /= float(n_init);
osaka 84:028bd650e8bc 52 magref.z /= float(n_init);
osaka 84:028bd650e8bc 53 palt0 /= float(n_init);
osaka 85:0b14a2a600fc 54 */
cocorlow 56:888379912f81 55
cocorlow 56:888379912f81 56 switch(pos_tail){
cocorlow 56:888379912f81 57 case 0:
cocorlow 56:888379912f81 58 pc.printf("This MBED is Located at Left \r\n");
cocorlow 56:888379912f81 59 break;
cocorlow 56:888379912f81 60 case 1:
cocorlow 56:888379912f81 61 pc.printf("This MBED is Located at Center \r\n");
cocorlow 56:888379912f81 62 break;
cocorlow 56:888379912f81 63 case 2:
cocorlow 56:888379912f81 64 pc.printf("This MBED is Located at Right \r\n");
cocorlow 56:888379912f81 65 break;
cocorlow 56:888379912f81 66 default: // error situation
cocorlow 56:888379912f81 67 pc.printf("error\r\n");
cocorlow 56:888379912f81 68 break;
cocorlow 56:888379912f81 69 }
cocorlow 56:888379912f81 70 pc.printf("tail_address : %d\r\n", tail_address[pos_tail]);
NaotoMorita 71:62eb45ecffe9 71 pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",rpy_align.x*180.0f/M_PI,rpy_align.y*180.0f/M_PI);
NaotoMorita 69:0caaad87cf1d 72
osaka 84:028bd650e8bc 73 //ESKFの共分散設定
osaka 84:028bd650e8bc 74 eskf.setGravity(0.0f,0.0f,9.8f);
osaka 84:028bd650e8bc 75 eskf.setPhatPosition(0.1f);
osaka 84:028bd650e8bc 76 eskf.setPhatVelocity(0.1f);
osaka 84:028bd650e8bc 77 eskf.setPhatAngleError(0.5f);
osaka 84:028bd650e8bc 78 eskf.setPhatAccBias(0.001f);
osaka 84:028bd650e8bc 79 eskf.setPhatGyroBias(0.001f);
osaka 84:028bd650e8bc 80 eskf.setPhatGravity(0.0000001f);
osaka 84:028bd650e8bc 81
osaka 84:028bd650e8bc 82 eskf.setQVelocity(0.001f);
osaka 84:028bd650e8bc 83 eskf.setQAngleError(0.0000025f);
osaka 84:028bd650e8bc 84 eskf.setQAccBias(0.000001f);
osaka 84:028bd650e8bc 85 eskf.setQGyroBias(0.000001f);
osaka 84:028bd650e8bc 86
osaka 84:028bd650e8bc 87 Matrix Rgpspos(3,3);
osaka 84:028bd650e8bc 88 setDiag(Rgpspos,1.0f);
osaka 84:028bd650e8bc 89
osaka 84:028bd650e8bc 90 Matrix Rgpsvel(3,3);
osaka 84:028bd650e8bc 91 Rgpsvel(1,1) = 0.01f;
osaka 84:028bd650e8bc 92 Rgpsvel(2,2) = 0.01f;
osaka 84:028bd650e8bc 93 Rgpsvel(3,3) = 0.1f;
osaka 84:028bd650e8bc 94
osaka 84:028bd650e8bc 95 Matrix Rgps(5,5);
osaka 84:028bd650e8bc 96 setDiag(Rgps,0.05f);
osaka 84:028bd650e8bc 97 Rgps(4,4) = 0.1f;
osaka 84:028bd650e8bc 98 Rgps(5,5) = 0.1f;
osaka 84:028bd650e8bc 99
osaka 84:028bd650e8bc 100 float dynAccCriteria = 0.4f;
osaka 84:028bd650e8bc 101 Matrix Racc(3,3);
osaka 84:028bd650e8bc 102 setDiag(Racc,100.0f);
osaka 84:028bd650e8bc 103 Matrix RaccDyn(3,3);
osaka 84:028bd650e8bc 104 setDiag(RaccDyn,5000.0f);
osaka 84:028bd650e8bc 105
osaka 84:028bd650e8bc 106 Matrix Rheading(1,1);
osaka 84:028bd650e8bc 107 Rheading(1,1) = 0.01f;
osaka 84:028bd650e8bc 108
NaotoMorita 71:62eb45ecffe9 109 wait(0.5);
NaotoMorita 69:0caaad87cf1d 110 Timer _t;
NaotoMorita 69:0caaad87cf1d 111 _t.start();
osaka 84:028bd650e8bc 112
osaka 84:028bd650e8bc 113 /*
NaotoMorita 71:62eb45ecffe9 114 magCalibrator.setExtremes(magbiasMin,magbiasMax);
NaotoMorita 71:62eb45ecffe9 115 for(int i = 0; i < 1000; i++){
NaotoMorita 71:62eb45ecffe9 116 getIMUval();
NaotoMorita 71:62eb45ecffe9 117 }
NaotoMorita 71:62eb45ecffe9 118 ekf.defineQhat(rpy_align);
osaka 84:028bd650e8bc 119 float sum2accnorm = 0;
NaotoMorita 71:62eb45ecffe9 120 float sumaccnorm = 0;
cocorlow 56:888379912f81 121 for(int i = 0; i < 1000; i++){
NaotoMorita 69:0caaad87cf1d 122 float tstart = _t.read();
cocorlow 56:888379912f81 123 getIMUval();
NaotoMorita 69:0caaad87cf1d 124 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 71:62eb45ecffe9 125 ekf.updateGyroBiasConstraints(gyro);
NaotoMorita 71:62eb45ecffe9 126 ekf.fuseErr2Nominal();
NaotoMorita 71:62eb45ecffe9 127 ekf.resetBias();
NaotoMorita 71:62eb45ecffe9 128 //ekf.updateMagMeasures(mag);
NaotoMorita 69:0caaad87cf1d 129 ekf.computeAngles(rpy, rpy_align);
NaotoMorita 69:0caaad87cf1d 130 sumaccnorm += acc.Norm();
NaotoMorita 69:0caaad87cf1d 131 sum2accnorm += acc.Norm()*acc.Norm();
NaotoMorita 69:0caaad87cf1d 132 float tend = _t.read();
NaotoMorita 69:0caaad87cf1d 133 att_dt = (tend-tstart);
cocorlow 56:888379912f81 134 }
NaotoMorita 69:0caaad87cf1d 135 accref.z = sumaccnorm / 1000.0f;
osaka 84:028bd650e8bc 136 */
osaka 84:028bd650e8bc 137
osaka 84:028bd650e8bc 138 while(1)
osaka 84:028bd650e8bc 139 {
osaka 84:028bd650e8bc 140 float tstart = _t.read();
osaka 84:028bd650e8bc 141 getIMUval();
osaka 84:028bd650e8bc 142 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
osaka 84:028bd650e8bc 143 eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
osaka 84:028bd650e8bc 144 Vector3 pi(updateValues.pi[0], updateValues.pi[1], updateValues.pi[2]);
osaka 84:028bd650e8bc 145 Vector3 vi(updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
osaka 84:028bd650e8bc 146 eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
osaka 84:028bd650e8bc 147 float heading = atan2f(-mag.y,mag.x);
osaka 84:028bd650e8bc 148 eskf.updateHeading(heading,Rheading);
osaka 84:028bd650e8bc 149 Matrix Raccpreflight(3,3);
osaka 84:028bd650e8bc 150 setDiag(Raccpreflight,5.0f);
osaka 84:028bd650e8bc 151 eskf.updateAcc(MatrixMath::Vector2mat(acc),Raccpreflight);
osaka 84:028bd650e8bc 152
osaka 84:028bd650e8bc 153 bool preflightCheck = true;
osaka 84:028bd650e8bc 154 Matrix gyroBias = eskf.getGyroBias();
osaka 84:028bd650e8bc 155 if(fabsf(gyro.x-gyroBias(1,1))>2.0f || fabsf(gyro.y-gyroBias(2,1))>2.0f || fabsf(gyro.z-gyroBias(3,1))>2.0f){
osaka 84:028bd650e8bc 156 preflightCheck = false;
osaka 84:028bd650e8bc 157 //twelite.serial.printf("PreFlight Check : high gyro value\r\n");
osaka 84:028bd650e8bc 158 }
osaka 84:028bd650e8bc 159 Matrix accBias = eskf.getAccBias();
osaka 84:028bd650e8bc 160 if(fabsf(acc.x-accBias(1,1))>2.0f || fabsf(acc.y-gyroBias(2,1))>2.0f){
osaka 84:028bd650e8bc 161 preflightCheck = false;
osaka 84:028bd650e8bc 162 //twelite.serial.printf("PreFlight Check : high acc value\r\n");
osaka 84:028bd650e8bc 163 }
osaka 84:028bd650e8bc 164 Matrix vihat = eskf.getVihat();
osaka 84:028bd650e8bc 165 if(fabsf(vihat(1,1))>2.0f || fabsf(vihat(2,1))>2.0f||fabsf(vihat(3,1))>2.0f){
osaka 84:028bd650e8bc 166 preflightCheck = false;
osaka 84:028bd650e8bc 167 //twelite.serial.printf("PreFlight Check : high velocity value\r\n");
osaka 84:028bd650e8bc 168 }
osaka 84:028bd650e8bc 169 Matrix pihat = eskf.getPihat();
osaka 84:028bd650e8bc 170 if(fabsf(pihat(1,1))>2.0f || fabsf(pihat(2,1))>2.0f||fabsf(pihat(3,1))>2.0f){
osaka 84:028bd650e8bc 171 preflightCheck = false;
osaka 84:028bd650e8bc 172 //twelite.serial.printf("PreFlight Check : not home position\r\n");
osaka 84:028bd650e8bc 173 }
osaka 84:028bd650e8bc 174 if(sbus.failSafe){
osaka 84:028bd650e8bc 175 preflightCheck = false;
osaka 84:028bd650e8bc 176 //twelite.serial.printf("PreFlight Check : no RC\r\n");
osaka 84:028bd650e8bc 177 }
osaka 84:028bd650e8bc 178 // sbusデータの読み込み
osaka 84:028bd650e8bc 179 for (int i =0 ; i < 16;i ++){
osaka 84:028bd650e8bc 180 rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
osaka 84:028bd650e8bc 181 }
osaka 84:028bd650e8bc 182 if (rc[4]>-0.3f && rc[6] < -0.3f){
osaka 84:028bd650e8bc 183 preflightCheck = false;
osaka 84:028bd650e8bc 184 //twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n");
osaka 84:028bd650e8bc 185 }
osaka 84:028bd650e8bc 186 if(!(updateValues.gps_fix == 0x02 || updateValues.gps_fix == 0x03)){
osaka 84:028bd650e8bc 187 preflightCheck = false;
osaka 84:028bd650e8bc 188 //twelite.serial.printf("PreFlight Check : no gps lock\r\n");
osaka 84:028bd650e8bc 189 }
osaka 84:028bd650e8bc 190 if(preflightCheck == true){
osaka 84:028bd650e8bc 191 break;
osaka 84:028bd650e8bc 192 }
osaka 85:0b14a2a600fc 193 break; //pre-flight checkなし
osaka 84:028bd650e8bc 194 }
NaotoMorita 73:be7a8b8188de 195
NaotoMorita 73:be7a8b8188de 196 tail.Subscribe(tail_address[pos_tail], &(updateValues));
cocorlow 56:888379912f81 197
cocorlow 56:888379912f81 198 LoopTicker PIDtick;
cocorlow 56:888379912f81 199 PIDtick.attach(calcServoOut,PID_dt);
cocorlow 56:888379912f81 200
osaka 84:028bd650e8bc 201 float tgps = _t.read();
osaka 84:028bd650e8bc 202 float theading = _t.read();
cocorlow 56:888379912f81 203
cocorlow 56:888379912f81 204 while(1)
cocorlow 56:888379912f81 205 {
cocorlow 56:888379912f81 206 float tstart = _t.read();
osaka 84:028bd650e8bc 207 //センサの値を取得
cocorlow 56:888379912f81 208 getIMUval();
osaka 84:028bd650e8bc 209
osaka 84:028bd650e8bc 210 //ekfの更新
osaka 84:028bd650e8bc 211 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
osaka 84:028bd650e8bc 212 eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
osaka 84:028bd650e8bc 213
osaka 84:028bd650e8bc 214 headingUpdateFlag = false;
osaka 84:028bd650e8bc 215 if(tstart-theading>0.05f){
osaka 84:028bd650e8bc 216 Matrix euler = eskf.computeAngles();
osaka 84:028bd650e8bc 217 if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
osaka 84:028bd650e8bc 218 headingUpdateFlag = true;
osaka 84:028bd650e8bc 219 theading = _t.read();
osaka 84:028bd650e8bc 220 }
NaotoMorita 79:aa2631950f46 221 }
osaka 87:981895e1d4f2 222 //eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
NaotoMorita 89:316c20d3184c 223 //pc.printf("%f %f %f %f %f %f\r\n", updateValues.pi[0], updateValues.pi[1], updateValues.pi[2], updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
osaka 87:981895e1d4f2 224
osaka 87:981895e1d4f2 225 if((gpsitow != updateValues.gps_itow) && (updateValues.gps_fix == 0x03)){
osaka 87:981895e1d4f2 226 gpsitow = updateValues.gps_itow;
osaka 84:028bd650e8bc 227 Vector3 pi(updateValues.pi[0], updateValues.pi[1], updateValues.pi[2]);
osaka 84:028bd650e8bc 228 Vector3 vi(updateValues.vi[0], updateValues.vi[1], updateValues.vi[2]);
osaka 84:028bd650e8bc 229 eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
osaka 84:028bd650e8bc 230 //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
osaka 84:028bd650e8bc 231 //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
osaka 84:028bd650e8bc 232 }else if(headingUpdateFlag == true){
osaka 84:028bd650e8bc 233 float heading = atan2f(-mag.y,mag.x);
osaka 84:028bd650e8bc 234 eskf.updateHeading(heading,Rheading);
osaka 84:028bd650e8bc 235 }else{
osaka 84:028bd650e8bc 236 Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
osaka 84:028bd650e8bc 237 dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1);
osaka 84:028bd650e8bc 238 //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2));
osaka 84:028bd650e8bc 239 if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
osaka 84:028bd650e8bc 240 eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn);
osaka 84:028bd650e8bc 241 }else{
osaka 84:028bd650e8bc 242 eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
osaka 84:028bd650e8bc 243 }
osaka 84:028bd650e8bc 244 }
osaka 87:981895e1d4f2 245
cocorlow 56:888379912f81 246 PIDtick.loop();
cocorlow 56:888379912f81 247
osaka 84:028bd650e8bc 248 //制御時間を計測
cocorlow 56:888379912f81 249 float tend = _t.read();
cocorlow 56:888379912f81 250 att_dt = (tend-tstart);
cocorlow 56:888379912f81 251 }
cocorlow 56:888379912f81 252 }