Diff: global.cpp

Revision:

139:b378528c05f2

Parent:

137:240588882ae2

Child:

140:53dbdb207542

--- a/global.cpp	Wed Dec 01 19:17:36 2021 +0000
+++ b/global.cpp	Mon Dec 06 08:26:16 2021 +0000
@@ -25,9 +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
-solaESKF eskf; // ESKF class
+//solaESKF eskf; // ESKF class
 int obsCount = 0;
-Autopilot autopilot;
+//Autopilot autopilot;
 float roll_obj;
 float pitch_obj;
 float dT_obj;
@@ -36,17 +36,17 @@
 int loop_count = 0;
 float att_dt = 0.01f;
 // position
-Matrix SensorAlignmentAG(3,3);
-Matrix SensorAlignmentMAG(3,3);
-Matrix euler(3,1);
-Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll  y:pitch  z:yaw
-Vector3 acc;
-Vector3 accref(0.0f, 0.0f, 9.8f);
-Vector3 mag;
-Vector3 magref(0.0f, 0.0f, 0.0f);
-Vector3 gyro;
-Vector3 vi;
-Vector3 pi;
+Matrix3f SensorAlignmentAG(3,3);
+Matrix3f SensorAlignmentMAG(3,3);
+Vector3f euler(3,1);
+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;
+Vector3f vi;
+Vector3f pi;
 float palt;
 float palt0;
 float dynaccnorm2;
@@ -76,7 +76,7 @@
 float accMin[3] = {-0.963692, -0.974141, -1.012899};
 float accMax[3] = { 1.025954, 0.974748, 0.987155};
 
-Vector3 rpy_align( 0.0f*M_PI/180.0f, 5.0f*M_PI/180.0f, 0.0f*M_PI/180.0f);
+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
@@ -96,14 +96,14 @@
     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-void setDiag(Matrix& mat, float val){
-    for (int i = 1; i < mat.getCols()+1; i++){
-            mat(i,i) = val;
-    }
-}
-
-void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){
-    for (int i = startIndex; i < endIndex+1; i++){
-            mat(i,i) = val;
-    }
-}
+//void setDiag(Matrix3f& mat, float val){
+//    for (int i = 1; i < mat.getCols()+1; i++){
+//            mat(i,i) = val;
+//    }
+//}
+//
+//void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex){
+//    for (int i = startIndex; i < endIndex+1; i++){
+//            mat(i,i) = val;
+//    }
+//}