From a33daf2a0176992087e20e8bbd8a54fc3d5af75c Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Tue, 6 May 2025 13:48:27 +0200 Subject: [PATCH] updated code to use internal pwm timers of esp instead of software pwm --- src/drone/src/main.cpp | 113 ++++++++++++++--------------------------- 1 file changed, 38 insertions(+), 75 deletions(-) diff --git a/src/drone/src/main.cpp b/src/drone/src/main.cpp index 481cc5f..8d8e68b 100644 --- a/src/drone/src/main.cpp +++ b/src/drone/src/main.cpp @@ -4,6 +4,7 @@ #include #include #include +#include // 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"); - // Initialize all pins - // 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("Initiating ESC"); + setupMotorPWM(); 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() -{ - // 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; +void commandMotors() { + //https://docs.espressif.com/projects/arduino-esp32/en/latest/api/ledc.html - // Write all motor pins high - digitalWrite(m1Pin, HIGH); - digitalWrite(m2Pin, HIGH); - digitalWrite(m3Pin, HIGH); - digitalWrite(m4Pin, HIGH); - digitalWrite(m5Pin, HIGH); - digitalWrite(m6Pin, HIGH); - 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 - timer = micros(); - if ((m1_command_PWM <= timer - pulseStart) && (flagM1 == 0)) - { - 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; - } - } + // Convert OneShot125 pulse values (125-250μs) to duty cycle values (1100-2000) for my ESC's + uint32_t m1_duty = map(m1_command_PWM, 125, 250, 1100, 2000); + uint32_t m2_duty = map(m2_command_PWM, 125, 250, 1100, 2000); + uint32_t m3_duty = map(m3_command_PWM, 125, 250, 1100, 2000); + uint32_t m4_duty = map(m4_command_PWM, 125, 250, 1100, 2000); + uint32_t m5_duty = map(m5_command_PWM, 125, 250, 1100, 2000); + uint32_t m6_duty = map(m6_command_PWM, 125, 250, 1100, 2000); + + // Write the duty cycle values directly to each pin + ledcWrite(m1Pin, m1_duty); + ledcWrite(m2Pin, m2_duty); + ledcWrite(m3Pin, m3_duty); + ledcWrite(m4Pin, m4_duty); + ledcWrite(m5Pin, m5_duty); + ledcWrite(m6Pin, m6_duty); } 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