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
Diff: run.cpp
- Revision:
- 143:53808e4e684c
- Parent:
- 141:725321fe2949
diff -r e1b8357d1ea4 -r 53808e4e684c run.cpp --- a/run.cpp Fri Dec 10 11:20:13 2021 +0000 +++ b/run.cpp Fri Jun 24 05:44:34 2022 +0000 @@ -2,137 +2,40 @@ void run() { - wait(0.5); - - //センサの初期化・ジャイロバイアス・加速度スケールの取得 - int n_init = 1000; - for(int i = 0;i<n_init;i++){ - lsm.readAccel(); - lsm.readMag(); - lsm.readGyro(); - agoffset[0] += lsm.ax * 9.8f; - agoffset[1] += lsm.ay * 9.8f; - agoffset[2] += lsm.az * 9.8f-9.8f; - agoffset[3] +=(lsm.gx * M_PI_F / 180.0f); - agoffset[4] +=(lsm.gy * M_PI_F / 180.0f); - agoffset[5] +=(lsm.gz * M_PI_F / 180.0f); - palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars()); - magref(0) += lsm.mx; - magref(1) += lsm.my; - magref(2) += lsm.mz; - } - for(int i = 0;i<6;i++){ - agoffset[i] /= float(n_init); - } - magref /= float(n_init); - palt0 /= float(n_init); - 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]); + preflightCalibration(); - //ESKFの共分散設定 - eskf.setGravity(0.0f,0.0f,9.8f); - eskf.setPhatPosition(0.1f); - eskf.setPhatVelocity(0.1f); - eskf.setPhatAngleError(0.5f); - eskf.setPhatAccBias(0.001f); - eskf.setPhatGyroBias(0.001f); - eskf.setPhatGravity(0.0000001f); - - eskf.setQVelocity(0.001f); - eskf.setQAngleError(0.0000025f); - eskf.setQAccBias(0.000001f); - eskf.setQGyroBias(0.000001f); - - Matrix3f Rgpspos; - setDiag(Rgpspos,1.0f); - - Matrix3f Rgpsvel; - Rgpsvel(0,0) = 0.01f; - Rgpsvel(1,1) = 0.01f; - Rgpsvel(2,2) = 0.1f; + setEskfCov(); MatrixXf Rgps(5,5); - setDiag(Rgps,0.05f); + setDiag(Rgps,1.5f); + Rgps(2,2) = 0.1f; Rgps(3,3) = 0.1f; Rgps(4,4) = 0.1f; - float dynAccCriteria = 0.4f; - Matrix3f Racc; - setDiag(Racc,100.0f); - Matrix3f RaccDyn; - setDiag(RaccDyn,5000.0f); - - Matrix<float, 1, 1> Rheading; - Rheading(0, 0) = 0.01f; + MatrixXf Rwhole = MatrixXf::Zero(9,9); + Rwhole(0,0) = 1.5f; + Rwhole(1,1) = 1.5f; + Rwhole(2,2) = 0.1f; + Rwhole(3,3) = 0.1f; + Rwhole(4,4) = 0.1f; + Rwhole(5,5) = 5000.0f; + Rwhole(6,6) = 5000.0f; + Rwhole(7,7) = 5000.0f; + Rwhole(8,8) = 0.007f; + _t.start(); - //センサ正常性チェック - if(hilFlag == false){ - while(1){ - float tstart = _t.read(); - getIMUval(); - getGPSval(); - eskf.updateNominal(acc, gyro, att_dt); - eskf.updateErrState(acc, gyro, att_dt); - eskf.updateGPS(pi, palt, vi, Rgps); - float heading = std::atan2(-mag(2), mag(0)); - eskf.updateHeading(heading, Rheading); - Matrix3f Raccpreflight; - setDiag(Raccpreflight, 5.0f); - eskf.updateAcc(acc, Raccpreflight); - - bool preflightCheck = true; - Vector3f gyroBias = eskf.getGyroBias(); - if(fabsf(gyro(0)-gyroBias(0))>2.0f || fabsf(gyro(1)-gyroBias(1))>2.0f || fabsf(gyro(2)-gyroBias(2))>2.0f){ - preflightCheck = false; - twelite.serial.printf("PreFlight Check : high gyro value\r\n"); - } - Vector3f accBias = eskf.getAccBias(); - if(fabsf(acc(0)-accBias(0))>2.0f || fabsf(acc(1)-gyroBias(1))>2.0f){ - preflightCheck = false; - twelite.serial.printf("PreFlight Check : high acc value\r\n"); - } - Vector3f vihat = eskf.getVihat(); - if(fabsf(vihat(0))>2.0f || fabsf(vihat(1))>2.0f||fabsf(vihat(2))>2.0f){ - preflightCheck = false; - twelite.serial.printf("PreFlight Check : high velocity value\r\n"); - } - Vector3f pihat = eskf.getPihat(); - if(fabsf(pihat(0))>2.0f || fabsf(pihat(1))>2.0f||fabsf(pihat(2))>2.0f){ - preflightCheck = false; - twelite.serial.printf("PreFlight Check : not home position\r\n"); - } - if(sbus.failSafe){ - preflightCheck = false; - twelite.serial.printf("PreFlight Check : no RC\r\n"); - } - // sbusデータの読み込み - for (int i =0 ; i < 16;i ++){ - rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input - } - if (rc[4]>-0.3f && rc[6] < -0.3f){ - preflightCheck = false; - twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n"); - } - if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){ - preflightCheck = false; - twelite.serial.printf("PreFlight Check : no gps lock\r\n"); - } - - if(preflightCheck == true){ - break; - } - } - } - twelite.serial.printf("PreFlight Check Completed\r\n"); - //usaPack通信開始 + + preflightCheck(); + wait(1.0f); + //usaPack通信開始 制御ループのアタッチ pc.Subscribe(0000, &(vp)); - - //制御ループのアタッチ LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); float tgps = _t.read(); float theading = _t.read(); + float tstart = _t.read(); while(1) { float tstart = _t.read(); @@ -162,32 +65,22 @@ getGPSval(); } - headingUpdateFlag = false; - if(tstart-theading>0.05f){ - Vector3f euler = eskf.computeAngles(); - if(fabsf(euler(1))<30.0f*M_PI_F/180.0f){ - headingUpdateFlag = true; - theading = _t.read(); + if(gpsUpdateFlag == true){ + float heading = std::atan2(-mag(1),mag(0)); + Vector3f dynacc = eskf.calcDynAcc(acc); + dynaccnorm2 = dynacc.squaredNorm(); + if(dynaccnorm2 > 0.16f){ + Rwhole(5,5) = 100.0f; + Rwhole(6,6) = 100.0f; + Rwhole(7,7) = 100.0f; + }else{ + Rwhole(5,5) = 5000.0f; + Rwhole(6,6) = 5000.0f; + Rwhole(7,7) = 5000.0f; } + eskf.updateWhole(pi, palt, vi,acc,heading, Rwhole); } - if(gpsUpdateFlag == true){ - eskf.updateGPS(pi, palt, vi, Rgps); - //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos); - //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel); - }else if(headingUpdateFlag == true){ - float heading = std::atan2(-mag(1),mag(0)); - eskf.updateHeading(heading,Rheading); - }else{ - Vector3f dynacc = eskf.calcDynAcc(acc); - dynaccnorm2 = dynacc.squaredNorm(); - //twelite.serial.printf("%f\r\n",sqrt(dynaccnorm2)); - if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){ - eskf.updateAcc(acc, RaccDyn); - }else{ - eskf.updateAcc(acc, Racc); - } - } PIDtick.loop(); //制御時間を計測