From f05c78e40073c8b515eb8b1c1c787aced2ad8b5b Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 17 Mar 2025 16:42:34 +0100 Subject: [PATCH] drone driver seems to work --- src/drone/src/main.cpp | 105 ++++++++++++++++++++++++++++++++--------- 1 file changed, 83 insertions(+), 22 deletions(-) diff --git a/src/drone/src/main.cpp b/src/drone/src/main.cpp index 922294a..ac9ea7b 100644 --- a/src/drone/src/main.cpp +++ b/src/drone/src/main.cpp @@ -48,16 +48,18 @@ static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to ma #define USE_BNO085_I2C // Custom // Uncomment only one full scale gyro range (deg/sec) -#define GYRO_250DPS // Default +// #define GYRO_250DPS // Default // #define GYRO_500DPS // #define GYRO_1000DPS // #define GYRO_2000DPS +#define GYRO_SCALE_BNO085 // Uncomment only one full scale accelerometer range (G's) -#define ACCEL_2G // Default +// #define ACCEL_2G // Default // #define ACCEL_4G // #define ACCEL_8G // #define ACCEL_16G +#define ACCEL_SCALE_BNO085 //========================================================================================================================// @@ -110,11 +112,12 @@ BNO080 myIMU; #define ACCEL_FS_SEL_8 mpu9250.ACCEL_RANGE_8G #define ACCEL_FS_SEL_16 mpu9250.ACCEL_RANGE_16G #elif defined USE_BNO085_I2C +// these are all placeholders since the bno does this all by itself #define GYRO_FS_SEL_250 0 #define GYRO_FS_SEL_500 1 #define GYRO_FS_SEL_1000 2 #define GYRO_FS_SEL_2000 3 -#define ACCEL_FS_SEL_2 0 +#define ACCEL_FS_SEL_2 0 #define ACCEL_FS_SEL_4 1 #define ACCEL_FS_SEL_8 2 #define ACCEL_FS_SEL_16 3 @@ -132,6 +135,9 @@ BNO080 myIMU; #elif defined GYRO_2000DPS #define GYRO_SCALE GYRO_FS_SEL_2000 #define GYRO_SCALE_FACTOR 16.4 +#elif defined GYRO_SCALE_BNO085 +#define GYRO_SCALE 0 +#define GYRO_SCALE_FACTOR 1.0 // BNO085 gives values already in deg/s when converted in getIMUdata #endif #if defined ACCEL_2G @@ -146,6 +152,9 @@ BNO080 myIMU; #elif defined ACCEL_16G #define ACCEL_SCALE ACCEL_FS_SEL_16 #define ACCEL_SCALE_FACTOR 2048.0 +#elif defined ACCEL_SCALE_BNO085 +#define ACCEL_SCALE 0 +#define ACCEL_SCALE_FACTOR 1.0 // BNO085 gives values already in g's when converted in getIMUdata #endif //========================================================================================================================// @@ -468,9 +477,9 @@ void loop() // 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) // 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) + // 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) // printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180) @@ -631,7 +640,6 @@ void IMUinit() myIMU.enableMagnetometer(50); delay(500); - #endif } @@ -651,14 +659,13 @@ void getIMUdata() mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); #elif defined USE_MPU9250_SPI mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); -#elif defined USE_BNO085_I2C - + #elif defined USE_BNO085_I2C // Keep waiting until data becomes available while (!myIMU.dataAvailable()) { - delay(10); // Small delay to avoid hammering the I2C bus too hard + delay(1); // Small delay to avoid hammering the I2C bus too hard } - + // All the error checking is done here because the BNO085 handles all the sensor fusion internally // BNO magnetometer MgX = myIMU.getMagX(); MgY = myIMU.getMagY(); @@ -670,10 +677,66 @@ void getIMUdata() GyZ = myIMU.getGyroZ() * 180 / PI; // Acceleration - AcX = myIMU.getAccelX(); - AcY = myIMU.getAccelY(); - AcZ = myIMU.getAccelZ(); + // 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; + + // Correct with gyro error values + 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_prev = GyroX; + GyroY_prev = GyroY; + GyroZ_prev = GyroZ; + + // 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; #endif // Accelerometer @@ -751,11 +814,10 @@ void calculate_IMU_error() mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); #elif defined USE_BNO085_I2C -while (!myIMU.dataAvailable()) -{ - delay(10); // Small delay to avoid hammering the I2C bus too hard -} - + while (!myIMU.dataAvailable()) + { + delay(1); // Small delay to avoid hammering the I2C bus too hard + } // bno magnetometer MgX = myIMU.getMagX(); @@ -768,11 +830,10 @@ while (!myIMU.dataAvailable()) GyZ = myIMU.getGyroZ() * 180 / PI; // Acelleration (dont know if I need linear or normal. One way to find out) - AcX = myIMU.getAccelX(); - AcY = myIMU.getAccelY(); - AcZ = myIMU.getAccelZ(); + 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 - #endif AccX = AcX / ACCEL_SCALE_FACTOR;