Diff: global.cpp

Revision:

93:b827f78a717a

Parent:

92:00460f6df439

Child:

94:579e875a4244

Child:

95:43717535c354

--- a/global.cpp	Thu Oct 28 09:44:47 2021 +0000
+++ b/global.cpp	Fri Oct 29 13:30:24 2021 +0000
@@ -31,6 +31,7 @@
 float att_dt = 0.01f;
 
 // position
+Matrix SensorAlignment(3,3); 
 Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll  y:pitch  z:yaw
 Vector3 acc;
 Vector3 accref(0.0f, 0.0f, 0.98f);
@@ -54,7 +55,7 @@
 float motorOut[1] = {1100.0f};
 
 int calibrationFlag = 0;
-int agoffset[6] = {0, 0, 0, -584, -94, 37};
+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};
@@ -70,7 +71,7 @@
 sendPack sp;
 
 // HIL
-bool hilFlag = true;
+bool hilFlag = false;
 bool serialControlSource = false;
 bool serialParamSource = false;
 int checkParamSerial[5] = {0,0,0,0,0};
@@ -79,3 +80,15 @@
 {
     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;
+    }
+}