Diff: global.cpp

Revision:

70:99f974d8960e

Parent:

68:b9f6938fab9d

Child:

72:267e2cfddb0b

--- a/global.cpp	Mon Jun 28 01:45:12 2021 +0000
+++ b/global.cpp	Tue Jun 29 08:07:55 2021 +0000
@@ -14,17 +14,21 @@
 
 // io
 DigitalIn userButton(USER_BUTTON);
-CalibrateMagneto joyCalibrator;
+SBUS sbus(PD_5, PD_6);
+
 // control
-FastPWM elevServo(PE_11);
-FastPWM rudServo(PE_13);
-PID pitchPID(5.0, 0,0, PID_dt); // rad
-PID pitchratePID(0.5, 0.0, 0.0, PID_dt); // rad/s
+FastPWM servoRight(PE_9);
+FastPWM servoLeft(PE_11);
+FastPWM servoThrust(PE_13);
+PID pitchPID(5.0, 0,0,PID_dt); //rad
+PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
+PID rollPID(5.0,0.0,0.0,PID_dt);
+PID rollratePID(5, 0.0, 0.0, PID_dt);//rad/s
 errStateEKF ekf; // EKF class
 
+float rc[16];
 int loop_count = 0;
 float att_dt = 0.01f;
-
 int16_t ax, ay, az;
 int16_t gx, gy, gz;
 MotionSensorDataUnits mdata;
@@ -38,16 +42,23 @@
 Vector3 mag;
 Vector3 magref(0.65f, 0.0f, 0.75f);
 Vector3 gyro;
-
-float scaledServoOut[2] = {0.0f,0.0f};
+MedianFilter accMedian(3);
+MedianFilter gyroMedian(3);
+MedianFilter magMedian(3);
+float scaledServoOut[2]= {0.0f,0.0f};
+float scaledMotorOut[1]= {-1.0f};
 float servoOut[2] = {1500.0f,1500.0f};
+float motorOut[1] = {1100.0f};
 
 int calibrationFlag = 0;
-int agoffset[6] = {0, 0, 0, -121, -12, 24};
-float magbiasMin[3] = {-45.639572, -0.059049, -208.120300};
-float magbiasMax[3] = {28.209938, 65.421822, -131.388748};
+int agoffset[6] = {0, 0, 0, -585, -83, 38 };
+float magbiasMin[3] = {-177.574768, 90.129456, -199.611603};
+float magbiasMax[3] = {-108.064606, 160.956711, -125.601234};
+float accMin[3] = {-0.958801, -1.016878, -1.054199};
+float accMax[3] = { 1.050249, 0.965697, 0.946194};
 
-Vector3 rpy_align(-1.192014f*M_PI/180.0f, -2.639991f*M_PI/180.0f, 0.0f*M_PI/180.0f);
+Vector3 rpy_align(8.798061f*M_PI/180.0f, 0.953862f*M_PI/180.0f, 0.0f*M_PI/180.0f
+);
 
 
 // UsaPack