notes about platformio

docs/Assignments/week_4_programming/programming.md

@@ -3,6 +3,63 @@
 ## Introduction
 I'm very familiar with coding in C++ because of my university. At the moment im studying computer sciences at the Applied University of Amsterdam.
 
+
+### Switching to platformIO.
+At the local lecture we got thought how to use platformIO. I was already used to Arduino IDE v2. But I found the UI and the speeds of PlatformIO very nice so that's why I decided to switch to PlatformIO.
+
+First off. I created a new project with the Xiao espC3 because the espC6 isn't registered on the list. I've googled a bit and that's because PlatformIO needs to get profit so they ask money to the board developers to add the board to their platform. So we need to set the correct board later on.
+![alt text](image-3.jpg)
+When it created the project. I copied all the files over to the src folder and renament my drone.ino to main.cpp. Now I had a lot of include errors because Arduino IDE automatically imports everything and with PlatformIO you need to define everything yourself. So I started out in the Library manager installing all the libraries to the project I needed.
+
+![alt text](image-4.jpg)
+
+#### Setting the correct board
+
+Before
+```ini
+[env:seeed_xiao_esp32c3]
+platform = espressif32
+board = seeed_xiao_esp32c3
+framework = arduino
+```
+
+After
+```ini
+[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 = arduino
+board = seeed_xiao_esp32c6
+```
+
+To get the correct configuration if it isn't listed on PlatformIO, is to go to the Board manufactors website and hope there is a PlatformIO example configuration. For example this example was listed on the Xiao website. [Link](https://wiki.seeedstudio.com/xiao_esp32c6_with_platform_io/).
+
+#### Installing libraries in platformIO
+To install a library click the platformIO icon. In there click the libraries button. Then you can search a library and click add to project. Now you have added a library to your project!
+
+![alt text](image.jpg)
+
+After that it will edit platformio.ini and there you will see the library added to the project. It's also in `.pio/libdeps`
+
+### Important for linux users.
+When setting up PlatformIO on another system we ran into the issue that the machine couldn't upload the code properly. We first checked how the OS interacted with `dmesg`. `dmesg` on linux shows the kernel activity. After a while we found out we needed to install the package `platformio-udev`. What it does it add some rules for the kernel on how to handle the mcu's.
+
+### Programming and compiling and uploading
+Using platformIO is really straight forward. The only folder you should look at in your platformIO project is the `src` folder. 
+
+![alt text](image-5.jpg)
+
+In the main.cpp file you can write your program to run on a microcontroller.
+
+On the bottom of your screen you see that there are a few new buttons.
+
+![alt text](image-6.jpg)
+
+The Compile and Upload and Serial console buttons are going to be the most used. Compile compiles the code and checks if it's correct. If it has incorrect syntax you need to correct it. The upload button compiles the code first and then sends it to the Microcontroller. The Serial console is a really nice debug tool to see what the microcontroller is doing if you added print statements in your code.
+
+
 ## First code to read from the BNO085
 I first wanted to create the drone firmware myself. But then I looked through some research papers and existing programs and saw all the math that was needed to keep it upright. Then I decided to modify an existing program.
 This was my first attempts at getting the sensor to read
