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..78d96ad4cd6 100644
--- a/src/main/drivers/pwm_output.h
+++ b/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;
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 3e81b028de2..979c2937650 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,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:
diff --git a/src/main/fc/motor_locate.c b/src/main/fc/motor_locate.c
new file mode 100644
index 00000000000..61f6e6113c6
--- /dev/null
+++ b/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 .
+ */
+
+#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_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
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