solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
preflight.cpp@144:b3a713b4f1c4, 2022-06-29 (annotated)
- Committer:
- NaotoMorita
- Date:
- Wed Jun 29 07:57:10 2022 +0000
- Revision:
- 144:b3a713b4f1c4
- Parent:
- 143:53808e4e684c
can fly
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 | 144:b3a713b4f1c4 | 45 | rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368.0f,1680.0f,-1.0f,1.0f) + (1.0f - 0.65f) * rc[i]; // mapped input |
NaotoMorita | 143:53808e4e684c | 46 | } |
NaotoMorita | 144:b3a713b4f1c4 | 47 | if (rc[7] > 0.3f){ |
NaotoMorita | 143:53808e4e684c | 48 | preflightCheck = false; |
NaotoMorita | 144:b3a713b4f1c4 | 49 | motor1.pulsewidth_us(motorPwmMin); |
NaotoMorita | 144:b3a713b4f1c4 | 50 | motor2.pulsewidth_us(motorPwmMin); |
NaotoMorita | 144:b3a713b4f1c4 | 51 | motor3.pulsewidth_us(motorPwmMin); |
NaotoMorita | 144:b3a713b4f1c4 | 52 | motor4.pulsewidth_us(motorPwmMin); |
NaotoMorita | 144:b3a713b4f1c4 | 53 | motor5.pulsewidth_us(motorPwmMin); |
NaotoMorita | 144:b3a713b4f1c4 | 54 | motor6.pulsewidth_us(motorPwmMin); |
NaotoMorita | 144:b3a713b4f1c4 | 55 | twelite.serial.printf("PreFlight Check : switch arming position\r\n"); |
NaotoMorita | 144:b3a713b4f1c4 | 56 | pc.serial.printf("PreFlight Check : switch arming position\r\n"); |
NaotoMorita | 143:53808e4e684c | 57 | } |
NaotoMorita | 144:b3a713b4f1c4 | 58 | |
NaotoMorita | 144:b3a713b4f1c4 | 59 | |
NaotoMorita | 143:53808e4e684c | 60 | if(preflightCheck == true){ |
NaotoMorita | 143:53808e4e684c | 61 | break; |
NaotoMorita | 143:53808e4e684c | 62 | } |
NaotoMorita | 144:b3a713b4f1c4 | 63 | pc.serial.printf("PreFlight Check : failed\r\n"); |
NaotoMorita | 143:53808e4e684c | 64 | } |
NaotoMorita | 143:53808e4e684c | 65 | } |
NaotoMorita | 143:53808e4e684c | 66 | twelite.serial.printf("PreFlight Check Completed\r\n"); |
NaotoMorita | 143:53808e4e684c | 67 | } |
NaotoMorita | 143:53808e4e684c | 68 | |
NaotoMorita | 143:53808e4e684c | 69 | void setEskfCov(){ |
NaotoMorita | 143:53808e4e684c | 70 | |
NaotoMorita | 143:53808e4e684c | 71 | //ESKFの共分散設定 |
NaotoMorita | 143:53808e4e684c | 72 | eskf.setGravity(0.0f,0.0f,9.8f); |
NaotoMorita | 143:53808e4e684c | 73 | eskf.setPhatPosition(2.0f,2.0f); |
NaotoMorita | 143:53808e4e684c | 74 | eskf.setPhatVelocity(1.01f,1.01f); |
NaotoMorita | 143:53808e4e684c | 75 | eskf.setPhatAngleError(0.1f); |
NaotoMorita | 144:b3a713b4f1c4 | 76 | eskf.setPhatAccBias(0.0001f); |
NaotoMorita | 143:53808e4e684c | 77 | eskf.setPhatGyroBias(0.0001f); |
NaotoMorita | 143:53808e4e684c | 78 | eskf.setPhatGravity(0.0000001f); |
NaotoMorita | 143:53808e4e684c | 79 | |
NaotoMorita | 143:53808e4e684c | 80 | eskf.setQVelocity(0.1f,0.2f); |
NaotoMorita | 143:53808e4e684c | 81 | eskf.setQAngleError(0.0000025f); |
NaotoMorita | 144:b3a713b4f1c4 | 82 | eskf.setQAccBias(0.0000001f); |
NaotoMorita | 143:53808e4e684c | 83 | eskf.setQGyroBias(0.0000001f); |
NaotoMorita | 143:53808e4e684c | 84 | } |