solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: preflight.cpp
- Revision:
- 143:53808e4e684c
- Child:
- 144:b3a713b4f1c4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/preflight.cpp Fri Jun 24 05:44:34 2022 +0000 @@ -0,0 +1,79 @@ +#include "global.hpp" + +void preflightCalibration() +{ + wait(0.5f); + //センサの初期化・ジャイロバイアス・加速度スケールの取得 + 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]); + wait(1.0f); +} + + +void preflightCheck() +{ + //センサ正常性チェック + if(hilFlag == false){ + while(1){ + bool preflightCheck = true; + 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"); + } + preflightCheck = true; + if(preflightCheck == true){ + break; + } + } + } + twelite.serial.printf("PreFlight Check Completed\r\n"); +} + +void setEskfCov(){ + + //ESKFの共分散設定 + eskf.setGravity(0.0f,0.0f,9.8f); + eskf.setPhatPosition(2.0f,2.0f); + eskf.setPhatVelocity(1.01f,1.01f); + eskf.setPhatAngleError(0.1f); + eskf.setPhatAccBias(0.5f); + eskf.setPhatGyroBias(0.0001f); + eskf.setPhatGravity(0.0000001f); + + eskf.setQVelocity(0.1f,0.2f); + eskf.setQAngleError(0.0000025f); + eskf.setQAccBias(0.000001f); + eskf.setQGyroBias(0.0000001f); + } \ No newline at end of file