Motor output reordering for CLI and MSP

Fixed some code formatting

+fixReorderingArray() for EEPROM load/save and renamings

fix brace new line

moving MOTOR_OUTPUT_REORDERING to MSP2 betaflight specific

validateAndfixConfig now resets reordering motor array to default in case it is invalid

src/main/cli/settings.c

@@ -827,7 +827,8 @@ const clivalue_t valueTable[] = {
     { "motor_pwm_protocol",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmProtocol) },
     { "motor_pwm_rate",             VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 32000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmRate) },
     { "motor_pwm_inversion",        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmInversion) },
-    { "motor_poles",                VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 4, UINT8_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, motorPoleCount) },
+    { "motor_poles",                VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 4, UINT8_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, motorPoleCount) },
+    { "motor_output_reordering",    VAR_UINT8  | MASTER_VALUE | MODE_ARRAY, .config.array.length = MAX_SUPPORTED_MOTORS, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorOutputReordering)},
 
 // PG_THROTTLE_CORRECTION_CONFIG
     { "thr_corr_value",             VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0,  150 }, PG_THROTTLE_CORRECTION_CONFIG, offsetof(throttleCorrectionConfig_t, throttle_correction_value) },

src/main/config/config.c

@@ -89,6 +89,8 @@
 
 #include "config.h"
 
+#include "drivers/dshot.h"
+
 static bool configIsDirty; /* someone indicated that the config is modified and it is not yet saved */
 
 static bool rebootRequired = false;  // set if a config change requires a reboot to take effect
@@ -596,6 +598,7 @@ static void validateAndFixConfig(void)
             }
         }
     }
+
 #if defined(USE_RX_MSP_OVERRIDE)
     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
         const modeActivationCondition_t *mac = modeActivationConditions(i);
@@ -604,6 +607,8 @@ static void validateAndFixConfig(void)
         }
     }
 #endif
+
+    validateAndfixMotorOutputReordering(motorConfigMutable()->dev.motorOutputReordering, MAX_SUPPORTED_MOTORS);
 }
 
 void validateAndFixGyroConfig(void)

src/main/drivers/dshot.c

@@ -164,3 +164,39 @@ void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool pac
 #endif // USE_DSHOT_TELEMETRY_STATS
 
 #endif // USE_DSHOT
+
+// temporarly here, needs to be moved during refactoring
+void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size)
+{
+    bool invalid = false;
+
+    for (unsigned i = 0; i < size; i++) {
+        if (array[i] >= size) {
+            invalid = true;
+            break;
+        }
+    }
+
+    int valuesAsIndexes[size];
+
+    for (unsigned i = 0; i < size; i++) {
+        valuesAsIndexes[i] = -1;
+    }
+
+    if (!invalid) {
+        for (unsigned i = 0; i < size; i++) {
+            if (-1 != valuesAsIndexes[array[i]]) {
+                invalid = true;
+                break;
+            }
+
+            valuesAsIndexes[array[i]] = array[i];
+        }
+    }
+
+    if (invalid) {
+        for (unsigned i = 0; i < size; i++) {
+            array[i] = i;
+        }
+    }
+}

src/main/drivers/dshot.h

@@ -90,3 +90,5 @@ bool isDshotMotorTelemetryActive(uint8_t motorIndex);
 bool isDshotTelemetryActive(void);
 
 int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex);
+
+void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size);

src/main/drivers/dshot_bitbang.c

@@ -650,8 +650,9 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
 #endif
 
     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
-        const timerHardware_t *timerHardware = timerGetByTag(motorConfig->ioTags[motorIndex]);
+        const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
-        const IO_t io = IOGetByTag(motorConfig->ioTags[motorIndex]);
+        const timerHardware_t *timerHardware = timerGetByTag(motorConfig->ioTags[reorderedMotorIndex]);
+        const IO_t io = IOGetByTag(motorConfig->ioTags[reorderedMotorIndex]);
 
         uint8_t output = motorConfig->motorPwmInversion ?  timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output;
         bbPuPdMode = (output & TIMER_OUTPUT_INVERTED) ? BB_GPIO_PULLDOWN : BB_GPIO_PULLUP;

src/main/drivers/dshot_dpwm.c

@@ -179,15 +179,17 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
     }
 
     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
