From 7628c25294e04995c2f8990c12f66049a77ecec3 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 7 Jan 2026 12:27:34 -0600 Subject: [PATCH 1/4] Add MSP2_INAV_MOTOR_LOCATE command for motor identification Add new MSP command (0x2042) that spins a specific motor for identification: - New motor_locate module with state machine for jerk + beep pattern - 2-second cycle: 100ms motor jerk at 12% throttle, then 3 beeps - Only works when disarmed and with DShot protocol - Integrates with mixer to override motor outputs during locate Used by configurator's motor wizard to help users identify motor positions. --- src/main/CMakeLists.txt | 2 + src/main/drivers/pwm_output.h | 9 ++ src/main/fc/fc_msp.c | 15 ++ src/main/fc/motor_locate.c | 207 ++++++++++++++++++++++++++++ src/main/fc/motor_locate.h | 39 ++++++ src/main/flight/mixer.c | 7 + src/main/msp/msp_protocol_v2_inav.h | 1 + 7 files changed, 280 insertions(+) create mode 100644 src/main/fc/motor_locate.c create mode 100644 src/main/fc/motor_locate.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 101c7223372..7e2cb1306b8 100755 --- a/src/main/CMakeLists.txt +++ b/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 diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 1041ace04fa..b21f64019c2 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -27,10 +27,18 @@ #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; +#define DSHOT_CMD_ALL_MOTORS 255 + typedef struct { dshotCommands_e cmd; int remainingRepeats; @@ -61,6 +69,7 @@ void pwmWriteBeeper(bool onoffBeep); void beeperPwmInit(ioTag_t tag, uint16_t frequency); void sendDShotCommand(dshotCommands_e cmd); +void sendDShotCommandToMotor(uint8_t motorIndex, dshotCommands_e cmd); void initDShotCommands(void); uint32_t getEscUpdateFrequency(void); \ No newline at end of file diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3e81b028de2..abba65382b5 100644 --- a/src/main/fc/fc_msp.c +++ b/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,20 @@ 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 + if (dataSize < 1) { + return MSP_RESULT_ERROR; + } + uint8_t motorIndex = sbufReadU8(src); + bool success = motorLocateStart(motorIndex); + sbufWriteU8(dst, success ? 1 : 0); + } + break; +#endif + #ifdef USE_EZ_TUNE case MSP2_INAV_EZ_TUNE: diff --git a/src/main/fc/motor_locate.c b/src/main/fc/motor_locate.c new file mode 100644 index 00000000000..e5271f6d99b --- /dev/null +++ b/src/main/fc/motor_locate.c @@ -0,0 +1,207 @@ +/* + * 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 . + */ + +#include +#include + +#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_BEEP1_ON, + LOCATE_STATE_BEEP1_OFF, + LOCATE_STATE_BEEP2_ON, + LOCATE_STATE_BEEP2_OFF, + LOCATE_STATE_BEEP3_ON, + LOCATE_STATE_BEEP3_PAUSE, +} motorLocateState_e; + +// Global flag for fast inline check in FAST_CODE path +bool motorLocateActive = false; + +static struct { + motorLocateState_e state; + uint8_t motorIndex; + timeUs_t stateStartTime; + timeUs_t cycleStartTime; +} locateState = { + .state = LOCATE_STATE_IDLE, + .motorIndex = 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.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_BEEP1_ON: + case LOCATE_STATE_BEEP2_ON: + case LOCATE_STATE_BEEP3_ON: + return LOCATE_BEEP_ON_US; + case LOCATE_STATE_BEEP1_OFF: + case LOCATE_STATE_BEEP2_OFF: + return LOCATE_BEEP_OFF_US; + case LOCATE_STATE_BEEP3_PAUSE: + return LOCATE_JERK_PAUSE_US; + default: + return 0; + } +} + +static motorLocateState_e getNextState(motorLocateState_e state) +{ + switch (state) { + case LOCATE_STATE_JERK: return LOCATE_STATE_JERK_PAUSE; + case LOCATE_STATE_JERK_PAUSE: return LOCATE_STATE_BEEP1_ON; + case LOCATE_STATE_BEEP1_ON: return LOCATE_STATE_BEEP1_OFF; + case LOCATE_STATE_BEEP1_OFF: return LOCATE_STATE_BEEP2_ON; + case LOCATE_STATE_BEEP2_ON: return LOCATE_STATE_BEEP2_OFF; + case LOCATE_STATE_BEEP2_OFF: return LOCATE_STATE_BEEP3_ON; + case LOCATE_STATE_BEEP3_ON: return LOCATE_STATE_BEEP3_PAUSE; + case LOCATE_STATE_BEEP3_PAUSE: return LOCATE_STATE_JERK; // Loop back + default: return LOCATE_STATE_IDLE; + } +} + +static uint16_t getMotorValueForState(motorLocateState_e state, uint8_t motorIdx, uint8_t targetMotorIdx) +{ + // All motors except target get MOTOR_STOP + if (motorIdx != targetMotorIdx) { + return DSHOT_CMD_MOTOR_STOP; + } + + // Target motor value depends on state + switch (state) { + case LOCATE_STATE_JERK: + return LOCATE_JERK_THROTTLE; + case LOCATE_STATE_BEEP1_ON: + case LOCATE_STATE_BEEP2_ON: + case LOCATE_STATE_BEEP3_ON: + return DSHOT_CMD_BEACON1; + default: + return DSHOT_CMD_MOTOR_STOP; + } +} + +bool motorLocateUpdate(void) +{ + if (locateState.state == LOCATE_STATE_IDLE) { + 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(getNextState(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 diff --git a/src/main/fc/motor_locate.h b/src/main/fc/motor_locate.h new file mode 100644 index 00000000000..9319fdf5bd5 --- /dev/null +++ b/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 . + */ + +#pragma once + +#include +#include + +// 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); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80992b772d..0f75f45be68 100644 --- a/src/main/flight/mixer.c +++ b/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 diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 0b893916895..0df7d977c51 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/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 From 2bfa7ba8dce62d59132a2f3ca3db8d2b567e15f6 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 7 Jan 2026 13:40:53 -0600 Subject: [PATCH 2/4] Remove unused DSHOT_CMD_ALL_MOTORS and sendDShotCommandToMotor declaration --- src/main/drivers/pwm_output.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index b21f64019c2..78d96ad4cd6 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -37,8 +37,6 @@ typedef enum { DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21, } dshotCommands_e; -#define DSHOT_CMD_ALL_MOTORS 255 - typedef struct { dshotCommands_e cmd; int remainingRepeats; @@ -69,7 +67,6 @@ void pwmWriteBeeper(bool onoffBeep); void beeperPwmInit(ioTag_t tag, uint16_t frequency); void sendDShotCommand(dshotCommands_e cmd); -void sendDShotCommandToMotor(uint8_t motorIndex, dshotCommands_e cmd); void initDShotCommands(void); uint32_t getEscUpdateFrequency(void); \ No newline at end of file From abf3a19a91630f6227ff4b3b80ac5f6aab330fe2 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 7 Jan 2026 14:29:30 -0600 Subject: [PATCH 3/4] Improve motor locate beacon pattern for better audible localization - Use ascending tones (BEACON1-4) instead of single repeated tone - Ascending sweep leverages multiple human hearing mechanisms: low freq for ITD, high freq for ILD localization - Simplify state machine using beepCount instead of separate states - Rename getNextState to advanceToNextState (has side effects) - Add safety check: immediately stop if aircraft becomes armed --- src/main/fc/motor_locate.c | 66 ++++++++++++++++++++------------------ 1 file changed, 35 insertions(+), 31 deletions(-) diff --git a/src/main/fc/motor_locate.c b/src/main/fc/motor_locate.c index e5271f6d99b..61f6e6113c6 100644 --- a/src/main/fc/motor_locate.c +++ b/src/main/fc/motor_locate.c @@ -43,25 +43,25 @@ typedef enum { LOCATE_STATE_IDLE, LOCATE_STATE_JERK, LOCATE_STATE_JERK_PAUSE, - LOCATE_STATE_BEEP1_ON, - LOCATE_STATE_BEEP1_OFF, - LOCATE_STATE_BEEP2_ON, - LOCATE_STATE_BEEP2_OFF, - LOCATE_STATE_BEEP3_ON, - LOCATE_STATE_BEEP3_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, }; @@ -94,6 +94,7 @@ bool motorLocateStart(uint8_t motorIndex) timeUs_t now = micros(); locateState.motorIndex = motorIndex; + locateState.beepCount = 0; locateState.cycleStartTime = now; transitionToState(LOCATE_STATE_JERK, now); motorLocateActive = true; @@ -125,50 +126,47 @@ static timeUs_t getStateDuration(motorLocateState_e state) return LOCATE_JERK_DURATION_US; case LOCATE_STATE_JERK_PAUSE: return LOCATE_JERK_PAUSE_US; - case LOCATE_STATE_BEEP1_ON: - case LOCATE_STATE_BEEP2_ON: - case LOCATE_STATE_BEEP3_ON: + case LOCATE_STATE_BEEP_ON: return LOCATE_BEEP_ON_US; - case LOCATE_STATE_BEEP1_OFF: - case LOCATE_STATE_BEEP2_OFF: - return LOCATE_BEEP_OFF_US; - case LOCATE_STATE_BEEP3_PAUSE: - return LOCATE_JERK_PAUSE_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 getNextState(motorLocateState_e state) +static motorLocateState_e advanceToNextState(motorLocateState_e state) { switch (state) { - case LOCATE_STATE_JERK: return LOCATE_STATE_JERK_PAUSE; - case LOCATE_STATE_JERK_PAUSE: return LOCATE_STATE_BEEP1_ON; - case LOCATE_STATE_BEEP1_ON: return LOCATE_STATE_BEEP1_OFF; - case LOCATE_STATE_BEEP1_OFF: return LOCATE_STATE_BEEP2_ON; - case LOCATE_STATE_BEEP2_ON: return LOCATE_STATE_BEEP2_OFF; - case LOCATE_STATE_BEEP2_OFF: return LOCATE_STATE_BEEP3_ON; - case LOCATE_STATE_BEEP3_ON: return LOCATE_STATE_BEEP3_PAUSE; - case LOCATE_STATE_BEEP3_PAUSE: return LOCATE_STATE_JERK; // Loop back - default: return LOCATE_STATE_IDLE; + 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) { - // All motors except target get MOTOR_STOP if (motorIdx != targetMotorIdx) { return DSHOT_CMD_MOTOR_STOP; } - // Target motor value depends on state switch (state) { case LOCATE_STATE_JERK: return LOCATE_JERK_THROTTLE; - case LOCATE_STATE_BEEP1_ON: - case LOCATE_STATE_BEEP2_ON: - case LOCATE_STATE_BEEP3_ON: - return DSHOT_CMD_BEACON1; + 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; } @@ -180,6 +178,12 @@ bool motorLocateUpdate(void) 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 @@ -191,7 +195,7 @@ bool motorLocateUpdate(void) // Check for state transition timeUs_t stateDuration = getStateDuration(locateState.state); if (cmpTimeUs(now, locateState.stateStartTime) >= stateDuration) { - transitionToState(getNextState(locateState.state), now); + transitionToState(advanceToNextState(locateState.state), now); } // Apply motor values for current state From 8d4c6ce4f13185400e3d72574d2c110c96214111 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 7 Jan 2026 15:39:25 -0600 Subject: [PATCH 4/4] Add motor locate stop command (index 255) Sending motor index 255 to MSP2_INAV_MOTOR_LOCATE now explicitly stops any running motor locate sequence. This allows the configurator to cleanly cancel the operation. --- src/main/fc/fc_msp.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index abba65382b5..979c2937650 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1731,12 +1731,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #ifdef USE_DSHOT case MSP2_INAV_MOTOR_LOCATE: { - // Motor locate requires 1 byte: motor index + // Motor locate requires 1 byte: motor index (255 = stop) if (dataSize < 1) { return MSP_RESULT_ERROR; } uint8_t motorIndex = sbufReadU8(src); - bool success = motorLocateStart(motorIndex); + bool success; + if (motorIndex == 255) { + motorLocateStop(); + success = true; + } else { + success = motorLocateStart(motorIndex); + } sbufWriteU8(dst, success ? 1 : 0); } break;