Diff: preflight.cpp

Revision:

143:53808e4e684c

--- /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