@@ -117,93 +174,6 @@ First used the wrong library I used the Adafruit bno0xx library instead of the S
 ## New driver
 After researching for a while and looking through other fab academy projects I found out that other people also made drones with micro controllers and used a pre-made driver that they customized (https://fab.cba.mit.edu/classes/863.23/Architecture/people/Zhixing/finalproject.html). After doing some research on how to keep the drone upright I also decided to use an existing driver because the math required for that is way above my level. Im gonna be using the [dRhemFlightVTOL](https://github.com/nickrehm/dRehmFlight/tree/master) driver. The only problem is that it doesn't support my Inertial measuring unit (BNO085). So I will have to customize the driver to make it work with it.
 
-### Implementing the BNO085
-
-### Hijacking the controls
-I need to reverse engineer the controls because now it just calls a function called radiocontrols() that manages the controls. There are 4 pwm channel that controls where the drone is going. I wanna make my own controller and simulate these pwm signals.
-```cpp
-  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
-```
-From looking further in the code it has safety protocols when the channels go too low or too high so using that we have a range where the pwm speed should be. 
-
-```cpp
-  // Triggers for failure criteria
-  // Line 1260
-  if (channel_1_pwm > maxVal || channel_1_pwm < minVal)
-    check1 = 1;
-  if (channel_2_pwm > maxVal || channel_2_pwm < minVal)
-    check2 = 1;
-  if (channel_3_pwm > maxVal || channel_3_pwm < minVal)
-    check3 = 1;
-  if (channel_4_pwm > maxVal || channel_4_pwm < minVal)
-    check4 = 1;
-  if (channel_5_pwm > maxVal || channel_5_pwm < minVal)
-    check5 = 1;
-  if (channel_6_pwm > maxVal || channel_6_pwm < minVal)
-    check6 = 1;
-```
-In this case minVal is 800 an maxVal is 2200. So we need to stay between these ranges to control the drone. In theory you can send any value to the ESC because it calibrates to the lowest and highest values you give it but there is a range. From past experiences I have experienced that 12bit PWM is not sufficient, 16 bit is needed.
-
-[ESC calibration](https://ardupilot.org/copter/docs/esc-calibration.html)
-
-### Switching to platformIO.
-At the local lecture we got thought how to use platformIO. I was already used to Arduino IDE v2. But I found the UI and the speeds of PlatformIO very nice so that's why I decided to switch to PlatformIO.
-
-First off. I created a new project with the Xiao espC3 because the espC6 isn't registered on the list. I've googled a bit and that's because PlatformIO needs to get profit so they ask money to the board developers to add the board to their platform. So we need to set the correct board later on.
-![alt text](image-3.jpg)
-When it created the project. I copied all the files over to the src folder and renament my drone.ino to main.cpp. Now I had a lot of include errors because Arduino IDE automatically imports everything and with PlatformIO you need to define everything yourself. So I started out in the Library manager installing all the libraries to the project I needed.
-
-![alt text](image-4.jpg)
-
-#### Setting the correct board
-
-Before
-```ini
-[env:seeed_xiao_esp32c3]
-platform = espressif32
-board = seeed_xiao_esp32c3
-framework = arduino
-```
-
-After
-```ini
-[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 = arduino
-board = seeed_xiao_esp32c6
-```
-
-To get the correct configuration if it isn't listed on PlatformIO, is to go to the Board manufactors website and hope there is a PlatformIO example configuration. For example this example was listed on the Xiao website. [Link](https://wiki.seeedstudio.com/xiao_esp32c6_with_platform_io/).
-
-#### Installing libraries in platformIO
-To install a library click the platformIO icon. In there click the libraries button. Then you can search a library and click add to project. Now you have added a library to your project!
-
-![alt text](image.jpg)
-
-After that it will edit platformio.ini and there you will see the library added to the project. It's also in `.pio/libdeps`
-
-### Important for linux users.
-When setting up PlatformIO on another system we ran into the issue that the machine couldn't upload the code properly. We first checked how the OS interacted with `dmesg`. `dmesg` on linux shows the kernel activity.
-
-### Programming and compiling and uploading
-Using platformIO is really straight forward. The only folder you should look at in your platformIO project is the `src` folder. 
-
-![alt text](image-5.jpg)
-
-In the main.cpp file you can write your program to run on a microcontroller.
-
-On the bottom of your screen you see that there are a few new buttons.
-
-![alt text](image-6.jpg)
-
-The Compile and Upload and Serial console buttons are going to be the most used. Compile compiles the code and checks if it's correct. If it has incorrect syntax you need to correct it. The upload button compiles the code first and then sends it to the Microcontroller. The Serial console is a really nice debug tool to see what the microcontroller is doing if you added print statements in your code.
-
 ## Editing the new driver
 Adding in support for the BNO085. The original driver doesn't support the BNO085. 
 I first started off searching for every instance of an existing sensor.
@@ -222,7 +192,7 @@ The purple text are compiler flags. They tell the compiler what to do with certa
 | #elif                 | Same as #if except using this you can have multiple cases                                                                                                                                                                     |
 | #include              | Include a library into the code                                                                                                                                                                                               |
 | #else                 | If none of the cases are met this is used                                                                                                                                                                                     |
-| #error                | Throws an error during compiling when it is compiled                                                                                                                                                                          |
+| #error                | Throws an error during compiling when ran by the compiler                                                                                                                                                                          |
 | #endif                | end of a #if, #elif or #else |
 
 ### Setting the correct library
@@ -277,6 +247,38 @@ When attempting to compile it still throws some errors so there are still functi
 ![alt text](image-19.jpg)
 So I will be needing to make my own case to control the drone.
 
+
+### Hijacking the controls from the original driver.
+I need to reverse engineer the controls because now it just calls a function called radiocontrols() that manages the controls. There are 4 pwm channel that controls where the drone is going. I wanna make my own controller and simulate these pwm signals.
+```cpp
+  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
+```
+From looking further in the code it has safety protocols when the channels go too low or too high so using that we have a range where the pwm speed should be. 
+
+```cpp
+  // Triggers for failure criteria
+  // Line 1260
+  if (channel_1_pwm > maxVal || channel_1_pwm < minVal)
+    check1 = 1;
+  if (channel_2_pwm > maxVal || channel_2_pwm < minVal)
+    check2 = 1;
+  if (channel_3_pwm > maxVal || channel_3_pwm < minVal)
+    check3 = 1;
+  if (channel_4_pwm > maxVal || channel_4_pwm < minVal)
+    check4 = 1;
+  if (channel_5_pwm > maxVal || channel_5_pwm < minVal)
+    check5 = 1;
+  if (channel_6_pwm > maxVal || channel_6_pwm < minVal)
+    check6 = 1;
+```
+In this case minVal is 800 an maxVal is 2200. So we need to stay between these ranges to control the drone. In theory you can send any value to the ESC because it calibrates to the lowest and highest values you give it but there is a range. From past experiences I have experienced that 12bit PWM is not sufficient, 16 bit is needed.
+
+[ESC calibration](https://ardupilot.org/copter/docs/esc-calibration.html)
+
+
 I wanna control to drone using another microcontroller, specifically an esp because then I can use the espNOW protocol to connect them together and get communication running between them. First I wanna make something functional before I upgrade it so that's why I'm starting out easy.
 
 
@@ -286,8 +288,8 @@ Im going to start off with 2 sliding potentiometers and a espC3 for throttle and
 ![alt text](IMG_8180.jpg)
 
 * [PotSlider](https://nl.aliexpress.com/item/1005006733220962.html) x2
-* [espC3 supermini](https://nl.aliexpress.com/item/1005008125438785.html) x1
-
+* ~~[espC3 supermini](https://nl.aliexpress.com/item/1005008125438785.html) x1~~
+* [MCU changed to espC3 XIAO](https://www.seeedstudio.com/Seeed-XIAO-ESP32C3-p-5431.html)
 
 ### Wiring the controller
 To read the Pot meter values we need to read the resistance from the potslider. So we need analog read. When looking at the data sheet I can see that I can use pins from 0 to 5.
@@ -457,7 +459,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 +575,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
 
@@ -647,16 +649,16 @@ For the drone I'm going to start of with a seperate script to see if it works an
 
 A weird thing I found out is that my esp32C3 Supermini doesn't work wirelessly that well. When I uploaded the code it still didn't work untill I held my finger on the antenna.
 
-<video controls src="IMG_8197(1).mp4" title="Title" type="video/mp4"></video>
+<video controls src="../../../Assignments/week_4_programming/IMG_8197(1).mp4" title="Title" type="video/mp4"></video>
 
-I've delved a bit deeper into it and then I found online that with the first batches of espC3 supermini's have issues with the antenna. When I was debugging it I noticed that connecting the ground to the antenne fixed it when I tried it with a pin. But when soldered on it didn't work anymore and then only thing that worked was my finger.
+I've delved a bit deeper into it and then I found online that with the first batches of espC3 supermini's have issues with the antenna. When I was debugging it I noticed that connecting the ground to the antenna 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.
+Now I can start integrating the code into the drone driver.
 
 The first thing I did is add the correct libraries at the top of the file. So it looks like this now
 
@@ -670,25 +672,57 @@ 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.
+At the end of the program I added the ESPNow Initialization 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 finally compiles!
+
+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.
+I did that by adding this line in my platformio.ini file. [Source](https://community.platformio.org/t/how-to-set-up-log-level-to-be-able-to-debug-the-esp32/8278)
+```ini
+build_flags = -DCORE_DEBUG_LEVEL=5
+```
+
+Now 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 accidentally 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 +731,8 @@ 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/)
+* [PlatformIO Debug level change for ESP](https://community.platformio.org/t/how-to-set-up-log-level-to-be-able-to-debug-the-esp32/8278)
 
 ### 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));
+}