diff --git a/src/drone/src/main.cpp b/src/drone/src/main.cpp index 093203e..79703f7 100644 --- a/src/drone/src/main.cpp +++ b/src/drone/src/main.cpp @@ -442,11 +442,11 @@ void loop() // Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE: // printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) - // printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle) + printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle) // printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) // printAccelData(); // Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) // printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) - printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) + // printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) // printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) // printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) // printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) @@ -457,7 +457,6 @@ void loop() // Get vehicle state 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 angle estimates (degrees) // Compute desired state getDesState(); // Convert raw commands to normalized values based on saturated control limits @@ -481,9 +480,6 @@ void loop() // Get vehicle commands for next loop iteration getCommands(); // Pulls current available radio commands failSafe(); // Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup - - // Regulate loop rate - loopRate(2000); // Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default } //========================================================================================================================// @@ -599,6 +595,8 @@ void IMUinit() myIMU.enableAccelerometer(50); delay(100); myIMU.enableMagnetometer(50); + delay(100); + myIMU.enableGyroIntegratedRotationVector(50); delay(500); #endif @@ -622,79 +620,63 @@ void getIMUdata() mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); #elif defined USE_BNO085_I2C // Keep waiting until data becomes available - while (!myIMU.dataAvailable()) - { - delay(1); // Small delay to avoid hammering the I2C bus too hard + // Wait for data with timeout + unsigned long startTime = millis(); + while (!myIMU.dataAvailable()) { + delay(1); + if (millis() - startTime > 500) { + Serial.println("BNO085 timeout"); + return; + } } - // All the error checking is done here because the BNO085 handles all the sensor fusion internally - // BNO magnetometer - MgX = myIMU.getMagX(); - MgY = myIMU.getMagY(); - MgZ = myIMU.getMagZ(); - - // convert radians to degrees - GyX = myIMU.getGyroX() * 180 / PI; - GyY = myIMU.getGyroY() * 180 / PI; - GyZ = myIMU.getGyroZ() * 180 / PI; - - // Acceleration - // Convert acceleration m/s² to g (9.81 m/s² = 1g) - AcX = myIMU.getAccelX() / 9.80665; // m/s² to g - AcY = myIMU.getAccelY() / 9.80665; // m/s² to g - AcZ = myIMU.getAccelZ() / 9.80665; // m/s² to g - - AccX = AcX; - AccY = AcY; - AccZ = AcZ; - - // Correct the outputs with the calculated error values - AccX = AccX - AccErrorX; - AccY = AccY - AccErrorY; - AccZ = AccZ - AccErrorZ; - // LP filter accelerometer data - AccX = (1.0 - B_accel) * AccX_prev + B_accel * AccX; - AccY = (1.0 - B_accel) * AccY_prev + B_accel * AccY; - AccZ = (1.0 - B_accel) * AccZ_prev + B_accel * AccZ; - AccX_prev = AccX; - AccY_prev = AccY; - AccZ_prev = AccZ; - // Process gyro data - GyroX = GyX; - GyroY = GyY; - GyroZ = GyZ; + // 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; + - // Correct with gyro error values + // Get raw sensor data for PID controller + GyroX = myIMU.getGyroX() * 57.29577951f; // Convert rad/s to deg/s + GyroY = myIMU.getGyroY() * 57.29577951f; + GyroZ = myIMU.getGyroZ() * 57.29577951f; + + // Apply error corrections and filtering GyroX = GyroX - GyroErrorX; GyroY = GyroY - GyroErrorY; GyroZ = GyroZ - GyroErrorZ; - // LP filter gyro data - GyroX = (1.0 - B_gyro) * GyroX_prev + B_gyro * GyroX; - GyroY = (1.0 - B_gyro) * GyroY_prev + B_gyro * GyroY; - GyroZ = (1.0 - B_gyro) * GyroZ_prev + B_gyro * GyroZ; + GyroX = (1.0f - B_gyro) * GyroX_prev + B_gyro * GyroX; + GyroY = (1.0f - B_gyro) * GyroY_prev + B_gyro * GyroY; + GyroZ = (1.0f - B_gyro) * GyroZ_prev + B_gyro * GyroZ; GyroX_prev = GyroX; GyroY_prev = GyroY; GyroZ_prev = GyroZ; + + // Get accelerometer data (still useful for PID control) + AccX = myIMU.getAccelX() / 9.80665f; // Convert m/s² to g + AccY = myIMU.getAccelY() / 9.80665f; + AccZ = myIMU.getAccelZ() / 9.80665f; + + // Apply corrections and filtering + AccX = AccX - AccErrorX; + AccY = AccY - AccErrorY; + AccZ = AccZ - AccErrorZ; + + AccX = (1.0f - B_accel) * AccX_prev + B_accel * AccX; + AccY = (1.0f - B_accel) * AccY_prev + B_accel * AccY; + AccZ = (1.0f - B_accel) * AccZ_prev + B_accel * AccZ; + AccX_prev = AccX; + AccY_prev = AccY; + AccZ_prev = AccZ; - // Process mag data - MagX = MgX / 6.0; // uT - MagY = MgY / 6.0; - MagZ = MgZ / 6.0; - - // Correct the outputs with the calculated error values - MagX = (MagX - MagErrorX) * MagScaleX; - MagY = (MagY - MagErrorY) * MagScaleY; - MagZ = (MagZ - MagErrorZ) * MagScaleZ; - - // LP filter magnetometer data - MagX = (1.0 - B_mag) * MagX_prev + B_mag * MagX; - MagY = (1.0 - B_mag) * MagY_prev + B_mag * MagY; - MagZ = (1.0 - B_mag) * MagZ_prev + B_mag * MagZ; - MagX_prev = MagX; - MagY_prev = MagY; - MagZ_prev = MagZ; // Return to avoid executing code for other IMUs return; @@ -795,6 +777,8 @@ void calculate_IMU_error() AcY = myIMU.getAccelY() / 9.80665; // m/s² to g AcZ = myIMU.getAccelZ() / 9.80665; // m/s² to g + + #endif AccX = AcX / ACCEL_SCALE_FACTOR;