docs - it finally compiles now except the IMU is dead

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) 

src/drone/platformio.ini

@@ -18,3 +18,5 @@ 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

src/drone/src/main.cpp

@@ -1,6 +1,8 @@
 #include <Arduino.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
 // Author: Nicholas Rehm
 // 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
 // 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
+#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
@@ -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 <SPI.h>      //SPI communication
-#include <PWMServo.h>  //Commanding any extra actuators, installed with teensyduino installer
 
 #if defined USE_SBUS_RX
 #include "src/SBUS/SBUS.h" //sBus interface
@@ -216,16 +218,16 @@ 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 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;
@@ -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);
+  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
+#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,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.
 
   // Arm servo channels
+  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
@@ -410,9 +438,11 @@ void setup() {
   m4_command_PWM = 125;
   m5_command_PWM = 125;
   m6_command_PWM = 125;
+  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)
 
   // 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                                                        //
 //========================================================================================================================//
 
-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();
@@ -432,8 +463,8 @@ void loop() {
   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)
@@ -487,7 +518,8 @@ void loop() {
 //                                                      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
@@ -522,14 +554,17 @@ 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.
@@ -540,10 +575,12 @@ void IMUinit() {
 
   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
@@ -577,15 +616,16 @@ void IMUinit() {
   Wire.begin();
   myIMU.begin();
   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.
@@ -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,7 +725,8 @@ 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
@@ -699,6 +741,8 @@ void calculate_IMU_error() {
     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)
     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.");
 }
 
-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,7 +806,8 @@ 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;
@@ -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
   /*
    * 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);
@@ -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
 }
 
-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;
@@ -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
 }
 
-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
@@ -1000,7 +1052,8 @@ void getDesState() {
   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,7 +1069,8 @@ 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
@@ -1026,7 +1080,8 @@ void controlANGLE() {
   // 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
@@ -1036,7 +1091,8 @@ void controlANGLE() {
   // 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
@@ -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,7 +1120,8 @@ 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
@@ -1073,7 +1131,8 @@ void controlANGLE2() {
   // 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
@@ -1093,7 +1152,8 @@ 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
@@ -1103,7 +1163,8 @@ void controlANGLE2() {
   // 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
@@ -1113,7 +1174,8 @@ void controlANGLE2() {
   // 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
@@ -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,7 +1208,8 @@ 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
@@ -1155,7 +1219,8 @@ void controlRATE() {
   // 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
@@ -1165,7 +1230,8 @@ void controlRATE() {
   // 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
@@ -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,6 +1342,15 @@ 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
@@ -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,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()
   /*
    *  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
@@ -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
   /*
    *  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
 
-  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;
   }
 
@@ -1477,7 +1576,8 @@ float floatFaderLinear(float param, float param_min, float param_max, float fade
   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,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.
    *
    */
-  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;
   }
@@ -1500,7 +1603,8 @@ float floatFaderLinear2(float param, float param_des, float param_lower, float p
   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,7 +1691,9 @@ 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.");
   }
 
@@ -1595,7 +1705,8 @@ void calibrateMagnetometer() {
     ; // 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
 
-    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;
@@ -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
 }
+
+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));
+}