FIX: Serial ESC communication when using digital protocols (#14214)

* FIX: Serial ESC communication when using digital protocols

* Fix error following rebase, and amending based on comments from @ledvinap

src/main/drivers/motor.c

@@ -362,6 +362,14 @@ timeMs_t motorGetMotorEnableTimeMs(void)
 }
 #endif
 
+IO_t motorGetIo(unsigned index)
+{
+    if (index >= motorDevice.count) {
+        return IO_NONE;
+    }
+    return motorDevice.vTable->getMotorIO ? motorDevice.vTable->getMotorIO(index) : IO_NONE;
+}
+
 /* functions below for empty methods and no active motors */
 void motorPostInitNull(void)
 {
@@ -433,6 +441,7 @@ static const motorVTable_t motorNullVTable = {
     .shutdown = motorShutdownNull,
     .requestTelemetry = NULL,
     .isMotorIdle = NULL,
+    .getMotorIO = NULL,
 };
 
 void motorNullDevInit(motorDevice_t *device)

src/main/drivers/motor.h

@@ -54,5 +54,7 @@ float motorEstimateMaxRpm(void);
 bool motorIsEnabled(void);
 bool motorIsMotorEnabled(unsigned index);
 bool motorIsMotorIdle(unsigned index);
+IO_t motorGetIo(unsigned index);
+
 timeMs_t motorGetMotorEnableTimeMs(void);
 void motorShutdown(void); // Replaces stopPwmAllMotors

src/main/drivers/motor_types.h

@@ -22,6 +22,7 @@
 #pragma once
 
 #include "common/time.h"
+#include "drivers/io_types.h"
 
 #define ALL_MOTORS 255
 #define MOTOR_OUTPUT_LIMIT_PERCENT_MIN 1
@@ -64,6 +65,7 @@ typedef struct motorVTable_s {
     void (*updateComplete)(void);
     void (*shutdown)(void);
     bool (*isMotorIdle)(unsigned index);
+    IO_t (*getMotorIO)(unsigned index);
 
     // Digital commands
     void (*requestTelemetry)(unsigned index);

src/main/drivers/pwm_output.c

@@ -48,4 +48,23 @@ void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, fl
     }
 }
 
+IO_t pwmGetMotorIO(unsigned index)
+{
+    if (index >= pwmMotorCount) {
+        return IO_NONE;
+    }
+    return motors[index].io;
+}
+
+bool pwmIsMotorEnabled(unsigned index)
+{
+    return motors[index].enabled;
+}
+
+bool pwmEnableMotors(void)
+{
+    /* check motors can be enabled */
+    return pwmMotorCount > 0;
+}
+
 #endif

src/main/drivers/pwm_output_impl.h

@@ -0,0 +1,28 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "drivers/io_types.h"
+
+IO_t pwmGetMotorIO(unsigned index);
+bool pwmIsMotorEnabled(unsigned index);
+bool pwmEnableMotors(void);

src/main/io/serial_4way.c

@@ -36,7 +36,7 @@
 #include "drivers/time.h"
 #include "drivers/timer.h"
 #include "drivers/light_led.h"
-#include "drivers/pwm_output.h"
+#include "drivers/motor.h"
 #include "flight/mixer.h"
 
 #include "io/beeper.h"
@@ -143,23 +143,23 @@ inline void setEscOutput(uint8_t selEsc)
 
 uint8_t esc4wayInit(void)
 {
-    // StopPwmAllMotors();
+    motorShutdown();
-    // XXX Review effect of motor refactor
+    uint8_t escIndex = 0;
-    //pwmDisableMotors();
+
-    escCount = 0;
     memset(&escHardware, 0, sizeof(escHardware));
-    pwmOutputPort_t *pwmMotors = pwmGetMotors();
     for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
-        if (pwmMotors[i].enabled) {
+        if (motorIsMotorEnabled(i)) {
-            if (pwmMotors[i].io != IO_NONE) {
+            const IO_t io = motorGetIo(i);
-                escHardware[escCount].io = pwmMotors[i].io;
+            if (io != IO_NONE) {
-                setEscInput(escCount);
+                escHardware[escIndex].io = io;
-                setEscHi(escCount);
+                setEscInput(escIndex);
-                escCount++;
+                setEscHi(escIndex);
+                escIndex++;
             }
         }
     }
     motorDisable();
+    escCount = escIndex;
     return escCount;
 }
 

src/platform/APM32/dshot_bitbang.c

@@ -669,6 +669,14 @@ static void bbPostInit(void)
     }
 }
 
+static IO_t bbGetMotorIO(unsigned index)
+{
+    if (index >= dshotMotorCount) {
+        return IO_NONE;
+    }
+    return bbMotors[index].io;
+}
+
 static motorVTable_t bbVTable = {
     .postInit = bbPostInit,
     .enable = bbEnableMotors,
@@ -685,6 +693,7 @@ static motorVTable_t bbVTable = {
     .shutdown = bbShutdown,
     .isMotorIdle = bbDshotIsMotorIdle,
     .requestTelemetry = bbDshotRequestTelemetry,
+    .getMotorIO = bbGetMotorIO,
 };
 
 dshotBitbangStatus_e dshotBitbangGetStatus(void)

src/platform/APM32/pwm_output_apm32.c

@@ -31,6 +31,7 @@
 #include "drivers/io.h"
 #include "drivers/motor.h"
 #include "drivers/pwm_output.h"
+#include "drivers/pwm_output_impl.h"
 #include "drivers/time.h"
 #include "drivers/timer.h"
 
@@ -108,16 +109,6 @@ static void pwmDisableMotors(void)
 }
 
 static motorVTable_t motorPwmVTable;