-        const ioTag_t tag = motorConfig->ioTags[motorIndex];
+        const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
-        const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+        const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
+        const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 
         if (timerHardware != NULL) {
             motors[motorIndex].io = IOGetByTag(tag);
-            IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+            IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 
             if (pwmDshotMotorHardwareConfig(timerHardware,
                 motorIndex,
+                reorderedMotorIndex,
                 motorConfig->motorPwmProtocol,
                 motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output)) {
                 motors[motorIndex].enabled = true;

src/main/drivers/dshot_dpwm.h

@@ -157,7 +157,7 @@ typedef struct motorDmaOutput_s {
 motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
 
 void pwmWriteDshotInt(uint8_t index, uint16_t value);
-bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
+bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
 #ifdef USE_DSHOT_TELEMETRY
 bool pwmStartDshotMotorUpdate(void);
 #endif

src/main/drivers/pwm_output.c

@@ -219,8 +219,9 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
     motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
 
     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
-        const ioTag_t tag = motorConfig->ioTags[motorIndex];
+        const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
-        const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+        const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
+        const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 
         if (timerHardware == NULL) {
             /* not enough motors initialised for the mixer or a break in the motors */
@@ -231,7 +232,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
         }
 
         motors[motorIndex].io = IOGetByTag(tag);
-        IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+        IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 
 #if defined(STM32F1)
         IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);

src/main/drivers/pwm_output_dshot.c

@@ -224,7 +224,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
     }
 }
 
-bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
+bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 {
 #ifdef USE_DSHOT_TELEMETRY
 #define OCINIT motor->ocInitStruct
@@ -377,7 +377,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     } else
 #endif
     {
-        dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+        dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 
         motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
 

src/main/drivers/pwm_output_dshot_hal.c

@@ -197,7 +197,7 @@ FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
     }
 }
 
-bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
+bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 {
 #ifdef USE_DSHOT_TELEMETRY
 #define OCINIT motor->ocInitStruct
@@ -347,7 +347,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     } else
 #endif
     {
-        dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+        dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 
         motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
 

src/main/drivers/pwm_output_dshot_hal_hal.c

@@ -233,7 +233,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
     }
 }
 
-bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
+bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 {
     dmaResource_t *dmaRef = NULL;
     uint32_t dmaChannel;
@@ -285,7 +285,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     const uint8_t timerIndex = getTimerIndex(timer);
     const bool configureTimer = (timerIndex == dmaMotorTimerCount - 1);
 
-    IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+    IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
     IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
 
     // Configure time base
@@ -373,7 +373,7 @@ P    -    High -     High -
     } else
 #endif
     {
-        dmaInit(identifier, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+        dmaInit(identifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
         dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motorIndex);
     }
 

src/main/msp/msp.c

@@ -1190,6 +1190,16 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
         }
         break;
 
+    case MSP2_MOTOR_OUTPUT_REORDERING:
+        {
+            sbufWriteU8(dst, MAX_SUPPORTED_MOTORS);
+
+            for (unsigned i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
+                sbufWriteU8(dst, motorConfig()->dev.motorOutputReordering[i]);
+            }
+        }
+        break;
+
     case MSP_RC:
         for (int i = 0; i < rxRuntimeState.channelCount; i++) {
             sbufWriteU16(dst, rcData[i]);
@@ -2964,6 +2974,22 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         break;
 #endif
 
+    case MSP2_SET_MOTOR_OUTPUT_REORDERING:
+        {
+            const uint8_t arraySize = sbufReadU8(src);
+
+            for (unsigned i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
+                uint8_t value = i;
+
+                if (i < arraySize) {
+                    value = sbufReadU8(src);
+                }
+
+                motorConfigMutable()->dev.motorOutputReordering[i] = value;
+            }
+        }
+        break;
+
 #ifdef USE_CAMERA_CONTROL
     case MSP_CAMERA_CONTROL:
         {

src/main/msp/msp_protocol_v2_betaflight.h

@@ -18,4 +18,6 @@
  * If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define MSP2_BETAFLIGHT_BIND            0x3000
+#define MSP2_BETAFLIGHT_BIND                0x3000
+#define MSP2_MOTOR_OUTPUT_REORDERING        0x3001
+#define MSP2_SET_MOTOR_OUTPUT_REORDERING    0x3002

src/main/pg/motor.c

@@ -74,6 +74,10 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig)
 
     motorConfig->motorPoleCount = 14;   // Most brushes motors that we use are 14 poles
 
+    for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
+        motorConfig->dev.motorOutputReordering[i] = i;
+    }
+
 #ifdef USE_DSHOT_BITBANG
     motorConfig->dev.useDshotBitbang = DSHOT_BITBANG_DEFAULT;
     motorConfig->dev.useDshotBitbangedTimer = DSHOT_BITBANGED_TIMER_DEFAULT;

src/main/pg/motor.h

