added bno085 sensor functionality

This commit is contained in:
2025-01-29 11:31:15 +01:00
parent 05ede91a79
commit 1397d66b54

View File

@@ -24,12 +24,10 @@ Everyone that sends me pictures and videos of your flying creations! -Nick
*/ */
//========================================================================================================================// //========================================================================================================================//
// USER-SPECIFIED DEFINES // // USER-SPECIFIED DEFINES //
//========================================================================================================================// //========================================================================================================================//
#define PI 3.141592653589793238462643383279502884197
// Uncomment only one receiver type // Uncomment only one receiver type
#define USE_PWM_RX #define USE_PWM_RX
// #define USE_PPM_RX // #define USE_PPM_RX
@@ -38,8 +36,9 @@ Everyone that sends me pictures and videos of your flying creations! -Nick
static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to match the number of transmitter channels you have static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to match the number of transmitter channels you have
// Uncomment only one IMU // Uncomment only one IMU
#define USE_MPU6050_I2C //Default // #define USE_MPU6050_I2C // Default
// #define USE_MPU9250_SPI // #define USE_MPU9250_SPI
#define USE_BNO085_I2C //Custom
// Uncomment only one full scale gyro range (deg/sec) // Uncomment only one full scale gyro range (deg/sec)
#define GYRO_250DPS // Default #define GYRO_250DPS // Default
@@ -53,12 +52,8 @@ static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to mat
// #define ACCEL_8G // #define ACCEL_8G
// #define ACCEL_16G // #define ACCEL_16G
//========================================================================================================================// //========================================================================================================================//
// REQUIRED LIBRARIES (included with download in main sketch folder) // REQUIRED LIBRARIES (included with download in main sketch folder)
#include <Wire.h> //I2c communication #include <Wire.h> //I2c communication
@@ -79,16 +74,15 @@ static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to mat
#elif defined USE_MPU9250_SPI #elif defined USE_MPU9250_SPI
#include "src/MPU9250/MPU9250.h" #include "src/MPU9250/MPU9250.h"
MPU9250 mpu9250(SPI2, 36); MPU9250 mpu9250(SPI2, 36);
#elif defined USE_BNO085_I2C
#include <SparkFun_BNO080_Arduino_Library.h>
BNO080 myIMU;
#else #else
#error No MPU defined... #error No MPU defined...
#endif #endif
//========================================================================================================================// //========================================================================================================================//
// Setup gyro and accel full scale value selection and scale factor // Setup gyro and accel full scale value selection and scale factor
#if defined USE_MPU6050_I2C #if defined USE_MPU6050_I2C
@@ -109,6 +103,15 @@ static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to mat
#define ACCEL_FS_SEL_4 mpu9250.ACCEL_RANGE_4G #define ACCEL_FS_SEL_4 mpu9250.ACCEL_RANGE_4G
#define ACCEL_FS_SEL_8 mpu9250.ACCEL_RANGE_8G #define ACCEL_FS_SEL_8 mpu9250.ACCEL_RANGE_8G
#define ACCEL_FS_SEL_16 mpu9250.ACCEL_RANGE_16G #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
#endif #endif
#if defined GYRO_250DPS #if defined GYRO_250DPS
@@ -139,8 +142,6 @@ static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to mat
#define ACCEL_SCALE_FACTOR 2048.0 #define ACCEL_SCALE_FACTOR 2048.0
#endif #endif
//========================================================================================================================// //========================================================================================================================//
// USER-SPECIFIED VARIABLES // // USER-SPECIFIED VARIABLES //
//========================================================================================================================// //========================================================================================================================//
@@ -201,8 +202,6 @@ float Kp_yaw = 0.3; //Yaw P-gain
float Ki_yaw = 0.05; // Yaw I-gain float Ki_yaw = 0.05; // Yaw I-gain
float Kd_yaw = 0.00015; // Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) float Kd_yaw = 0.00015; // Yaw D-gain (be careful when increasing too high, motors will begin to overheat!)
//========================================================================================================================// //========================================================================================================================//
// DECLARE PINS // // DECLARE PINS //
//========================================================================================================================// //========================================================================================================================//
@@ -240,12 +239,8 @@ PWMServo servo5;
PWMServo servo6; PWMServo servo6;
PWMServo servo7; PWMServo servo7;
//========================================================================================================================// //========================================================================================================================//
// DECLARE GLOBAL VARIABLES // DECLARE GLOBAL VARIABLES
// General stuff // General stuff
@@ -377,11 +372,8 @@ void setup() {
// If using MPU9250 IMU, uncomment for one-time magnetometer calibration (may need to repeat for new locations) // If using MPU9250 IMU, uncomment for one-time magnetometer calibration (may need to repeat for new locations)
// calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section // calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section
} }
//========================================================================================================================// //========================================================================================================================//
// MAIN LOOP // // MAIN LOOP //
//========================================================================================================================// //========================================================================================================================//
@@ -446,14 +438,10 @@ void loop() {
loopRate(2000); // Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default loopRate(2000); // Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default
} }
//========================================================================================================================// //========================================================================================================================//
// FUNCTIONS // // FUNCTIONS //
//========================================================================================================================// //========================================================================================================================//
void controlMixer() { void controlMixer() {
// DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration // DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration
/* /*
@@ -487,7 +475,6 @@ void controlMixer() {
s5_command_scaled = 0; s5_command_scaled = 0;
s6_command_scaled = 0; s6_command_scaled = 0;
s7_command_scaled = 0; s7_command_scaled = 0;
} }
void armedStatus() { void armedStatus() {
@@ -511,7 +498,8 @@ void IMUinit() {
if (mpu6050.testConnection() == false) { if (mpu6050.testConnection() == false) {
Serial.println("MPU6050 initialization unsuccessful"); Serial.println("MPU6050 initialization unsuccessful");
Serial.println("Check MPU6050 wiring or try cycling power"); Serial.println("Check MPU6050 wiring or try cycling power");
while(1) {} while (1) {
}
} }
// From the reset state all registers should be 0x00, so we should be at // From the reset state all registers should be 0x00, so we should be at
@@ -528,9 +516,9 @@ void IMUinit() {
Serial.println("Check MPU9250 wiring or try cycling power"); Serial.println("Check MPU9250 wiring or try cycling power");
Serial.print("Status: "); Serial.print("Status: ");
Serial.println(status); Serial.println(status);
while(1) {} while (1) {
}
} }
// From the reset state all registers should be 0x00, so we should be at // From the reset state all registers should be 0x00, so we should be at
// max sample rate with digital low pass filter(s) off. All we need to // max sample rate with digital low pass filter(s) off. All we need to
// do is set the desired fullscale ranges // do is set the desired fullscale ranges
@@ -540,6 +528,15 @@ void IMUinit() {
mpu9250.setMagCalY(MagErrorY, MagScaleY); mpu9250.setMagCalY(MagErrorY, MagScaleY);
mpu9250.setMagCalZ(MagErrorZ, MagScaleZ); mpu9250.setMagCalZ(MagErrorZ, MagScaleZ);
mpu9250.setSrd(0); // sets gyro and accel read to 1khz, magnetometer read to 100hz mpu9250.setSrd(0); // sets gyro and accel read to 1khz, magnetometer read to 100hz
#elif defined USE_BNO085_I2C
Wire.begin();
myIMU.begin();
Wire.setClock(400000); // Increase I2C data rate to 400kHz
myIMU.enableMagnetometer(50);
myIMU.enableGyro(50);
myIMU.enableAccelerometer(50);
#endif #endif
} }
@@ -559,6 +556,21 @@ void getIMUdata() {
mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ);
#elif defined USE_MPU9250_SPI #elif defined USE_MPU9250_SPI
mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ);
#elif defined USE_BNO085_I2C
//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;
//Acelleration (dont know if I need linear or normal. One way to find out)
AcX = myIMU.getAccelX();
AcY = myIMU.getAccelY();
AcZ = myIMU.getAccelZ();
#endif #endif
// Accelerometer // Accelerometer
@@ -632,6 +644,21 @@ void calculate_IMU_error() {
mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ);
#elif defined USE_MPU9250_SPI #elif defined USE_MPU9250_SPI
mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ);
#elif defined USE_BNO085_I2C
//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;
//Acelleration (dont know if I need linear or normal. One way to find out)
AcX = myIMU.getAccelX();
AcY = myIMU.getAccelY();
AcZ = myIMU.getAccelZ();
#endif #endif
AccX = AcX / ACCEL_SCALE_FACTOR; AccX = AcX / ACCEL_SCALE_FACTOR;
@@ -1063,7 +1090,6 @@ void controlANGLE2() {
// Update yaw variables // Update yaw variables
error_yaw_prev = error_yaw; error_yaw_prev = error_yaw;
integral_yaw_prev = integral_yaw; integral_yaw_prev = integral_yaw;
} }
void controlRATE() { void controlRATE() {
@@ -1153,7 +1179,6 @@ void scaleCommands() {
s5_command_PWM = constrain(s5_command_PWM, 0, 180); s5_command_PWM = constrain(s5_command_PWM, 0, 180);
s6_command_PWM = constrain(s6_command_PWM, 0, 180); s6_command_PWM = constrain(s6_command_PWM, 0, 180);
s7_command_PWM = constrain(s7_command_PWM, 0, 180); s7_command_PWM = constrain(s7_command_PWM, 0, 180);
} }
void getCommands() { void getCommands() {
@@ -1174,8 +1199,7 @@ void getCommands() {
channel_6_pwm = getRadioPWM(6); channel_6_pwm = getRadioPWM(6);
#elif defined USE_SBUS_RX #elif defined USE_SBUS_RX
if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame)) if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame)) {
{
// sBus scaling below is for Taranis-Plus and X4R-SB // sBus scaling below is for Taranis-Plus and X4R-SB
float scale = 0.615; float scale = 0.615;
float bias = 895.0; float bias = 895.0;
@@ -1190,8 +1214,7 @@ void getCommands() {
#elif defined USE_DSM_RX #elif defined USE_DSM_RX
if (DSM.timedOut(micros())) { if (DSM.timedOut(micros())) {
// Serial.println("*** DSM RX TIMED OUT ***"); // Serial.println("*** DSM RX TIMED OUT ***");
} } else if (DSM.gotNewFrame()) {
else if (DSM.gotNewFrame()) {
uint16_t values[num_DSM_channels]; uint16_t values[num_DSM_channels];
DSM.getChannelValues(values, num_DSM_channels); DSM.getChannelValues(values, num_DSM_channels);
@@ -1235,12 +1258,18 @@ void failSafe() {
int check6 = 0; int check6 = 0;
// Triggers for failure criteria // Triggers for failure criteria
if (channel_1_pwm > maxVal || channel_1_pwm < minVal) check1 = 1; if (channel_1_pwm > maxVal || channel_1_pwm < minVal)
if (channel_2_pwm > maxVal || channel_2_pwm < minVal) check2 = 1; check1 = 1;
if (channel_3_pwm > maxVal || channel_3_pwm < minVal) check3 = 1; if (channel_2_pwm > maxVal || channel_2_pwm < minVal)
if (channel_4_pwm > maxVal || channel_4_pwm < minVal) check4 = 1; check2 = 1;
if (channel_5_pwm > maxVal || channel_5_pwm < minVal) check5 = 1; if (channel_3_pwm > maxVal || channel_3_pwm < minVal)
if (channel_6_pwm > maxVal || channel_6_pwm < minVal) check6 = 1; check3 = 1;
if (channel_4_pwm > maxVal || channel_4_pwm < minVal)
check4 = 1;
if (channel_5_pwm > maxVal || channel_5_pwm < minVal)
check5 = 1;
if (channel_6_pwm > maxVal || channel_6_pwm < minVal)
check6 = 1;
// If any failures, set to default failsafe values // If any failures, set to default failsafe values
if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) { if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) {
@@ -1394,8 +1423,7 @@ float floatFaderLinear(float param, float param_min, float param_max, float fade
if (state == 1) { // Maximum param bound desired, increase param by diffParam for each loop iteration if (state == 1) { // Maximum param bound desired, increase param by diffParam for each loop iteration
param = param + diffParam; param = param + diffParam;
} } else if (state == 0) { // Minimum param bound desired, decrease param by diffParam for each loop iteration
else if (state == 0) { //Minimum param bound desired, decrease param by diffParam for each loop iteration
param = param - diffParam; param = param - diffParam;
} }
@@ -1417,8 +1445,7 @@ float floatFaderLinear2(float param, float param_des, float param_lower, float p
if (param > param_des) { // Need to fade down to get to desired if (param > param_des) { // Need to fade down to get to desired
float diffParam = (param_upper - param_des) / (fadeTime_down * loopFreq); float diffParam = (param_upper - param_des) / (fadeTime_down * loopFreq);
param = param - diffParam; param = param - diffParam;
} } else if (param < param_des) { // Need to fade up to get to desired
else if (param < param_des) { //Need to fade up to get to desired
float diffParam = (param_des - param_lower) / (fadeTime_up * loopFreq); float diffParam = (param_des - param_lower) / (fadeTime_up * loopFreq);
param = param + diffParam; param = param + diffParam;
} }
@@ -1511,15 +1538,16 @@ void calibrateMagnetometer() {
Serial.println(";"); Serial.println(";");
Serial.println(" "); Serial.println(" ");
Serial.println("If you are having trouble with your attitude estimate at a new flying location, repeat this process as needed."); Serial.println("If you are having trouble with your attitude estimate at a new flying location, repeat this process as needed.");
} } else {
else {
Serial.println("Calibration Unsuccessful. Please reset the board and try again."); Serial.println("Calibration Unsuccessful. Please reset the board and try again.");
} }
while(1); //Halt code so it won't enter main loop until this function commented out while (1)
; // Halt code so it won't enter main loop until this function commented out
#endif #endif
Serial.println("Error: MPU9250 not selected. Cannot calibrate non-existent magnetometer."); Serial.println("Error: MPU9250 not selected. Cannot calibrate non-existent magnetometer.");
while(1); //Halt code so it won't enter main loop until this function commented out while (1)
; // Halt code so it won't enter main loop until this function commented out
} }
void loopRate(int freq) { void loopRate(int freq) {
@@ -1552,8 +1580,7 @@ void loopBlink() {
if (blinkAlternate == 1) { if (blinkAlternate == 1) {
blinkAlternate = 0; blinkAlternate = 0;
blink_delay = 100000; blink_delay = 100000;
} } else if (blinkAlternate == 0) {
else if (blinkAlternate == 0) {
blinkAlternate = 1; blinkAlternate = 1;
blink_delay = 2000000; blink_delay = 2000000;
} }