mirror of
https://gitlab.waag.org/make/fablab/interns/2025/sam.git
synced 2025-08-04 04:14:56 +00:00
added bno085 sensor functionality
This commit is contained in:
@@ -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;
|
||||||
}
|
}
|
||||||
|
Reference in New Issue
Block a user