docs - it finally compiles now except the IMU is dead

This commit is contained in:
2025-02-19 10:56:04 +01:00
parent a08ed6c5a8
commit df4145c734
19 changed files with 461 additions and 274 deletions

Binary file not shown.

After

Width:  |  Height:  |  Size: 176 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 361 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 389 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 54 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 14 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 16 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 64 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 215 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 102 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 122 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 212 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 212 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 29 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 31 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 60 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 39 KiB

View File

@@ -457,7 +457,7 @@ I am following [this](https://randomnerdtutorials.com/esp-now-esp32-arduino-ide/
What I first need to do according to the tutorial is get the receiver esp their mac adress. Luckily that gets displayed when uploading to the esp. What I first need to do according to the tutorial is get the receiver esp their mac adress. Luckily that gets displayed when uploading to the esp.
![alt text](image-26.png) ![alt text](image-26.jpg)
After `MAC` there is the mac address. I've saved it in my controller code as a compiler flag `#define receiverMAC "d8:3b:da:37:66:00";` So I could use it later on. I don't know if the libraries accepts this compiler flag but I will find out soon enough. After `MAC` there is the mac address. I've saved it in my controller code as a compiler flag `#define receiverMAC "d8:3b:da:37:66:00";` So I could use it later on. I don't know if the libraries accepts this compiler flag but I will find out soon enough.
I first need to create a struct with the data I wanna send. A stuct is a collection of variables named under one big variable. I first need to create a struct with the data I wanna send. A stuct is a collection of variables named under one big variable.
@@ -573,7 +573,7 @@ I added that it could connect to the internet and that it attempts to send data.
Now when I compile it I get errors that the data isn't being send correctly. The issue could be that the second microcontroller isn't turned on with the receiver code. Now when I compile it I get errors that the data isn't being send correctly. The issue could be that the second microcontroller isn't turned on with the receiver code.
![alt text](image-27.png) ![alt text](image-27.jpg)
#### Drone side #### Drone side
@@ -651,10 +651,10 @@ A weird thing I found out is that my esp32C3 Supermini doesn't work wirelessly t
I've delved a bit deeper into it and then I found online that with the first batches of espC3 supermini's have issues with the antenna. When I was debugging it I noticed that connecting the ground to the antenne fixed it when I tried it with a pin. But when soldered on it didn't work anymore and then only thing that worked was my finger. I've delved a bit deeper into it and then I found online that with the first batches of espC3 supermini's have issues with the antenna. When I was debugging it I noticed that connecting the ground to the antenne fixed it when I tried it with a pin. But when soldered on it didn't work anymore and then only thing that worked was my finger.
![alt text](IMG_8200.JPEG) ![alt text](IMG_8200.jpg)
Result of bridging ground and the antenna. Sadly didn't work. After that I grabbed the Xiao espC3 and connected it. It almost worked like a drop in replacement I only needed to change the Potpin. After that it worked flawlessly. Result of bridging ground and the antenna. Sadly didn't work. After that I grabbed the Xiao espC3 and connected it. It almost worked like a drop in replacement I only needed to change the Potpin. After that it worked flawlessly.
![alt text](image-28.png) ![alt text](image-28.jpg)
Now I can start intergrating the code into the drone driver. Now I can start intergrating the code into the drone driver.
@@ -670,25 +670,51 @@ The first thing I did is add the correct libraries at the top of the file. So it
After that I added my own ESPNow define so I could use that later on in the code. I want the code to stay variable and user specified so thats why im continuing the way it was made. After that I added my own ESPNow define so I could use that later on in the code. I want the code to stay variable and user specified so thats why im continuing the way it was made.
![alt text](image-29.png) ![alt text](image-29.jpg)
After that I added the global variables for the ESPNow protocol. After that I added the global variables for the ESPNow protocol.
![alt text](image-30.png) ![alt text](image-30.jpg)
Then I uncommented `RadioSetup()` again so it could be used again and I added a case for my ESPNow protocol. Then I uncommented `RadioSetup()` again so it could be used again and I added a case for my ESPNow protocol.
![alt text](image-31.png) ![alt text](image-31.jpg)
In the PWM receive function I added an additional case so my PWM values actually get intergrated into the code. In the PWM receive function I added an additional case so my PWM values actually get intergrated into the code.
![alt text](image-32.png) ![alt text](image-32.jpg)
At the end of the program I added the ESPNow Initialisation function and the callback function for when the data is received. At the end of the program I added the ESPNow Initialisation function and the callback function for when the data is received.
![alt text](image-33.png) ![alt text](image-33.jpg)
When I comppile it now it should work. When I compile it now it should work. Except that it doesn't work.
![alt text](image-34.jpg)
From looking at the errors it looked like the PWMServo library wasn't compatible with the ESP32. When looking around on google from PWMservo libraries for the ESP32 I found the correct library on [this](https://randomnerdtutorials.com/esp32-servo-motor-web-server-arduino-ide/) website. When I added that I needed to edit the class creation to the correct library and then it finaly compiled!
From:
![alt text](image-35.jpg)
To:
![alt text](image-36.jpg)
Result:
![alt text](image-37.jpg)
It compiles!
The next day when testing the code. The microcontroller would suddenly disconnect. First I tried setting every pin to something else because there where pins being defined the espC6 doesn't have. When I finally set the debug level to verbose. So I can see everything that the microcontroller is doing. I saw that it used pin 12 and 13 for USB. And somewhere in the program pin 12 and 13 where re-assigned. So I removed that it got reassigned and then the communication worked flawlessly again.
![alt text](image-38.jpg)
Now I can finally test. When adding debug print statements for my sensor I found out it wasn't responding. Then I looked at the pins of the sensor and found out when I re-seated it I acciddentally killed it.
I had it like this:
![alt text](IMG_8212.jpg)
Instead of like this.
![alt text](IMG_8213.jpg)
So I had the 3v3 connected to the sensor it's ground and the ground connected to the SCL pin. So that killed it.
## Sources ## Sources
### Code ### Code
@@ -697,6 +723,7 @@ When I comppile it now it should work.
* [BNO085 library examples](https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library/tree/main/examples) * [BNO085 library examples](https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library/tree/main/examples)
* [PlatformIO espC6](https://wiki.seeedstudio.com/xiao_esp32c6_with_platform_io) * [PlatformIO espC6](https://wiki.seeedstudio.com/xiao_esp32c6_with_platform_io)
* [ESC calibration and PWM](https://ardupilot.org/copter/docs/esc-calibration.html) * [ESC calibration and PWM](https://ardupilot.org/copter/docs/esc-calibration.html)
* [ESP32PWMServo lib](https://randomnerdtutorials.com/esp32-servo-motor-web-server-arduino-ide/)
### Parts ### Parts
* [PotSlider](https://nl.aliexpress.com/item/1005006733220962.html) * [PotSlider](https://nl.aliexpress.com/item/1005006733220962.html)

View File

@@ -18,3 +18,5 @@ board = seeed_xiao_esp32c6
lib_deps = lib_deps =
paulstoffregen/PWMServo@^2.1 paulstoffregen/PWMServo@^2.1
sparkfun/SparkFun BNO080 Cortex Based IMU@^1.1.12 sparkfun/SparkFun BNO080 Cortex Based IMU@^1.1.12
madhephaestus/ESP32Servo@^3.0.6
build_flags = -DCORE_DEBUG_LEVEL=5

View File

@@ -1,6 +1,8 @@
#include <Arduino.h> #include <Arduino.h>
#include <SparkFun_BNO080_Arduino_Library.h> #include <SparkFun_BNO080_Arduino_Library.h>
#include <PWMServo.h> #include <ESP32Servo.h>
#include <esp_now.h>
#include <WiFi.h>
// Arduino/Teensy Flight Controller - dRehmFlight // Arduino/Teensy Flight Controller - dRehmFlight
// Author: Nicholas Rehm // Author: Nicholas Rehm
// Project Start: 1/6/2020 // Project Start: 1/6/2020
@@ -32,10 +34,11 @@ Everyone that sends me pictures and videos of your flying creations! -Nick
//========================================================================================================================// //========================================================================================================================//
#define PI 3.141592653589793238462643383279502884197 #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
// #define USE_SBUS_RX // #define USE_SBUS_RX
// #define USE_DSM_RX // #define USE_DSM_RX
#define USE_ESPNow
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
@@ -61,7 +64,6 @@ static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to m
#include <Wire.h> //I2c communication #include <Wire.h> //I2c communication
#include <SPI.h> //SPI communication #include <SPI.h> //SPI communication
#include <PWMServo.h> //Commanding any extra actuators, installed with teensyduino installer
#if defined USE_SBUS_RX #if defined USE_SBUS_RX
#include "src/SBUS/SBUS.h" //sBus interface #include "src/SBUS/SBUS.h" //sBus interface
@@ -216,16 +218,16 @@ const int ch1Pin = 15; // throttle
const int ch2Pin = 16; // ail const int ch2Pin = 16; // ail
const int ch3Pin = 17; // ele const int ch3Pin = 17; // ele
const int ch4Pin = 20; // rudd const int ch4Pin = 20; // rudd
const int ch5Pin = 21; // gear (throttle cut) const int ch5Pin = D7; // gear (throttle cut)
const int ch6Pin = 22; // aux1 (free aux channel) const int ch6Pin = D7; // aux1 (free aux channel)
const int PPM_Pin = 23; const int PPM_Pin = 23;
// OneShot125 ESC pin outputs: // OneShot125 ESC pin outputs:
const int m1Pin = D0; const int m1Pin = D0;
const int m2Pin = D1; const int m2Pin = D1;
const int m3Pin = D2; const int m3Pin = D2;
const int m4Pin = D3; const int m4Pin = D3;
const int m5Pin = D4; const int m5Pin = D7;
const int m6Pin = D5; const int m6Pin = D8;
// PWM servo or ESC outputs: // PWM servo or ESC outputs:
const int servo1Pin = 6; const int servo1Pin = 6;
const int servo2Pin = 7; const int servo2Pin = 7;
@@ -234,13 +236,13 @@ const int servo4Pin = 9;
const int servo5Pin = 10; const int servo5Pin = 10;
const int servo6Pin = 11; const int servo6Pin = 11;
const int servo7Pin = 12; const int servo7Pin = 12;
PWMServo servo1; // Create servo objects to control a servo or ESC with PWM Servo servo1; // Create servo objects to control a servo or ESC with PWM
PWMServo servo2; Servo servo2;
PWMServo servo3; Servo servo3;
PWMServo servo4; Servo servo4;
PWMServo servo5; Servo servo5;
PWMServo servo6; Servo servo6;
PWMServo servo7; Servo servo7;
//========================================================================================================================// //========================================================================================================================//
@@ -266,6 +268,20 @@ bool sbusLostFrame;
#if defined USE_DSM_RX #if defined USE_DSM_RX
DSM1024 DSM; DSM1024 DSM;
#endif #endif
#if defined USE_ESPNow
// Structure example to receive data
// Must match the sender structure
typedef struct struct_message
{
int PWMCH1;
int PWMCH2;
int PWMCH3;
int PWMCH4;
} struct_message;
// Create a struct_message called myData
struct_message myData;
#endif
// IMU: // IMU:
float AccX, AccY, AccZ; float AccX, AccY, AccZ;
@@ -339,39 +355,50 @@ void printServoCommands();
void printLoopRate(); void printLoopRate();
float invSqrt(float x); float invSqrt(float x);
void controlMixer(); void controlMixer();
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len);
void ESPNowSetup();
//========================================================================================================================// //========================================================================================================================//
// VOID SETUP // // VOID SETUP //
//========================================================================================================================// //========================================================================================================================//
void setup() { void setup()
Serial.begin(500000); // USB serial {
delay(500); Serial.begin(9600); // USB serial
Serial.println("Serial started");
delay(10000);
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
pinMode(m1Pin, OUTPUT); pinMode(m1Pin, OUTPUT);
pinMode(m2Pin, OUTPUT); pinMode(m2Pin, OUTPUT);
pinMode(m3Pin, OUTPUT); pinMode(m3Pin, OUTPUT);
pinMode(m4Pin, OUTPUT); pinMode(m4Pin, OUTPUT);
pinMode(m5Pin, OUTPUT); pinMode(m5Pin, OUTPUT);
pinMode(m6Pin, OUTPUT); pinMode(m6Pin, OUTPUT);
Serial.println("Attach servos");
servo1.attach(servo1Pin, 900, 2100); // Pin, min PWM value, max PWM value servo1.attach(servo1Pin, 900, 2100); // Pin, min PWM value, max PWM value
servo2.attach(servo2Pin, 900, 2100); servo2.attach(servo2Pin, 900, 2100);
servo3.attach(servo3Pin, 900, 2100); servo3.attach(servo3Pin, 900, 2100);
servo4.attach(servo4Pin, 900, 2100); servo4.attach(servo4Pin, 900, 2100);
servo5.attach(servo5Pin, 900, 2100); servo5.attach(servo5Pin, 900, 2100);
servo6.attach(servo6Pin, 900, 2100); servo6.attach(servo6Pin, 900, 2100);
servo7.attach(servo7Pin, 900, 2100); // servo7.attach(servo7Pin, 900, 2100);
// Set built in LED to turn on to signal startup // Set built in LED to turn on to signal startup
digitalWrite(13, HIGH); // digitalWrite(13, HIGH);
delay(5); delay(5);
// Initialize radio communication // Initialize radio communication
// radioSetup(); //commented out because I'm making my own controller #if defined USE_SBUS_RX || USE_DSM_RX || USE_PWM_RX || USE_PPM_RX
radioSetup();
#elif defined USE_ESPNow
Serial.println("ESPNow setup");
ESPNowSetup();
#else
#error No radio defined...
#endif
// Set radio channels to default (safe) values before entering main loop // Set radio channels to default (safe) values before entering main loop
channel_1_pwm = channel_1_fs; channel_1_pwm = channel_1_fs;
@@ -390,6 +417,7 @@ void setup() {
// calculate_IMU_error(); //Calibration parameters printed to serial monitor. Paste these in the user specified variables section, then comment this out forever. // calculate_IMU_error(); //Calibration parameters printed to serial monitor. Paste these in the user specified variables section, then comment this out forever.
// Arm servo channels // Arm servo channels
Serial.println("Arming servos");
servo1.write(0); // Command servo angle from 0-180 degrees (1000 to 2000 PWM) servo1.write(0); // Command servo angle from 0-180 degrees (1000 to 2000 PWM)
servo2.write(0); // Set these to 90 for servos if you do not want them to briefly max out on startup servo2.write(0); // Set these to 90 for servos if you do not want them to briefly max out on startup
servo3.write(0); // Keep these at 0 if you are using servo outputs for motors servo3.write(0); // Keep these at 0 if you are using servo outputs for motors
@@ -410,9 +438,11 @@ void setup() {
m4_command_PWM = 125; m4_command_PWM = 125;
m5_command_PWM = 125; m5_command_PWM = 125;
m6_command_PWM = 125; m6_command_PWM = 125;
Serial.println("Arming motors");
armMotors(); // Loop over commandMotors() until ESCs happily arm armMotors(); // Loop over commandMotors() until ESCs happily arm
// Indicate entering main loop with 3 quick blinks // Indicate entering main loop with 3 quick blinks
//
setupBlink(3, 160, 70); // numBlinks, upTime (ms), downTime (ms) setupBlink(3, 160, 70); // numBlinks, upTime (ms), downTime (ms)
// 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)
@@ -423,7 +453,8 @@ void setup() {
// MAIN LOOP // // MAIN LOOP //
//========================================================================================================================// //========================================================================================================================//
void loop() { void loop()
{
// Keep track of what time it is and how much time has elapsed since the last loop // Keep track of what time it is and how much time has elapsed since the last loop
prev_time = current_time; prev_time = current_time;
current_time = micros(); current_time = micros();
@@ -432,8 +463,8 @@ 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)
@@ -487,7 +518,8 @@ void loop() {
// 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
/* /*
* Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired * Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired
@@ -522,14 +554,17 @@ void controlMixer() {
s7_command_scaled = 0; s7_command_scaled = 0;
} }
void armedStatus() { void armedStatus()
{
// DESCRIPTION: Check if the throttle cut is off and the throttle input is low to prepare for flight. // DESCRIPTION: Check if the throttle cut is off and the throttle input is low to prepare for flight.
if ((channel_5_pwm < 1500) && (channel_1_pwm < 1050)) { if ((channel_5_pwm < 1500) && (channel_1_pwm < 1050))
{
armedFly = true; armedFly = true;
} }
} }
void IMUinit() { void IMUinit()
{
// DESCRIPTION: Initialize IMU // DESCRIPTION: Initialize IMU
/* /*
* Don't worry about how this works. * Don't worry about how this works.
@@ -540,10 +575,12 @@ void IMUinit() {
mpu6050.initialize(); mpu6050.initialize();
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)
{
} }
} }
@@ -556,12 +593,14 @@ void IMUinit() {
#elif defined USE_MPU9250_SPI #elif defined USE_MPU9250_SPI
int status = mpu9250.begin(); int status = mpu9250.begin();
if (status < 0) { if (status < 0)
{
Serial.println("MPU9250 initialization unsuccessful"); Serial.println("MPU9250 initialization unsuccessful");
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
@@ -577,15 +616,16 @@ void IMUinit() {
Wire.begin(); Wire.begin();
myIMU.begin(); myIMU.begin();
Wire.setClock(400000); // Increase I2C data rate to 400kHz Wire.setClock(400000); // Increase I2C data rate to 400kHz
Serial.println("IMU initialized");
myIMU.enableMagnetometer(50); myIMU.enableMagnetometer(50);
myIMU.enableGyro(50); myIMU.enableGyro(50);
myIMU.enableAccelerometer(50); myIMU.enableAccelerometer(50);
#endif #endif
} }
void getIMUdata() { void getIMUdata()
{
// DESCRIPTION: Request full dataset from IMU and LP filter gyro, accelerometer, and magnetometer data // DESCRIPTION: Request full dataset from IMU and LP filter gyro, accelerometer, and magnetometer data
/* /*
* Reads accelerometer, gyro, and magnetometer data from IMU as AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ. * Reads accelerometer, gyro, and magnetometer data from IMU as AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ.
@@ -667,7 +707,8 @@ void getIMUdata() {
MagZ_prev = MagZ; MagZ_prev = MagZ;
} }
void calculate_IMU_error() { void calculate_IMU_error()
{
// DESCRIPTION: Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface // DESCRIPTION: Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface
/* /*
* Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and * Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and
@@ -684,7 +725,8 @@ void calculate_IMU_error() {
// Read IMU values 12000 times // Read IMU values 12000 times
int c = 0; int c = 0;
while (c < 12000) { while (c < 12000)
{
#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
@@ -699,6 +741,8 @@ 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();
@@ -753,7 +797,8 @@ void calculate_IMU_error() {
Serial.println("Paste these values in user specified variables section and comment out calculate_IMU_error() in void setup."); Serial.println("Paste these values in user specified variables section and comment out calculate_IMU_error() in void setup.");
} }
void calibrateAttitude() { void calibrateAttitude()
{
// DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators // DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators
// Assuming vehicle is powered up on level surface! // Assuming vehicle is powered up on level surface!
/* /*
@@ -761,7 +806,8 @@ void calibrateAttitude() {
* to boot. * to boot.
*/ */
// Warm up IMU and madgwick filter in simulated main loop // Warm up IMU and madgwick filter in simulated main loop
for (int i = 0; i <= 10000; i++) { for (int i = 0; i <= 10000; i++)
{
prev_time = current_time; prev_time = current_time;
current_time = micros(); current_time = micros();
dt = (current_time - prev_time) / 1000000.0; dt = (current_time - prev_time) / 1000000.0;
@@ -771,7 +817,8 @@ void calibrateAttitude() {
} }
} }
void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float invSampleFreq) { void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float invSampleFreq)
{
// DESCRIPTION: Attitude estimation through sensor fusion - 9DOF // DESCRIPTION: Attitude estimation through sensor fusion - 9DOF
/* /*
* This function fuses the accelerometer gyro, and magnetometer readings AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, and MagZ for attitude estimation. * This function fuses the accelerometer gyro, and magnetometer readings AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, and MagZ for attitude estimation.
@@ -793,7 +840,8 @@ void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float
#endif #endif
// Use 6DOF algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) // Use 6DOF algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))
{
Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq); Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq);
return; return;
} }
@@ -810,7 +858,8 @@ void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
// Normalise accelerometer measurement // Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az); recipNorm = invSqrt(ax * ax + ay * ay + az * az);
@@ -891,7 +940,8 @@ void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float
yaw_IMU = -atan2(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3) * 57.29577951; // degrees yaw_IMU = -atan2(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3) * 57.29577951; // degrees
} }
void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq) { void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq)
{
// DESCRIPTION: Attitude estimation through sensor fusion - 6DOF // DESCRIPTION: Attitude estimation through sensor fusion - 6DOF
/* /*
* See description of Madgwick() for more information. This is a 6DOF implimentation for when magnetometer data is not * See description of Madgwick() for more information. This is a 6DOF implimentation for when magnetometer data is not
@@ -914,7 +964,8 @@ void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, fl
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
// Normalise accelerometer measurement // Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az); recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm; ax *= recipNorm;
@@ -973,7 +1024,8 @@ void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, fl
yaw_IMU = -atan2(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3) * 57.29577951; // degrees yaw_IMU = -atan2(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3) * 57.29577951; // degrees
} }
void getDesState() { void getDesState()
{
// DESCRIPTION: Normalizes desired control values to appropriate values // DESCRIPTION: Normalizes desired control values to appropriate values
/* /*
* Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw
@@ -1000,7 +1052,8 @@ void getDesState() {
yaw_passthru = constrain(yaw_passthru, -0.5, 0.5); yaw_passthru = constrain(yaw_passthru, -0.5, 0.5);
} }
void controlANGLE() { void controlANGLE()
{
// DESCRIPTION: Computes control commands based on state error (angle) // DESCRIPTION: Computes control commands based on state error (angle)
/* /*
* Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in
@@ -1016,7 +1069,8 @@ void controlANGLE() {
// Roll // Roll
error_roll = roll_des - roll_IMU; error_roll = roll_des - roll_IMU;
integral_roll = integral_roll_prev + error_roll * dt; integral_roll = integral_roll_prev + error_roll * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_roll = 0; integral_roll = 0;
} }
integral_roll = constrain(integral_roll, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup integral_roll = constrain(integral_roll, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup
@@ -1026,7 +1080,8 @@ void controlANGLE() {
// Pitch // Pitch
error_pitch = pitch_des - pitch_IMU; error_pitch = pitch_des - pitch_IMU;
integral_pitch = integral_pitch_prev + error_pitch * dt; integral_pitch = integral_pitch_prev + error_pitch * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_pitch = 0; integral_pitch = 0;
} }
integral_pitch = constrain(integral_pitch, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup integral_pitch = constrain(integral_pitch, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup
@@ -1036,7 +1091,8 @@ void controlANGLE() {
// Yaw, stablize on rate from GyroZ // Yaw, stablize on rate from GyroZ
error_yaw = yaw_des - GyroZ; error_yaw = yaw_des - GyroZ;
integral_yaw = integral_yaw_prev + error_yaw * dt; integral_yaw = integral_yaw_prev + error_yaw * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_yaw = 0; integral_yaw = 0;
} }
integral_yaw = constrain(integral_yaw, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup integral_yaw = constrain(integral_yaw, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup
@@ -1052,7 +1108,8 @@ void controlANGLE() {
integral_yaw_prev = integral_yaw; integral_yaw_prev = integral_yaw;
} }
void controlANGLE2() { void controlANGLE2()
{
// DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme // DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme
/* /*
* Gives better performance than controlANGLE() but requires much more tuning. Not recommended for first-time setup. * Gives better performance than controlANGLE() but requires much more tuning. Not recommended for first-time setup.
@@ -1063,7 +1120,8 @@ void controlANGLE2() {
// Roll // Roll
error_roll = roll_des - roll_IMU; error_roll = roll_des - roll_IMU;
integral_roll_ol = integral_roll_prev_ol + error_roll * dt; integral_roll_ol = integral_roll_prev_ol + error_roll * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_roll_ol = 0; integral_roll_ol = 0;
} }
integral_roll_ol = constrain(integral_roll_ol, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup integral_roll_ol = constrain(integral_roll_ol, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup
@@ -1073,7 +1131,8 @@ void controlANGLE2() {
// Pitch // Pitch
error_pitch = pitch_des - pitch_IMU; error_pitch = pitch_des - pitch_IMU;
integral_pitch_ol = integral_pitch_prev_ol + error_pitch * dt; integral_pitch_ol = integral_pitch_prev_ol + error_pitch * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_pitch_ol = 0; integral_pitch_ol = 0;
} }
integral_pitch_ol = constrain(integral_pitch_ol, -i_limit, i_limit); // saturate integrator to prevent unsafe buildup integral_pitch_ol = constrain(integral_pitch_ol, -i_limit, i_limit); // saturate integrator to prevent unsafe buildup
@@ -1093,7 +1152,8 @@ void controlANGLE2() {
// Roll // Roll
error_roll = roll_des_ol - GyroX; error_roll = roll_des_ol - GyroX;
integral_roll_il = integral_roll_prev_il + error_roll * dt; integral_roll_il = integral_roll_prev_il + error_roll * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_roll_il = 0; integral_roll_il = 0;
} }
integral_roll_il = constrain(integral_roll_il, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup integral_roll_il = constrain(integral_roll_il, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup
@@ -1103,7 +1163,8 @@ void controlANGLE2() {
// Pitch // Pitch
error_pitch = pitch_des_ol - GyroY; error_pitch = pitch_des_ol - GyroY;
integral_pitch_il = integral_pitch_prev_il + error_pitch * dt; integral_pitch_il = integral_pitch_prev_il + error_pitch * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_pitch_il = 0; integral_pitch_il = 0;
} }
integral_pitch_il = constrain(integral_pitch_il, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup integral_pitch_il = constrain(integral_pitch_il, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup
@@ -1113,7 +1174,8 @@ void controlANGLE2() {
// Yaw // Yaw
error_yaw = yaw_des - GyroZ; error_yaw = yaw_des - GyroZ;
integral_yaw = integral_yaw_prev + error_yaw * dt; integral_yaw = integral_yaw_prev + error_yaw * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_yaw = 0; integral_yaw = 0;
} }
integral_yaw = constrain(integral_yaw, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup integral_yaw = constrain(integral_yaw, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup
@@ -1137,7 +1199,8 @@ void controlANGLE2() {
integral_yaw_prev = integral_yaw; integral_yaw_prev = integral_yaw;
} }
void controlRATE() { void controlRATE()
{
// DESCRIPTION: Computes control commands based on state error (rate) // DESCRIPTION: Computes control commands based on state error (rate)
/* /*
* See explanation for controlANGLE(). Everything is the same here except the error is now the desired rate - raw gyro reading. * See explanation for controlANGLE(). Everything is the same here except the error is now the desired rate - raw gyro reading.
@@ -1145,7 +1208,8 @@ void controlRATE() {
// Roll // Roll
error_roll = roll_des - GyroX; error_roll = roll_des - GyroX;
integral_roll = integral_roll_prev + error_roll * dt; integral_roll = integral_roll_prev + error_roll * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_roll = 0; integral_roll = 0;
} }
integral_roll = constrain(integral_roll, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup integral_roll = constrain(integral_roll, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup
@@ -1155,7 +1219,8 @@ void controlRATE() {
// Pitch // Pitch
error_pitch = pitch_des - GyroY; error_pitch = pitch_des - GyroY;
integral_pitch = integral_pitch_prev + error_pitch * dt; integral_pitch = integral_pitch_prev + error_pitch * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_pitch = 0; integral_pitch = 0;
} }
integral_pitch = constrain(integral_pitch, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup integral_pitch = constrain(integral_pitch, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup
@@ -1165,7 +1230,8 @@ void controlRATE() {
// Yaw, stablize on rate from GyroZ // Yaw, stablize on rate from GyroZ
error_yaw = yaw_des - GyroZ; error_yaw = yaw_des - GyroZ;
integral_yaw = integral_yaw_prev + error_yaw * dt; integral_yaw = integral_yaw_prev + error_yaw * dt;
if (channel_1_pwm < 1060) { // Don't let integrator build if throttle is too low if (channel_1_pwm < 1060)
{ // Don't let integrator build if throttle is too low
integral_yaw = 0; integral_yaw = 0;
} }
integral_yaw = constrain(integral_yaw, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup integral_yaw = constrain(integral_yaw, -i_limit, i_limit); // Saturate integrator to prevent unsafe buildup
@@ -1185,7 +1251,8 @@ void controlRATE() {
integral_yaw_prev = integral_yaw; integral_yaw_prev = integral_yaw;
} }
void scaleCommands() { void scaleCommands()
{
// DESCRIPTION: Scale normalized actuator commands to values for ESC/Servo protocol // DESCRIPTION: Scale normalized actuator commands to values for ESC/Servo protocol
/* /*
* mX_command_scaled variables from the mixer function are scaled to 125-250us for OneShot125 protocol. sX_command_scaled variables from * mX_command_scaled variables from the mixer function are scaled to 125-250us for OneShot125 protocol. sX_command_scaled variables from
@@ -1226,7 +1293,8 @@ void scaleCommands() {
s7_command_PWM = constrain(s7_command_PWM, 0, 180); s7_command_PWM = constrain(s7_command_PWM, 0, 180);
} }
void getCommands() { void getCommands()
{
// DESCRIPTION: Get raw PWM values for every channel from the radio // DESCRIPTION: Get raw PWM values for every channel from the radio
/* /*
* Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of
@@ -1244,7 +1312,8 @@ 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;
@@ -1257,9 +1326,12 @@ 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);
@@ -1270,6 +1342,15 @@ void getCommands() {
channel_5_pwm = values[4]; channel_5_pwm = values[4];
channel_6_pwm = values[5]; channel_6_pwm = values[5];
} }
#elif defined USE_ESPNow
channel_1_pwm = myData.PWMCH1;
channel_2_pwm = myData.PWMCH2;
channel_3_pwm = myData.PWMCH3;
channel_4_pwm = myData.PWMCH4;
// channel_5_pwm = getRadioPWM(5); //commented out because ESPnow only sends 4 channels
// channel_6_pwm = getRadioPWM(6);
#endif #endif
// Low-pass the critical commands and update previous values // Low-pass the critical commands and update previous values
@@ -1284,7 +1365,8 @@ void getCommands() {
channel_4_pwm_prev = channel_4_pwm; channel_4_pwm_prev = channel_4_pwm;
} }
void failSafe() { void failSafe()
{
// DESCRIPTION: If radio gives garbage values, set all commands to default values // DESCRIPTION: If radio gives garbage values, set all commands to default values
/* /*
* Radio connection failsafe used to check if the getCommands() function is returning acceptable pwm values. If any of * Radio connection failsafe used to check if the getCommands() function is returning acceptable pwm values. If any of
@@ -1317,7 +1399,8 @@ void failSafe() {
check6 = 1; 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)
{
channel_1_pwm = channel_1_fs; channel_1_pwm = channel_1_fs;
channel_2_pwm = channel_2_fs; channel_2_pwm = channel_2_fs;
channel_3_pwm = channel_3_fs; channel_3_pwm = channel_3_fs;
@@ -1327,7 +1410,8 @@ void failSafe() {
} }
} }
void commandMotors() { void commandMotors()
{
// DESCRIPTION: Send pulses to motor pins, oneshot125 protocol // DESCRIPTION: Send pulses to motor pins, oneshot125 protocol
/* /*
* My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being
@@ -1352,34 +1436,41 @@ void commandMotors() {
pulseStart = micros(); pulseStart = micros();
// Write each motor pin low as correct pulse length is reached // Write each motor pin low as correct pulse length is reached
while (wentLow < 6) { // Keep going until final (6th) pulse is finished, then done while (wentLow < 6)
{ // Keep going until final (6th) pulse is finished, then done
timer = micros(); timer = micros();
if ((m1_command_PWM <= timer - pulseStart) && (flagM1 == 0)) { if ((m1_command_PWM <= timer - pulseStart) && (flagM1 == 0))
{
digitalWrite(m1Pin, LOW); digitalWrite(m1Pin, LOW);
wentLow = wentLow + 1; wentLow = wentLow + 1;
flagM1 = 1; flagM1 = 1;
} }
if ((m2_command_PWM <= timer - pulseStart) && (flagM2 == 0)) { if ((m2_command_PWM <= timer - pulseStart) && (flagM2 == 0))
{
digitalWrite(m2Pin, LOW); digitalWrite(m2Pin, LOW);
wentLow = wentLow + 1; wentLow = wentLow + 1;
flagM2 = 1; flagM2 = 1;
} }
if ((m3_command_PWM <= timer - pulseStart) && (flagM3 == 0)) { if ((m3_command_PWM <= timer - pulseStart) && (flagM3 == 0))
{
digitalWrite(m3Pin, LOW); digitalWrite(m3Pin, LOW);
wentLow = wentLow + 1; wentLow = wentLow + 1;
flagM3 = 1; flagM3 = 1;
} }
if ((m4_command_PWM <= timer - pulseStart) && (flagM4 == 0)) { if ((m4_command_PWM <= timer - pulseStart) && (flagM4 == 0))
{
digitalWrite(m4Pin, LOW); digitalWrite(m4Pin, LOW);
wentLow = wentLow + 1; wentLow = wentLow + 1;
flagM4 = 1; flagM4 = 1;
} }
if ((m5_command_PWM <= timer - pulseStart) && (flagM5 == 0)) { if ((m5_command_PWM <= timer - pulseStart) && (flagM5 == 0))
{
digitalWrite(m5Pin, LOW); digitalWrite(m5Pin, LOW);
wentLow = wentLow + 1; wentLow = wentLow + 1;
flagM5 = 1; flagM5 = 1;
} }
if ((m6_command_PWM <= timer - pulseStart) && (flagM6 == 0)) { if ((m6_command_PWM <= timer - pulseStart) && (flagM6 == 0))
{
digitalWrite(m6Pin, LOW); digitalWrite(m6Pin, LOW);
wentLow = wentLow + 1; wentLow = wentLow + 1;
flagM6 = 1; flagM6 = 1;
@@ -1387,32 +1478,36 @@ void commandMotors() {
} }
} }
void armMotors() { void armMotors()
{
// DESCRIPTION: Sends many command pulses to the motors, to be used to arm motors in the void setup() // DESCRIPTION: Sends many command pulses to the motors, to be used to arm motors in the void setup()
/* /*
* Loops over the commandMotors() function 50 times with a delay in between, simulating how the commandMotors() * Loops over the commandMotors() function 50 times with a delay in between, simulating how the commandMotors()
* function is used in the main loop. Ensures motors arm within the void setup() where there are some delays * function is used in the main loop. Ensures motors arm within the void setup() where there are some delays
* for other processes that sometimes prevent motors from arming. * for other processes that sometimes prevent motors from arming.
*/ */
for (int i = 0; i <= 50; i++) { for (int i = 0; i <= 50; i++)
{
commandMotors(); commandMotors();
delay(2); delay(2);
} }
} }
void calibrateESCs() { void calibrateESCs()
{
// DESCRIPTION: Used in void setup() to allow standard ESC calibration procedure with the radio to take place. // DESCRIPTION: Used in void setup() to allow standard ESC calibration procedure with the radio to take place.
/* /*
* Simulates the void loop(), but only for the purpose of providing throttle pass through to the motors, so that you can * Simulates the void loop(), but only for the purpose of providing throttle pass through to the motors, so that you can
* power up with throttle at full, let ESCs begin arming sequence, and lower throttle to zero. This function should only be * power up with throttle at full, let ESCs begin arming sequence, and lower throttle to zero. This function should only be
* uncommented when performing an ESC calibration. * uncommented when performing an ESC calibration.
*/ */
while (true) { while (true)
{
prev_time = current_time; prev_time = current_time;
current_time = micros(); current_time = micros();
dt = (current_time - prev_time) / 1000000.0; dt = (current_time - prev_time) / 1000000.0;
digitalWrite(13, HIGH); // LED on to indicate we are not in main loop // digitalWrite(13, HIGH); // LED on to indicate we are not in main loop
getCommands(); // Pulls current available radio commands getCommands(); // Pulls current available radio commands
failSafe(); // Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup failSafe(); // Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup
@@ -1453,7 +1548,8 @@ void calibrateESCs() {
} }
} }
float floatFaderLinear(float param, float param_min, float param_max, float fadeTime, int state, int loopFreq) { float floatFaderLinear(float param, float param_min, float param_max, float fadeTime, int state, int loopFreq)
{
// DESCRIPTION: Linearly fades a float type variable between min and max bounds based on desired high or low state and time // DESCRIPTION: Linearly fades a float type variable between min and max bounds based on desired high or low state and time
/* /*
* Takes in a float variable, desired minimum and maximum bounds, fade time, high or low desired state, and the loop frequency * Takes in a float variable, desired minimum and maximum bounds, fade time, high or low desired state, and the loop frequency
@@ -1466,9 +1562,12 @@ float floatFaderLinear(float param, float param_min, float param_max, float fade
*/ */
float diffParam = (param_max - param_min) / (fadeTime * loopFreq); // Difference to add or subtract from param for each loop iteration for desired fadeTime float diffParam = (param_max - param_min) / (fadeTime * loopFreq); // Difference to add or subtract from param for each loop iteration for desired fadeTime
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;
} }
@@ -1477,7 +1576,8 @@ float floatFaderLinear(float param, float param_min, float param_max, float fade
return param; return param;
} }
float floatFaderLinear2(float param, float param_des, float param_lower, float param_upper, float fadeTime_up, float fadeTime_down, int loopFreq) { float floatFaderLinear2(float param, float param_des, float param_lower, float param_upper, float fadeTime_up, float fadeTime_down, int loopFreq)
{
// DESCRIPTION: Linearly fades a float type variable from its current value to the desired value, up or down // DESCRIPTION: Linearly fades a float type variable from its current value to the desired value, up or down
/* /*
* Takes in a float variable to be modified, desired new position, upper value, lower value, fade time, and the loop frequency * Takes in a float variable to be modified, desired new position, upper value, lower value, fade time, and the loop frequency
@@ -1487,10 +1587,13 @@ float floatFaderLinear2(float param, float param_des, float param_lower, float p
* statements in order to fade controller gains, for example between the two dynamic configurations. * statements in order to fade controller gains, for example between the two dynamic configurations.
* *
*/ */
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;
} }
@@ -1500,7 +1603,8 @@ float floatFaderLinear2(float param, float param_des, float param_lower, float p
return param; return param;
} }
void switchRollYaw(int reverseRoll, int reverseYaw) { void switchRollYaw(int reverseRoll, int reverseYaw)
{
// DESCRIPTION: Switches roll_des and yaw_des variables for tailsitter-type configurations // DESCRIPTION: Switches roll_des and yaw_des variables for tailsitter-type configurations
/* /*
* Takes in two integers (either 1 or -1) corresponding to the desired reversing of the roll axis and yaw axis, respectively. * Takes in two integers (either 1 or -1) corresponding to the desired reversing of the roll axis and yaw axis, respectively.
@@ -1516,7 +1620,8 @@ void switchRollYaw(int reverseRoll, int reverseYaw) {
roll_des = reverseRoll * switch_holder; roll_des = reverseRoll * switch_holder;
} }
void throttleCut() { void throttleCut()
{
// DESCRIPTION: Directly set actuator outputs to minimum value if triggered // DESCRIPTION: Directly set actuator outputs to minimum value if triggered
/* /*
Monitors the state of radio command channel_5_pwm and directly sets the mx_command_PWM values to minimum (120 is Monitors the state of radio command channel_5_pwm and directly sets the mx_command_PWM values to minimum (120 is
@@ -1527,7 +1632,8 @@ void throttleCut() {
channel_5_pwm is LOW then throttle cut is OFF and throttle value can change. (ThrottleCut is DEACTIVATED) channel_5_pwm is LOW then throttle cut is OFF and throttle value can change. (ThrottleCut is DEACTIVATED)
channel_5_pwm is HIGH then throttle cut is ON and throttle value = 120 only. (ThrottleCut is ACTIVATED), (drone is DISARMED) channel_5_pwm is HIGH then throttle cut is ON and throttle value = 120 only. (ThrottleCut is ACTIVATED), (drone is DISARMED)
*/ */
if ((channel_5_pwm > 1500) || (armedFly == false)) { if ((channel_5_pwm > 1500) || (armedFly == false))
{
armedFly = false; armedFly = false;
m1_command_PWM = 120; m1_command_PWM = 120;
m2_command_PWM = 120; m2_command_PWM = 120;
@@ -1547,7 +1653,8 @@ void throttleCut() {
} }
} }
void calibrateMagnetometer() { void calibrateMagnetometer()
{
#if defined USE_MPU9250_SPI #if defined USE_MPU9250_SPI
float success; float success;
Serial.println("Beginning magnetometer calibration in"); Serial.println("Beginning magnetometer calibration in");
@@ -1560,7 +1667,8 @@ void calibrateMagnetometer() {
Serial.println("Rotate the IMU about all axes until complete."); Serial.println("Rotate the IMU about all axes until complete.");
Serial.println(" "); Serial.println(" ");
success = mpu9250.calibrateMag(); success = mpu9250.calibrateMag();
if (success) { if (success)
{
Serial.println("Calibration Successful!"); Serial.println("Calibration Successful!");
Serial.println("Please comment out the calibrateMagnetometer() function and copy these values into the code:"); Serial.println("Please comment out the calibrateMagnetometer() function and copy these values into the code:");
Serial.print("float MagErrorX = "); Serial.print("float MagErrorX = ");
@@ -1583,7 +1691,9 @@ 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.");
} }
@@ -1595,7 +1705,8 @@ void calibrateMagnetometer() {
; // Halt code so it won't enter main loop until this function commented out ; // Halt code so it won't enter main loop until this function commented out
} }
void loopRate(int freq) { void loopRate(int freq)
{
// DESCRIPTION: Regulate main loop rate to specified frequency in Hz // DESCRIPTION: Regulate main loop rate to specified frequency in Hz
/* /*
* It's good to operate at a constant loop rate for filters to remain stable and whatnot. Interrupt routines running in the * It's good to operate at a constant loop rate for filters to remain stable and whatnot. Interrupt routines running in the
@@ -1608,42 +1719,52 @@ void loopRate(int freq) {
unsigned long checker = micros(); unsigned long checker = micros();
// Sit in loop until appropriate time has passed // Sit in loop until appropriate time has passed
while (invFreq > (checker - current_time)) { while (invFreq > (checker - current_time))
{
checker = micros(); checker = micros();
} }
} }
void loopBlink() { void loopBlink()
{
// DESCRIPTION: Blink LED on board to indicate main loop is running // DESCRIPTION: Blink LED on board to indicate main loop is running
/* /*
* It looks cool. * It looks cool.
*/ */
if (current_time - blink_counter > blink_delay) { if (current_time - blink_counter > blink_delay)
{
blink_counter = micros(); blink_counter = micros();
digitalWrite(13, blinkAlternate); // Pin 13 is built in LED digitalWrite(13, blinkAlternate); // Pin 13 is built in LED
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;
} }
} }
} }
void setupBlink(int numBlinks, int upTime, int downTime) { void setupBlink(int numBlinks, int upTime, int downTime)
{
// DESCRIPTION: Simple function to make LED on board blink as desired // DESCRIPTION: Simple function to make LED on board blink as desired
for (int j = 1; j <= numBlinks; j++) { for (int j = 1; j <= numBlinks; j++)
digitalWrite(13, LOW); {
// digitalWrite(13, LOW);
delay(downTime); delay(downTime);
digitalWrite(13, HIGH); // digitalWrite(13, HIGH);
delay(upTime); delay(upTime);
} }
} }
void printRadioData() { void printRadioData()
if (current_time - print_counter > 10000) { {
if (current_time - print_counter > 10000)
{
print_counter = micros(); print_counter = micros();
Serial.print(F(" CH1:")); Serial.print(F(" CH1:"));
Serial.print(channel_1_pwm); Serial.print(channel_1_pwm);
@@ -1660,8 +1781,10 @@ void printRadioData() {
} }
} }
void printDesiredState() { void printDesiredState()
if (current_time - print_counter > 10000) { {
if (current_time - print_counter > 10000)
{
print_counter = micros(); print_counter = micros();
Serial.print(F("thro_des:")); Serial.print(F("thro_des:"));
Serial.print(thro_des); Serial.print(thro_des);
@@ -1674,8 +1797,10 @@ void printDesiredState() {
} }
} }
void printGyroData() { void printGyroData()
if (current_time - print_counter > 10000) { {
if (current_time - print_counter > 10000)
{
print_counter = micros(); print_counter = micros();
Serial.print(F("GyroX:")); Serial.print(F("GyroX:"));
Serial.print(GyroX); Serial.print(GyroX);
@@ -1686,8 +1811,10 @@ void printGyroData() {
} }
} }
void printAccelData() { void printAccelData()
if (current_time - print_counter > 10000) { {
if (current_time - print_counter > 10000)
{
print_counter = micros(); print_counter = micros();
Serial.print(F("AccX:")); Serial.print(F("AccX:"));
Serial.print(AccX); Serial.print(AccX);
@@ -1698,8 +1825,10 @@ void printAccelData() {
} }
} }
void printMagData() { void printMagData()
if (current_time - print_counter > 10000) { {
if (current_time - print_counter > 10000)
{
print_counter = micros(); print_counter = micros();
Serial.print(F("MagX:")); Serial.print(F("MagX:"));
Serial.print(MagX); Serial.print(MagX);
@@ -1710,8 +1839,10 @@ void printMagData() {
} }
} }
void printRollPitchYaw() { void printRollPitchYaw()
if (current_time - print_counter > 10000) { {
if (current_time - print_counter > 10000)
{
print_counter = micros(); print_counter = micros();
Serial.print(F("roll:")); Serial.print(F("roll:"));
Serial.print(roll_IMU); Serial.print(roll_IMU);
@@ -1722,8 +1853,10 @@ void printRollPitchYaw() {
} }
} }
void printPIDoutput() { void printPIDoutput()
if (current_time - print_counter > 10000) { {
if (current_time - print_counter > 10000)
{
print_counter = micros(); print_counter = micros();
Serial.print(F("roll_PID:")); Serial.print(F("roll_PID:"));
Serial.print(roll_PID); Serial.print(roll_PID);
@@ -1734,8 +1867,10 @@ void printPIDoutput() {
} }
} }
void printMotorCommands() { void printMotorCommands()
if (current_time - print_counter > 10000) { {
if (current_time - print_counter > 10000)
{
print_counter = micros(); print_counter = micros();
Serial.print(F("m1_command:")); Serial.print(F("m1_command:"));
Serial.print(m1_command_PWM); Serial.print(m1_command_PWM);
@@ -1752,8 +1887,10 @@ void printMotorCommands() {
} }
} }
void printServoCommands() { void printServoCommands()
if (current_time - print_counter > 10000) { {
if (current_time - print_counter > 10000)
{
print_counter = micros(); print_counter = micros();
Serial.print(F("s1_command:")); Serial.print(F("s1_command:"));
Serial.print(s1_command_PWM); Serial.print(s1_command_PWM);
@@ -1772,8 +1909,10 @@ void printServoCommands() {
} }
} }
void printLoopRate() { void printLoopRate()
if (current_time - print_counter > 10000) { {
if (current_time - print_counter > 10000)
{
print_counter = micros(); print_counter = micros();
Serial.print(F("dt:")); Serial.print(F("dt:"));
Serial.println(dt * 1000000.0); Serial.println(dt * 1000000.0);
@@ -1784,7 +1923,8 @@ void printLoopRate() {
// HELPER FUNCTIONS // HELPER FUNCTIONS
float invSqrt(float x) { float invSqrt(float x)
{
// Fast inverse sqrt for madgwick filter // Fast inverse sqrt for madgwick filter
/* /*
float halfx = 0.5f * x; float halfx = 0.5f * x;
@@ -1805,3 +1945,21 @@ float invSqrt(float x) {
*/ */
return 1.0 / sqrtf(x); // Teensy is fast enough to just take the compute penalty lol suck it arduino nano return 1.0 / sqrtf(x); // Teensy is fast enough to just take the compute penalty lol suck it arduino nano
} }
void ESPNowSetup()
{
WiFi.mode(WIFI_STA);
// Init ESP-NOW
if (esp_now_init() != ESP_OK)
{
Serial.println("Error initializing ESP-NOW");
return;
}
esp_now_register_recv_cb(esp_now_recv_cb_t(OnDataRecv));
}
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len)
{
memcpy(&myData, incomingData, sizeof(myData));
}