Smikkelbakje/Drone-FabAcademy-2025
1
0
Fork 0
mirror of https://gitlab.waag.org/make/fablab/interns/2025/sam.git synced 2025-08-03 11:54:58 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

drone driver seems to work

Browse Source
This commit is contained in:
Sam Hos
2025-03-17 16:42:34 +01:00
parent 49fe84e1a6
commit f05c78e400
1 changed files with 83 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
src/drone/src/main.cpp
View File

@@ -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;