Diff: global.hpp

Revision:

143:53808e4e684c

Parent:

141:725321fe2949

Child:

144:b3a713b4f1c4

--- a/global.hpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/global.hpp	Fri Jun 24 05:44:34 2022 +0000
@@ -8,12 +8,10 @@
 #include "FastPWM.h"
 #include <cmath>
 #include "UsaPack.hpp"
-#include "Vector3.hpp"
 #include "LSM9DS1.h"
 #include "LPS.h"
 #include "CalibrateMagneto.h"
 #include "solaESKF.hpp"
-#include "MedianFilter.hpp"
 #include "GPSUBX_UART.hpp"
 #include "Autopilot.hpp"
 
@@ -38,6 +36,7 @@
 #define M_PI_F 3.141592f
 #endif
 
+
 struct valuePack
 {
     int16_t accData[3];
@@ -45,7 +44,8 @@
     int16_t magData[3];
     int16_t viData[3];
     int16_t piData[3];
-    int16_t actData[4];
+    //int16_t airspeed;
+    //int16_t actData[4];
     int16_t commandIndex;
     int16_t commandVal;
 };
@@ -55,38 +55,57 @@
     float da;
     float de;
     float dT;
-    float rpy[3];
-    float vihat[3];
+    float euler[3];
+    float state[4];
+    float variance[3];
+    
 };
 struct logPack
 {
-    float time;
-    float hertz;
-    float gpsFix;
-    float da;
-    float de;
-    float dT;
-    float rpy[3];
-    float pihat[3];
-    float vihat[3];
-    float pi[3];
-    float vi[3];
-    float palt;
-    float acc[3];
-    float gyro[3];
-    float mag[3];
-    float rc[16];
+    float time; // 1
+    float hertz; // 2
+    float gpsFix; // 3
+    float da; // 4
+    float de; // 5
+    float dT; // 6
+    float rpy[3]; // 7 8 9
+    float pihat[3]; // 10 11 12
+    float vihat[3]; // 13 14 15
+    float pi[3]; // 16 17 18
+    float vi[3]; // 19 20 21
+    float palt; // 22
+    float acc[3]; // 23 24 25
+    float gyro[3]; // 26 27 28
+    float mag[3]; // 29 30 31
+    float rc[16]; // 32 ... 47
+    float wind[3]; // 48 49 50
+    float pitch_obj; // 51
+    float roll_obj; // 52
+    float u_pitot; // 53
 };
 
 struct telemetryPack
 {
+    float pi[3];
+    float rpy_l[3];
+    float rpy_c[3];
+    float rpy_r[3];
+    float vi[3];
+    float palt;
+    float gps_fix;
+    float mode; //preflight:0, flight:1
     float time;
-    float hertz;
-    float gpsFix;
-    float rpy[3];
-    float pihat[3];
-    float vihat[3];
-    float dynaccNorm;
+    float gps_acc;
+    float vx_opt;
+    float vy_opt;
+    float dist_ir;
+    float voltage[2];
+    float current[2];
+    float wind[3];
+    float pitch_obj;
+    float roll_obj;
+    float hinf;
+    float u_pitot;
 };
 
 // var
@@ -105,7 +124,7 @@
 extern LPS lps;
 extern GPSUBX_UART gps;
 extern CalibrateMagneto magCalibrator;
-extern float magres;
+
 // control
 extern Timer _t;
 extern FastPWM servoRight;
@@ -115,8 +134,9 @@
 extern PID pitchratePID;//rad/s
 extern PID rollPID;
 extern PID rollratePID;//rad/s
+extern PID yawratePID;//rad/s
+extern PID climbratePID;//rad/s
 extern solaESKF eskf; // EKF class
-extern int obsCount;
 extern Autopilot autopilot;
 extern float roll_obj;
 extern float pitch_obj;
@@ -125,16 +145,13 @@
 extern float rc[16];
 extern int loop_count;
 extern float att_dt;
-extern bool accmagSwitch;
 
 
 // position
 extern Matrix3f SensorAlignmentAG;
 extern Matrix3f SensorAlignmentMAG; 
 extern Vector3f euler;
-extern Vector3f rpy; // x:roll  y:pitch  z:yaw
 extern Vector3f acc;
-extern Vector3f accref;
 extern Vector3f mag;
 extern Vector3f magref;
 extern Vector3f gyro;
@@ -147,13 +164,11 @@
 extern bool gpsUpdateFlag;
 extern bool gpsLlh0Fixed;
 extern bool headingUpdateFlag;
+extern bool hinf_flag;
 
 extern float de;
 extern float da;
 extern float dT;
-extern MedianFilter accMedian;
-extern MedianFilter gyroMedian;
-extern MedianFilter magMedian;
 
 extern float scaledServoOut[2];
 extern float scaledMotorOut[1];
@@ -163,13 +178,6 @@
 
 extern int calibrationFlag;
 extern float agoffset[6];
-extern float magbiasMin[3];
-extern float magbiasMax[3];
-extern float magbias[4];
-extern float accMin[3];
-extern float accMax[3];
-
-extern Vector3f rpy_align;
 
 
 //// UsaPack
@@ -180,9 +188,8 @@
 
 // HIL
 extern bool hilFlag;
-extern bool serialControlSource;
-extern bool serialParamSource;
-extern int checkParamSerial[5];
+extern int16_t hilDataOut;
+
 // function
 
 // main.cpp
@@ -190,6 +197,12 @@
 // setup.cpp
 extern void setup();
 extern void calibrate();
+
+// preflight.cpp
+extern void preflightCalibration();
+extern void preflightCheck();
+extern void setEskfCov();
+
 // run.cpp
 extern void run();