Diff: global.cpp

Revision:

71:62eb45ecffe9

Parent:

70:9e7be21475f8

Child:

73:be7a8b8188de

--- a/global.cpp	Tue Jun 29 10:53:06 2021 +0000
+++ b/global.cpp	Fri Aug 20 07:48:41 2021 +0000
@@ -22,10 +22,10 @@
 FastPWM servo(PE_11);
 PID pitchPID(5.0, 0,0, PID_dt); // rad
 PID pitchratePID(0.5, 0.0, 0.0, PID_dt); // rad/s
-errStateEKF ekf; // EKF class
+ScErrStateEKF ekf; // EKF class
+int obsCount = 0;
 
 int loop_count = 0;
-int obs_count = 0;
 float att_dt = 0.01f;
 float rc[16];
 
@@ -40,13 +40,12 @@
 
 // position
 Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll  y:pitch  z:yaw
-Vector3 rpy_g(0.0f, 0.0f, 0.0f); // x:roll  y:pitch  z:yaw
 Vector3 acc;
-Vector3 accref(0.0f, 0.0f, -0.98f);
+Vector3 accref(0.0f, 0.0f,  0.98f);
 Vector3 mag;
 Vector3 magref(0.65f, 0.0f, 0.75f);
-Vector3 dynacc;
 Vector3 gyro;
+Vector3 vb;
 MedianFilter accMedian(3);
 MedianFilter gyroMedian(3);
 MedianFilter magMedian(3);
@@ -63,11 +62,11 @@
 
 int calibrationFlag = 0;
 int pos_tail = 0; // 0:left 1:center 2:right
-int agoffset[6] = {0, 0, 0, 386, -450, 48};
+int agoffset[6] = {0, 0, 0, 0, 0, 0};
 float magbiasMin[3] = {0.0f, 0.0f, 0.0f};
 float magbiasMax[3] = {0.0f, 0.0f, 0.0f};
-float accMin[3] = {-0.974521, -1.010208, -1.055415};
-float accMax[3] = { 1.036735, 0.973873, 0.969753};
+float accMin[3] = {-1.0f, -1.0f, -1.0f};
+float accMax[3] = { 1.0f,  1.0f,  1.0f};
 
 Vector3 rpy_align(0.0f*M_PI/180.0f, 0.0f*M_PI/180.0f, 0.0f);