Diff: global.cpp

Revision:

143:53808e4e684c

Parent:

141:725321fe2949

--- a/global.cpp	Fri Dec 10 11:20:13 2021 +0000
+++ b/global.cpp	Fri Jun 24 05:44:34 2022 +0000
@@ -25,8 +25,9 @@
 PID pitchratePID(1.0f, 0.0f, 0.0f, PID_dt);//rad/s
 PID rollPID(5.0f,0.0f,0.0f,PID_dt);
 PID rollratePID(0.05f, 0.0, 0.0, PID_dt);//rad/s
+PID yawratePID(2.0f, 0.0, 0.0, PID_dt);//rad/s
+PID climbratePID(1.0f, 0.0, 0.0, PID_dt);//rad/s
 solaESKF eskf; // ESKF class
-int obsCount = 0;
 Autopilot autopilot;
 float roll_obj;
 float pitch_obj;
@@ -39,9 +40,7 @@
 Matrix3f SensorAlignmentAG;
 Matrix3f SensorAlignmentMAG;
 Vector3f euler;
-Vector3f rpy(0.0f, 0.0f, 0.0f); // x:roll  y:pitch  z:yaw
 Vector3f acc;
-Vector3f accref(0.0f, 0.0f, 9.8f);
 Vector3f mag;
 Vector3f magref(0.0f, 0.0f, 0.0f);
 Vector3f gyro;
@@ -54,13 +53,11 @@
 bool gpsUpdateFlag = false;
 bool gpsLlh0Fixed = false;
 bool headingUpdateFlag = false;
+bool hinf_flag = false;
 
 float de = 0.0f;
 float da = 0.0f;
 float dT = 0.0f;
-MedianFilter accMedian(3);
-MedianFilter gyroMedian(3);
-MedianFilter magMedian(3);
 
 float scaledServoOut[2]= {0.0f,0.0f};
 float scaledMotorOut[1]= {-1.0f};
@@ -70,14 +67,6 @@
 int calibrationFlag = 0;
 float agoffset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
 
-float magbias[4] = {-134.817047, 127.833481, -152.631836, 44.508556};
-float magbiasMin[3] = {-174.831711, 93.465691, -188.617172};
-float magbiasMax[3] = {-110.413963, 162.321548, -122.566071};
-float accMin[3] = {-0.963692, -0.974141, -1.012899};
-float accMax[3] = { 1.025954, 0.974748, 0.987155};
-
-Vector3f rpy_align( 0.0f*M_PI_F/180.0f, 5.0f*M_PI_F/180.0f, 0.0f*M_PI_F/180.0f);
-
 
 // UsaPack
 valuePack vp;
@@ -87,9 +76,7 @@
 
 // HIL
 bool hilFlag = true;
-bool serialControlSource = false;
-bool serialParamSource = false;
-int checkParamSerial[5] = {0,0,0,0,0};
+int16_t hilDataOut = 0;
 
 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
 {
@@ -98,11 +85,21 @@
 
 void setDiag(Matrix3f& mat, float val){
     for (int i = 0; i < mat.cols(); i++){
+            for (int j = 0; j < mat.rows(); j++){
+                mat(i,j) = 0.0f;
+            }
+    }
+    for (int i = 0; i < mat.cols(); i++){
             mat(i,i) = val;
     }
 }
 
 void setDiag(MatrixXf& mat, float val){
+    for (int i = 0; i < mat.cols(); i++){
+        for (int j = 0; j < mat.rows(); j++){
+            mat(i,j) = 0.0f;
+        }
+    }
     for (int i = 0; i < mat.cols(); i++)
     {
         mat(i, i) = val;