From 2d1a6ae1f61f4b6644db943e6d431c0214bbc2ca Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Tue, 3 Jun 2025 12:26:06 +0200 Subject: [PATCH] stable flight in theory been achieved --- src/drone/src/main.cpp | 60 ++++++++++++++++++++++++++---------------- 1 file changed, 37 insertions(+), 23 deletions(-) diff --git a/src/drone/src/main.cpp b/src/drone/src/main.cpp index a5d7446..f1a5048 100644 --- a/src/drone/src/main.cpp +++ b/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. @@ -508,12 +510,13 @@ void controlMixer() *roll_passthru, pitch_passthru, yaw_passthru - direct unstabilized command passthrough *channel_6_pwm - free auxillary channel, can be used to toggle things with an 'if' statement */ - + // Quad mixer with pitch bias because the sensor isnt flat on the board m1_command_scaled = thro_des - pitch_PID + roll_PID - yaw_PID; // Front Left CCW 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 - - // 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; - + // 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 + + // 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