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@92:6a2319f7f6ed, 2022-01-13 (annotated)

Committer:
NaotoMorita
Date:
Thu Jan 13 11:15:34 2022 +0000
Revision:
92:6a2319f7f6ed
Parent:
91:393b9ae62681
Child:
93:b0f7a1e91476
for 1/14 flight test

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