diff --git a/docs/Assignments/week_4_programming/IMG_8200.jpg b/docs/Assignments/week_4_programming/IMG_8200.jpg new file mode 100644 index 0000000..7cb73d1 Binary files /dev/null and b/docs/Assignments/week_4_programming/IMG_8200.jpg differ diff --git a/docs/Assignments/week_4_programming/IMG_8212.jpg b/docs/Assignments/week_4_programming/IMG_8212.jpg new file mode 100644 index 0000000..68c4f32 Binary files /dev/null and b/docs/Assignments/week_4_programming/IMG_8212.jpg differ diff --git a/docs/Assignments/week_4_programming/IMG_8213.jpg b/docs/Assignments/week_4_programming/IMG_8213.jpg new file mode 100644 index 0000000..448f036 Binary files /dev/null and b/docs/Assignments/week_4_programming/IMG_8213.jpg differ diff --git a/docs/Assignments/week_4_programming/image-26.jpg b/docs/Assignments/week_4_programming/image-26.jpg new file mode 100644 index 0000000..8467bad Binary files /dev/null and b/docs/Assignments/week_4_programming/image-26.jpg differ diff --git a/docs/Assignments/week_4_programming/image-27.jpg b/docs/Assignments/week_4_programming/image-27.jpg new file mode 100644 index 0000000..5bc717a Binary files /dev/null and b/docs/Assignments/week_4_programming/image-27.jpg differ diff --git a/docs/Assignments/week_4_programming/image-28.jpg b/docs/Assignments/week_4_programming/image-28.jpg new file mode 100644 index 0000000..5a26872 Binary files /dev/null and b/docs/Assignments/week_4_programming/image-28.jpg differ diff --git a/docs/Assignments/week_4_programming/image-29.jpg b/docs/Assignments/week_4_programming/image-29.jpg new file mode 100644 index 0000000..bf71356 Binary files /dev/null and b/docs/Assignments/week_4_programming/image-29.jpg differ diff --git a/docs/Assignments/week_4_programming/image-30.jpg b/docs/Assignments/week_4_programming/image-30.jpg new file mode 100644 index 0000000..41a2899 Binary files /dev/null and b/docs/Assignments/week_4_programming/image-30.jpg differ diff --git a/docs/Assignments/week_4_programming/image-31.jpg b/docs/Assignments/week_4_programming/image-31.jpg new file mode 100644 index 0000000..96b1602 Binary files /dev/null and b/docs/Assignments/week_4_programming/image-31.jpg differ diff --git a/docs/Assignments/week_4_programming/image-32.jpg b/docs/Assignments/week_4_programming/image-32.jpg new file mode 100644 index 0000000..827bc31 Binary files /dev/null and b/docs/Assignments/week_4_programming/image-32.jpg differ diff --git a/docs/Assignments/week_4_programming/image-33.jpg b/docs/Assignments/week_4_programming/image-33.jpg new file mode 100644 index 0000000..ec9f9f7 Binary files /dev/null and b/docs/Assignments/week_4_programming/image-33.jpg differ diff --git a/docs/Assignments/week_4_programming/image-34.jpg b/docs/Assignments/week_4_programming/image-34.jpg new file mode 100644 index 0000000..a719fb2 Binary files /dev/null and b/docs/Assignments/week_4_programming/image-34.jpg differ diff --git a/docs/Assignments/week_4_programming/image-35.jpg b/docs/Assignments/week_4_programming/image-35.jpg new file mode 100644 index 0000000..2440e71 Binary files /dev/null and b/docs/Assignments/week_4_programming/image-35.jpg differ diff --git a/docs/Assignments/week_4_programming/image-36.jpg b/docs/Assignments/week_4_programming/image-36.jpg new file mode 100644 index 0000000..64bac19 Binary files /dev/null and b/docs/Assignments/week_4_programming/image-36.jpg differ diff --git a/docs/Assignments/week_4_programming/image-37.jpg b/docs/Assignments/week_4_programming/image-37.jpg new file mode 100644 index 0000000..22af9fc Binary files /dev/null and b/docs/Assignments/week_4_programming/image-37.jpg differ diff --git a/docs/Assignments/week_4_programming/image-38.jpg b/docs/Assignments/week_4_programming/image-38.jpg new file mode 100644 index 0000000..200fbb3 Binary files /dev/null and b/docs/Assignments/week_4_programming/image-38.jpg differ diff --git a/docs/Assignments/week_4_programming/programming.md b/docs/Assignments/week_4_programming/programming.md index 421ca5f..9ebc956 100644 --- a/docs/Assignments/week_4_programming/programming.md +++ b/docs/Assignments/week_4_programming/programming.md @@ -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. -![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. 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. -![alt text](image-27.png) +![alt text](image-27.jpg) #### 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. -![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. -![alt text](image-28.png) +![alt text](image-28.jpg) 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. -![alt text](image-29.png) +![alt text](image-29.jpg) 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. - ![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. -![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. -![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 ### 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) * [PlatformIO espC6](https://wiki.seeedstudio.com/xiao_esp32c6_with_platform_io) * [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 * [PotSlider](https://nl.aliexpress.com/item/1005006733220962.html) diff --git a/src/drone/platformio.ini b/src/drone/platformio.ini index 0a02393..10c3b2e 100644 --- a/src/drone/platformio.ini +++ b/src/drone/platformio.ini @@ -11,10 +11,12 @@ [env:seeed_xiao_esp32c6] platform = https://github.com/mnowak32/platform-espressif32.git#boards/seeed_xiao_esp32c6 platform_packages = - framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.2 - framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.2/esp32-arduino-libs-3.0.2.zip + framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#3.0.2 + framework-arduinoespressif32-libs @ https://github.com/espressif/arduino-esp32/releases/download/3.0.2/esp32-arduino-libs-3.0.2.zip framework = arduino board = seeed_xiao_esp32c6 lib_deps = paulstoffregen/PWMServo@^2.1 sparkfun/SparkFun BNO080 Cortex Based IMU@^1.1.12 + madhephaestus/ESP32Servo@^3.0.6 +build_flags = -DCORE_DEBUG_LEVEL=5 \ No newline at end of file diff --git a/src/drone/src/main.cpp b/src/drone/src/main.cpp index b89d5bf..f15016f 100644 --- a/src/drone/src/main.cpp +++ b/src/drone/src/main.cpp @@ -1,6 +1,8 @@ #include #include -#include +#include +#include +#include // Arduino/Teensy Flight Controller - dRehmFlight // Author: Nicholas Rehm // Project Start: 1/6/2020 @@ -32,19 +34,20 @@ Everyone that sends me pictures and videos of your flying creations! -Nick //========================================================================================================================// #define PI 3.141592653589793238462643383279502884197 // Uncomment only one receiver type -#define USE_PWM_RX +// #define USE_PWM_RX // #define USE_PPM_RX // #define USE_SBUS_RX // #define USE_DSM_RX -static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to match the number of transmitter channels you have +#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 // Uncomment only one IMU // #define USE_MPU6050_I2C // Default // #define USE_MPU9250_SPI -#define USE_BNO085_I2C //Custom +#define USE_BNO085_I2C // Custom // Uncomment only one full scale gyro range (deg/sec) -#define GYRO_250DPS // Default +#define GYRO_250DPS // Default // #define GYRO_500DPS // #define GYRO_1000DPS // #define GYRO_2000DPS @@ -59,12 +62,11 @@ static const uint8_t num_DSM_channels = 6; // If using DSM RX, change this to m // REQUIRED LIBRARIES (included with download in main sketch folder) -#include //I2c communication -#include //SPI communication -#include //Commanding any extra actuators, installed with teensyduino installer +#include //I2c communication +#include //SPI communication #if defined USE_SBUS_RX -#include "src/SBUS/SBUS.h" //sBus interface +#include "src/SBUS/SBUS.h" //sBus interface #endif #if defined USE_DSM_RX @@ -150,18 +152,18 @@ BNO080 myIMU; //========================================================================================================================// // Radio failsafe values for every channel in the event that bad reciever data is detected. Recommended defaults: -unsigned long channel_1_fs = 1000; // thro -unsigned long channel_2_fs = 1500; // ail -unsigned long channel_3_fs = 1500; // elev -unsigned long channel_4_fs = 1500; // rudd -unsigned long channel_5_fs = 2000; // gear, greater than 1500 = throttle cut -unsigned long channel_6_fs = 2000; // aux1 +unsigned long channel_1_fs = 1000; // thro +unsigned long channel_2_fs = 1500; // ail +unsigned long channel_3_fs = 1500; // elev +unsigned long channel_4_fs = 1500; // rudd +unsigned long channel_5_fs = 2000; // gear, greater than 1500 = throttle cut +unsigned long channel_6_fs = 2000; // aux1 // Filter parameters - Defaults tuned for 2kHz loop rate; Do not touch unless you know what you are doing: -float B_madgwick = 0.04; // Madgwick filter parameter -float B_accel = 0.14; // Accelerometer LP filter paramter, (MPU6050 default: 0.14. MPU9250 default: 0.2) -float B_gyro = 0.1; // Gyro LP filter paramter, (MPU6050 default: 0.1. MPU9250 default: 0.17) -float B_mag = 1.0; // Magnetometer LP filter parameter +float B_madgwick = 0.04; // Madgwick filter parameter +float B_accel = 0.14; // Accelerometer LP filter paramter, (MPU6050 default: 0.14. MPU9250 default: 0.2) +float B_gyro = 0.1; // Gyro LP filter paramter, (MPU6050 default: 0.1. MPU9250 default: 0.17) +float B_mag = 1.0; // Magnetometer LP filter parameter // Magnetometer calibration parameters - if using MPU9250, uncomment calibrateMagnetometer() in void setup() to get these values, else just ignore these float MagErrorX = 0.0; @@ -180,30 +182,30 @@ float GyroErrorY = 0.0; float GyroErrorZ = 0.0; // Controller parameters (take note of defaults before modifying!): -float i_limit = 25.0; // Integrator saturation level, mostly for safety (default 25.0) -float maxRoll = 30.0; // Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode -float maxPitch = 30.0; // Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode -float maxYaw = 160.0; // Max yaw rate in deg/sec +float i_limit = 25.0; // Integrator saturation level, mostly for safety (default 25.0) +float maxRoll = 30.0; // Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode +float maxPitch = 30.0; // Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode +float maxYaw = 160.0; // Max yaw rate in deg/sec -float Kp_roll_angle = 0.2; // Roll P-gain - angle mode -float Ki_roll_angle = 0.3; // Roll I-gain - angle mode -float Kd_roll_angle = 0.05; // Roll D-gain - angle mode (has no effect on controlANGLE2) -float B_loop_roll = 0.9; // Roll damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) -float Kp_pitch_angle = 0.2; // Pitch P-gain - angle mode -float Ki_pitch_angle = 0.3; // Pitch I-gain - angle mode -float Kd_pitch_angle = 0.05; // Pitch D-gain - angle mode (has no effect on controlANGLE2) -float B_loop_pitch = 0.9; // Pitch damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) +float Kp_roll_angle = 0.2; // Roll P-gain - angle mode +float Ki_roll_angle = 0.3; // Roll I-gain - angle mode +float Kd_roll_angle = 0.05; // Roll D-gain - angle mode (has no effect on controlANGLE2) +float B_loop_roll = 0.9; // Roll damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) +float Kp_pitch_angle = 0.2; // Pitch P-gain - angle mode +float Ki_pitch_angle = 0.3; // Pitch I-gain - angle mode +float Kd_pitch_angle = 0.05; // Pitch D-gain - angle mode (has no effect on controlANGLE2) +float B_loop_pitch = 0.9; // Pitch damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) -float Kp_roll_rate = 0.15; // Roll P-gain - rate mode -float Ki_roll_rate = 0.2; // Roll I-gain - rate mode -float Kd_roll_rate = 0.0002; // Roll D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) -float Kp_pitch_rate = 0.15; // Pitch P-gain - rate mode -float Ki_pitch_rate = 0.2; // Pitch I-gain - rate mode -float Kd_pitch_rate = 0.0002; // Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) +float Kp_roll_rate = 0.15; // Roll P-gain - rate mode +float Ki_roll_rate = 0.2; // Roll I-gain - rate mode +float Kd_roll_rate = 0.0002; // Roll D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) +float Kp_pitch_rate = 0.15; // Pitch P-gain - rate mode +float Ki_pitch_rate = 0.2; // Pitch I-gain - rate mode +float Kd_pitch_rate = 0.0002; // Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) -float Kp_yaw = 0.3; // Yaw P-gain -float Ki_yaw = 0.05; // Yaw I-gain -float Kd_yaw = 0.00015; // Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) +float Kp_yaw = 0.3; // Yaw P-gain +float Ki_yaw = 0.05; // Yaw I-gain +float Kd_yaw = 0.00015; // Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) //========================================================================================================================// // DECLARE PINS // @@ -212,20 +214,20 @@ float Kd_yaw = 0.00015; // Yaw D-gain (be careful when increasing too high, mot // NOTE: Pin 13 is reserved for onboard LED, pins 18 and 19 are reserved for the MPU6050 IMU for default setup // Radio: // Note: If using SBUS, connect to pin 21 (RX5), if using DSM, connect to pin 15 (RX3) -const int ch1Pin = 15; // throttle -const int ch2Pin = 16; // ail -const int ch3Pin = 17; // ele -const int ch4Pin = 20; // rudd -const int ch5Pin = 21; // gear (throttle cut) -const int ch6Pin = 22; // aux1 (free aux channel) +const int ch1Pin = 15; // throttle +const int ch2Pin = 16; // ail +const int ch3Pin = 17; // ele +const int ch4Pin = 20; // rudd +const int ch5Pin = D7; // gear (throttle cut) +const int ch6Pin = D7; // aux1 (free aux channel) const int PPM_Pin = 23; // OneShot125 ESC pin outputs: const int m1Pin = D0; const int m2Pin = D1; const int m3Pin = D2; const int m4Pin = D3; -const int m5Pin = D4; -const int m6Pin = D5; +const int m5Pin = D7; +const int m6Pin = D8; // PWM servo or ESC outputs: const int servo1Pin = 6; const int servo2Pin = 7; @@ -234,13 +236,13 @@ const int servo4Pin = 9; const int servo5Pin = 10; const int servo6Pin = 11; const int servo7Pin = 12; -PWMServo servo1; // Create servo objects to control a servo or ESC with PWM -PWMServo servo2; -PWMServo servo3; -PWMServo servo4; -PWMServo servo5; -PWMServo servo6; -PWMServo servo7; +Servo servo1; // Create servo objects to control a servo or ESC with PWM +Servo servo2; +Servo servo3; +Servo servo4; +Servo servo5; +Servo servo6; +Servo servo7; //========================================================================================================================// @@ -266,6 +268,20 @@ bool sbusLostFrame; #if defined USE_DSM_RX DSM1024 DSM; #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: float AccX, AccY, AccZ; @@ -276,7 +292,7 @@ float MagX, MagY, MagZ; float MagX_prev, MagY_prev, MagZ_prev; float roll_IMU, pitch_IMU, yaw_IMU; float roll_IMU_prev, pitch_IMU_prev; -float q0 = 1.0f; // Initialize quaternion for madgwick filter +float q0 = 1.0f; // Initialize quaternion for madgwick filter float q1 = 0.0f; float q2 = 0.0f; float q3 = 0.0f; @@ -339,39 +355,50 @@ void printServoCommands(); void printLoopRate(); float invSqrt(float x); void controlMixer(); - +void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len); +void ESPNowSetup(); //========================================================================================================================// // VOID SETUP // //========================================================================================================================// -void setup() { - Serial.begin(500000); // USB serial - delay(500); - +void setup() +{ + Serial.begin(9600); // USB serial + Serial.println("Serial started"); + delay(10000); + Serial.println("Initiating 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(m2Pin, OUTPUT); pinMode(m3Pin, OUTPUT); pinMode(m4Pin, OUTPUT); pinMode(m5Pin, OUTPUT); pinMode(m6Pin, OUTPUT); - servo1.attach(servo1Pin, 900, 2100); // Pin, min PWM value, max PWM value + Serial.println("Attach servos"); + servo1.attach(servo1Pin, 900, 2100); // Pin, min PWM value, max PWM value servo2.attach(servo2Pin, 900, 2100); servo3.attach(servo3Pin, 900, 2100); servo4.attach(servo4Pin, 900, 2100); servo5.attach(servo5Pin, 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 - digitalWrite(13, HIGH); + // digitalWrite(13, HIGH); delay(5); - // Initialize radio communication - // radioSetup(); //commented out because I'm making my own controller +// Initialize radio communication +#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 channel_1_pwm = channel_1_fs; @@ -390,9 +417,10 @@ void setup() { // 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 - 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 - servo3.write(0); // Keep these at 0 if you are using servo outputs for motors + Serial.println("Arming servos"); + 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 + servo3.write(0); // Keep these at 0 if you are using servo outputs for motors servo4.write(0); servo5.write(0); servo6.write(0); @@ -404,16 +432,18 @@ void setup() { // Code will not proceed past here if this function is uncommented! // Arm OneShot125 motors - m1_command_PWM = 125; // Command OneShot125 ESC from 125 to 250us pulse length + m1_command_PWM = 125; // Command OneShot125 ESC from 125 to 250us pulse length m2_command_PWM = 125; m3_command_PWM = 125; m4_command_PWM = 125; m5_command_PWM = 125; m6_command_PWM = 125; - armMotors(); // Loop over commandMotors() until ESCs happily arm + Serial.println("Arming motors"); + armMotors(); // Loop over commandMotors() until ESCs happily arm // 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) // calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section @@ -423,17 +453,18 @@ void setup() { // MAIN LOOP // //========================================================================================================================// -void loop() { +void loop() +{ // Keep track of what time it is and how much time has elapsed since the last loop prev_time = current_time; current_time = micros(); dt = (current_time - prev_time) / 1000000.0; - 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: - // 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) + 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) // 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) // printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) @@ -444,30 +475,30 @@ void loop() { // printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) // Get arming status - armedStatus(); // Check if the throttle cut is off and throttle is low. + armedStatus(); // Check if the throttle cut is off and throttle is low. // Get vehicle state - getIMUdata(); // Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise - Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); // Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees) + getIMUdata(); // Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise + Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); // Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees) // Compute desired state - getDesState(); // Convert raw commands to normalized values based on saturated control limits + getDesState(); // Convert raw commands to normalized values based on saturated control limits // PID Controller - SELECT ONE: - controlANGLE(); // Stabilize on angle setpoint + controlANGLE(); // Stabilize on angle setpoint // controlANGLE2(); //Stabilize on angle setpoint using cascaded method. Rate controller must be tuned well first! // controlRATE(); //Stabilize on rate setpoint // Actuator mixing and scaling to PWM values - controlMixer(); // Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here - scaleCommands(); // Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) + controlMixer(); // Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here + scaleCommands(); // Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) // Throttle cut check - throttleCut(); // Directly sets motor commands to low based on state of ch5 + throttleCut(); // Directly sets motor commands to low based on state of ch5 // Command actuators - commandMotors(); // Sends command pulses to each motor pin using OneShot125 protocol - servo1.write(s1_command_PWM); // Writes PWM value to servo object + commandMotors(); // Sends command pulses to each motor pin using OneShot125 protocol + servo1.write(s1_command_PWM); // Writes PWM value to servo object servo2.write(s2_command_PWM); servo3.write(s3_command_PWM); servo4.write(s4_command_PWM); @@ -476,18 +507,19 @@ void loop() { servo7.write(s7_command_PWM); // Get vehicle commands for next loop iteration - getCommands(); // Pulls current available radio commands - failSafe(); // Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup + getCommands(); // Pulls current available radio commands + failSafe(); // Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup // Regulate loop rate - loopRate(2000); // Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default + loopRate(2000); // Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default } //========================================================================================================================// // FUNCTIONS // //========================================================================================================================// -void controlMixer() { +void controlMixer() +{ // 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 @@ -505,10 +537,10 @@ void controlMixer() { */ // Quad mixing - EXAMPLE - m1_command_scaled = thro_des - pitch_PID + roll_PID + yaw_PID; // Front Left - m2_command_scaled = thro_des - pitch_PID - roll_PID - yaw_PID; // Front Right - m3_command_scaled = thro_des + pitch_PID - roll_PID + yaw_PID; // Back Right - m4_command_scaled = thro_des + pitch_PID + roll_PID - yaw_PID; // Back Left + m1_command_scaled = thro_des - pitch_PID + roll_PID + yaw_PID; // Front Left + m2_command_scaled = thro_des - pitch_PID - roll_PID - yaw_PID; // Front Right + m3_command_scaled = thro_des + pitch_PID - roll_PID + yaw_PID; // Back Right + m4_command_scaled = thro_des + pitch_PID + roll_PID - yaw_PID; // Back Left m5_command_scaled = 0; m6_command_scaled = 0; @@ -522,28 +554,33 @@ void controlMixer() { 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. - if ((channel_5_pwm < 1500) && (channel_1_pwm < 1050)) { + if ((channel_5_pwm < 1500) && (channel_1_pwm < 1050)) + { armedFly = true; } } -void IMUinit() { +void IMUinit() +{ // DESCRIPTION: Initialize IMU /* * Don't worry about how this works. */ #if defined USE_MPU6050_I2C Wire.begin(); - Wire.setClock(1000000); // Note this is 2.5 times the spec sheet 400 kHz max... + Wire.setClock(1000000); // Note this is 2.5 times the spec sheet 400 kHz max... mpu6050.initialize(); - if (mpu6050.testConnection() == false) { + if (mpu6050.testConnection() == false) + { Serial.println("MPU6050 initialization unsuccessful"); Serial.println("Check MPU6050 wiring or try cycling power"); - while (1) { + while (1) + { } } @@ -556,12 +593,14 @@ void IMUinit() { #elif defined USE_MPU9250_SPI int status = mpu9250.begin(); - if (status < 0) { + if (status < 0) + { Serial.println("MPU9250 initialization unsuccessful"); Serial.println("Check MPU9250 wiring or try cycling power"); Serial.print("Status: "); Serial.println(status); - while (1) { + while (1) + { } } // From the reset state all registers should be 0x00, so we should be at @@ -572,20 +611,21 @@ void IMUinit() { mpu9250.setMagCalX(MagErrorX, MagScaleX); mpu9250.setMagCalY(MagErrorY, MagScaleY); mpu9250.setMagCalZ(MagErrorZ, MagScaleZ); - mpu9250.setSrd(0); // sets gyro and accel read to 1khz, magnetometer read to 100hz + mpu9250.setSrd(0); // sets gyro and accel read to 1khz, magnetometer read to 100hz #elif defined USE_BNO085_I2C Wire.begin(); myIMU.begin(); - Wire.setClock(400000); // Increase I2C data rate to 400kHz + Wire.setClock(400000); // Increase I2C data rate to 400kHz + Serial.println("IMU initialized"); myIMU.enableMagnetometer(50); myIMU.enableGyro(50); myIMU.enableAccelerometer(50); - #endif } -void getIMUdata() { +void getIMUdata() +{ // 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. @@ -602,24 +642,24 @@ void getIMUdata() { #elif defined USE_MPU9250_SPI mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); #elif defined USE_BNO085_I2C - //BNO magnetometer + // BNO magnetometer MgX = myIMU.getMagX(); MgY = myIMU.getMagY(); MgZ = myIMU.getMagZ(); - //convert radians to degrees + // convert radians to degrees GyX = myIMU.getGyroX() * 180 / PI; GyY = myIMU.getGyroY() * 180 / PI; GyZ = myIMU.getGyroZ() * 180 / PI; - //Acelleration (dont know if I need linear or normal. One way to find out) + // Acelleration (dont know if I need linear or normal. One way to find out) AcX = myIMU.getAccelX(); AcY = myIMU.getAccelY(); AcZ = myIMU.getAccelZ(); #endif // Accelerometer - AccX = AcX / ACCEL_SCALE_FACTOR; // G's + AccX = AcX / ACCEL_SCALE_FACTOR; // G's AccY = AcY / ACCEL_SCALE_FACTOR; AccZ = AcZ / ACCEL_SCALE_FACTOR; // Correct the outputs with the calculated error values @@ -635,7 +675,7 @@ void getIMUdata() { AccZ_prev = AccZ; // Gyro - GyroX = GyX / GYRO_SCALE_FACTOR; // deg/sec + GyroX = GyX / GYRO_SCALE_FACTOR; // deg/sec GyroY = GyY / GYRO_SCALE_FACTOR; GyroZ = GyZ / GYRO_SCALE_FACTOR; // Correct the outputs with the calculated error values @@ -651,7 +691,7 @@ void getIMUdata() { GyroZ_prev = GyroZ; // Magnetometer - MagX = MgX / 6.0; // uT + MagX = MgX / 6.0; // uT MagY = MgY / 6.0; MagZ = MgZ / 6.0; // Correct the outputs with the calculated error values @@ -667,7 +707,8 @@ void getIMUdata() { 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 /* * Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and @@ -684,23 +725,26 @@ void calculate_IMU_error() { // Read IMU values 12000 times int c = 0; - while (c < 12000) { + while (c < 12000) + { #if defined USE_MPU6050_I2C mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); #elif defined USE_MPU9250_SPI mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); #elif defined USE_BNO085_I2C - //bno magnetometer + // bno magnetometer MgX = myIMU.getMagX(); MgY = myIMU.getMagY(); MgZ = myIMU.getMagZ(); - //convert radians to degrees + // convert radians to degrees GyX = myIMU.getGyroX() * 180 / PI; GyY = myIMU.getGyroY() * 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(); AcY = myIMU.getAccelY(); AcZ = myIMU.getAccelZ(); @@ -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."); } -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 // Assuming vehicle is powered up on level surface! /* @@ -761,17 +806,19 @@ void calibrateAttitude() { * to boot. */ // 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; current_time = micros(); dt = (current_time - prev_time) / 1000000.0; getIMUdata(); Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); - loopRate(2000); // do not exceed 2000Hz + loopRate(2000); // do not exceed 2000Hz } } -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 /* * 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 // 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); 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); // 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 recipNorm = invSqrt(ax * ax + ay * ay + az * az); @@ -859,7 +908,7 @@ void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; @@ -886,12 +935,13 @@ void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float q3 *= recipNorm; // compute angles - NWU - roll_IMU = atan2(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2) * 57.29577951; // degrees - pitch_IMU = -asin(constrain(-2.0f * (q1 * q3 - q0 * q2), -0.999999, 0.999999)) * 57.29577951; // degrees - yaw_IMU = -atan2(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3) * 57.29577951; // degrees + roll_IMU = atan2(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2) * 57.29577951; // degrees + pitch_IMU = -asin(constrain(-2.0f * (q1 * q3 - q0 * q2), -0.999999, 0.999999)) * 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 /* * 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); // 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 recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; @@ -941,7 +992,7 @@ void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, fl s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; @@ -968,12 +1019,13 @@ void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, fl q3 *= recipNorm; // Compute angles - roll_IMU = atan2(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2) * 57.29577951; // degrees - pitch_IMU = -asin(constrain(-2.0f * (q1 * q3 - q0 * q2), -0.999999, 0.999999)) * 57.29577951; // degrees - yaw_IMU = -atan2(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3) * 57.29577951; // degrees + roll_IMU = atan2(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2) * 57.29577951; // degrees + pitch_IMU = -asin(constrain(-2.0f * (q1 * q3 - q0 * q2), -0.999999, 0.999999)) * 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 /* * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw @@ -982,25 +1034,26 @@ void getDesState() { * (rate mode). yaw_des is scaled to be within max yaw in degrees/sec. Also creates roll_passthru, pitch_passthru, and * yaw_passthru variables, to be used in commanding motors/servos with direct unstabilized commands in controlMixer(). */ - thro_des = (channel_1_pwm - 1000.0) / 1000.0; // Between 0 and 1 - roll_des = (channel_2_pwm - 1500.0) / 500.0; // Between -1 and 1 - pitch_des = (channel_3_pwm - 1500.0) / 500.0; // Between -1 and 1 - yaw_des = (channel_4_pwm - 1500.0) / 500.0; // Between -1 and 1 - roll_passthru = roll_des / 2.0; // Between -0.5 and 0.5 - pitch_passthru = pitch_des / 2.0; // Between -0.5 and 0.5 - yaw_passthru = yaw_des / 2.0; // Between -0.5 and 0.5 + thro_des = (channel_1_pwm - 1000.0) / 1000.0; // Between 0 and 1 + roll_des = (channel_2_pwm - 1500.0) / 500.0; // Between -1 and 1 + pitch_des = (channel_3_pwm - 1500.0) / 500.0; // Between -1 and 1 + yaw_des = (channel_4_pwm - 1500.0) / 500.0; // Between -1 and 1 + roll_passthru = roll_des / 2.0; // Between -0.5 and 0.5 + pitch_passthru = pitch_des / 2.0; // Between -0.5 and 0.5 + yaw_passthru = yaw_des / 2.0; // Between -0.5 and 0.5 // Constrain within normalized bounds - thro_des = constrain(thro_des, 0.0, 1.0); // Between 0 and 1 - roll_des = constrain(roll_des, -1.0, 1.0) * maxRoll; // Between -maxRoll and +maxRoll - pitch_des = constrain(pitch_des, -1.0, 1.0) * maxPitch; // Between -maxPitch and +maxPitch - yaw_des = constrain(yaw_des, -1.0, 1.0) * maxYaw; // Between -maxYaw and +maxYaw + thro_des = constrain(thro_des, 0.0, 1.0); // Between 0 and 1 + roll_des = constrain(roll_des, -1.0, 1.0) * maxRoll; // Between -maxRoll and +maxRoll + pitch_des = constrain(pitch_des, -1.0, 1.0) * maxPitch; // Between -maxPitch and +maxPitch + yaw_des = constrain(yaw_des, -1.0, 1.0) * maxYaw; // Between -maxYaw and +maxYaw roll_passthru = constrain(roll_passthru, -0.5, 0.5); pitch_passthru = constrain(pitch_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) /* * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in @@ -1016,32 +1069,35 @@ void controlANGLE() { // Roll error_roll = roll_des - roll_IMU; 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 = 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 derivative_roll = GyroX; - roll_PID = 0.01 * (Kp_roll_angle * error_roll + Ki_roll_angle * integral_roll - Kd_roll_angle * derivative_roll); // Scaled by .01 to bring within -1 to 1 range + roll_PID = 0.01 * (Kp_roll_angle * error_roll + Ki_roll_angle * integral_roll - Kd_roll_angle * derivative_roll); // Scaled by .01 to bring within -1 to 1 range // Pitch error_pitch = pitch_des - pitch_IMU; 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 = 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 derivative_pitch = GyroY; - pitch_PID = .01 * (Kp_pitch_angle * error_pitch + Ki_pitch_angle * integral_pitch - Kd_pitch_angle * derivative_pitch); // Scaled by .01 to bring within -1 to 1 range + pitch_PID = .01 * (Kp_pitch_angle * error_pitch + Ki_pitch_angle * integral_pitch - Kd_pitch_angle * derivative_pitch); // Scaled by .01 to bring within -1 to 1 range // Yaw, stablize on rate from GyroZ error_yaw = yaw_des - GyroZ; 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 = 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 derivative_yaw = (error_yaw - error_yaw_prev) / dt; - yaw_PID = .01 * (Kp_yaw * error_yaw + Ki_yaw * integral_yaw + Kd_yaw * derivative_yaw); // Scaled by .01 to bring within -1 to 1 range + yaw_PID = .01 * (Kp_yaw * error_yaw + Ki_yaw * integral_yaw + Kd_yaw * derivative_yaw); // Scaled by .01 to bring within -1 to 1 range // Update roll variables integral_roll_prev = integral_roll; @@ -1052,7 +1108,8 @@ void controlANGLE() { integral_yaw_prev = integral_yaw; } -void controlANGLE2() { +void controlANGLE2() +{ // 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. @@ -1063,22 +1120,24 @@ void controlANGLE2() { // Roll error_roll = roll_des - roll_IMU; 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 = 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 derivative_roll = (roll_IMU - roll_IMU_prev) / dt; - roll_des_ol = Kp_roll_angle * error_roll + Ki_roll_angle * integral_roll_ol; // - Kd_roll_angle*derivative_roll; + roll_des_ol = Kp_roll_angle * error_roll + Ki_roll_angle * integral_roll_ol; // - Kd_roll_angle*derivative_roll; // Pitch error_pitch = pitch_des - pitch_IMU; 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 = 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 derivative_pitch = (pitch_IMU - pitch_IMU_prev) / dt; - pitch_des_ol = Kp_pitch_angle * error_pitch + Ki_pitch_angle * integral_pitch_ol; // - Kd_pitch_angle*derivative_pitch; + pitch_des_ol = Kp_pitch_angle * error_pitch + Ki_pitch_angle * integral_pitch_ol; // - Kd_pitch_angle*derivative_pitch; // Apply loop gain, constrain, and LP filter for artificial damping float Kl = 30.0; @@ -1093,32 +1152,35 @@ void controlANGLE2() { // Roll error_roll = roll_des_ol - GyroX; 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 = 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 derivative_roll = (error_roll - error_roll_prev) / dt; - roll_PID = .01 * (Kp_roll_rate * error_roll + Ki_roll_rate * integral_roll_il + Kd_roll_rate * derivative_roll); // Scaled by .01 to bring within -1 to 1 range + roll_PID = .01 * (Kp_roll_rate * error_roll + Ki_roll_rate * integral_roll_il + Kd_roll_rate * derivative_roll); // Scaled by .01 to bring within -1 to 1 range // Pitch error_pitch = pitch_des_ol - GyroY; 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 = 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 derivative_pitch = (error_pitch - error_pitch_prev) / dt; - pitch_PID = .01 * (Kp_pitch_rate * error_pitch + Ki_pitch_rate * integral_pitch_il + Kd_pitch_rate * derivative_pitch); // Scaled by .01 to bring within -1 to 1 range + pitch_PID = .01 * (Kp_pitch_rate * error_pitch + Ki_pitch_rate * integral_pitch_il + Kd_pitch_rate * derivative_pitch); // Scaled by .01 to bring within -1 to 1 range // Yaw error_yaw = yaw_des - GyroZ; 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 = 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 derivative_yaw = (error_yaw - error_yaw_prev) / dt; - yaw_PID = .01 * (Kp_yaw * error_yaw + Ki_yaw * integral_yaw + Kd_yaw * derivative_yaw); // Scaled by .01 to bring within -1 to 1 range + yaw_PID = .01 * (Kp_yaw * error_yaw + Ki_yaw * integral_yaw + Kd_yaw * derivative_yaw); // Scaled by .01 to bring within -1 to 1 range // Update roll variables integral_roll_prev_ol = integral_roll_ol; @@ -1137,7 +1199,8 @@ void controlANGLE2() { integral_yaw_prev = integral_yaw; } -void controlRATE() { +void controlRATE() +{ // 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. @@ -1145,32 +1208,35 @@ void controlRATE() { // Roll error_roll = roll_des - GyroX; 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 = 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 derivative_roll = (error_roll - error_roll_prev) / dt; - roll_PID = .01 * (Kp_roll_rate * error_roll + Ki_roll_rate * integral_roll + Kd_roll_rate * derivative_roll); // Scaled by .01 to bring within -1 to 1 range + roll_PID = .01 * (Kp_roll_rate * error_roll + Ki_roll_rate * integral_roll + Kd_roll_rate * derivative_roll); // Scaled by .01 to bring within -1 to 1 range // Pitch error_pitch = pitch_des - GyroY; 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 = 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 derivative_pitch = (error_pitch - error_pitch_prev) / dt; - pitch_PID = .01 * (Kp_pitch_rate * error_pitch + Ki_pitch_rate * integral_pitch + Kd_pitch_rate * derivative_pitch); // Scaled by .01 to bring within -1 to 1 range + pitch_PID = .01 * (Kp_pitch_rate * error_pitch + Ki_pitch_rate * integral_pitch + Kd_pitch_rate * derivative_pitch); // Scaled by .01 to bring within -1 to 1 range // Yaw, stablize on rate from GyroZ error_yaw = yaw_des - GyroZ; 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 = 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 derivative_yaw = (error_yaw - error_yaw_prev) / dt; - yaw_PID = .01 * (Kp_yaw * error_yaw + Ki_yaw * integral_yaw + Kd_yaw * derivative_yaw); // Scaled by .01 to bring within -1 to 1 range + yaw_PID = .01 * (Kp_yaw * error_yaw + Ki_yaw * integral_yaw + Kd_yaw * derivative_yaw); // Scaled by .01 to bring within -1 to 1 range // Update roll variables error_roll_prev = error_roll; @@ -1185,7 +1251,8 @@ void controlRATE() { integral_yaw_prev = integral_yaw; } -void scaleCommands() { +void scaleCommands() +{ // 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 @@ -1226,7 +1293,8 @@ void scaleCommands() { s7_command_PWM = constrain(s7_command_PWM, 0, 180); } -void getCommands() { +void getCommands() +{ // 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 @@ -1244,7 +1312,8 @@ void getCommands() { channel_6_pwm = getRadioPWM(6); #elif defined USE_SBUS_RX - if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame)) { + if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame)) + { // sBus scaling below is for Taranis-Plus and X4R-SB float scale = 0.615; float bias = 895.0; @@ -1257,9 +1326,12 @@ void getCommands() { } #elif defined USE_DSM_RX - if (DSM.timedOut(micros())) { + if (DSM.timedOut(micros())) + { // Serial.println("*** DSM RX TIMED OUT ***"); - } else if (DSM.gotNewFrame()) { + } + else if (DSM.gotNewFrame()) + { uint16_t values[num_DSM_channels]; DSM.getChannelValues(values, num_DSM_channels); @@ -1270,10 +1342,19 @@ void getCommands() { channel_5_pwm = values[4]; 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 // Low-pass the critical commands and update previous values - float b = 0.7; // Lower=slower, higher=noiser + float b = 0.7; // Lower=slower, higher=noiser channel_1_pwm = (1.0 - b) * channel_1_pwm_prev + b * channel_1_pwm; channel_2_pwm = (1.0 - b) * channel_2_pwm_prev + b * channel_2_pwm; channel_3_pwm = (1.0 - b) * channel_3_pwm_prev + b * channel_3_pwm; @@ -1284,7 +1365,8 @@ void getCommands() { channel_4_pwm_prev = channel_4_pwm; } -void failSafe() { +void failSafe() +{ // 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 @@ -1317,7 +1399,8 @@ void failSafe() { check6 = 1; // 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_2_pwm = channel_2_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 /* * 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(); // 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(); - if ((m1_command_PWM <= timer - pulseStart) && (flagM1 == 0)) { + if ((m1_command_PWM <= timer - pulseStart) && (flagM1 == 0)) + { digitalWrite(m1Pin, LOW); wentLow = wentLow + 1; flagM1 = 1; } - if ((m2_command_PWM <= timer - pulseStart) && (flagM2 == 0)) { + if ((m2_command_PWM <= timer - pulseStart) && (flagM2 == 0)) + { digitalWrite(m2Pin, LOW); wentLow = wentLow + 1; flagM2 = 1; } - if ((m3_command_PWM <= timer - pulseStart) && (flagM3 == 0)) { + if ((m3_command_PWM <= timer - pulseStart) && (flagM3 == 0)) + { digitalWrite(m3Pin, LOW); wentLow = wentLow + 1; flagM3 = 1; } - if ((m4_command_PWM <= timer - pulseStart) && (flagM4 == 0)) { + if ((m4_command_PWM <= timer - pulseStart) && (flagM4 == 0)) + { digitalWrite(m4Pin, LOW); wentLow = wentLow + 1; flagM4 = 1; } - if ((m5_command_PWM <= timer - pulseStart) && (flagM5 == 0)) { + if ((m5_command_PWM <= timer - pulseStart) && (flagM5 == 0)) + { digitalWrite(m5Pin, LOW); wentLow = wentLow + 1; flagM5 = 1; } - if ((m6_command_PWM <= timer - pulseStart) && (flagM6 == 0)) { + if ((m6_command_PWM <= timer - pulseStart) && (flagM6 == 0)) + { digitalWrite(m6Pin, LOW); wentLow = wentLow + 1; flagM6 = 1; @@ -1387,39 +1478,43 @@ void commandMotors() { } } -void armMotors() { +void armMotors() +{ // 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() * 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 (int i = 0; i <= 50; i++) { + for (int i = 0; i <= 50; i++) + { commandMotors(); delay(2); } } -void calibrateESCs() { +void calibrateESCs() +{ // 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 * 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. */ - while (true) { + while (true) + { prev_time = current_time; current_time = micros(); 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 - failSafe(); // Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup - getDesState(); // Convert raw commands to normalized values based on saturated control limits - getIMUdata(); // Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise - Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); // Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) - getDesState(); // Convert raw commands to normalized values based on saturated control limits + getCommands(); // Pulls current available radio commands + failSafe(); // Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup + getDesState(); // Convert raw commands to normalized values based on saturated control limits + getIMUdata(); // Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise + Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); // Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) + getDesState(); // Convert raw commands to normalized values based on saturated control limits m1_command_scaled = thro_des; m2_command_scaled = thro_des; @@ -1434,7 +1529,7 @@ void calibrateESCs() { s5_command_scaled = thro_des; s6_command_scaled = thro_des; s7_command_scaled = thro_des; - scaleCommands(); // Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) + scaleCommands(); // Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) // throttleCut(); //Directly sets motor commands to low based on state of ch5 @@ -1445,15 +1540,16 @@ void calibrateESCs() { servo5.write(s5_command_PWM); servo6.write(s6_command_PWM); servo7.write(s7_command_PWM); - commandMotors(); // Sends command pulses to each motor pin using OneShot125 protocol + commandMotors(); // Sends command pulses to each motor pin using OneShot125 protocol // printRadioData(); //Radio pwm values (expected: 1000 to 2000) - loopRate(2000); // Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default + loopRate(2000); // Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default } } -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 /* * Takes in a float variable, desired minimum and maximum bounds, fade time, high or low desired state, and the loop frequency @@ -1464,20 +1560,24 @@ float floatFaderLinear(float param, float param_min, float param_max, float fade * to designate the two final options for that control gain based on the dynamic configuration assignment to the auxillary radio channel. * */ - 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; - } 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 = constrain(param, param_min, param_max); // Constrain param within max bounds + param = constrain(param, param_min, param_max); // Constrain param within max bounds 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 /* * Takes in a float variable to be modified, desired new position, upper value, lower value, fade time, and the loop frequency @@ -1487,20 +1587,24 @@ 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. * */ - 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); param = param - diffParam; - } else if (param < param_des) { // Need to fade up to get to desired + } + else if (param < param_des) + { // Need to fade up to get to desired float diffParam = (param_des - param_lower) / (fadeTime_up * loopFreq); param = param + diffParam; } - param = constrain(param, param_lower, param_upper); // Constrain param within max bounds + param = constrain(param, param_lower, param_upper); // Constrain param within max bounds 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 /* * 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; } -void throttleCut() { +void throttleCut() +{ // 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 @@ -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 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; m1_command_PWM = 120; m2_command_PWM = 120; @@ -1547,7 +1653,8 @@ void throttleCut() { } } -void calibrateMagnetometer() { +void calibrateMagnetometer() +{ #if defined USE_MPU9250_SPI float success; Serial.println("Beginning magnetometer calibration in"); @@ -1560,7 +1667,8 @@ void calibrateMagnetometer() { Serial.println("Rotate the IMU about all axes until complete."); Serial.println(" "); success = mpu9250.calibrateMag(); - if (success) { + if (success) + { Serial.println("Calibration Successful!"); Serial.println("Please comment out the calibrateMagnetometer() function and copy these values into the code:"); Serial.print("float MagErrorX = "); @@ -1583,19 +1691,22 @@ void calibrateMagnetometer() { Serial.println(";"); Serial.println(" "); Serial.println("If you are having trouble with your attitude estimate at a new flying location, repeat this process as needed."); - } else { + } + else + { Serial.println("Calibration Unsuccessful. Please reset the board and try again."); } while (1) - ; // Halt code so it won't enter main loop until this function commented out + ; // Halt code so it won't enter main loop until this function commented out #endif Serial.println("Error: MPU9250 not selected. Cannot calibrate non-existent magnetometer."); while (1) - ; // Halt code so it won't enter main loop until this function commented out + ; // 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 /* * 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(); // Sit in loop until appropriate time has passed - while (invFreq > (checker - current_time)) { + while (invFreq > (checker - current_time)) + { checker = micros(); } } -void loopBlink() { +void loopBlink() +{ // DESCRIPTION: Blink LED on board to indicate main loop is running /* * It looks cool. */ - if (current_time - blink_counter > blink_delay) { + if (current_time - blink_counter > blink_delay) + { 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; blink_delay = 100000; - } else if (blinkAlternate == 0) { + } + else if (blinkAlternate == 0) + { blinkAlternate = 1; 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 - for (int j = 1; j <= numBlinks; j++) { - digitalWrite(13, LOW); + for (int j = 1; j <= numBlinks; j++) + { + // digitalWrite(13, LOW); delay(downTime); - digitalWrite(13, HIGH); + // digitalWrite(13, HIGH); delay(upTime); } } -void printRadioData() { - if (current_time - print_counter > 10000) { +void printRadioData() +{ + if (current_time - print_counter > 10000) + { print_counter = micros(); Serial.print(F(" CH1:")); Serial.print(channel_1_pwm); @@ -1660,8 +1781,10 @@ void printRadioData() { } } -void printDesiredState() { - if (current_time - print_counter > 10000) { +void printDesiredState() +{ + if (current_time - print_counter > 10000) + { print_counter = micros(); Serial.print(F("thro_des:")); Serial.print(thro_des); @@ -1674,8 +1797,10 @@ void printDesiredState() { } } -void printGyroData() { - if (current_time - print_counter > 10000) { +void printGyroData() +{ + if (current_time - print_counter > 10000) + { print_counter = micros(); Serial.print(F("GyroX:")); Serial.print(GyroX); @@ -1686,8 +1811,10 @@ void printGyroData() { } } -void printAccelData() { - if (current_time - print_counter > 10000) { +void printAccelData() +{ + if (current_time - print_counter > 10000) + { print_counter = micros(); Serial.print(F("AccX:")); Serial.print(AccX); @@ -1698,8 +1825,10 @@ void printAccelData() { } } -void printMagData() { - if (current_time - print_counter > 10000) { +void printMagData() +{ + if (current_time - print_counter > 10000) + { print_counter = micros(); Serial.print(F("MagX:")); Serial.print(MagX); @@ -1710,8 +1839,10 @@ void printMagData() { } } -void printRollPitchYaw() { - if (current_time - print_counter > 10000) { +void printRollPitchYaw() +{ + if (current_time - print_counter > 10000) + { print_counter = micros(); Serial.print(F("roll:")); Serial.print(roll_IMU); @@ -1722,8 +1853,10 @@ void printRollPitchYaw() { } } -void printPIDoutput() { - if (current_time - print_counter > 10000) { +void printPIDoutput() +{ + if (current_time - print_counter > 10000) + { print_counter = micros(); Serial.print(F("roll_PID:")); Serial.print(roll_PID); @@ -1734,8 +1867,10 @@ void printPIDoutput() { } } -void printMotorCommands() { - if (current_time - print_counter > 10000) { +void printMotorCommands() +{ + if (current_time - print_counter > 10000) + { print_counter = micros(); Serial.print(F("m1_command:")); Serial.print(m1_command_PWM); @@ -1752,8 +1887,10 @@ void printMotorCommands() { } } -void printServoCommands() { - if (current_time - print_counter > 10000) { +void printServoCommands() +{ + if (current_time - print_counter > 10000) + { print_counter = micros(); Serial.print(F("s1_command:")); Serial.print(s1_command_PWM); @@ -1772,8 +1909,10 @@ void printServoCommands() { } } -void printLoopRate() { - if (current_time - print_counter > 10000) { +void printLoopRate() +{ + if (current_time - print_counter > 10000) + { print_counter = micros(); Serial.print(F("dt:")); Serial.println(dt * 1000000.0); @@ -1784,7 +1923,8 @@ void printLoopRate() { // HELPER FUNCTIONS -float invSqrt(float x) { +float invSqrt(float x) +{ // Fast inverse sqrt for madgwick filter /* float halfx = 0.5f * x; @@ -1803,5 +1943,23 @@ float invSqrt(float x) { float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp); return y; */ - 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)); } \ No newline at end of file