docs - it finally compiles now except the IMU is dead
BIN
docs/Assignments/week_4_programming/IMG_8200.jpg
Normal file
After Width: | Height: | Size: 176 KiB |
BIN
docs/Assignments/week_4_programming/IMG_8212.jpg
Normal file
After Width: | Height: | Size: 361 KiB |
BIN
docs/Assignments/week_4_programming/IMG_8213.jpg
Normal file
After Width: | Height: | Size: 389 KiB |
BIN
docs/Assignments/week_4_programming/image-26.jpg
Normal file
After Width: | Height: | Size: 54 KiB |
BIN
docs/Assignments/week_4_programming/image-27.jpg
Normal file
After Width: | Height: | Size: 14 KiB |
BIN
docs/Assignments/week_4_programming/image-28.jpg
Normal file
After Width: | Height: | Size: 16 KiB |
BIN
docs/Assignments/week_4_programming/image-29.jpg
Normal file
After Width: | Height: | Size: 64 KiB |
BIN
docs/Assignments/week_4_programming/image-30.jpg
Normal file
After Width: | Height: | Size: 215 KiB |
BIN
docs/Assignments/week_4_programming/image-31.jpg
Normal file
After Width: | Height: | Size: 102 KiB |
BIN
docs/Assignments/week_4_programming/image-32.jpg
Normal file
After Width: | Height: | Size: 122 KiB |
BIN
docs/Assignments/week_4_programming/image-33.jpg
Normal file
After Width: | Height: | Size: 212 KiB |
BIN
docs/Assignments/week_4_programming/image-34.jpg
Normal file
After Width: | Height: | Size: 212 KiB |
BIN
docs/Assignments/week_4_programming/image-35.jpg
Normal file
After Width: | Height: | Size: 29 KiB |
BIN
docs/Assignments/week_4_programming/image-36.jpg
Normal file
After Width: | Height: | Size: 31 KiB |
BIN
docs/Assignments/week_4_programming/image-37.jpg
Normal file
After Width: | Height: | Size: 60 KiB |
BIN
docs/Assignments/week_4_programming/image-38.jpg
Normal file
After Width: | Height: | Size: 39 KiB |
@@ -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.
|
||||||
|
|
||||||

|

|
||||||
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.
|
||||||
|
|
||||||

|

|
||||||
|
|
||||||
#### 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.
|
||||||
|
|
||||||

|

|
||||||
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.
|
||||||
|
|
||||||

|

|
||||||
|
|
||||||
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.
|
||||||
|
|
||||||

|

|
||||||
|
|
||||||
After that I added the global variables for the ESPNow protocol.
|
After that I added the global variables for the ESPNow protocol.
|
||||||
|
|
||||||

|

|
||||||
|
|
||||||
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.
|
||||||

|

|
||||||
|
|
||||||
|
|
||||||
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.
|
||||||
|
|
||||||

|

|
||||||
|
|
||||||
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.
|
||||||
|
|
||||||

|

|
||||||
|
|
||||||
When I comppile it now it should work.
|
When I compile it now it should work. Except that it doesn't work.
|
||||||
|
|
||||||
|

|
||||||
|
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:
|
||||||
|

|
||||||
|
|
||||||
|
To:
|
||||||
|

|
||||||
|
|
||||||
|
Result:
|
||||||
|

|
||||||
|
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.
|
||||||
|
|
||||||
|

|
||||||
|
|
||||||
|
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:
|
||||||
|

|
||||||
|
|
||||||
|
Instead of like this.
|
||||||
|

|
||||||
|
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)
|
||||||
|
@@ -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
|
@@ -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));
|
||||||
|
}
|