solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
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();