HAPSRG / Mbed 2 deprecated hexaTest_Eigen

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, 22 months ago (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?

UserRevisionLine numberNew 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 }