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

run.cpp@141:725321fe2949, 2021-12-10 (annotated)

Committer:
cocorlow
Date:
Fri Dec 10 10:43:50 2021 +0000
Revision:
141:725321fe2949
Parent:
139:b378528c05f2
Child:
143:53808e4e684c
Eigen completed

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 {
cocorlow 141:725321fe2949 5 wait(0.5);
cocorlow 141:725321fe2949 6
cocorlow 141:725321fe2949 7 //センサの初期化・ジャイロバイアス・加速度スケールの取得
cocorlow 141:725321fe2949 8 int n_init = 1000;
cocorlow 141:725321fe2949 9 for(int i = 0;i<n_init;i++){
cocorlow 141:725321fe2949 10 lsm.readAccel();
cocorlow 141:725321fe2949 11 lsm.readMag();
cocorlow 141:725321fe2949 12 lsm.readGyro();
cocorlow 141:725321fe2949 13 agoffset[0] += lsm.ax * 9.8f;
cocorlow 141:725321fe2949 14 agoffset[1] += lsm.ay * 9.8f;
cocorlow 141:725321fe2949 15 agoffset[2] += lsm.az * 9.8f-9.8f;
cocorlow 141:725321fe2949 16 agoffset[3] +=(lsm.gx * M_PI_F / 180.0f);
cocorlow 141:725321fe2949 17 agoffset[4] +=(lsm.gy * M_PI_F / 180.0f);
cocorlow 141:725321fe2949 18 agoffset[5] +=(lsm.gz * M_PI_F / 180.0f);
cocorlow 141:725321fe2949 19 palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars());
cocorlow 141:725321fe2949 20 magref(0) += lsm.mx;
cocorlow 141:725321fe2949 21 magref(1) += lsm.my;
cocorlow 141:725321fe2949 22 magref(2) += lsm.mz;
cocorlow 141:725321fe2949 23 }
cocorlow 141:725321fe2949 24 for(int i = 0;i<6;i++){
cocorlow 141:725321fe2949 25 agoffset[i] /= float(n_init);
cocorlow 141:725321fe2949 26 }
cocorlow 141:725321fe2949 27 magref /= float(n_init);
cocorlow 141:725321fe2949 28 palt0 /= float(n_init);
cocorlow 141:725321fe2949 29 twelite.serial.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
cocorlow 141:725321fe2949 30
cocorlow 141:725321fe2949 31 //ESKFの共分散設定
cocorlow 141:725321fe2949 32 eskf.setGravity(0.0f,0.0f,9.8f);
cocorlow 141:725321fe2949 33 eskf.setPhatPosition(0.1f);
cocorlow 141:725321fe2949 34 eskf.setPhatVelocity(0.1f);
cocorlow 141:725321fe2949 35 eskf.setPhatAngleError(0.5f);
cocorlow 141:725321fe2949 36 eskf.setPhatAccBias(0.001f);
cocorlow 141:725321fe2949 37 eskf.setPhatGyroBias(0.001f);
cocorlow 141:725321fe2949 38 eskf.setPhatGravity(0.0000001f);
cocorlow 141:725321fe2949 39
cocorlow 141:725321fe2949 40 eskf.setQVelocity(0.001f);
cocorlow 141:725321fe2949 41 eskf.setQAngleError(0.0000025f);
cocorlow 141:725321fe2949 42 eskf.setQAccBias(0.000001f);
cocorlow 141:725321fe2949 43 eskf.setQGyroBias(0.000001f);
cocorlow 141:725321fe2949 44
cocorlow 141:725321fe2949 45 Matrix3f Rgpspos;
cocorlow 141:725321fe2949 46 setDiag(Rgpspos,1.0f);
cocorlow 141:725321fe2949 47
cocorlow 141:725321fe2949 48 Matrix3f Rgpsvel;
cocorlow 141:725321fe2949 49 Rgpsvel(0,0) = 0.01f;
cocorlow 141:725321fe2949 50 Rgpsvel(1,1) = 0.01f;
cocorlow 141:725321fe2949 51 Rgpsvel(2,2) = 0.1f;
cocorlow 141:725321fe2949 52
cocorlow 141:725321fe2949 53 MatrixXf Rgps(5,5);
cocorlow 141:725321fe2949 54 setDiag(Rgps,0.05f);
cocorlow 141:725321fe2949 55 Rgps(3,3) = 0.1f;
cocorlow 141:725321fe2949 56 Rgps(4,4) = 0.1f;
cocorlow 141:725321fe2949 57
cocorlow 141:725321fe2949 58 float dynAccCriteria = 0.4f;
cocorlow 141:725321fe2949 59 Matrix3f Racc;
cocorlow 141:725321fe2949 60 setDiag(Racc,100.0f);
cocorlow 141:725321fe2949 61 Matrix3f RaccDyn;
cocorlow 141:725321fe2949 62 setDiag(RaccDyn,5000.0f);
cocorlow 141:725321fe2949 63
cocorlow 141:725321fe2949 64 Matrix<float, 1, 1> Rheading;
cocorlow 141:725321fe2949 65 Rheading(0, 0) = 0.01f;
cocorlow 141:725321fe2949 66
cocorlow 141:725321fe2949 67 _t.start();
cocorlow 141:725321fe2949 68 //センサ正常性チェック
cocorlow 141:725321fe2949 69 if(hilFlag == false){
cocorlow 141:725321fe2949 70 while(1){
cocorlow 141:725321fe2949 71 float tstart = _t.read();
cocorlow 141:725321fe2949 72 getIMUval();
cocorlow 141:725321fe2949 73 getGPSval();
cocorlow 141:725321fe2949 74 eskf.updateNominal(acc, gyro, att_dt);
cocorlow 141:725321fe2949 75 eskf.updateErrState(acc, gyro, att_dt);
cocorlow 141:725321fe2949 76 eskf.updateGPS(pi, palt, vi, Rgps);
cocorlow 141:725321fe2949 77 float heading = std::atan2(-mag(2), mag(0));
cocorlow 141:725321fe2949 78 eskf.updateHeading(heading, Rheading);
cocorlow 141:725321fe2949 79 Matrix3f Raccpreflight;
cocorlow 141:725321fe2949 80 setDiag(Raccpreflight, 5.0f);
cocorlow 141:725321fe2949 81 eskf.updateAcc(acc, Raccpreflight);
cocorlow 141:725321fe2949 82
cocorlow 141:725321fe2949 83 bool preflightCheck = true;
cocorlow 141:725321fe2949 84 Vector3f gyroBias = eskf.getGyroBias();
cocorlow 141:725321fe2949 85 if(fabsf(gyro(0)-gyroBias(0))>2.0f || fabsf(gyro(1)-gyroBias(1))>2.0f || fabsf(gyro(2)-gyroBias(2))>2.0f){
cocorlow 141:725321fe2949 86 preflightCheck = false;
cocorlow 141:725321fe2949 87 twelite.serial.printf("PreFlight Check : high gyro value\r\n");
cocorlow 141:725321fe2949 88 }
cocorlow 141:725321fe2949 89 Vector3f accBias = eskf.getAccBias();
cocorlow 141:725321fe2949 90 if(fabsf(acc(0)-accBias(0))>2.0f || fabsf(acc(1)-gyroBias(1))>2.0f){
cocorlow 141:725321fe2949 91 preflightCheck = false;
cocorlow 141:725321fe2949 92 twelite.serial.printf("PreFlight Check : high acc value\r\n");
cocorlow 141:725321fe2949 93 }
cocorlow 141:725321fe2949 94 Vector3f vihat = eskf.getVihat();
cocorlow 141:725321fe2949 95 if(fabsf(vihat(0))>2.0f || fabsf(vihat(1))>2.0f||fabsf(vihat(2))>2.0f){
cocorlow 141:725321fe2949 96 preflightCheck = false;
cocorlow 141:725321fe2949 97 twelite.serial.printf("PreFlight Check : high velocity value\r\n");
cocorlow 141:725321fe2949 98 }
cocorlow 141:725321fe2949 99 Vector3f pihat = eskf.getPihat();
cocorlow 141:725321fe2949 100 if(fabsf(pihat(0))>2.0f || fabsf(pihat(1))>2.0f||fabsf(pihat(2))>2.0f){
cocorlow 141:725321fe2949 101 preflightCheck = false;
cocorlow 141:725321fe2949 102 twelite.serial.printf("PreFlight Check : not home position\r\n");
cocorlow 141:725321fe2949 103 }
cocorlow 141:725321fe2949 104 if(sbus.failSafe){
cocorlow 141:725321fe2949 105 preflightCheck = false;
cocorlow 141:725321fe2949 106 twelite.serial.printf("PreFlight Check : no RC\r\n");
cocorlow 141:725321fe2949 107 }
cocorlow 141:725321fe2949 108 // sbusデータの読み込み
cocorlow 141:725321fe2949 109 for (int i =0 ; i < 16;i ++){
cocorlow 141:725321fe2949 110 rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
cocorlow 141:725321fe2949 111 }
cocorlow 141:725321fe2949 112 if (rc[4]>-0.3f && rc[6] < -0.3f){
cocorlow 141:725321fe2949 113 preflightCheck = false;
cocorlow 141:725321fe2949 114 twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n");
cocorlow 141:725321fe2949 115 }
cocorlow 141:725321fe2949 116 if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){
cocorlow 141:725321fe2949 117 preflightCheck = false;
cocorlow 141:725321fe2949 118 twelite.serial.printf("PreFlight Check : no gps lock\r\n");
cocorlow 141:725321fe2949 119 }
cocorlow 141:725321fe2949 120
cocorlow 141:725321fe2949 121 if(preflightCheck == true){
cocorlow 141:725321fe2949 122 break;
cocorlow 141:725321fe2949 123 }
cocorlow 141:725321fe2949 124 }
cocorlow 141:725321fe2949 125 }
cocorlow 141:725321fe2949 126 twelite.serial.printf("PreFlight Check Completed\r\n");
cocorlow 141:725321fe2949 127 //usaPack通信開始
cocorlow 141:725321fe2949 128 pc.Subscribe(0000, &(vp));
cocorlow 141:725321fe2949 129
cocorlow 141:725321fe2949 130 //制御ループのアタッチ
cocorlow 141:725321fe2949 131 LoopTicker PIDtick;
cocorlow 141:725321fe2949 132 PIDtick.attach(calcServoOut,PID_dt);
cocorlow 141:725321fe2949 133
cocorlow 141:725321fe2949 134 float tgps = _t.read();
cocorlow 141:725321fe2949 135 float theading = _t.read();
cocorlow 141:725321fe2949 136 while(1)
cocorlow 141:725321fe2949 137 {
cocorlow 141:725321fe2949 138 float tstart = _t.read();
cocorlow 141:725321fe2949 139 //センサの値を取得
cocorlow 141:725321fe2949 140 if(hilFlag == true){
cocorlow 141:725321fe2949 141 getHilIMUval();
cocorlow 141:725321fe2949 142 }else{
cocorlow 141:725321fe2949 143 getIMUval();
cocorlow 141:725321fe2949 144 }
cocorlow 141:725321fe2949 145 //ekfの更新
cocorlow 141:725321fe2949 146 eskf.updateNominal(acc, gyro, att_dt);
cocorlow 141:725321fe2949 147 eskf.updateErrState(acc, gyro, att_dt);
cocorlow 141:725321fe2949 148
cocorlow 141:725321fe2949 149 if(hilFlag == true){
cocorlow 141:725321fe2949 150 if(tstart-tgps>0.2f){
cocorlow 141:725321fe2949 151 getHilGPSval();
cocorlow 141:725321fe2949 152 tgps = _t.read();
cocorlow 141:725321fe2949 153 gpsUpdateFlag = true;
cocorlow 141:725321fe2949 154 }else{
cocorlow 141:725321fe2949 155 gpsUpdateFlag = false;
cocorlow 141:725321fe2949 156 }
cocorlow 141:725321fe2949 157 }else{
cocorlow 141:725321fe2949 158 if(userButton.read()==1)
cocorlow 141:725321fe2949 159 {
cocorlow 141:725321fe2949 160 gpsLlh0Fixed = false;
cocorlow 141:725321fe2949 161 };
cocorlow 141:725321fe2949 162 getGPSval();
cocorlow 141:725321fe2949 163 }
cocorlow 141:725321fe2949 164
cocorlow 141:725321fe2949 165 headingUpdateFlag = false;
cocorlow 141:725321fe2949 166 if(tstart-theading>0.05f){
cocorlow 141:725321fe2949 167 Vector3f euler = eskf.computeAngles();
cocorlow 141:725321fe2949 168 if(fabsf(euler(1))<30.0f*M_PI_F/180.0f){
cocorlow 141:725321fe2949 169 headingUpdateFlag = true;
cocorlow 141:725321fe2949 170 theading = _t.read();
cocorlow 141:725321fe2949 171 }
cocorlow 141:725321fe2949 172 }
cocorlow 141:725321fe2949 173
cocorlow 141:725321fe2949 174 if(gpsUpdateFlag == true){
cocorlow 141:725321fe2949 175 eskf.updateGPS(pi, palt, vi, Rgps);
cocorlow 141:725321fe2949 176 //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
cocorlow 141:725321fe2949 177 //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
cocorlow 141:725321fe2949 178 }else if(headingUpdateFlag == true){
cocorlow 141:725321fe2949 179 float heading = std::atan2(-mag(1),mag(0));
cocorlow 141:725321fe2949 180 eskf.updateHeading(heading,Rheading);
cocorlow 141:725321fe2949 181 }else{
cocorlow 141:725321fe2949 182 Vector3f dynacc = eskf.calcDynAcc(acc);
cocorlow 141:725321fe2949 183 dynaccnorm2 = dynacc.squaredNorm();
cocorlow 141:725321fe2949 184 //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2));
cocorlow 141:725321fe2949 185 if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
cocorlow 141:725321fe2949 186 eskf.updateAcc(acc, RaccDyn);
cocorlow 141:725321fe2949 187 }else{
cocorlow 141:725321fe2949 188 eskf.updateAcc(acc, Racc);
cocorlow 141:725321fe2949 189 }
cocorlow 141:725321fe2949 190 }
cocorlow 141:725321fe2949 191 PIDtick.loop();
cocorlow 141:725321fe2949 192
cocorlow 141:725321fe2949 193 //制御時間を計測
cocorlow 141:725321fe2949 194 float tend = _t.read();
cocorlow 141:725321fe2949 195 att_dt = (tend-tstart);
cocorlow 141:725321fe2949 196 }
osaka 87:89bbbcdb667b 197 }