-static bool pwmEnableMotors(void)
-{
-    /* check motors can be enabled */
-    return pwmMotorCount > 0;
-}
-
-static bool pwmIsMotorEnabled(unsigned index)
-{
-    return motors[index].enabled;
-}
 
 static void pwmCompleteMotorUpdate(void)
 {
@@ -158,6 +149,7 @@ static motorVTable_t motorPwmVTable = {
     .updateComplete = pwmCompleteMotorUpdate,
     .requestTelemetry = NULL,
     .isMotorIdle = NULL,
+    .getMotorIO = pwmGetMotorIO,
 };
 
 bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)

src/platform/AT32/pwm_output_at32bsp.c

@@ -31,6 +31,7 @@
 #include "drivers/io.h"
 #include "drivers/motor_impl.h"
 #include "drivers/pwm_output.h"
+#include "drivers/pwm_output_impl.h"
 #include "drivers/time.h"
 #include "drivers/timer.h"
 
@@ -98,18 +99,6 @@ static void pwmDisableMotors(void)
     pwmShutdownPulsesForAllMotors();
 }
 
-static motorVTable_t motorPwmVTable;
-static bool pwmEnableMotors(void)
-{
-    /* check motors can be enabled */
-    return (pwmMotorDevice->vTable);
-}
-
-static bool pwmIsMotorEnabled(unsigned index)
-{
-    return motors[index].enabled;
-}
-
 static bool useContinuousUpdate = true;
 
 static void pwmCompleteMotorUpdate(void)
@@ -151,6 +140,7 @@ static motorVTable_t motorPwmVTable = {
     .updateComplete = pwmCompleteMotorUpdate,
     .requestTelemetry = NULL,
     .isMotorIdle = NULL,
+    .getMotorIO = pwmGetMotorIO,
 };
 
 bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)

src/platform/SIMULATOR/sitl.c

@@ -41,6 +41,7 @@
 #include "drivers/system.h"
 #include "drivers/time.h"
 #include "drivers/pwm_output.h"
+#include "drivers/pwm_output_impl.h"
 #include "drivers/light_led.h"
 
 #include "drivers/timer.h"
@@ -543,8 +544,6 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
     }
 }
 
-static motorDevice_t pwmMotorDevice; // Forward
-
 pwmOutputPort_t *pwmGetMotors(void)
 {
     return motors;
@@ -562,14 +561,7 @@ static uint16_t pwmConvertToExternal(float motorValue)
 
 static void pwmDisableMotors(void)
 {
-    pwmMotorDevice.enabled = false;
+    // NOOP
-}
-
-static bool pwmEnableMotors(void)
-{
-    pwmMotorDevice.enabled = true;
-
-    return true;
 }
 
 static void pwmWriteMotor(uint8_t index, float value)
@@ -594,12 +586,7 @@ static void pwmWriteMotorInt(uint8_t index, uint16_t value)
 
 static void pwmShutdownPulsesForAllMotors(void)
 {
-    pwmMotorDevice.enabled = false;
+    // NOOP
-}
-
-static bool pwmIsMotorEnabled(unsigned index)
-{
-    return motors[index].enabled;
 }
 
 static void pwmCompleteMotorUpdate(void)
@@ -647,7 +634,7 @@ static const motorVTable_t vTable = {
     .shutdown = pwmShutdownPulsesForAllMotors,
     .requestTelemetry = NULL,
     .isMotorIdle = NULL,
-
+    .getMotorIO = NULL,
 };
 
 bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t _idlePulse)

src/platform/STM32/pwm_output_hw.c

@@ -29,6 +29,7 @@
 
 #include "drivers/io.h"
 #include "drivers/pwm_output.h"
+#include "drivers/pwm_output_impl.h"
 #include "drivers/time.h"
 #include "drivers/timer.h"
 
@@ -128,17 +129,6 @@ static void pwmDisableMotors(void)
     pwmShutdownPulsesForAllMotors();
 }
 
-static bool pwmEnableMotors(void)
-{
-    /* check motors can be enabled */
-    return pwmMotorCount > 0;
-}
-
-static bool pwmIsMotorEnabled(unsigned index)
-{
-    return motors[index].enabled;
-}
-
 static void pwmCompleteMotorUpdate(void)
 {
     if (useContinuousUpdate) {
@@ -178,6 +168,7 @@ static const motorVTable_t motorPwmVTable = {
     .updateComplete = pwmCompleteMotorUpdate,
     .requestTelemetry = NULL,
     .isMotorIdle = NULL,
+    .getMotorIO = pwmGetMotorIO,
 };
 
 bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)

src/platform/common/stm32/dshot_dpwm.c

@@ -126,6 +126,14 @@ static bool dshotPwmIsMotorEnabled(unsigned index)
     return motors[index].enabled;
 }
 
+static IO_t pwmDshotGetMotorIO(unsigned index)
+{
+    if (index >= dshotMotorCount) {
+        return IO_NONE;
+    }
+    return motors[index].io;
+}
+
 static FAST_CODE void dshotWriteInt(uint8_t index, uint16_t value)
 {
     pwmWriteDshotInt(index, value);
@@ -150,6 +158,7 @@ static const motorVTable_t dshotPwmVTable = {
     .shutdown = dshotPwmShutdown,
     .requestTelemetry = pwmDshotRequestTelemetry,
     .isMotorIdle = pwmDshotIsMotorIdle,
+    .getMotorIO = pwmDshotGetMotorIO,
 };
 
 bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig)