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
preflight.cpp@143:53808e4e684c, 22 months ago (annotated)
- Committer:
- NaotoMorita
- Date:
- Fri Jun 24 05:44:34 2022 +0000
- Revision:
- 143:53808e4e684c
complete
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
NaotoMorita | 143:53808e4e684c | 1 | #include "global.hpp" |
NaotoMorita | 143:53808e4e684c | 2 | |
NaotoMorita | 143:53808e4e684c | 3 | void preflightCalibration() |
NaotoMorita | 143:53808e4e684c | 4 | { |
NaotoMorita | 143:53808e4e684c | 5 | wait(0.5f); |
NaotoMorita | 143:53808e4e684c | 6 | //センサの初期化・ジャイロバイアス・加速度スケールの取得 |
NaotoMorita | 143:53808e4e684c | 7 | int n_init = 1000; |
NaotoMorita | 143:53808e4e684c | 8 | for(int i = 0;i<n_init;i++){ |
NaotoMorita | 143:53808e4e684c | 9 | lsm.readAccel(); |
NaotoMorita | 143:53808e4e684c | 10 | lsm.readMag(); |
NaotoMorita | 143:53808e4e684c | 11 | lsm.readGyro(); |
NaotoMorita | 143:53808e4e684c | 12 | //agoffset[0] += lsm.ax * 9.8f; |
NaotoMorita | 143:53808e4e684c | 13 | //agoffset[1] += lsm.ay * 9.8f; |
NaotoMorita | 143:53808e4e684c | 14 | //agoffset[2] += lsm.az * 9.8f-9.8f; |
NaotoMorita | 143:53808e4e684c | 15 | agoffset[3] +=(lsm.gx * M_PI_F / 180.0f); |
NaotoMorita | 143:53808e4e684c | 16 | agoffset[4] +=(lsm.gy * M_PI_F / 180.0f); |
NaotoMorita | 143:53808e4e684c | 17 | agoffset[5] +=(lsm.gz * M_PI_F / 180.0f); |
NaotoMorita | 143:53808e4e684c | 18 | palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars()); |
NaotoMorita | 143:53808e4e684c | 19 | magref(0) += lsm.mx; |
NaotoMorita | 143:53808e4e684c | 20 | magref(1) += lsm.my; |
NaotoMorita | 143:53808e4e684c | 21 | magref(2) += lsm.mz; |
NaotoMorita | 143:53808e4e684c | 22 | } |
NaotoMorita | 143:53808e4e684c | 23 | for(int i = 0;i<6;i++){ |
NaotoMorita | 143:53808e4e684c | 24 | agoffset[i] /= float(n_init); |
NaotoMorita | 143:53808e4e684c | 25 | } |
NaotoMorita | 143:53808e4e684c | 26 | magref /= float(n_init); |
NaotoMorita | 143:53808e4e684c | 27 | palt0 /= float(n_init); |
NaotoMorita | 143:53808e4e684c | 28 | 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]); |
NaotoMorita | 143:53808e4e684c | 29 | wait(1.0f); |
NaotoMorita | 143:53808e4e684c | 30 | } |
NaotoMorita | 143:53808e4e684c | 31 | |
NaotoMorita | 143:53808e4e684c | 32 | |
NaotoMorita | 143:53808e4e684c | 33 | void preflightCheck() |
NaotoMorita | 143:53808e4e684c | 34 | { |
NaotoMorita | 143:53808e4e684c | 35 | //センサ正常性チェック |
NaotoMorita | 143:53808e4e684c | 36 | if(hilFlag == false){ |
NaotoMorita | 143:53808e4e684c | 37 | while(1){ |
NaotoMorita | 143:53808e4e684c | 38 | bool preflightCheck = true; |
NaotoMorita | 143:53808e4e684c | 39 | if(sbus.failSafe){ |
NaotoMorita | 143:53808e4e684c | 40 | preflightCheck = false; |
NaotoMorita | 143:53808e4e684c | 41 | twelite.serial.printf("PreFlight Check : no RC\r\n"); |
NaotoMorita | 143:53808e4e684c | 42 | } |
NaotoMorita | 143:53808e4e684c | 43 | // sbusデータの読み込み |
NaotoMorita | 143:53808e4e684c | 44 | for (int i =0 ; i < 16;i ++){ |
NaotoMorita | 143:53808e4e684c | 45 | rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input |
NaotoMorita | 143:53808e4e684c | 46 | } |
NaotoMorita | 143:53808e4e684c | 47 | if (rc[4]>-0.3f && rc[6] < -0.3f){ |
NaotoMorita | 143:53808e4e684c | 48 | preflightCheck = false; |
NaotoMorita | 143:53808e4e684c | 49 | twelite.serial.printf("PreFlight Check : autoPilot enabled\r\n"); |
NaotoMorita | 143:53808e4e684c | 50 | } |
NaotoMorita | 143:53808e4e684c | 51 | if(!(gps.gpsFix == 0x02 || gps.gpsFix == 0x03)){ |
NaotoMorita | 143:53808e4e684c | 52 | preflightCheck = false; |
NaotoMorita | 143:53808e4e684c | 53 | twelite.serial.printf("PreFlight Check : no gps lock\r\n"); |
NaotoMorita | 143:53808e4e684c | 54 | } |
NaotoMorita | 143:53808e4e684c | 55 | preflightCheck = true; |
NaotoMorita | 143:53808e4e684c | 56 | if(preflightCheck == true){ |
NaotoMorita | 143:53808e4e684c | 57 | break; |
NaotoMorita | 143:53808e4e684c | 58 | } |
NaotoMorita | 143:53808e4e684c | 59 | } |
NaotoMorita | 143:53808e4e684c | 60 | } |
NaotoMorita | 143:53808e4e684c | 61 | twelite.serial.printf("PreFlight Check Completed\r\n"); |
NaotoMorita | 143:53808e4e684c | 62 | } |
NaotoMorita | 143:53808e4e684c | 63 | |
NaotoMorita | 143:53808e4e684c | 64 | void setEskfCov(){ |
NaotoMorita | 143:53808e4e684c | 65 | |
NaotoMorita | 143:53808e4e684c | 66 | //ESKFの共分散設定 |
NaotoMorita | 143:53808e4e684c | 67 | eskf.setGravity(0.0f,0.0f,9.8f); |
NaotoMorita | 143:53808e4e684c | 68 | eskf.setPhatPosition(2.0f,2.0f); |
NaotoMorita | 143:53808e4e684c | 69 | eskf.setPhatVelocity(1.01f,1.01f); |
NaotoMorita | 143:53808e4e684c | 70 | eskf.setPhatAngleError(0.1f); |
NaotoMorita | 143:53808e4e684c | 71 | eskf.setPhatAccBias(0.5f); |
NaotoMorita | 143:53808e4e684c | 72 | eskf.setPhatGyroBias(0.0001f); |
NaotoMorita | 143:53808e4e684c | 73 | eskf.setPhatGravity(0.0000001f); |
NaotoMorita | 143:53808e4e684c | 74 | |
NaotoMorita | 143:53808e4e684c | 75 | eskf.setQVelocity(0.1f,0.2f); |
NaotoMorita | 143:53808e4e684c | 76 | eskf.setQAngleError(0.0000025f); |
NaotoMorita | 143:53808e4e684c | 77 | eskf.setQAccBias(0.000001f); |
NaotoMorita | 143:53808e4e684c | 78 | eskf.setQGyroBias(0.0000001f); |
NaotoMorita | 143:53808e4e684c | 79 | } |