stable flight in theory been achieved

src/drone/src/main.cpp

@@ -189,10 +189,10 @@ float MagScaleY = 1.0;
 float MagScaleZ = 1.0;
 
 // IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error()
-float AccErrorX = 0.05;
-float AccErrorY = 0.01;
+float AccErrorX = 0.07;
+float AccErrorY = -0.01;
 float AccErrorZ = -0.01;
-float GyroErrorX = 0.00;
+float GyroErrorX = -0.00;
 float GyroErrorY = 0.00;
 float GyroErrorZ = 0.00;
 
@@ -359,6 +359,7 @@ void setupMotorPWM();
 int pulseWidthToDutyCycle(int pulseWidth);
 void setupMatrixSerial();
 void printPitchInfo();
+void printAttitudeDebug();
 //========================================================================================================================//
 //                                                      VOID SETUP                                                        //
 //========================================================================================================================//
@@ -456,6 +457,7 @@ void loop()
   // printLoopRate();      //Prints the time between loops in microseconds (expected: microseconds between loop iterations)
   // printDebugInfo(); //PWM 1 and 5 + Armed conditio
   // printPitchInfo();
+  // printAttitudeDebug();
 
   // Get arming status
   armedStatus(); // Check if the throttle cut is off and throttle is low.
@@ -514,6 +516,7 @@ void controlMixer()
   m2_command_scaled = thro_des - pitch_PID - roll_PID + yaw_PID; // Front Right CW
   m3_command_scaled = thro_des + pitch_PID - roll_PID - yaw_PID; // Back Right CCW
   m4_command_scaled = thro_des + pitch_PID + roll_PID + yaw_PID; // Back Left CW
+  
   m5_command_scaled = 0;
   m6_command_scaled = 0;
   
@@ -624,7 +627,6 @@ void getIMUdata()
   mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ);
   #elif defined USE_BNO085_I2C
   // Keep waiting until data becomes available
-  // Wait for data with timeout
   unsigned long startTime = millis();
   while (!myIMU.dataAvailable()) {
     delay(1);
@@ -634,18 +636,16 @@ void getIMUdata()
     }
   }
   
+  // Get quaternion directly from BNO085
+  q0 = myIMU.getQuatReal();  // w component
+  q1 = myIMU.getQuatI();     // x component
+  q2 = myIMU.getQuatJ();     // y component
+  q3 = myIMU.getQuatK();     // z component
   
-    // Get quaternion directly from BNO085
-    q0 = myIMU.getQuatReal();  // w component
-    q1 = myIMU.getQuatI();     // x component
-    q2 = myIMU.getQuatJ();     // y component
-    q3 = myIMU.getQuatK();     // z component
-    
-    // Calculate Euler angles from quaternion
-    roll_IMU = atan2(2.0f * (q0*q1 + q2*q3), 1.0f - 2.0f * (q1*q1 + q2*q2)) * 57.29577951f;
-    pitch_IMU = asin(2.0f * (q0*q2 - q3*q1)) * 57.29577951f;
-    yaw_IMU = atan2(2.0f * (q0*q3 + q1*q2), 1.0f - 2.0f * (q2*q2 + q3*q3)) * 57.29577951f;
-
+  // CORRECTED Euler angle calculation for BNO085
+  roll_IMU = atan2(2.0f * (q0*q1 + q2*q3), 1.0f - 2.0f * (q1*q1 + q2*q2)) * 57.29577951f;
+  pitch_IMU = asin(constrain(2.0f * (q0*q2 - q3*q1), -0.999999, 0.999999)) * 57.29577951f;
+  yaw_IMU = atan2(2.0f * (q0*q3 + q1*q2), 1.0f - 2.0f * (q2*q2 + q3*q3)) * 57.29577951f;
   
   // Get raw sensor data for PID controller
   GyroX = myIMU.getGyroX() * 57.29577951f; // Convert rad/s to deg/s
@@ -664,7 +664,7 @@ void getIMUdata()
   GyroY_prev = GyroY;
   GyroZ_prev = GyroZ;
   
-  // Get accelerometer data (still useful for PID control)
+  // Get accelerometer data (still useful for validation)
   AccX = myIMU.getAccelX() / 9.80665f; // Convert m/s² to g
   AccY = myIMU.getAccelY() / 9.80665f;
   AccZ = myIMU.getAccelZ() / 9.80665f;
@@ -681,7 +681,6 @@ void getIMUdata()
   AccY_prev = AccY;
   AccZ_prev = AccZ;
 
-
   // Return to avoid executing code for other IMUs
   return;
 #endif
@@ -1486,9 +1485,7 @@ void calibrateESCs()
    */
   while (true)
   {
-    prev_time = current_time;
-    current_time = micros();
-    dt = (current_time - prev_time) / 1000000.0;
+
 
     // digitalWrite(13, HIGH); // LED on to indicate we are not in main loop
 
@@ -1496,7 +1493,6 @@ void calibrateESCs()
     failSafe();                                                                // Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup
     getDesState();                                                             // Convert raw commands to normalized values based on saturated control limits
     getIMUdata();                                                              // Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise
-    Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); // Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees)
     getDesState();                                                             // Convert raw commands to normalized values based on saturated control limits
 
     m1_command_scaled = thro_des;
@@ -1915,6 +1911,24 @@ void printPitchInfo() {
   }
 }
 
+void printAttitudeDebug() {
+  if (current_time - print_counter > 10000) {
+    print_counter = micros();
+    Serial.print("q0:");
+    Serial.print(q0, 4);
+    Serial.print(" q1:");
+    Serial.print(q1, 4);
+    Serial.print(" q2:");
+    Serial.print(q2, 4);
+    Serial.print(" q3:");
+    Serial.print(q3, 4);
+    Serial.print(" roll:");
+    Serial.print(roll_IMU);
+    Serial.print(" pitch:");
+    Serial.println(pitch_IMU);
+  }
+}
+
 //=========================================================================================//
 
 // HELPER FUNCTIONS