From 76ebfd9677b37d0481bd5eca1e08a61d67ef975b Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 17 Mar 2025 15:06:52 +0100 Subject: [PATCH] It finally outputs data now need to get it parsed correctly --- src/drone/src/main.cpp | 79 +++++++++++++++++++++++++++--------------- 1 file changed, 52 insertions(+), 27 deletions(-) diff --git a/src/drone/src/main.cpp b/src/drone/src/main.cpp index f15016f..922294a 100644 --- a/src/drone/src/main.cpp +++ b/src/drone/src/main.cpp @@ -1,5 +1,6 @@ #include -#include +#include +#include "SparkFun_BNO080_Arduino_Library.h" #include #include #include @@ -53,17 +54,16 @@ static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to ma // #define GYRO_2000DPS // 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_8G // #define ACCEL_16G //========================================================================================================================// // REQUIRED LIBRARIES (included with download in main sketch folder) -#include //I2c communication -#include //SPI communication +#include //I2c communication #if defined USE_SBUS_RX #include "src/SBUS/SBUS.h" //sBus interface @@ -77,6 +77,7 @@ static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to ma #include "src/MPU6050/MPU6050.h" MPU6050 mpu6050; #elif defined USE_MPU9250_SPI +#include //SPI communication #include "src/MPU9250/MPU9250.h" MPU9250 mpu9250(SPI2, 36); #elif defined USE_BNO085_I2C @@ -109,14 +110,14 @@ 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 -#define GYRO_FS_SEL_250 MPU6050_GYRO_FS_250 -#define GYRO_FS_SEL_500 MPU6050_GYRO_FS_500 -#define GYRO_FS_SEL_1000 MPU6050_GYRO_FS_1000 -#define GYRO_FS_SEL_2000 MPU6050_GYRO_FS_2000 -#define ACCEL_FS_SEL_2 MPU6050_ACCEL_FS_2 -#define ACCEL_FS_SEL_4 MPU6050_ACCEL_FS_4 -#define ACCEL_FS_SEL_8 MPU6050_ACCEL_FS_8 -#define ACCEL_FS_SEL_16 MPU6050_ACCEL_FS_16 +#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_4 1 +#define ACCEL_FS_SEL_8 2 +#define ACCEL_FS_SEL_16 3 #endif #if defined GYRO_250DPS @@ -220,7 +221,7 @@ const int ch3Pin = 17; // ele const int ch4Pin = 20; // rudd const int ch5Pin = D7; // gear (throttle cut) const int ch6Pin = D7; // aux1 (free aux channel) -const int PPM_Pin = 23; +const int PPM_Pin = D7; // OneShot125 ESC pin outputs: const int m1Pin = D0; const int m2Pin = D1; @@ -366,7 +367,11 @@ void setup() { Serial.begin(9600); // USB serial Serial.println("Serial started"); - delay(10000); + + // Initialize IMU communication + Serial.println("Initiating IMU"); + IMUinit(); + Serial.println("Initiating pins"); // Initialize all pins // pinMode(13, OUTPUT); // Pin 13 LED blinker on board, do not modify @@ -408,9 +413,6 @@ void setup() channel_5_pwm = channel_5_fs; channel_6_pwm = channel_6_fs; - // Initialize IMU communication - IMUinit(); - delay(5); // Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level when powered up @@ -463,10 +465,10 @@ void loop() loopBlink(); // Indicate we are in main loop with short blink every 1.5 seconds // 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) + // 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) // printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) @@ -614,12 +616,21 @@ void IMUinit() mpu9250.setSrd(0); // sets gyro and accel read to 1khz, magnetometer read to 100hz #elif defined USE_BNO085_I2C Wire.begin(); - myIMU.begin(); + if (myIMU.begin() == false) // from sparkfun example + { + Serial.println("BNO080 not detected at default I2C address. Check your jumpers and the hookup guide. Freezing..."); + while (1) + ; + } Wire.setClock(400000); // Increase I2C data rate to 400kHz Serial.println("IMU initialized"); - myIMU.enableMagnetometer(50); myIMU.enableGyro(50); + delay(100); myIMU.enableAccelerometer(50); + delay(100); + myIMU.enableMagnetometer(50); + delay(500); + #endif } @@ -636,12 +647,18 @@ void getIMUdata() * the constant errors found in calculate_IMU_error() on startup are subtracted from the accelerometer and gyro readings. */ int16_t AcX, AcY, AcZ, GyX, GyY, GyZ, MgX, MgY, MgZ; - #if defined USE_MPU6050_I2C 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 + + // Keep waiting until data becomes available + while (!myIMU.dataAvailable()) + { + delay(10); // Small delay to avoid hammering the I2C bus too hard + } + // BNO magnetometer MgX = myIMU.getMagX(); MgY = myIMU.getMagY(); @@ -652,10 +669,11 @@ void getIMUdata() GyY = myIMU.getGyroY() * 180 / PI; GyZ = myIMU.getGyroZ() * 180 / PI; - // Acelleration (dont know if I need linear or normal. One way to find out) + // Acceleration AcX = myIMU.getAccelX(); AcY = myIMU.getAccelY(); AcZ = myIMU.getAccelZ(); + #endif // Accelerometer @@ -732,6 +750,13 @@ void calculate_IMU_error() #elif defined USE_MPU9250_SPI 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 +} + + // bno magnetometer MgX = myIMU.getMagX(); MgY = myIMU.getMagY(); @@ -741,13 +766,13 @@ void calculate_IMU_error() GyX = myIMU.getGyroX() * 180 / PI; GyY = myIMU.getGyroY() * 180 / PI; GyZ = myIMU.getGyroZ() * 180 / PI; - Serial.println(GyZ); - // Acelleration (dont know if I need linear or normal. One way to find out) AcX = myIMU.getAccelX(); AcY = myIMU.getAccelY(); AcZ = myIMU.getAccelZ(); + + #endif AccX = AcX / ACCEL_SCALE_FACTOR;