@@ -48,6 +48,7 @@ typedef struct motorDevConfig_s {
     uint8_t  motorTransportProtocol;
     uint8_t  useDshotBitbang;
     uint8_t  useDshotBitbangedTimer;
+    uint8_t  motorOutputReordering[MAX_SUPPORTED_MOTORS]; // Reindexing motors for "remap motors" feature in Configurator
 } motorDevConfig_t;
 
 typedef struct motorConfig_s {

src/test/Makefile

@@ -174,13 +174,17 @@ ledstrip_unittest_SRC := \
 		$(USER_DIR)/io/ledstrip.c
 
 ledstrip_unittest_DEFINES := \
-        USE_LED_STRIP=
+		USE_LED_STRIP=
-       
+
-       
+
 maths_unittest_SRC := \
 		$(USER_DIR)/common/maths.c
 
 
+motor_output_unittest_SRC := \
+		$(USER_DIR)/drivers/dshot.c
+
+
 osd_unittest_SRC := \
 		$(USER_DIR)/osd/osd.c \
 		$(USER_DIR)/osd/osd_elements.c \

src/test/unit/motor_output_unittest.cc

@@ -0,0 +1,79 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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/>.
+ */
+
+#include <stdint.h>
+#include <iostream>
+
+extern "C" {
+    #include "drivers/dshot.h"
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+TEST(MotorOutputUnittest, TestFixMotorOutputReordering)
+{
+    const unsigned size = 8;
+
+    uint8_t  a1_initial[size] = {0, 1, 2, 3, 4, 5, 6, 7};
+    uint8_t a1_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
+    validateAndfixMotorOutputReordering(a1_initial, size);
+    EXPECT_TRUE( 0 == memcmp(a1_expected, a1_initial, sizeof(a1_expected)));
+
+    uint8_t  a2_initial[size] = {3, 2, 1, 4, 5, 6, 7, 0};
+    uint8_t a2_expected[size] = {3, 2, 1, 4, 5, 6, 7, 0};
+    validateAndfixMotorOutputReordering(a2_initial, size);
+    EXPECT_TRUE( 0 == memcmp(a2_expected, a2_initial, sizeof(a2_expected)));
+
+    uint8_t  a3_initial[size] = {3, 2, 1, 100, 5, 6, 7, 0};
+    uint8_t a3_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
+    validateAndfixMotorOutputReordering(a3_initial, size);
+    EXPECT_TRUE( 0 == memcmp(a3_expected, a3_initial, sizeof(a3_expected)));
+
+    uint8_t  a4_initial[size] = {3, 2, 1, 100, 5, 6, 200, 0};
+    uint8_t a4_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
+    validateAndfixMotorOutputReordering(a4_initial, size);
+    EXPECT_TRUE( 0 == memcmp(a4_expected, a4_initial, sizeof(a4_expected)));
+
+    uint8_t  a5_initial[size] = {0, 0, 0, 0, 0, 0, 0, 0};
+    uint8_t a5_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
+    validateAndfixMotorOutputReordering(a5_initial, size);
+    EXPECT_TRUE( 0 == memcmp(a5_expected, a5_initial, sizeof(a5_expected)));
+
+    uint8_t  a6_initial[size] = {0, 0, 0, 1, 0, 99, 0, 0};
+    uint8_t a6_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
+    validateAndfixMotorOutputReordering(a6_initial, size);
+    EXPECT_TRUE( 0 == memcmp(a6_expected, a6_initial, sizeof(a5_expected)));
+
+    uint8_t  a7_initial[size] = {1, 5, 3, 4, 5, 6, 7, 0};
+    uint8_t a7_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
+    validateAndfixMotorOutputReordering(a7_initial, size);
+    EXPECT_TRUE( 0 == memcmp(a7_expected, a7_initial, sizeof(a7_expected)));
+
+    uint8_t  a8_initial[size] = {1, 5, 1, 5, 3, 3, 3, 3};
+    uint8_t a8_expected[size] = {0, 1, 2, 3, 4, 5, 6, 7};
+    validateAndfixMotorOutputReordering(a8_initial, size);
+    EXPECT_TRUE( 0 == memcmp(a8_expected, a8_initial, sizeof(a8_expected)));
+
+    uint8_t  a9_initial[size] = {7, 6, 5, 4, 3, 2, 1, 0};
+    uint8_t a9_expected[size] = {7, 6, 5, 4, 3, 2, 1, 0};
+    validateAndfixMotorOutputReordering(a9_initial, size);
+    EXPECT_TRUE( 0 == memcmp(a9_expected, a9_initial, sizeof(a9_expected)));
+}