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

added bno085 sensor functionality

Browse Source
This commit is contained in:
Sam Hos
2025-01-29 11:31:15 +01:00
parent 05ede91a79
commit 1397d66b54
1 changed files with 774 additions and 747 deletions
Download Patch File Download Diff File Expand all files Collapse all files

133
src/drone/drone.ino
View File

@@ -24,12 +24,10 @@ Everyone that sends me pictures and videos of your flying creations! -Nick
*/
//========================================================================================================================//
// USER-SPECIFIED DEFINES //
//========================================================================================================================//
#define PI 3.141592653589793238462643383279502884197
// Uncomment only one receiver type
#define USE_PWM_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
// Uncomment only one IMU
#define USE_MPU6050_I2C //Default
// #define USE_MPU6050_I2C // Default
// #define USE_MPU9250_SPI
#define USE_BNO085_I2C //Custom
// Uncomment only one full scale gyro range (deg/sec)
#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_16G
//========================================================================================================================//
// REQUIRED LIBRARIES (included with download in main sketch folder)
#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
#include "src/MPU9250/MPU9250.h"
MPU9250 mpu9250(SPI2, 36);
#elif defined USE_BNO085_I2C
#include <SparkFun_BNO080_Arduino_Library.h>
BNO080 myIMU;
#else
#error No MPU defined...
#endif
//========================================================================================================================//
// Setup gyro and accel full scale value selection and scale factor
#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_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
#endif
#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
#endif
//========================================================================================================================//
// USER-SPECIFIED VARIABLES //
//========================================================================================================================//
@@ -201,8 +202,6 @@ float Kp_yaw = 0.3; //Yaw P-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!)
//========================================================================================================================//
// DECLARE PINS //
//========================================================================================================================//
@@ -240,12 +239,8 @@ PWMServo servo5;
PWMServo servo6;
PWMServo servo7;
//========================================================================================================================//
// DECLARE GLOBAL VARIABLES
// 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)
// calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section
}
//========================================================================================================================//
// MAIN LOOP //
//========================================================================================================================//
@@ -446,14 +438,10 @@ void loop() {
loopRate(2000); // Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default
}
//========================================================================================================================//
// FUNCTIONS //
//========================================================================================================================//
void controlMixer() {
// DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration
/*
@@ -487,7 +475,6 @@ void controlMixer() {
s5_command_scaled = 0;
s6_command_scaled = 0;
s7_command_scaled = 0;
}
void armedStatus() {
@@ -511,7 +498,8 @@ void IMUinit() {
if (mpu6050.testConnection() == false) {
Serial.println("MPU6050 initialization unsuccessful");
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
@@ -528,9 +516,9 @@ void IMUinit() {
Serial.println("Check MPU9250 wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
while (1) {
}
}
// 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
// do is set the desired fullscale ranges
@@ -540,6 +528,15 @@ void IMUinit() {
mpu9250.setMagCalY(MagErrorY, MagScaleY);
mpu9250.setMagCalZ(MagErrorZ, MagScaleZ);
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
}
@@ -559,6 +556,21 @@ 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
//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
// Accelerometer
@@ -632,6 +644,21 @@ void calculate_IMU_error() {
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
//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
AccX = AcX / ACCEL_SCALE_FACTOR;
@@ -1063,7 +1090,6 @@ void controlANGLE2() {
// Update yaw variables
error_yaw_prev = error_yaw;
integral_yaw_prev = integral_yaw;
}
void controlRATE() {
@@ -1153,7 +1179,6 @@ void scaleCommands() {
s5_command_PWM = constrain(s5_command_PWM, 0, 180);
s6_command_PWM = constrain(s6_command_PWM, 0, 180);
s7_command_PWM = constrain(s7_command_PWM, 0, 180);
}
void getCommands() {
@@ -1174,8 +1199,7 @@ void getCommands() {
channel_6_pwm = getRadioPWM(6);
#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
float scale = 0.615;
float bias = 895.0;
@@ -1190,8 +1214,7 @@ void getCommands() {
#elif defined USE_DSM_RX
if (DSM.timedOut(micros())) {
// Serial.println("*** DSM RX TIMED OUT ***");
}
else if (DSM.gotNewFrame()) {
} else if (DSM.gotNewFrame()) {
uint16_t values[num_DSM_channels];
DSM.getChannelValues(values, num_DSM_channels);
@@ -1235,12 +1258,18 @@ void failSafe() {
int check6 = 0;
// Triggers for failure criteria
if (channel_1_pwm > maxVal || channel_1_pwm < minVal) check1 = 1;
if (channel_2_pwm > maxVal || channel_2_pwm < minVal) check2 = 1;
if (channel_3_pwm > maxVal || channel_3_pwm < minVal) 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 (channel_1_pwm > maxVal || channel_1_pwm < minVal)
check1 = 1;
if (channel_2_pwm > maxVal || channel_2_pwm < minVal)
check2 = 1;
if (channel_3_pwm > maxVal || channel_3_pwm < minVal)
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 ((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
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;
}
@@ -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
float diffParam = (param_upper - param_des) / (fadeTime_down * loopFreq);
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);
param = param + diffParam;
}
@@ -1511,15 +1538,16 @@ void calibrateMagnetometer() {
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.");
}
else {
} else {
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
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) {
@@ -1552,8 +1580,7 @@ void loopBlink() {
if (blinkAlternate == 1) {
blinkAlternate = 0;
blink_delay = 100000;
}
else if (blinkAlternate == 0) {
} else if (blinkAlternate == 0) {
blinkAlternate = 1;
blink_delay = 2000000;
}