Firmware support for improved mixer wizard (MSP2_INAV_MOTOR_LOCATE command for motor identification)

src/main/CMakeLists.txt

@@ -298,6 +298,8 @@ main_sources(COMMON_SRC
     fc/firmware_update.h
     fc/firmware_update_common.c
     fc/firmware_update_common.h
+    fc/motor_locate.c
+    fc/motor_locate.h
     fc/multifunction.c
     fc/multifunction.h
     fc/rc_smoothing.c

src/main/drivers/pwm_output.h

@@ -27,6 +27,12 @@
 #define MAX_PWM_OUTPUTS (MAX_PWM_OUTPUT_PORTS)
 #endif
 typedef enum {
+    DSHOT_CMD_MOTOR_STOP = 0,
+    DSHOT_CMD_BEACON1 = 1,
+    DSHOT_CMD_BEACON2 = 2,
+    DSHOT_CMD_BEACON3 = 3,
+    DSHOT_CMD_BEACON4 = 4,
+    DSHOT_CMD_BEACON5 = 5,
     DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
     DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
 } dshotCommands_e;

src/main/fc/fc_msp.c

@@ -68,6 +68,7 @@
 #include "fc/fc_msp.h"
 #include "fc/fc_msp_box.h"
 #include "fc/firmware_update.h"
+#include "fc/motor_locate.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
@@ -1727,6 +1728,26 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
 #endif
 
+#ifdef USE_DSHOT
+    case MSP2_INAV_MOTOR_LOCATE:
+        {
+            // Motor locate requires 1 byte: motor index (255 = stop)
+            if (dataSize < 1) {
+                return MSP_RESULT_ERROR;
+            }
+            uint8_t motorIndex = sbufReadU8(src);
+            bool success;
+            if (motorIndex == 255) {
+                motorLocateStop();
+                success = true;
+            } else {
+                success = motorLocateStart(motorIndex);
+            }
+            sbufWriteU8(dst, success ? 1 : 0);
+        }
+        break;
+#endif
+
 #ifdef USE_EZ_TUNE
 
     case MSP2_INAV_EZ_TUNE:

src/main/fc/motor_locate.c

@@ -0,0 +1,211 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it 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.
+ *
+ * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef USE_DSHOT
+
+#include "drivers/pwm_output.h"
+#include "drivers/time.h"
+#include "flight/mixer.h"
+#include "fc/motor_locate.h"
+#include "fc/runtime_config.h"
+
+// Timing constants (in microseconds)
+#define LOCATE_JERK_DURATION_US     100000  // 100ms motor jerk
+#define LOCATE_JERK_PAUSE_US        100000  // 100ms pause after jerk
+#define LOCATE_BEEP_ON_US            80000  // 80ms beep on
+#define LOCATE_BEEP_OFF_US           80000  // 80ms beep off
+#define LOCATE_CYCLE_DURATION_US   2000000  // 2 seconds total cycle
+
+// Motor throttle for jerk (~12% throttle)
+// DShot range: 48 (0%) to 2047 (100%)
+#define LOCATE_JERK_THROTTLE    288  // 48 + (1999 * 0.12) ≈ 288
+
+typedef enum {
+    LOCATE_STATE_IDLE,
+    LOCATE_STATE_JERK,
+    LOCATE_STATE_JERK_PAUSE,
+    LOCATE_STATE_BEEP_ON,
+    LOCATE_STATE_BEEP_OFF,
+} motorLocateState_e;
+
+#define LOCATE_NUM_BEEPS 4
+
+// Global flag for fast inline check in FAST_CODE path
+bool motorLocateActive = false;
+
+static struct {
+    motorLocateState_e state;
+    uint8_t motorIndex;
+    uint8_t beepCount;
+    timeUs_t stateStartTime;
+    timeUs_t cycleStartTime;
+} locateState = {
+    .state = LOCATE_STATE_IDLE,
+    .motorIndex = 0,
+    .beepCount = 0,
+    .stateStartTime = 0,
+    .cycleStartTime = 0,
+};
+
+// Forward declarations
+static void transitionToState(motorLocateState_e newState, timeUs_t now);
+static timeUs_t getStateDuration(motorLocateState_e state);
+
+bool motorLocateStart(uint8_t motorIndex)
+{
+    // Don't allow if already running
+    if (locateState.state != LOCATE_STATE_IDLE) {
+        return false;
+    }
+
+    // Validate motor index
+    if (motorIndex >= getMotorCount()) {
+        return false;
+    }
+
+    // Only allow when disarmed
+    if (ARMING_FLAG(ARMED)) {
+        return false;
+    }
+
+    // Only allow with DShot protocol
+    if (!isMotorProtocolDshot()) {
+        return false;
+    }
+
+    timeUs_t now = micros();
+    locateState.motorIndex = motorIndex;
+    locateState.beepCount = 0;
+    locateState.cycleStartTime = now;
+    transitionToState(LOCATE_STATE_JERK, now);
+    motorLocateActive = true;
+
+    return true;
+}
+
+void motorLocateStop(void)
+{
+    locateState.state = LOCATE_STATE_IDLE;
+    motorLocateActive = false;
+}
+
+bool motorLocateIsActive(void)
+{
+    return locateState.state != LOCATE_STATE_IDLE;
+}
+
+static void transitionToState(motorLocateState_e newState, timeUs_t now)
+{
+    locateState.state = newState;
+    locateState.stateStartTime = now;
+}
+
+static timeUs_t getStateDuration(motorLocateState_e state)
+{
+    switch (state) {
+        case LOCATE_STATE_JERK:
+            return LOCATE_JERK_DURATION_US;
+        case LOCATE_STATE_JERK_PAUSE:
+            return LOCATE_JERK_PAUSE_US;
+        case LOCATE_STATE_BEEP_ON:
+            return LOCATE_BEEP_ON_US;
+        case LOCATE_STATE_BEEP_OFF:
+            return (locateState.beepCount >= LOCATE_NUM_BEEPS - 1) ? LOCATE_JERK_PAUSE_US : LOCATE_BEEP_OFF_US;
+        default:
+            return 0;
+    }
+}
+
+static motorLocateState_e advanceToNextState(motorLocateState_e state)
+{
+    switch (state) {
+        case LOCATE_STATE_JERK:
+            return LOCATE_STATE_JERK_PAUSE;
+        case LOCATE_STATE_JERK_PAUSE:
+            locateState.beepCount = 0;
+            return LOCATE_STATE_BEEP_ON;
+        case LOCATE_STATE_BEEP_ON:
+            return LOCATE_STATE_BEEP_OFF;
+        case LOCATE_STATE_BEEP_OFF:
+            if (++locateState.beepCount >= LOCATE_NUM_BEEPS) {
+                return LOCATE_STATE_JERK;
+            }
+            return LOCATE_STATE_BEEP_ON;
+        default:
+            return LOCATE_STATE_IDLE;
+    }
+}
+
+static uint16_t getMotorValueForState(motorLocateState_e state, uint8_t motorIdx, uint8_t targetMotorIdx)
+{
+    if (motorIdx != targetMotorIdx) {
+        return DSHOT_CMD_MOTOR_STOP;
+    }
+
+    switch (state) {
+        case LOCATE_STATE_JERK:
+            return LOCATE_JERK_THROTTLE;
+        case LOCATE_STATE_BEEP_ON:
+            // Ascending tones help humans locate sound using multiple hearing mechanisms
+            return DSHOT_CMD_BEACON1 + locateState.beepCount;
+        default:
+            return DSHOT_CMD_MOTOR_STOP;
+    }
+}
+
+bool motorLocateUpdate(void)
+{
+    if (locateState.state == LOCATE_STATE_IDLE) {
+        return false;
+    }
+
+    // Immediately stop if aircraft becomes armed
+    if (ARMING_FLAG(ARMED)) {
+        motorLocateStop();
+        return false;
+    }
+
+    timeUs_t now = micros();
+
+    // Check if total cycle time exceeded
+    if (cmpTimeUs(now, locateState.cycleStartTime) >= LOCATE_CYCLE_DURATION_US) {
+        motorLocateStop();
+        return false;
+    }
+
+    // Check for state transition
+    timeUs_t stateDuration = getStateDuration(locateState.state);
+    if (cmpTimeUs(now, locateState.stateStartTime) >= stateDuration) {
+        transitionToState(advanceToNextState(locateState.state), now);
+    }
+
+    // Apply motor values for current state
+    uint8_t motorCount = getMotorCount();
+    for (uint8_t i = 0; i < motorCount; i++) {
+        uint16_t value = getMotorValueForState(locateState.state, i, locateState.motorIndex);
+        pwmWriteMotor(i, value);
+    }
+
+    return true;
+}
+
+#endif // USE_DSHOT

src/main/fc/motor_locate.h

@@ -0,0 +1,39 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it 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.
+ *
+ * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+// Global flag for fast inline check - avoids function call in FAST_CODE path
+extern bool motorLocateActive;
+
+// Start motor locate cycle for specified motor index
+// Runs jerk+beep pattern for ~2 seconds then stops automatically
+// Returns false if locate is already running or motor index invalid
+bool motorLocateStart(uint8_t motorIndex);
+
+// Stop any running motor locate cycle
+void motorLocateStop(void);
+
+// Check if motor locate is currently active
+bool motorLocateIsActive(void);
+
+// Called from motor update loop to apply locate overrides
+// Returns true if locate is active and motor values were overridden
+bool motorLocateUpdate(void);

src/main/flight/mixer.c

@@ -39,6 +39,7 @@
 #include "drivers/time.h"
 
 #include "fc/config.h"
+#include "fc/motor_locate.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
@@ -369,6 +370,12 @@ static void applyTurtleModeToMotors(void) {
 void FAST_CODE writeMotors(void)
 {
 #if !defined(SITL_BUILD)
+#ifdef USE_DSHOT
+    if (motorLocateActive && motorLocateUpdate()) {
+        return;
+    }
+#endif
+
     for (int i = 0; i < motorCount; i++) {
         uint16_t motorValue;
 #ifdef USE_DSHOT

src/main/msp/msp_protocol_v2_inav.h

@@ -93,6 +93,7 @@
 
 #define MSP2_INAV_ESC_RPM                       0x2040
 #define MSP2_INAV_ESC_TELEM                     0x2041
+#define MSP2_INAV_MOTOR_LOCATE                  0x2042
 
 #define MSP2_INAV_LED_STRIP_CONFIG_EX           0x2048
 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX       0x2049