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@127:839ae718713b, 2022-07-08 (annotated)

Committer:
NaotoMorita
Date:
Fri Jul 08 08:25:42 2022 +0000
Revision:
127:839ae718713b
Parent:
122:2e77adcf0c0d 
new

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
NaotoMorita 104:f81befbc5ab7 6 preflightCalibration();
NaotoMorita 69:0caaad87cf1d 7
osaka 84:028bd650e8bc 8 //ESKFの共分散設定
osaka 84:028bd650e8bc 9 eskf.setGravity(0.0f,0.0f,9.8f);
NaotoMorita 101:d9895b8eb49f 10 eskf.setPhatPosition(0.1f);
NaotoMorita 101:d9895b8eb49f 11 eskf.setPhatVelocity(0.1f);
NaotoMorita 101:d9895b8eb49f 12 eskf.setPhatAngleError(0.5f);
NaotoMorita 101:d9895b8eb49f 13 eskf.setPhatAccBias(0.001f);
NaotoMorita 101:d9895b8eb49f 14 eskf.setPhatGyroBias(0.001f);
osaka 84:028bd650e8bc 15 eskf.setPhatGravity(0.0000001f);
NaotoMorita 101:d9895b8eb49f 16
osaka 84:028bd650e8bc 17 eskf.setQVelocity(0.001f);
osaka 84:028bd650e8bc 18 eskf.setQAngleError(0.0000025f);
osaka 84:028bd650e8bc 19 eskf.setQAccBias(0.000001f);
osaka 84:028bd650e8bc 20 eskf.setQGyroBias(0.000001f);
osaka 84:028bd650e8bc 21
osaka 84:028bd650e8bc 22 Matrix Rgps(5,5);
osaka 84:028bd650e8bc 23 setDiag(Rgps,0.05f);
osaka 84:028bd650e8bc 24 Rgps(4,4) = 0.1f;
osaka 84:028bd650e8bc 25 Rgps(5,5) = 0.1f;
osaka 84:028bd650e8bc 26
osaka 84:028bd650e8bc 27 float dynAccCriteria = 0.4f;
osaka 84:028bd650e8bc 28 Matrix Racc(3,3);
osaka 84:028bd650e8bc 29 setDiag(Racc,100.0f);
osaka 84:028bd650e8bc 30 Matrix RaccDyn(3,3);
osaka 84:028bd650e8bc 31 setDiag(RaccDyn,5000.0f);
osaka 84:028bd650e8bc 32
osaka 84:028bd650e8bc 33 Matrix Rheading(1,1);
osaka 84:028bd650e8bc 34 Rheading(1,1) = 0.01f;
osaka 84:028bd650e8bc 35
NaotoMorita 105:aaaed895ffaf 36 Matrix Ropt(2,2);
NaotoMorita 105:aaaed895ffaf 37 setDiag(Ropt,10.0f);
NaotoMorita 105:aaaed895ffaf 38
NaotoMorita 71:62eb45ecffe9 39 wait(0.5);
NaotoMorita 104:f81befbc5ab7 40
NaotoMorita 69:0caaad87cf1d 41 _t.start();
NaotoMorita 73:be7a8b8188de 42 tail.Subscribe(tail_address[pos_tail], &(updateValues));
NaotoMorita 118:97ffe77b6f38 43 pc.Subscribe(0000,&(rp));
cocorlow 56:888379912f81 44
NaotoMorita 121:1d5b3e1f0d21 45 if(hilFlag == false){
NaotoMorita 121:1d5b3e1f0d21 46 preflightCheck();
NaotoMorita 121:1d5b3e1f0d21 47 }else{
NaotoMorita 121:1d5b3e1f0d21 48 prefligt_finished = true;
NaotoMorita 121:1d5b3e1f0d21 49 };
NaotoMorita 120:9f4725deb5a6 50
osaka 91:393b9ae62681 51
cocorlow 56:888379912f81 52 LoopTicker PIDtick;
cocorlow 56:888379912f81 53 PIDtick.attach(calcServoOut,PID_dt);
cocorlow 56:888379912f81 54
cocorlow 56:888379912f81 55 while(1)
cocorlow 56:888379912f81 56 {
NaotoMorita 104:f81befbc5ab7 57 tstart = _t.read();
osaka 84:028bd650e8bc 58 //センサの値を取得
NaotoMorita 120:9f4725deb5a6 59 if(hilFlag==false){
NaotoMorita 120:9f4725deb5a6 60 getIMUval();
NaotoMorita 120:9f4725deb5a6 61 }else{
NaotoMorita 120:9f4725deb5a6 62 getHilIMUval();
NaotoMorita 120:9f4725deb5a6 63 }
osaka 103:34e911b94440 64 calcOpticalVel();
osaka 84:028bd650e8bc 65 //ekfの更新
osaka 84:028bd650e8bc 66 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
osaka 84:028bd650e8bc 67 eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
osaka 116:248f2d8cc11e 68 Matrix pihat = eskf.getPihat();
osaka 116:248f2d8cc11e 69 //pc.printf("pihat: %f %f %f\r\n", pihat(1, 1), pihat(2, 1), pihat(3, 1));
osaka 84:028bd650e8bc 70
NaotoMorita 90:0b1f62f7a83a 71 //キャリブレーション中
NaotoMorita 90:0b1f62f7a83a 72 if(updateValues.calibrationFlag == 1111){
NaotoMorita 104:f81befbc5ab7 73 mainloopCalibration();
osaka 84:028bd650e8bc 74 }else{
NaotoMorita 90:0b1f62f7a83a 75 gpsUpdateFlag = false;
NaotoMorita 101:d9895b8eb49f 76 //pc.printf("%d %d \r\n",updateValues.gps_itow,updateValues.gps_fix );
NaotoMorita 120:9f4725deb5a6 77 if(hilFlag==false){
NaotoMorita 120:9f4725deb5a6 78 if((gpsitow != updateValues.gps_itow) && (updateValues.gps_fix == 0x03)){
NaotoMorita 120:9f4725deb5a6 79 getGPSval();
NaotoMorita 120:9f4725deb5a6 80 gpsUpdateFlag = true;
NaotoMorita 120:9f4725deb5a6 81 gpsitow = updateValues.gps_itow;
NaotoMorita 120:9f4725deb5a6 82 tgps = _t.read();
NaotoMorita 120:9f4725deb5a6 83 }
NaotoMorita 120:9f4725deb5a6 84 }else{
NaotoMorita 120:9f4725deb5a6 85 if(tstart-tgps>0.2f){
NaotoMorita 120:9f4725deb5a6 86 getHilGPSval();
NaotoMorita 120:9f4725deb5a6 87 tgps = _t.read();
NaotoMorita 120:9f4725deb5a6 88 gpsUpdateFlag = true;
NaotoMorita 120:9f4725deb5a6 89 }
NaotoMorita 90:0b1f62f7a83a 90 }
NaotoMorita 90:0b1f62f7a83a 91
NaotoMorita 106:36458fb9b5b7 92 sensorUpdateFlag = false;
NaotoMorita 106:36458fb9b5b7 93 if(tstart-tsensors>0.05f){
NaotoMorita 106:36458fb9b5b7 94 sensorUpdateFlag = true;
NaotoMorita 106:36458fb9b5b7 95 tsensors = _t.read();
NaotoMorita 90:0b1f62f7a83a 96 }
NaotoMorita 90:0b1f62f7a83a 97
NaotoMorita 90:0b1f62f7a83a 98 if(gpsUpdateFlag==true){
NaotoMorita 122:2e77adcf0c0d 99 if(hilFlag==false){
NaotoMorita 122:2e77adcf0c0d 100 Rgps(1, 1) = (updateValues.gps_acc * 0.849f) * (updateValues.gps_acc * 0.849f);
NaotoMorita 122:2e77adcf0c0d 101 Rgps(2, 2) = (updateValues.gps_acc * 0.849f) * (updateValues.gps_acc * 0.849f);
NaotoMorita 122:2e77adcf0c0d 102 }else{
NaotoMorita 122:2e77adcf0c0d 103 Rgps(1, 1) = 1.1f;
NaotoMorita 122:2e77adcf0c0d 104 Rgps(2, 2) = 1.1f;
NaotoMorita 122:2e77adcf0c0d 105 }
NaotoMorita 90:0b1f62f7a83a 106 eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
NaotoMorita 106:36458fb9b5b7 107 //pc.printf("gps\r\n");
NaotoMorita 106:36458fb9b5b7 108 }else if(sensorUpdateFlag == true){
NaotoMorita 106:36458fb9b5b7 109 if(sensorUpdateID==1){
NaotoMorita 106:36458fb9b5b7 110 Matrix euler = eskf.computeAngles();
NaotoMorita 106:36458fb9b5b7 111 if(fabsf(euler(2,1))<30.0f*M_PI/180.0f){
osaka 114:ba221936d53a 112 //float heading = atan2f(-mag.y,mag.x);
osaka 114:ba221936d53a 113 eskf.updateHeading(updateValues.heading,Rheading);
NaotoMorita 106:36458fb9b5b7 114 //pc.printf("heading\r\n");
NaotoMorita 106:36458fb9b5b7 115 }
NaotoMorita 106:36458fb9b5b7 116 sensorUpdateID += 1;
NaotoMorita 106:36458fb9b5b7 117 }else{
NaotoMorita 109:27ae949bc38e 118 //float vnorm_opt2 = updateValues.vx_opt*updateValues.vx_opt+updateValues.vy_opt*updateValues.vy_opt;
NaotoMorita 109:27ae949bc38e 119 //if(updateValues.dist_ir<1.0f && vnorm_opt2 < 400.0f){
NaotoMorita 109:27ae949bc38e 120 //if(vnorm_opt2 < 0.1f){
NaotoMorita 109:27ae949bc38e 121 //Matrix vbOpt(2,1);
NaotoMorita 109:27ae949bc38e 122 //vbOpt(1,1) = updateValues.vx_opt;
NaotoMorita 109:27ae949bc38e 123 //vbOpt(2,1) = updateValues.vy_opt;
NaotoMorita 109:27ae949bc38e 124 //eskf.updateVb(vbOpt,Ropt*0.1f);
NaotoMorita 109:27ae949bc38e 125 //}else{
NaotoMorita 108:e582f8bd4729 126 //Matrix vbOpt(2,1);
NaotoMorita 108:e582f8bd4729 127 //vbOpt(1,1) = updateValues.vx_opt;
NaotoMorita 108:e582f8bd4729 128 //vbOpt(2,1) = updateValues.vy_opt;
NaotoMorita 108:e582f8bd4729 129 //eskf.updateVb(vbOpt,Ropt);
NaotoMorita 109:27ae949bc38e 130 //}
NaotoMorita 106:36458fb9b5b7 131 //pc.printf("opt\r\n");
NaotoMorita 109:27ae949bc38e 132 //}
NaotoMorita 106:36458fb9b5b7 133 sensorUpdateID=1;
NaotoMorita 106:36458fb9b5b7 134 }
osaka 84:028bd650e8bc 135 }else{
NaotoMorita 90:0b1f62f7a83a 136 Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
NaotoMorita 90:0b1f62f7a83a 137 dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1);
NaotoMorita 90:0b1f62f7a83a 138 if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
NaotoMorita 90:0b1f62f7a83a 139 eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn);
NaotoMorita 106:36458fb9b5b7 140 //pc.printf("dyn acc\r\n");
NaotoMorita 90:0b1f62f7a83a 141 }else{
NaotoMorita 90:0b1f62f7a83a 142 eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
NaotoMorita 106:36458fb9b5b7 143 //pc.printf("static acc\r\n");
NaotoMorita 106:36458fb9b5b7 144 }
NaotoMorita 106:36458fb9b5b7 145
osaka 84:028bd650e8bc 146 }
NaotoMorita 106:36458fb9b5b7 147
NaotoMorita 105:aaaed895ffaf 148
osaka 84:028bd650e8bc 149 }
osaka 87:981895e1d4f2 150
cocorlow 56:888379912f81 151 PIDtick.loop();
cocorlow 56:888379912f81 152
osaka 84:028bd650e8bc 153 //制御時間を計測
cocorlow 56:888379912f81 154 float tend = _t.read();
cocorlow 56:888379912f81 155 att_dt = (tend-tstart);
cocorlow 56:888379912f81 156 }
cocorlow 56:888379912f81 157 }