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

replace how data get's received and passed on the rest of the code

Browse Source
This commit is contained in:
Sam Hos
2025-05-08 16:02:04 +02:00
parent 1519d1adad
commit a2111224b3
1 changed files with 51 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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