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@139:b378528c05f2, 2021-12-06 (annotated)

Committer:
cocorlow
Date:
Mon Dec 06 08:26:16 2021 +0000
Revision:
139:b378528c05f2
Parent:
135:49f8916588da
Child:
141:725321fe2949
Eigen

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