updated code to use internal pwm timers of esp instead of software pwm

src/drone/src/main.cpp

@@ -4,6 +4,7 @@
 #include <ESP32Servo.h>
 #include <esp_now.h>
 #include <WiFi.h>
+#include <driver/ledc.h>
 // Arduino/Teensy Flight Controller - dRehmFlight
 // Author: Nicholas Rehm
 // Project Start: 1/6/2020
@@ -157,6 +158,12 @@ BNO080 myIMU;
 #define ACCEL_SCALE_FACTOR 1.0 // BNO085 gives values already in g's when converted in getIMUdata
 #endif
 
+#define LEDC_FREQ 500             // PWM frequency in Hz for OneShot125 (can be higher)
+#define LEDC_RESOLUTION 13        // 13-bit resolution gives values from 0-8191
+#define LEDC_TIMER LEDC_TIMER_0   // Use timer 0
+#define LEDC_MODE LEDC_HIGH_SPEED_MODE  // Use high-speed mode
+
+
 //========================================================================================================================//
 //                                               USER-SPECIFIED VARIABLES                                                 //
 //========================================================================================================================//
@@ -365,7 +372,7 @@ void controlMixer();
 void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len);
 void ESPNowSetup();
 void printDebugInfo();
-
+void setupMotorPWM();
 //========================================================================================================================//
 //                                                      VOID SETUP                                                        //
 //========================================================================================================================//
@@ -379,15 +386,8 @@ void setup()
   Serial.println("Initiating IMU");
   IMUinit();
 
-  Serial.println("Initiating pins");
+  Serial.println("Initiating ESC");
-  // Initialize all pins
+  setupMotorPWM();
-  // 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);
@@ -1487,72 +1487,24 @@ void failSafe()
   }
 }
 
-void commandMotors()
+void commandMotors() {
-{
+  //https://docs.espressif.com/projects/arduino-esp32/en/latest/api/ledc.html
-  // 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
-   * sent are mX_command_PWM, computed in scaleCommands(). This may be replaced by something more efficient in the future.
-   */
-  int wentLow = 0;
-  int pulseStart, timer;
-  int flagM1 = 0;
-  int flagM2 = 0;
-  int flagM3 = 0;
-  int flagM4 = 0;
-  int flagM5 = 0;
-  int flagM6 = 0;
 
-  // Write all motor pins high
+  // Convert OneShot125 pulse values (125-250μs) to duty cycle values (1100-2000) for my ESC's
-  digitalWrite(m1Pin, HIGH);
+  uint32_t m1_duty = map(m1_command_PWM, 125, 250, 1100, 2000);
-  digitalWrite(m2Pin, HIGH);
+  uint32_t m2_duty = map(m2_command_PWM, 125, 250, 1100, 2000);
-  digitalWrite(m3Pin, HIGH);
+  uint32_t m3_duty = map(m3_command_PWM, 125, 250, 1100, 2000);
-  digitalWrite(m4Pin, HIGH);
+  uint32_t m4_duty = map(m4_command_PWM, 125, 250, 1100, 2000);
-  digitalWrite(m5Pin, HIGH);
+  uint32_t m5_duty = map(m5_command_PWM, 125, 250, 1100, 2000);
-  digitalWrite(m6Pin, HIGH);
+  uint32_t m6_duty = map(m6_command_PWM, 125, 250, 1100, 2000);
-  pulseStart = micros();
+  
-
+  // Write the duty cycle values directly to each pin
-  // Write each motor pin low as correct pulse length is reached
+  ledcWrite(m1Pin, m1_duty);
-  while (wentLow < 6)
+  ledcWrite(m2Pin, m2_duty);
-  { // Keep going until final (6th) pulse is finished, then done
+  ledcWrite(m3Pin, m3_duty);
-    timer = micros();
+  ledcWrite(m4Pin, m4_duty);
-    if ((m1_command_PWM <= timer - pulseStart) && (flagM1 == 0))
+  ledcWrite(m5Pin, m5_duty);
-    {
+  ledcWrite(m6Pin, m6_duty);
-      digitalWrite(m1Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM1 = 1;
-    }
-    if ((m2_command_PWM <= timer - pulseStart) && (flagM2 == 0))
-    {
-      digitalWrite(m2Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM2 = 1;
-    }
-    if ((m3_command_PWM <= timer - pulseStart) && (flagM3 == 0))
-    {
-      digitalWrite(m3Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM3 = 1;
-    }
-    if ((m4_command_PWM <= timer - pulseStart) && (flagM4 == 0))
-    {
-      digitalWrite(m4Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM4 = 1;
-    }
-    if ((m5_command_PWM <= timer - pulseStart) && (flagM5 == 0))
-    {
-      digitalWrite(m5Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM5 = 1;
-    }
-    if ((m6_command_PWM <= timer - pulseStart) && (flagM6 == 0))
-    {
-      digitalWrite(m6Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM6 = 1;
-    }
-  }
 }
 
 void armMotors()
@@ -1730,6 +1682,17 @@ void throttleCut()
   }
 }
 
+// Add this function to initialize LEDC for each motor
+void setupMotorPWM() {
+  // Configure LEDC pins with frequency and resolution, channels selected automatically
+  ledcAttach(m1Pin, LEDC_FREQ, LEDC_RESOLUTION); // Channel selected automatically
+  ledcAttach(m2Pin, LEDC_FREQ, LEDC_RESOLUTION);
+  ledcAttach(m3Pin, LEDC_FREQ, LEDC_RESOLUTION);
+  ledcAttach(m4Pin, LEDC_FREQ, LEDC_RESOLUTION);
+  ledcAttach(m5Pin, LEDC_FREQ, LEDC_RESOLUTION);
+  ledcAttach(m6Pin, LEDC_FREQ, LEDC_RESOLUTION);
+}
+
 void calibrateMagnetometer()
 {
 #if defined USE_MPU9250_SPI