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

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;