Diff: global.cpp

Revision:

144:b3a713b4f1c4

Parent:

143:53808e4e684c

--- a/global.cpp	Fri Jun 24 05:44:34 2022 +0000
+++ b/global.cpp	Wed Jun 29 07:57:10 2022 +0000
@@ -18,34 +18,35 @@
 float magres = 0.0f;
 // control
 Timer _t;
-FastPWM servoRight(PE_9);
-FastPWM servoLeft(PE_11);
-FastPWM servoThrust(PE_13);
-PID pitchPID(10.0f,0.0f,0.0f,PID_dt); //rad
-PID pitchratePID(1.0f, 0.0f, 0.0f, PID_dt);//rad/s
-PID rollPID(5.0f,0.0f,0.0f,PID_dt);
+FastPWM motor1(PE_9);
+FastPWM motor2(PE_11);
+FastPWM motor3(PE_13);
+FastPWM motor4(PA_6);
+FastPWM motor5(PA_0);
+FastPWM motor6(PB_10);
+PID pitchPID(0.05f,0.0f,0.0f,PID_dt); //rad
+PID pitchratePID(0.05f, 0.0f, 0.0f, PID_dt);//rad/s
+PID rollPID(0.05f,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
+PID yawratePID(0.3f, 0.0, 0.0, PID_dt);//rad/s
+PID vxPID(0.5f, 0.1f, 0.0f, PID_dt);
+PID vyPID(0.5f, 0.1f, 0.0f, PID_dt);
+PID vzPID(0.05f, 0.0f, 0.0f, PID_dt);
 solaESKF eskf; // ESKF class
-Autopilot autopilot;
-float roll_obj;
-float pitch_obj;
-float dT_obj;
 
 float rc[16];
 int loop_count = 0;
 float att_dt = 0.01f;
 // position
-Matrix3f SensorAlignmentAG;
-Matrix3f SensorAlignmentMAG;
-Vector3f euler;
-Vector3f acc;
-Vector3f mag;
-Vector3f magref(0.0f, 0.0f, 0.0f);
-Vector3f gyro;
-Vector3f vi;
-Vector3f pi;
+Matrix3f SensorAlignmentAG = Matrix3f::Zero();
+Matrix3f SensorAlignmentMAG = Matrix3f::Zero();
+Vector3f euler = Vector3f::Zero(); 
+Vector3f acc = Vector3f::Zero();
+Vector3f mag = Vector3f::Zero();
+Vector3f magref = Vector3f::Zero();
+Vector3f gyro = Vector3f::Zero();
+Vector3f vi = Vector3f::Zero();
+Vector3f pi = Vector3f::Zero();
 float palt;
 float palt0;
 float dynaccnorm2;
@@ -57,14 +58,11 @@
 
 float de = 0.0f;
 float da = 0.0f;
+float dr = 0.0f;
 float dT = 0.0f;
 
-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;
+float scaledMotorOut[6]= {-1.0f,-1.0f,-1.0f,-1.0f,-1.0f,-1.0f};
+float motorOut[6] = {1100.0f,1100.0f,1100.0f,1100.0f,1100.0f,1100.0f};
 float agoffset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
 
 
@@ -75,7 +73,7 @@
 telemetryPack tp;
 
 // HIL
-bool hilFlag = true;
+bool hilFlag = false;
 int16_t hilDataOut = 0;
 
 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)