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

It finally outputs data

Browse Source 
now need to get it parsed  correctly
This commit is contained in:
Sam Hos
2025-03-17 15:06:52 +01:00
parent 011cc8ccfd
commit 76ebfd9677
1 changed files with 52 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
src/drone/src/main.cpp
View File

@@ -1,5 +1,6 @@
#include <Arduino.h> #include <Arduino.h>
#include <SparkFun_BNO080_Arduino_Library.h> #include <Wire.h>
#include "SparkFun_BNO080_Arduino_Library.h"
#include <ESP32Servo.h> #include <ESP32Servo.h>
#include <esp_now.h> #include <esp_now.h>
#include <WiFi.h> #include <WiFi.h>
@@ -53,17 +54,16 @@ static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to ma
// #define GYRO_2000DPS // #define GYRO_2000DPS
// Uncomment only one full scale accelerometer range (G's) // Uncomment only one full scale accelerometer range (G's)
// #define ACCEL_2G // Default #define ACCEL_2G // Default
// #define ACCEL_4G // #define ACCEL_4G
#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
#include <SPI.h> //SPI communication
#if defined USE_SBUS_RX #if defined USE_SBUS_RX
#include "src/SBUS/SBUS.h" //sBus interface #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" #include "src/MPU6050/MPU6050.h"
MPU6050 mpu6050; MPU6050 mpu6050;
#elif defined USE_MPU9250_SPI #elif defined USE_MPU9250_SPI
#include <SPI.h> //SPI communication
#include "src/MPU9250/MPU9250.h" #include "src/MPU9250/MPU9250.h"
MPU9250 mpu9250(SPI2, 36); MPU9250 mpu9250(SPI2, 36);
#elif defined USE_BNO085_I2C #elif defined USE_BNO085_I2C
@@ -109,14 +110,14 @@ BNO080 myIMU;
#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 #elif defined USE_BNO085_I2C
#define GYRO_FS_SEL_250 MPU6050_GYRO_FS_250 #define GYRO_FS_SEL_250 0
#define GYRO_FS_SEL_500 MPU6050_GYRO_FS_500 #define GYRO_FS_SEL_500 1
#define GYRO_FS_SEL_1000 MPU6050_GYRO_FS_1000 #define GYRO_FS_SEL_1000 2
#define GYRO_FS_SEL_2000 MPU6050_GYRO_FS_2000 #define GYRO_FS_SEL_2000 3
#define ACCEL_FS_SEL_2 MPU6050_ACCEL_FS_2 #define ACCEL_FS_SEL_2 0
#define ACCEL_FS_SEL_4 MPU6050_ACCEL_FS_4 #define ACCEL_FS_SEL_4 1
#define ACCEL_FS_SEL_8 MPU6050_ACCEL_FS_8 #define ACCEL_FS_SEL_8 2
#define ACCEL_FS_SEL_16 MPU6050_ACCEL_FS_16 #define ACCEL_FS_SEL_16 3
#endif #endif
#if defined GYRO_250DPS #if defined GYRO_250DPS
@@ -220,7 +221,7 @@ const int ch3Pin = 17; // ele
const int ch4Pin = 20; // rudd const int ch4Pin = 20; // rudd
const int ch5Pin = D7; // gear (throttle cut) const int ch5Pin = D7; // gear (throttle cut)
const int ch6Pin = D7; // aux1 (free aux channel) const int ch6Pin = D7; // aux1 (free aux channel)
const int PPM_Pin = 23; const int PPM_Pin = D7;
// OneShot125 ESC pin outputs: // OneShot125 ESC pin outputs:
const int m1Pin = D0; const int m1Pin = D0;
const int m2Pin = D1; const int m2Pin = D1;
@@ -366,7 +367,11 @@ void setup()
{ {
Serial.begin(9600); // USB serial Serial.begin(9600); // USB serial
Serial.println("Serial started"); Serial.println("Serial started");
delay(10000);
// Initialize IMU communication
Serial.println("Initiating IMU");
IMUinit();
Serial.println("Initiating pins"); Serial.println("Initiating pins");
// Initialize all pins // Initialize all pins
// pinMode(13, OUTPUT); // Pin 13 LED blinker on board, do not modify // 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_5_pwm = channel_5_fs;
channel_6_pwm = channel_6_fs; channel_6_pwm = channel_6_fs;
// Initialize IMU communication
IMUinit();
delay(5); delay(5);
// Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level when powered up // 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 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: // Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE:
printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) // 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) // 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) // 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) // 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 mpu9250.setSrd(0); // sets gyro and accel read to 1khz, magnetometer read to 100hz
#elif defined USE_BNO085_I2C #elif defined USE_BNO085_I2C
Wire.begin(); 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 Wire.setClock(400000); // Increase I2C data rate to 400kHz
Serial.println("IMU initialized"); Serial.println("IMU initialized");
myIMU.enableMagnetometer(50);
myIMU.enableGyro(50); myIMU.enableGyro(50);
delay(100);
myIMU.enableAccelerometer(50); myIMU.enableAccelerometer(50);
delay(100);
myIMU.enableMagnetometer(50);
delay(500);
#endif #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. * 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; int16_t AcX, AcY, AcZ, GyX, GyY, GyZ, MgX, MgY, MgZ;
#if defined USE_MPU6050_I2C #if defined USE_MPU6050_I2C
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 #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 // BNO magnetometer
MgX = myIMU.getMagX(); MgX = myIMU.getMagX();
MgY = myIMU.getMagY(); MgY = myIMU.getMagY();
@@ -652,10 +669,11 @@ void getIMUdata()
GyY = myIMU.getGyroY() * 180 / PI; GyY = myIMU.getGyroY() * 180 / PI;
GyZ = myIMU.getGyroZ() * 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(); AcX = myIMU.getAccelX();
AcY = myIMU.getAccelY(); AcY = myIMU.getAccelY();
AcZ = myIMU.getAccelZ(); AcZ = myIMU.getAccelZ();
#endif #endif
// Accelerometer // Accelerometer
@@ -732,6 +750,13 @@ void calculate_IMU_error()
#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 #elif defined USE_BNO085_I2C
while (!myIMU.dataAvailable())
{
delay(10); // Small delay to avoid hammering the I2C bus too hard
}
// bno magnetometer // bno magnetometer
MgX = myIMU.getMagX(); MgX = myIMU.getMagX();
MgY = myIMU.getMagY(); MgY = myIMU.getMagY();
@@ -741,13 +766,13 @@ void calculate_IMU_error()
GyX = myIMU.getGyroX() * 180 / PI; GyX = myIMU.getGyroX() * 180 / PI;
GyY = myIMU.getGyroY() * 180 / PI; GyY = myIMU.getGyroY() * 180 / PI;
GyZ = myIMU.getGyroZ() * 180 / PI; GyZ = myIMU.getGyroZ() * 180 / PI;
Serial.println(GyZ);
// Acelleration (dont know if I need linear or normal. One way to find out) // Acelleration (dont know if I need linear or normal. One way to find out)
AcX = myIMU.getAccelX(); AcX = myIMU.getAccelX();
AcY = myIMU.getAccelY(); AcY = myIMU.getAccelY();
AcZ = myIMU.getAccelZ(); AcZ = myIMU.getAccelZ();
#endif #endif
AccX = AcX / ACCEL_SCALE_FACTOR; AccX = AcX / ACCEL_SCALE_FACTOR;