I'm trying to build a quadcopter flight controller from scratch using an ESP32, BNO055 IMU, and Turnigy TGY-i6 Transmitter/Receiver. So far I've been able to get everything to work together and give me reasonable data using PID control loops, filters, and various libraries. Previously I was using an Arduino Nano in place of the ESP32 because it was what I had at the time and I thought it worked well with it.
After doing some (very rough) flight testing I ended up coming to the conclusion that the code was not executing fast enough with the Nano to sustainably keep the quadcopter in the air. I decided to measure the loop time by setting a t1 and t2 at the beginning and end of the loop, and took the difference to find the time it took to execute one loop of the code. With the Nano, it took roughly 80000 microseconds exactly with little deviation.
Seeing this, I bought the ESP32 knowing that it had a much faster CPU clock speed and overall performance compared against the Nano. After getting the code to work on the ESP32 with a few changes, I ran a speed test again, and got the same 80ms. I was a bit confused at first but I decided to try and isolate the problem by taking out chunks of the code to see if the loop time would change - and it did not. After reading about some specific inefficencies of Arduino IDE [such as digitalWrite()] I kept trying to take away specific parts of the code, and no difference arose as I kept measuring the clock speed. It kept at roughly 80000 microseconds no matter the changes I made.
This leads me to believe that there is something quite important that I'm unaware about in my code that is causing it to run so slow. Ideally, I would like to be able to lower the loop time to at most 10ms so that the quadcopter can autolevel with little to no oscillation.
I've used Arduino for a few years now, but by no means am an expert - so any help on optimizing the code and/or solving this odd problem would be very much appreciated, Thank you.
Note: I have a suspicion that it may be something related to the IMU (BNO055) because that is one part of the hardware and software I know very little about.
These are the libraries I'm using:
#include "ESC.h" //ESC
#include <ESP32Servo.h> //ESC
#include <Wire.h>
#include <Adafruit_Sensor.h> //IMU
#include <Adafruit_BNO055.h> //IMU
#include <utility/imumaths.h> //IMU
#include <PID_v1.h> //PID
Here is the rest of the code.
After reviewing the code and segmenting it further, I discovered the problem was coming from the pulseIn() functions I use to read the PPM signal from the receiver.
The function works by:
Reads a pulse (either HIGH or LOW) on a pin. For example, if value is HIGH, pulseIn() waits for the pin to go from LOW to HIGH, starts timing, then waits for the pin to go LOW and stops timing. Returns the length of the pulse in microseconds or gives up and returns 0 if no complete pulse was received within the timeout. Link
Looking back, this clearly causes a delay in the code.
Thank you to dimich for putting me on the right path to solve the problem.
Edit: How you can solve this:
Rather than use pulseIn(), you can use interrupts to solve the problem. I did this by attaching an interrupt to the pin that is sending the PPM signal and listening for a CHANGE to happen on it. You then check whether the pin is HIGH or LOW and either begin or stop your timer. This avoids using delays in the program and allows for a clean signal to be read.
Note: Some receivers use a PPMsum on one pin rather than individual pins. On my receiver, I use 4 different channels and 4 different GPIO pins for each axis + throttle.