From 11994e6ab81e05139c8abaab041f8d900e345e6d Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Wed, 14 Aug 2019 23:49:25 +0900
Subject: [PATCH] Isolate MSP_MOTOR from pwm_output (pwmGetMotors)

---
 src/main/drivers/dshot_dpwm.c | 6 ++++++
 src/main/drivers/motor.c      | 5 +++++
 src/main/drivers/motor.h      | 2 ++
 src/main/drivers/pwm_output.c | 6 ++++++
 src/main/msp/msp.c            | 4 ++--
 5 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/dshot_dpwm.c b/src/main/drivers/dshot_dpwm.c
index 70f233f0e..f12ed200b 100644
--- a/src/main/drivers/dshot_dpwm.c
+++ b/src/main/drivers/dshot_dpwm.c
@@ -122,6 +122,11 @@ static bool dshotPwmEnableMotors(void)
     return true;
 }
 
+static bool dshotPwmIsMotorEnabled(uint8_t index)
+{
+    return motors[index].enabled;
+}
+
 static FAST_CODE void dshotWriteInt(uint8_t index, uint16_t value)
 {
     pwmWriteDshotInt(index, value);
@@ -135,6 +140,7 @@ static FAST_CODE void dshotWrite(uint8_t index, float value)
 static motorVTable_t dshotPwmVTable = {
     .enable = dshotPwmEnableMotors,
     .disable = dshotPwmDisableMotors,
+    .isMotorEnabled = dshotPwmIsMotorEnabled,
     .updateStart = motorUpdateStartNull, // May be updated after copying
     .write = dshotWrite,
     .writeInt = dshotWriteInt,
diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c
index 96e96a3eb..12fd331b5 100644
--- a/src/main/drivers/motor.c
+++ b/src/main/drivers/motor.c
@@ -246,6 +246,11 @@ bool motorIsEnabled(void)
     return motorDevice->enabled;
 }
 
+bool motorIsMotorEnabled(uint8_t index)
+{
+    return motorDevice->vTable.isMotorEnabled(index);
+}
+
 bool isMotorProtocolDshot(void)
 {
     return isDshot;
diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h
index f3692471e..5e2ba2682 100644
--- a/src/main/drivers/motor.h
+++ b/src/main/drivers/motor.h
@@ -45,6 +45,7 @@ typedef struct motorVTable_s {
     uint16_t (*convertMotorToExternal)(float motorValue);
     bool (*enable)(void);
     void (*disable)(void);
+    bool (*isMotorEnabled)(uint8_t index);
     bool (*updateStart)(void);
     void (*write)(uint8_t index, float value);
     void (*writeInt)(uint8_t index, uint16_t value);
@@ -80,4 +81,5 @@ bool isMotorProtocolDshot(void);
 void motorDisable(void);
 void motorEnable(void);
 bool motorIsEnabled(void);
+bool motorIsMotorEnabled(uint8_t index);
 void motorShutdown(void); // Replaces stopPwmAllMotors
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index f04ec5281..736a46a67 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -144,6 +144,11 @@ bool pwmEnableMotors(void)
     return (motorPwmVTable.write != &pwmWriteUnused);
 }
 
+bool pwmIsMotorEnabled(uint8_t index)
+{
+    return motors[index].enabled;
+}
+
 static void pwmCompleteOneshotMotorUpdate(void)
 {
     for (int index = 0; index < motorPwmDevice.count; index++) {
@@ -169,6 +174,7 @@ static uint16_t pwmConvertToExternal(float motorValue)
 static motorVTable_t motorPwmVTable = {
     .enable = pwmEnableMotors,
     .disable = pwmDisableMotors,
+    .isMotorEnabled = pwmIsMotorEnabled,
     .shutdown = pwmShutdownPulsesForAllMotors,
     .convertExternalToMotor = pwmConvertFromExternal,
     .convertMotorToExternal = pwmConvertToExternal,
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index c4fd8dc58..1e1b8ea55 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -962,8 +962,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
 
     case MSP_MOTOR:
         for (unsigned i = 0; i < 8; i++) {
-#ifdef USE_PWM_OUTPUT
-            if (i >= MAX_SUPPORTED_MOTORS || !pwmGetMotors()[i].enabled) {
+#ifdef USE_MOTOR
+            if (i >= MAX_SUPPORTED_MOTORS || !motorIsMotorEnabled(i)) {
                 sbufWriteU16(dst, 0);
                 continue;
             }