diff --git a/docs/Navigation.md b/docs/Navigation.md
index 4d7b9740d91..a1d32e7d4d6 100755
--- a/docs/Navigation.md
+++ b/docs/Navigation.md
@@ -38,6 +38,70 @@ PID meaning:
* POS - translated position error to desired velocity, uses P term only
* POSR - translates velocity error to desired acceleration
+## Marker Guidance Target Consumer (MSP)
+
+INAV can consume externally computed marker offsets over MSP and use them for:
+1. precision landing alignment
+2. marker-relative position hold in POSHOLD
+3. marker-relative containment (indoor limiter behavior)
+
+### Build-time availability
+This feature is compiled only when `USE_MARKER_GUIDANCE` is enabled for the target.
+On flash-constrained targets, it can be excluded at build time to preserve headroom.
+
+### MSP payload
+`MSP2_INAV_SET_MARKER_GUIDANCE_TARGET` request payload is 4 bytes:
+* `int16_t offsetForwardCm`
+* `int16_t offsetRightCm`
+
+* every received packet is treated as a fresh target sample
+* target freshness is based on FC receive time (`nav_marker_guidance_max_target_age_ms`)
+
+### Mode gating
+Marker guidance can influence navigation only when:
+* active profile is MC/VTOL-hover-capable
+* `nav_marker_guidance_mode` is not `OFF`
+
+Outside those contexts, updates may still be cached but do not affect navigation loops.
+
+### POSHOLD behavior
+When `nav_marker_guidance_mode = PL`:
+* FC uses marker offsets to center above the target in POSHOLD.
+
+When `nav_marker_guidance_mode = CONTAINMENT`:
+* FC uses marker-relative hold target:
+ * `nav_marker_containment_hold_north_cm`
+ * `nav_marker_containment_hold_east_cm`
+* FC applies containment behavior with `nav_marker_guidance_radius_cm`:
+ * inside radius: no correction
+ * outside radius: FC corrects back toward allowed boundary
+
+### LAND behavior
+When `nav_marker_guidance_mode = PL` and target is fresh:
+* FC performs precision horizontal alignment to marker center during LAND
+* vertical descent profile remains normal LAND behavior (`nav_land_*`)
+
+With stale/lost target:
+* FC enters hold for `nav_marker_guidance_lost_hold_time_ms`
+* optionally performs climb-and-retry up to `nav_marker_guidance_retry_count`
+* then falls back to normal LAND behavior
+
+Retry safety rule:
+* retry is only entered if target was acquired at least once in the current LAND context
+* if no target was ever acquired in that LAND context, no retry is performed
+
+### Shared radius setting
+`nav_marker_guidance_radius_cm` is used by both modes:
+* `PL`: center-alignment deadband around marker center
+* `CONTAINMENT`: allowed radius around marker-containment hold target
+* `0`: continuous correction (no deadband/boundary allowance)
+
+### Core safety semantics
+* new packet == fresh target sample
+* no packet inside timeout window == target lost
+* horizontal marker-guidance correction is capped by current active navigation speed limit (`getActiveSpeed()`)
+* no dynamic allocation in the runtime path
+
## NAV RTH - return to home mode
Home for RTH is the position where vehicle was first armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. This position may also be overridden with Safehomes. RTH requires accelerometer, compass and GPS sensors.
diff --git a/docs/Settings.md b/docs/Settings.md
index d49067357bc..0b599d8c3eb 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -4434,6 +4434,118 @@ If GPS fails wait for this much seconds before switching to emergency landing mo
---
+### nav_marker_guidance_mode
+
+Marker-guidance mode selector (master enable gate). `OFF` disables marker guidance.
+
+| Allowed Values | |
+| --- | --- |
+| OFF | Default |
+| PL | |
+| CONTAINMENT | |
+
+---
+
+### nav_marker_guidance_radius_cm
+
+Marker-guidance radius [cm]. In PL, this is the center-alignment deadband around marker center. In CONTAINMENT, this is the allowed radius around marker-containment hold target. Set 0 for continuous correction.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 80 | 0 | 5000 |
+
+---
+
+### nav_marker_guidance_lost_hold_time_ms
+
+Hold duration after target loss before falling back to normal LAND behavior [ms].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1500 | 100 | 10000 |
+
+---
+
+### nav_marker_containment_hold_north_cm
+
+Marker-relative hold target for vehicle North position [cm], relative to marker. Positive means vehicle should stay North of marker.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -5000 | 5000 |
+
+---
+
+### nav_marker_containment_hold_east_cm
+
+Marker-relative hold target for vehicle East position [cm], relative to marker. Positive means vehicle should stay East of marker.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -5000 | 5000 |
+
+---
+
+### nav_marker_guidance_max_offset_cm
+
+Maximum allowed horizontal target offset magnitude. Larger offsets are rejected [cm]. Set 0 to disable this check.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1500 | 0 | 5000 |
+
+---
+
+### nav_marker_guidance_max_target_age_ms
+
+Maximum age of cached marker data [ms]. If no fresh packet arrives inside this window, target becomes stale/lost and marker guidance stops affecting navigation.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 500 | 50 | 5000 |
+
+---
+
+### nav_marker_guidance_retry_altitude_cm
+
+Climb distance for each retry attempt after target loss in PL mode LAND context [cm].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 200 | 50 | 2000 |
+
+---
+
+### nav_marker_guidance_retry_count
+
+Maximum number of climb-and-retry attempts after target loss in PL mode LAND context.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 2 | 0 | 10 |
+
+---
+
+### nav_marker_guidance_retry_timeout_ms
+
+Timeout for each climb-and-retry phase in PL mode LAND context [ms]. Set to 0 for AUTO mode (computed as 2 x nav_marker_guidance_lost_hold_time_ms).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 60000 |
+
+---
+
+### nav_marker_guidance_source
+
+Marker-guidance target source.
+
+| Allowed Values | |
+| --- | --- |
+| MSP | Default |
+
+---
+
### nav_rth_abort_threshold
RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. Set to 0 to disable. [cm]
@@ -7127,4 +7239,3 @@ Defines rotation rate on YAW axis that UAV will try to archive on max. stick def
| 20 | 1 | 180 |
---
-
diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md
index a09b622b651..291b35b63af 100644
--- a/docs/development/msp/README.md
+++ b/docs/development/msp/README.md
@@ -449,6 +449,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version:
[8731 - MSP2_INAV_NAV_TARGET](#msp2_inav_nav_target)
[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose)
[8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index)
+[8753 - MSP2_INAV_SET_MARKER_GUIDANCE_TARGET](#msp2_inav_set_marker_guidance_target)
[8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading)
[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind)
[12289 - MSP2_RX_BIND](#msp2_rx_bind)
@@ -4698,6 +4699,26 @@ When the MSP JSON specification changes, bump `msp_messages.json` version:
**Notes:** Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target.
+## `MSP2_INAV_SET_MARKER_GUIDANCE_TARGET (8753 / 0x2231)`
+**Description:** Updates the external marker target sample used by marker-guidance NAV correction. This message does not arm/disarm, does not switch flight modes, and does not start landing by itself.
+
+**Request Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `offset_forward_cm` | `int16_t` | 2 | cm | Marker offset forward from vehicle body origin |
+| `offset_right_cm` | `int16_t` | 2 | cm | Marker offset right from vehicle body origin |
+
+**Reply Payload:**
+|Field|C Type|Size (Bytes)|Description|
+|---|---|---|---|
+| `accepted` | `uint8_t` | 1 | Payload accepted into target-processing path |
+| `used_now` | `uint8_t` | 1 | Target is currently influencing navigation correction |
+| `nav_guidance_state` | `uint8_t` | 1 | Internal marker-guidance state |
+| `reason` | `uint8_t` | 1 | Result reason (`OK`, `NOT_ENABLED`, `STALE`, `OFFSET_TOO_LARGE`, `NOT_MC_PROFILE`, `NOT_IN_POSHOLD_OR_LAND`, etc.) |
+| `retry_count` | `uint8_t` | 1 | Current retry-attempt counter in PL LAND retry flow |
+
+**Notes:** Hard break: request payload is now fixed at **4 bytes** (`int16_t`, `int16_t`). Legacy request fields `valid`, `confidence`, `frame`, `timestamp_ms`, and `distance_cm` were removed. Companion implementations must migrate to the new layout. Available only when firmware is built with `USE_MARKER_GUIDANCE`. Corrections are mode-gated to MC/VTOL-hover-capable POSHOLD/LAND contexts; outside those contexts updates may still be cached (`used_now = 0`). Freshness is evaluated using FC receive time.
+
## `MSP2_INAV_SET_CRUISE_HEADING (8739 / 0x2223)`
**Description:** Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading.
@@ -4735,4 +4756,3 @@ When the MSP JSON specification changes, bump `msp_messages.json` version:
| `reserved_for_custom_use` | `uint8_t[3]` | 3 | Reserved for custom use |
**Notes:** Requires a receiver using MSP as the protocol, sends MSP2_RX_BIND to the receiver.
-
diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index 813575ba8bc..5cf7cbd8282 100644
--- a/docs/development/msp/msp_messages.json
+++ b/docs/development/msp/msp_messages.json
@@ -2,7 +2,7 @@
"version": {
"major": 2,
"minor": 0,
- "patch": 0
+ "patch": 1
},
"messages": {
"MSP_API_VERSION": {
@@ -11260,6 +11260,62 @@
"notes": "Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target.",
"description": "Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint."
},
+ "MSP2_INAV_SET_MARKER_GUIDANCE_TARGET": {
+ "code": 8753,
+ "mspv": 2,
+ "request": {
+ "payload": [
+ {
+ "name": "offset_forward_cm",
+ "ctype": "int16_t",
+ "desc": "Marker forward offset relative to vehicle body frame.",
+ "units": "cm"
+ },
+ {
+ "name": "offset_right_cm",
+ "ctype": "int16_t",
+ "desc": "Marker right offset relative to vehicle body frame.",
+ "units": "cm"
+ }
+ ]
+ },
+ "reply": {
+ "payload": [
+ {
+ "name": "accepted",
+ "ctype": "uint8_t",
+ "desc": "Payload accepted by marker-guidance target consumer.",
+ "units": ""
+ },
+ {
+ "name": "used_now",
+ "ctype": "uint8_t",
+ "desc": "Target is currently influencing navigation correction.",
+ "units": ""
+ },
+ {
+ "name": "nav_guidance_state",
+ "ctype": "uint8_t",
+ "desc": "Current internal marker-guidance state.",
+ "units": "enum"
+ },
+ {
+ "name": "reason",
+ "ctype": "uint8_t",
+ "desc": "Processing reason/result code.",
+ "units": "enum"
+ },
+ {
+ "name": "retry_count",
+ "ctype": "uint8_t",
+ "desc": "Current retry-attempt counter in PL LAND retry flow.",
+ "units": ""
+ }
+ ]
+ },
+ "notes": "Hard-break payload format: request size is now exactly 4 bytes (`int16_t`, `int16_t`). Legacy fields (`valid`, `confidence`, `frame`, `timestamp_ms`, `distance_cm`) were removed. Every received packet is treated as a fresh target sample; freshness timeout is evaluated using FC receive time. This message updates marker target cache only. It does not arm/disarm, does not switch modes, and does not trigger landing by itself. Available only when firmware is built with USE_MARKER_GUIDANCE. Navigation correction influence is mode-gated to MC/VTOL-hover-capable POSHOLD/LAND contexts.",
+ "description": "Updates the external marker-relative target sample consumed by marker-guidance NAV correction."
+ },
"MSP2_INAV_SET_CRUISE_HEADING": {
"code": 8739,
"mspv": 2,
@@ -11354,4 +11410,4 @@
"description": "Initiates binding for MSP receivers (mLRS)."
}
}
-}
\ No newline at end of file
+}
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 4df7bf3e21c..00093f41403 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -585,6 +585,8 @@ main_sources(COMMON_SRC
navigation/navigation_fw_launch.c
navigation/navigation_geo.c
navigation/navigation_multicopter.c
+ navigation/marker_guidance.c
+ navigation/marker_guidance.h
navigation/navigation_pos_estimator.c
navigation/navigation_pos_estimator_private.h
navigation/navigation_pos_estimator_agl.c
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index d17512964a4..230dc8c0c34 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -109,6 +109,9 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h" //for MSP_SIMULATOR
+#ifdef USE_MARKER_GUIDANCE
+#include "navigation/marker_guidance.h"
+#endif
#include "navigation/navigation_pos_estimator_private.h" //for MSP_SIMULATOR
#include "rx/rx.h"
@@ -4367,6 +4370,34 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
break;
#endif
+#ifdef USE_MARKER_GUIDANCE
+ case MSP2_INAV_SET_MARKER_GUIDANCE_TARGET:
+ if (dataSize != (2 * sizeof(int16_t))) {
+ *ret = MSP_RESULT_ERROR;
+ break;
+ }
+ {
+ markerGuidanceTargetUpdate_t update = {
+ .offsetForwardCm = (int16_t)sbufReadU16(src),
+ .offsetRightCm = (int16_t)sbufReadU16(src),
+ };
+
+ markerGuidanceMspResponse_t response = { 0 };
+ if (!markerGuidanceHandleMspTargetUpdate(&update, &response)) {
+ *ret = MSP_RESULT_ERROR;
+ break;
+ }
+
+ sbufWriteU8(dst, response.accepted);
+ sbufWriteU8(dst, response.usedNow);
+ sbufWriteU8(dst, response.navGuidanceState);
+ sbufWriteU8(dst, response.reason);
+ sbufWriteU8(dst, response.retryCount);
+ *ret = MSP_RESULT_ACK;
+ }
+ break;
+#endif
+
case MSP2_INAV_SET_LOCAL_TARGET:
if (dataSize != 3 * sizeof(int32_t) || !isGCSValid()) {
*ret = MSP_RESULT_ERROR;
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index c53ef66852f..6f59cabe8b8 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -168,6 +168,10 @@ tables:
- name: nav_rth_climb_first
enum: navRTHClimbFirst_e
values: ["OFF", "ON", "ON_FW_SPIRAL"]
+ - name: nav_marker_guidance_source
+ values: ["MSP"]
+ - name: nav_marker_guidance_mode
+ values: ["OFF", "PL", "CONTAINMENT"]
- name: nav_wp_mission_restart
enum: navMissionRestart_e
values: ["START", "RESUME", "SWITCH"]
@@ -2759,6 +2763,81 @@ groups:
min: 0
max: 1800
field: general.rth_fs_landing_delay
+ - name: nav_marker_guidance_source
+ description: "Marker-guidance target source."
+ default_value: "MSP"
+ field: general.marker_guidance_source
+ table: nav_marker_guidance_source
+ condition: USE_MARKER_GUIDANCE
+ - name: nav_marker_guidance_mode
+ description: "Marker-guidance mode selector: OFF disables guidance, PL enables precision-centering for LAND/POSHOLD with retry-on-loss behavior in LAND, CONTAINMENT enables marker-relative POSHOLD containment using hold north/east and radius."
+ default_value: "OFF"
+ field: general.marker_guidance_mode
+ table: nav_marker_guidance_mode
+ condition: USE_MARKER_GUIDANCE
+ - name: nav_marker_guidance_max_target_age_ms
+ description: "Maximum age of cached marker data [ms]. If no fresh packet arrives inside this window, target becomes stale/lost and marker guidance stops affecting navigation."
+ default_value: 500
+ field: general.marker_guidance_max_target_age_ms
+ condition: USE_MARKER_GUIDANCE
+ min: 50
+ max: 5000
+ - name: nav_marker_guidance_max_offset_cm
+ description: "Maximum allowed horizontal target offset magnitude. Larger offsets are rejected [cm]. Set 0 to disable this check."
+ default_value: 1500
+ field: general.marker_guidance_max_offset_cm
+ condition: USE_MARKER_GUIDANCE
+ min: 0
+ max: 5000
+ - name: nav_marker_guidance_radius_cm
+ description: "Marker-guidance radius [cm]. In PL, this is the center-alignment deadband around marker center. In CONTAINMENT, this is the allowed radius around marker-containment hold target. Set 0 for continuous correction."
+ default_value: 80
+ field: general.marker_guidance_radius_cm
+ condition: USE_MARKER_GUIDANCE
+ min: 0
+ max: 5000
+ - name: nav_marker_containment_hold_north_cm
+ description: "Marker-relative hold target for vehicle North position [cm], relative to marker. Positive means vehicle should stay North of marker."
+ default_value: 0
+ field: general.marker_containment_hold_north_cm
+ condition: USE_MARKER_GUIDANCE
+ min: -5000
+ max: 5000
+ - name: nav_marker_containment_hold_east_cm
+ description: "Marker-relative hold target for vehicle East position [cm], relative to marker. Positive means vehicle should stay East of marker."
+ default_value: 0
+ field: general.marker_containment_hold_east_cm
+ condition: USE_MARKER_GUIDANCE
+ min: -5000
+ max: 5000
+ - name: nav_marker_guidance_lost_hold_time_ms
+ description: "Hold duration after target loss before falling back to normal LAND behavior [ms]."
+ default_value: 1500
+ field: general.marker_guidance_lost_hold_time_ms
+ condition: USE_MARKER_GUIDANCE
+ min: 100
+ max: 10000
+ - name: nav_marker_guidance_retry_count
+ description: "Maximum number of climb-and-retry attempts after target loss in PL mode LAND context."
+ default_value: 2
+ field: general.marker_guidance_retry_count
+ condition: USE_MARKER_GUIDANCE
+ min: 0
+ max: 10
+ - name: nav_marker_guidance_retry_altitude_cm
+ description: "Climb distance for each retry attempt after target loss in PL mode LAND context [cm]."
+ default_value: 200
+ field: general.marker_guidance_retry_altitude_cm
+ condition: USE_MARKER_GUIDANCE
+ min: 50
+ max: 2000
+ - name: nav_marker_guidance_retry_timeout_ms
+ description: "Timeout for each climb-and-retry phase in PL mode LAND context [ms]. Set to 0 for AUTO mode (computed as 2 x nav_marker_guidance_lost_hold_time_ms)."
+ default_value: 0
+ field: general.marker_guidance_retry_timeout_ms
+ condition: USE_MARKER_GUIDANCE
+ min: 0
+ max: 60000
- name: nav_rth_alt_mode
description: "Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details"
default_value: "AT_LEAST"
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 9d0dd55b344..cc7baf68b91 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1118,6 +1118,20 @@ static const char * navigationStateMessage(void)
case MW_NAV_STATE_LAND_START_DESCENT:
// Not used
break;
+#ifdef USE_MARKER_GUIDANCE
+ case MW_NAV_STATE_MARKER_GUIDANCE_STANDBY:
+ return OSD_MESSAGE_STR("MARKER STANDBY");
+ case MW_NAV_STATE_MARKER_GUIDANCE_POSHOLD_CORRECTION:
+ return OSD_MESSAGE_STR("MARKER POS ALIGN");
+ case MW_NAV_STATE_MARKER_GUIDANCE_LAND_CORRECTION:
+ return OSD_MESSAGE_STR("MARKER LAND ALIGN");
+ case MW_NAV_STATE_MARKER_GUIDANCE_TARGET_LOST_HOLD:
+ return OSD_MESSAGE_STR("MARKER LOST HOLD");
+ case MW_NAV_STATE_MARKER_GUIDANCE_CLIMB_AND_RETRY:
+ return OSD_MESSAGE_STR("MARKER RETRY");
+ case MW_NAV_STATE_MARKER_GUIDANCE_FALLBACK_NORMAL_LAND:
+ return OSD_MESSAGE_STR("MARKER FALLBACK");
+#endif
}
return NULL;
diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c
index a8adf663d57..ffc6bcf697c 100644
--- a/src/main/io/osd_dji_hd.c
+++ b/src/main/io/osd_dji_hd.c
@@ -638,6 +638,20 @@ static char * navigationStateMessage(void)
case MW_NAV_STATE_LAND_START_DESCENT:
// Not used
break;
+#ifdef USE_MARKER_GUIDANCE
+ case MW_NAV_STATE_MARKER_GUIDANCE_STANDBY:
+ return OSD_MESSAGE_STR("MARKER STANDBY");
+ case MW_NAV_STATE_MARKER_GUIDANCE_POSHOLD_CORRECTION:
+ return OSD_MESSAGE_STR("MARKER POS ALIGN");
+ case MW_NAV_STATE_MARKER_GUIDANCE_LAND_CORRECTION:
+ return OSD_MESSAGE_STR("MARKER LAND ALIGN");
+ case MW_NAV_STATE_MARKER_GUIDANCE_TARGET_LOST_HOLD:
+ return OSD_MESSAGE_STR("MARKER LOST HOLD");
+ case MW_NAV_STATE_MARKER_GUIDANCE_CLIMB_AND_RETRY:
+ return OSD_MESSAGE_STR("MARKER RETRY");
+ case MW_NAV_STATE_MARKER_GUIDANCE_FALLBACK_NORMAL_LAND:
+ return OSD_MESSAGE_STR("MARKER FALLBACK");
+#endif
}
return NULL;
}
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index b9aeb6b879c..59b204759b2 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -138,6 +138,7 @@
#define MSP2_INAV_FULL_LOCAL_POSE 0x2220
#define MSP2_INAV_SET_WP_INDEX 0x2221 //in message jump to waypoint N during active WP mission; payload: U8 wp_index (0-based, relative to mission start)
+#define MSP2_INAV_SET_MARKER_GUIDANCE_TARGET 0x2231 //in message marker-guidance target, hard-break payload (4 bytes): offsetForwardCm, offsetRightCm
#define MSP2_INAV_SET_CRUISE_HEADING 0x2223 //in message set heading while in Cruise/Course Hold mode; payload: I32 heading_centidegrees (0-35999)
-#define MSP2_INAV_SET_AUX_RC 0x2230
\ No newline at end of file
+#define MSP2_INAV_SET_AUX_RC 0x2230
diff --git a/src/main/navigation/marker_guidance.c b/src/main/navigation/marker_guidance.c
new file mode 100644
index 00000000000..062ee24a322
--- /dev/null
+++ b/src/main/navigation/marker_guidance.c
@@ -0,0 +1,510 @@
+#include
+#include
+
+#include "platform.h"
+
+#include "common/maths.h"
+
+#include "drivers/time.h"
+
+#include "fc/fc_core.h"
+#include "fc/runtime_config.h"
+
+#include "navigation/marker_guidance.h"
+
+#include "sensors/sensors.h"
+
+#ifdef USE_MARKER_GUIDANCE
+
+typedef enum {
+ MARKER_GUIDANCE_CONTEXT_NONE = 0,
+ MARKER_GUIDANCE_CONTEXT_POSHOLD,
+ MARKER_GUIDANCE_CONTEXT_LAND,
+} markerGuidanceContext_e;
+
+typedef struct {
+ int16_t offsetForwardCm;
+ int16_t offsetRightCm;
+ timeMs_t lastUpdateMs;
+} markerGuidanceTargetCache_t;
+
+typedef struct {
+ markerGuidanceState_e state;
+ markerGuidanceTargetCache_t target;
+ markerGuidanceContext_e activeContext;
+ timeMs_t stateDeadlineMs;
+ uint8_t retryCount;
+ bool targetAcquiredInContext;
+} markerGuidanceRuntime_t;
+
+static markerGuidanceRuntime_t markerGuidance;
+
+static navMarkerGuidanceMode_e markerGuidanceMode(void)
+{
+ return (navMarkerGuidanceMode_e)navConfig()->general.marker_guidance_mode;
+}
+
+static bool markerGuidanceFeatureEnabled(void)
+{
+ return navConfig()->general.marker_guidance_source == NAV_MARKER_GUIDANCE_SOURCE_MSP &&
+ markerGuidanceMode() != NAV_MARKER_GUIDANCE_MODE_OFF;
+}
+
+static bool markerGuidanceIsContainmentMode(void)
+{
+ return markerGuidanceMode() == NAV_MARKER_GUIDANCE_MODE_CONTAINMENT;
+}
+
+static bool markerGuidanceIsPlMode(void)
+{
+ return markerGuidanceMode() == NAV_MARKER_GUIDANCE_MODE_PL;
+}
+
+static bool isMcHoverCapableProfileActive(void)
+{
+ return STATE(MULTIROTOR) && !STATE(AIRPLANE);
+}
+
+static bool markerGuidanceIsPosholdContext(navigationFSMStateFlags_t navStateFlags)
+{
+ return (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS) ||
+ ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND) && FLIGHT_MODE(NAV_POSHOLD_MODE));
+}
+
+static bool markerGuidanceIsLandContext(navigationFSMStateFlags_t navStateFlags)
+{
+ return (navStateFlags & NAV_CTL_LAND) && !STATE(AIRPLANE);
+}
+
+static bool markerGuidanceManualTakeoverActive(void)
+{
+ return posControl.flags.isAdjustingAltitude ||
+ posControl.flags.isAdjustingPosition ||
+ posControl.flags.isAdjustingHeading;
+}
+
+static markerGuidanceContext_e markerGuidanceSelectContext(navigationFSMStateFlags_t navStateFlags)
+{
+ if (!isMcHoverCapableProfileActive()) {
+ return MARKER_GUIDANCE_CONTEXT_NONE;
+ }
+
+ switch (markerGuidanceMode()) {
+ case NAV_MARKER_GUIDANCE_MODE_PL:
+ if (markerGuidanceIsLandContext(navStateFlags)) {
+ return MARKER_GUIDANCE_CONTEXT_LAND;
+ }
+ if (markerGuidanceIsPosholdContext(navStateFlags)) {
+ return MARKER_GUIDANCE_CONTEXT_POSHOLD;
+ }
+ break;
+
+ case NAV_MARKER_GUIDANCE_MODE_CONTAINMENT:
+ if (markerGuidanceIsPosholdContext(navStateFlags)) {
+ return MARKER_GUIDANCE_CONTEXT_POSHOLD;
+ }
+ break;
+
+ default:
+ break;
+ }
+
+ return MARKER_GUIDANCE_CONTEXT_NONE;
+}
+
+static void setMarkerGuidanceState(markerGuidanceState_e state)
+{
+ markerGuidance.state = state;
+}
+
+static void clearTargetCache(void)
+{
+ markerGuidance.target.offsetForwardCm = 0;
+ markerGuidance.target.offsetRightCm = 0;
+ markerGuidance.target.lastUpdateMs = 0;
+}
+
+static void clearContextRuntime(void)
+{
+ markerGuidance.activeContext = MARKER_GUIDANCE_CONTEXT_NONE;
+ markerGuidance.stateDeadlineMs = 0;
+ markerGuidance.retryCount = 0;
+ markerGuidance.targetAcquiredInContext = false;
+}
+
+static void updateContextRuntime(markerGuidanceContext_e newContext)
+{
+ if (markerGuidance.activeContext == newContext) {
+ return;
+ }
+
+ markerGuidance.activeContext = newContext;
+ markerGuidance.stateDeadlineMs = 0;
+ markerGuidance.retryCount = 0;
+ markerGuidance.targetAcquiredInContext = false;
+}
+
+static float markerGuidanceTargetOffsetMagnitudeCm(void)
+{
+ return calc_length_pythagorean_2D((float)markerGuidance.target.offsetForwardCm, (float)markerGuidance.target.offsetRightCm);
+}
+
+static bool markerGuidanceTargetIsFresh(timeMs_t nowMs, markerGuidanceReason_e *reasonOut)
+{
+ if (markerGuidance.target.lastUpdateMs == 0) {
+ if (reasonOut) {
+ *reasonOut = MARKER_GUIDANCE_REASON_INVALID_TARGET;
+ }
+ return false;
+ }
+
+ if (navConfig()->general.marker_guidance_max_target_age_ms > 0 &&
+ (nowMs - markerGuidance.target.lastUpdateMs) > navConfig()->general.marker_guidance_max_target_age_ms) {
+ if (reasonOut) {
+ *reasonOut = MARKER_GUIDANCE_REASON_STALE;
+ }
+ return false;
+ }
+
+ if (navConfig()->general.marker_guidance_max_offset_cm > 0 &&
+ markerGuidanceTargetOffsetMagnitudeCm() > navConfig()->general.marker_guidance_max_offset_cm) {
+ if (reasonOut) {
+ *reasonOut = MARKER_GUIDANCE_REASON_OFFSET_TOO_LARGE;
+ }
+ return false;
+ }
+
+ if (reasonOut) {
+ *reasonOut = MARKER_GUIDANCE_REASON_OK;
+ }
+ return true;
+}
+
+static void markerGuidanceComputeMarkerErrorNeu(float desiredVehicleRelN, float desiredVehicleRelE, float *errorNOut, float *errorEOut)
+{
+ const float offsetForward = markerGuidance.target.offsetForwardCm;
+ const float offsetRight = markerGuidance.target.offsetRightCm;
+
+ const float cosYaw = posControl.actualState.cosYaw;
+ const float sinYaw = posControl.actualState.sinYaw;
+
+ // Marker offset in local NE from the vehicle perspective.
+ const float markerOffsetN = offsetForward * cosYaw - offsetRight * sinYaw;
+ const float markerOffsetE = offsetForward * sinYaw + offsetRight * cosYaw;
+
+ // error = desired(vehicle rel marker) - actual(vehicle rel marker)
+ // actual(vehicle rel marker) = -markerOffsetNE
+ *errorNOut = markerOffsetN + desiredVehicleRelN;
+ *errorEOut = markerOffsetE + desiredVehicleRelE;
+}
+
+static bool markerGuidanceComputeCorrectionVelocity(markerGuidanceContext_e context, float *velNOut, float *velEOut)
+{
+ if (!velNOut || !velEOut || context == MARKER_GUIDANCE_CONTEXT_NONE) {
+ return false;
+ }
+
+ *velNOut = 0.0f;
+ *velEOut = 0.0f;
+
+ const bool containmentMode = markerGuidanceIsContainmentMode() && (context == MARKER_GUIDANCE_CONTEXT_POSHOLD);
+
+ const float desiredRelN = containmentMode ? navConfig()->general.marker_containment_hold_north_cm : 0.0f;
+ const float desiredRelE = containmentMode ? navConfig()->general.marker_containment_hold_east_cm : 0.0f;
+
+ float errorN = 0.0f;
+ float errorE = 0.0f;
+ markerGuidanceComputeMarkerErrorNeu(desiredRelN, desiredRelE, &errorN, &errorE);
+
+ const float errorMagnitude = calc_length_pythagorean_2D(errorN, errorE);
+ if (errorMagnitude <= 0.0f) {
+ return false;
+ }
+
+ const float radiusCm = navConfig()->general.marker_guidance_radius_cm;
+ if (radiusCm > 0.0f) {
+ if (errorMagnitude <= radiusCm) {
+ return false;
+ }
+
+ // Shared radius behavior:
+ // PL mode: center-alignment deadband around marker center.
+ // CONTAINMENT mode: allowed boundary around configured hold point.
+ const float scale = (errorMagnitude - radiusCm) / errorMagnitude;
+ errorN *= scale;
+ errorE *= scale;
+ }
+
+ const float maxCorrectionSpeed = getActiveSpeed();
+ const float speed = calc_length_pythagorean_2D(errorN, errorE);
+
+ if (maxCorrectionSpeed > 0.0f && speed > maxCorrectionSpeed) {
+ const float scale = maxCorrectionSpeed / speed;
+ errorN *= scale;
+ errorE *= scale;
+ }
+
+ *velNOut = errorN;
+ *velEOut = errorE;
+
+ return true;
+}
+
+static uint16_t getRetryTimeoutMs(void)
+{
+ const uint16_t configuredRetryTimeoutMs = navConfig()->general.marker_guidance_retry_timeout_ms;
+ if (configuredRetryTimeoutMs > 0) {
+ return MAX(configuredRetryTimeoutMs, 100);
+ }
+
+ const uint16_t lostHoldMs = MAX(navConfig()->general.marker_guidance_lost_hold_time_ms, 100);
+ const uint32_t autoTimeoutMs = (uint32_t)lostHoldMs * 2U;
+ return (uint16_t)MAX(MIN(autoTimeoutMs, 60000U), 100U);
+}
+
+static float getRetryClimbRateCmS(void)
+{
+ const uint16_t retryTimeoutMs = getRetryTimeoutMs();
+ const float retryTimeoutS = retryTimeoutMs / 1000.0f;
+ const float climbRate = navConfig()->general.marker_guidance_retry_altitude_cm / retryTimeoutS;
+ return constrainf(climbRate, 20.0f, navConfig()->mc.max_auto_climb_rate);
+}
+
+static void setLostHoldState(timeMs_t nowMs)
+{
+ setMarkerGuidanceState(MARKER_GUIDANCE_TARGET_LOST_HOLD);
+ markerGuidance.stateDeadlineMs = nowMs + MAX(navConfig()->general.marker_guidance_lost_hold_time_ms, 100U);
+}
+
+static void setClimbRetryState(timeMs_t nowMs)
+{
+ setMarkerGuidanceState(MARKER_GUIDANCE_CLIMB_AND_RETRY);
+ markerGuidance.stateDeadlineMs = nowMs + getRetryTimeoutMs();
+}
+
+void markerGuidanceReset(void)
+{
+ clearTargetCache();
+ clearContextRuntime();
+ setMarkerGuidanceState(MARKER_GUIDANCE_IDLE);
+}
+
+void markerGuidanceUpdate(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+
+ const timeMs_t nowMs = millis();
+
+ if (!markerGuidanceFeatureEnabled()) {
+ clearContextRuntime();
+ setMarkerGuidanceState(MARKER_GUIDANCE_IDLE);
+ return;
+ }
+
+ if (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating() || STATE(LANDING_DETECTED)) {
+ markerGuidanceReset();
+ return;
+ }
+
+ markerGuidanceReason_e freshnessReason = MARKER_GUIDANCE_REASON_OK;
+ const bool targetFresh = markerGuidanceTargetIsFresh(nowMs, &freshnessReason);
+
+ const bool manualTakeover = markerGuidanceManualTakeoverActive();
+ const markerGuidanceContext_e selectedContext = manualTakeover ?
+ MARKER_GUIDANCE_CONTEXT_NONE :
+ markerGuidanceSelectContext(navStateFlags);
+
+ updateContextRuntime(selectedContext);
+
+ if (selectedContext == MARKER_GUIDANCE_CONTEXT_NONE) {
+ setMarkerGuidanceState(targetFresh ? MARKER_GUIDANCE_STANDBY : MARKER_GUIDANCE_IDLE);
+ return;
+ }
+
+ if (targetFresh) {
+ markerGuidance.targetAcquiredInContext = true;
+
+ float correctionVelN = 0.0f;
+ float correctionVelE = 0.0f;
+ if (markerGuidanceComputeCorrectionVelocity(selectedContext, &correctionVelN, &correctionVelE)) {
+ if (selectedContext == MARKER_GUIDANCE_CONTEXT_LAND) {
+ setMarkerGuidanceState(MARKER_GUIDANCE_LAND_CORRECTION);
+ } else {
+ setMarkerGuidanceState(MARKER_GUIDANCE_POSHOLD_CORRECTION);
+ }
+ } else {
+ setMarkerGuidanceState(MARKER_GUIDANCE_STANDBY);
+ }
+ return;
+ }
+
+ if (!markerGuidanceIsPlMode() || selectedContext != MARKER_GUIDANCE_CONTEXT_LAND || !markerGuidance.targetAcquiredInContext) {
+ if (markerGuidanceIsContainmentMode() && markerGuidance.target.lastUpdateMs != 0 && freshnessReason == MARKER_GUIDANCE_REASON_STALE) {
+ setMarkerGuidanceState(MARKER_GUIDANCE_TARGET_LOST_HOLD);
+ } else {
+ setMarkerGuidanceState(MARKER_GUIDANCE_STANDBY);
+ }
+ return;
+ }
+
+ switch (markerGuidance.state) {
+ case MARKER_GUIDANCE_TARGET_LOST_HOLD:
+ if (nowMs >= markerGuidance.stateDeadlineMs) {
+ if (markerGuidance.retryCount < navConfig()->general.marker_guidance_retry_count) {
+ setClimbRetryState(nowMs);
+ } else {
+ setMarkerGuidanceState(MARKER_GUIDANCE_FALLBACK_NORMAL_LAND);
+ }
+ }
+ break;
+
+ case MARKER_GUIDANCE_CLIMB_AND_RETRY:
+ if (nowMs >= markerGuidance.stateDeadlineMs) {
+ markerGuidance.retryCount++;
+ if (markerGuidance.retryCount >= navConfig()->general.marker_guidance_retry_count) {
+ setMarkerGuidanceState(MARKER_GUIDANCE_FALLBACK_NORMAL_LAND);
+ } else {
+ setLostHoldState(nowMs);
+ }
+ }
+ break;
+
+ case MARKER_GUIDANCE_FALLBACK_NORMAL_LAND:
+ break;
+
+ default:
+ setLostHoldState(nowMs);
+ break;
+ }
+}
+
+void markerGuidanceApplyHorizontalVelocityCorrection(float *velX, float *velY)
+{
+ if (!velX || !velY) {
+ return;
+ }
+
+ markerGuidanceContext_e correctionContext = MARKER_GUIDANCE_CONTEXT_NONE;
+ if (markerGuidance.state == MARKER_GUIDANCE_LAND_CORRECTION) {
+ correctionContext = MARKER_GUIDANCE_CONTEXT_LAND;
+ } else if (markerGuidance.state == MARKER_GUIDANCE_POSHOLD_CORRECTION) {
+ correctionContext = MARKER_GUIDANCE_CONTEXT_POSHOLD;
+ } else {
+ return;
+ }
+
+ if (!markerGuidanceTargetIsFresh(millis(), NULL)) {
+ return;
+ }
+
+ float velN = 0.0f;
+ float velE = 0.0f;
+ if (!markerGuidanceComputeCorrectionVelocity(correctionContext, &velN, &velE)) {
+ return;
+ }
+
+ *velX += velN;
+ *velY += velE;
+}
+
+void markerGuidanceGetLandControl(markerGuidanceLandControl_t *controlOut)
+{
+ if (!controlOut) {
+ return;
+ }
+
+ controlOut->mode = MARKER_GUIDANCE_LAND_CTRL_NONE;
+ controlOut->rateCmS = 0.0f;
+
+ if (!markerGuidanceIsPlMode() || markerGuidance.activeContext != MARKER_GUIDANCE_CONTEXT_LAND) {
+ return;
+ }
+
+ if (markerGuidance.state == MARKER_GUIDANCE_TARGET_LOST_HOLD) {
+ controlOut->mode = MARKER_GUIDANCE_LAND_CTRL_HOLD;
+ } else if (markerGuidance.state == MARKER_GUIDANCE_CLIMB_AND_RETRY) {
+ controlOut->mode = MARKER_GUIDANCE_LAND_CTRL_CLIMB;
+ controlOut->rateCmS = getRetryClimbRateCmS();
+ }
+}
+
+navSystemStatus_State_e markerGuidanceOverrideNavStatusState(navSystemStatus_State_e defaultState)
+{
+ switch (markerGuidance.state) {
+ case MARKER_GUIDANCE_STANDBY:
+ return MW_NAV_STATE_MARKER_GUIDANCE_STANDBY;
+ case MARKER_GUIDANCE_POSHOLD_CORRECTION:
+ return MW_NAV_STATE_MARKER_GUIDANCE_POSHOLD_CORRECTION;
+ case MARKER_GUIDANCE_LAND_CORRECTION:
+ return MW_NAV_STATE_MARKER_GUIDANCE_LAND_CORRECTION;
+ case MARKER_GUIDANCE_TARGET_LOST_HOLD:
+ return MW_NAV_STATE_MARKER_GUIDANCE_TARGET_LOST_HOLD;
+ case MARKER_GUIDANCE_CLIMB_AND_RETRY:
+ return MW_NAV_STATE_MARKER_GUIDANCE_CLIMB_AND_RETRY;
+ case MARKER_GUIDANCE_FALLBACK_NORMAL_LAND:
+ return MW_NAV_STATE_MARKER_GUIDANCE_FALLBACK_NORMAL_LAND;
+ default:
+ return defaultState;
+ }
+}
+
+bool markerGuidanceHandleMspTargetUpdate(const markerGuidanceTargetUpdate_t *update, markerGuidanceMspResponse_t *responseOut)
+{
+ if (!update || !responseOut) {
+ return false;
+ }
+
+ responseOut->accepted = 0;
+ responseOut->usedNow = 0;
+ responseOut->navGuidanceState = (uint8_t)markerGuidance.state;
+ responseOut->reason = MARKER_GUIDANCE_REASON_INVALID_TARGET;
+ responseOut->retryCount = markerGuidance.retryCount;
+
+ if (!markerGuidanceFeatureEnabled()) {
+ responseOut->accepted = 1;
+ responseOut->reason = MARKER_GUIDANCE_REASON_NOT_ENABLED;
+ return true;
+ }
+
+ const float offsetMagnitude = calc_length_pythagorean_2D((float)update->offsetForwardCm, (float)update->offsetRightCm);
+ if (navConfig()->general.marker_guidance_max_offset_cm > 0 &&
+ offsetMagnitude > navConfig()->general.marker_guidance_max_offset_cm) {
+ responseOut->reason = MARKER_GUIDANCE_REASON_OFFSET_TOO_LARGE;
+ return true;
+ }
+
+ markerGuidance.target.offsetForwardCm = update->offsetForwardCm;
+ markerGuidance.target.offsetRightCm = update->offsetRightCm;
+ markerGuidance.target.lastUpdateMs = millis();
+
+ responseOut->accepted = 1;
+ responseOut->reason = MARKER_GUIDANCE_REASON_OK;
+
+ const bool mcProfileActive = isMcHoverCapableProfileActive();
+ const navigationFSMStateFlags_t navStateFlags = navGetCurrentStateFlags();
+ const bool manualTakeover = markerGuidanceManualTakeoverActive();
+ const markerGuidanceContext_e selectedContext = manualTakeover ?
+ MARKER_GUIDANCE_CONTEXT_NONE :
+ markerGuidanceSelectContext(navStateFlags);
+
+ if (!ARMING_FLAG(ARMED)) {
+ responseOut->reason = MARKER_GUIDANCE_REASON_NOT_ARMED;
+ } else if (FLIGHT_MODE(FAILSAFE_MODE)) {
+ responseOut->reason = MARKER_GUIDANCE_REASON_FAILSAFE;
+ } else if (!mcProfileActive) {
+ responseOut->reason = MARKER_GUIDANCE_REASON_NOT_MC_PROFILE;
+ } else if (selectedContext == MARKER_GUIDANCE_CONTEXT_NONE) {
+ responseOut->reason = MARKER_GUIDANCE_REASON_NOT_IN_POSHOLD_OR_LAND;
+ } else {
+ float velN = 0.0f;
+ float velE = 0.0f;
+ responseOut->usedNow = markerGuidanceComputeCorrectionVelocity(selectedContext, &velN, &velE) ? 1 : 0;
+ responseOut->reason = MARKER_GUIDANCE_REASON_OK;
+ }
+
+ responseOut->navGuidanceState = (uint8_t)markerGuidance.state;
+ responseOut->retryCount = markerGuidance.retryCount;
+ return true;
+}
+
+#endif
diff --git a/src/main/navigation/marker_guidance.h b/src/main/navigation/marker_guidance.h
new file mode 100644
index 00000000000..0f980316da3
--- /dev/null
+++ b/src/main/navigation/marker_guidance.h
@@ -0,0 +1,67 @@
+#pragma once
+
+#ifdef USE_MARKER_GUIDANCE
+
+#include
+#include
+
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+
+typedef enum {
+ MARKER_GUIDANCE_REASON_OK = 0,
+ MARKER_GUIDANCE_REASON_NOT_ENABLED,
+ MARKER_GUIDANCE_REASON_LOW_CONFIDENCE,
+ MARKER_GUIDANCE_REASON_BAD_FRAME,
+ MARKER_GUIDANCE_REASON_STALE,
+ MARKER_GUIDANCE_REASON_OFFSET_TOO_LARGE,
+ MARKER_GUIDANCE_REASON_NOT_MC_PROFILE,
+ MARKER_GUIDANCE_REASON_NOT_IN_POSHOLD_OR_LAND,
+ MARKER_GUIDANCE_REASON_FAILSAFE,
+ MARKER_GUIDANCE_REASON_INVALID_TARGET,
+ MARKER_GUIDANCE_REASON_NOT_ARMED,
+} markerGuidanceReason_e;
+
+typedef enum {
+ MARKER_GUIDANCE_IDLE = 0,
+ MARKER_GUIDANCE_STANDBY,
+ MARKER_GUIDANCE_POSHOLD_CORRECTION,
+ MARKER_GUIDANCE_LAND_CORRECTION,
+ MARKER_GUIDANCE_TARGET_LOST_HOLD,
+ MARKER_GUIDANCE_CLIMB_AND_RETRY,
+ MARKER_GUIDANCE_FALLBACK_NORMAL_LAND,
+ MARKER_GUIDANCE_DONE,
+} markerGuidanceState_e;
+
+typedef enum {
+ MARKER_GUIDANCE_LAND_CTRL_NONE = 0,
+ MARKER_GUIDANCE_LAND_CTRL_HOLD,
+ MARKER_GUIDANCE_LAND_CTRL_CLIMB,
+} markerGuidanceLandControlMode_e;
+
+typedef struct {
+ int16_t offsetForwardCm;
+ int16_t offsetRightCm;
+} markerGuidanceTargetUpdate_t;
+
+typedef struct {
+ uint8_t accepted;
+ uint8_t usedNow;
+ uint8_t navGuidanceState;
+ uint8_t reason;
+ uint8_t retryCount;
+} markerGuidanceMspResponse_t;
+
+typedef struct {
+ markerGuidanceLandControlMode_e mode;
+ float rateCmS;
+} markerGuidanceLandControl_t;
+
+void markerGuidanceReset(void);
+void markerGuidanceUpdate(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
+void markerGuidanceApplyHorizontalVelocityCorrection(float *velX, float *velY);
+void markerGuidanceGetLandControl(markerGuidanceLandControl_t *controlOut);
+navSystemStatus_State_e markerGuidanceOverrideNavStatusState(navSystemStatus_State_e defaultState);
+bool markerGuidanceHandleMspTargetUpdate(const markerGuidanceTargetUpdate_t *update, markerGuidanceMspResponse_t *responseOut);
+
+#endif
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 8f60155b68c..5b53ea693e9 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -56,6 +56,9 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
+#ifdef USE_MARKER_GUIDANCE
+#include "navigation/marker_guidance.h"
+#endif
#include "navigation/rth_trackback.h"
#include "rx/rx.h"
@@ -119,7 +122,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
#endif
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@@ -177,6 +180,19 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
.rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing
+#ifdef USE_MARKER_GUIDANCE
+ .marker_guidance_mode = SETTING_NAV_MARKER_GUIDANCE_MODE_DEFAULT,
+ .marker_guidance_source = SETTING_NAV_MARKER_GUIDANCE_SOURCE_DEFAULT,
+ .marker_guidance_max_target_age_ms = SETTING_NAV_MARKER_GUIDANCE_MAX_TARGET_AGE_MS_DEFAULT,
+ .marker_guidance_max_offset_cm = SETTING_NAV_MARKER_GUIDANCE_MAX_OFFSET_CM_DEFAULT,
+ .marker_guidance_radius_cm = SETTING_NAV_MARKER_GUIDANCE_RADIUS_CM_DEFAULT,
+ .marker_containment_hold_north_cm = SETTING_NAV_MARKER_CONTAINMENT_HOLD_NORTH_CM_DEFAULT,
+ .marker_containment_hold_east_cm = SETTING_NAV_MARKER_CONTAINMENT_HOLD_EAST_CM_DEFAULT,
+ .marker_guidance_lost_hold_time_ms = SETTING_NAV_MARKER_GUIDANCE_LOST_HOLD_TIME_MS_DEFAULT,
+ .marker_guidance_retry_count = SETTING_NAV_MARKER_GUIDANCE_RETRY_COUNT_DEFAULT,
+ .marker_guidance_retry_altitude_cm = SETTING_NAV_MARKER_GUIDANCE_RETRY_ALTITUDE_CM_DEFAULT,
+ .marker_guidance_retry_timeout_ms = SETTING_NAV_MARKER_GUIDANCE_RETRY_TIMEOUT_MS_DEFAULT,
+#endif
},
// MC-specific
@@ -1884,7 +1900,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
}
- updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT);
+ bool markerGuidanceVerticalOverride = false;
+#ifdef USE_MARKER_GUIDANCE
+ markerGuidanceLandControl_t markerGuidanceControl = { 0 };
+ markerGuidanceGetLandControl(&markerGuidanceControl);
+
+ if (markerGuidanceControl.mode == MARKER_GUIDANCE_LAND_CTRL_HOLD) {
+ updateClimbRateToAltitudeController(0.0f, 0.0f, ROC_TO_ALT_CONSTANT);
+ markerGuidanceVerticalOverride = true;
+ } else if (markerGuidanceControl.mode == MARKER_GUIDANCE_LAND_CTRL_CLIMB) {
+ updateClimbRateToAltitudeController(markerGuidanceControl.rateCmS, 0.0f, ROC_TO_ALT_CONSTANT);
+ markerGuidanceVerticalOverride = true;
+ }
+#endif
+ if (!markerGuidanceVerticalOverride) {
+ updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT);
+ }
return NAV_FSM_EVENT_NONE;
}
@@ -2692,6 +2723,9 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
}
NAV_Status.state = navFSM[posControl.navState].mwState;
+#ifdef USE_MARKER_GUIDANCE
+ NAV_Status.state = markerGuidanceOverrideNavStatusState(NAV_Status.state);
+#endif
NAV_Status.error = navFSM[posControl.navState].mwError;
NAV_Status.flags = 0;
@@ -4385,6 +4419,9 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.activeWaypointIndex = posControl.startWpIndex;
// Reset RTH trackback
resetRthTrackBack();
+#ifdef USE_MARKER_GUIDANCE
+ markerGuidanceReset();
+#endif
#ifdef USE_GEOZONE
posControl.flags.sendToActive = false;
@@ -4405,6 +4442,10 @@ void applyWaypointNavigationAndAltitudeHold(void)
/* Process controllers */
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
+#ifdef USE_MARKER_GUIDANCE
+ markerGuidanceUpdate(navStateFlags, currentTimeUs);
+#endif
+
if (STATE(ROVER) || STATE(BOAT)) {
applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
} else if (STATE(FIXED_WING_LEGACY)) {
@@ -5097,6 +5138,9 @@ void navigationInit(void)
/* Reset statistics */
posControl.totalTripDistance = 0.0f;
+#ifdef USE_MARKER_GUIDANCE
+ markerGuidanceReset();
+#endif
/* Use system config */
navigationUsePIDs();
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 398781fa596..e45e3086092 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -386,6 +386,18 @@ typedef struct positionEstimationConfig_s {
PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig);
+#ifdef USE_MARKER_GUIDANCE
+typedef enum {
+ NAV_MARKER_GUIDANCE_SOURCE_MSP = 0,
+} navMarkerGuidanceSource_e;
+
+typedef enum {
+ NAV_MARKER_GUIDANCE_MODE_OFF = 0,
+ NAV_MARKER_GUIDANCE_MODE_PL,
+ NAV_MARKER_GUIDANCE_MODE_CONTAINMENT,
+} navMarkerGuidanceMode_e;
+#endif
+
typedef struct navConfig_s {
struct {
@@ -441,6 +453,19 @@ typedef struct navConfig_s {
uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately)
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
uint16_t rth_fs_landing_delay; // Delay upon reaching home before starting landing if in FS (0 = immediate)
+#ifdef USE_MARKER_GUIDANCE
+ uint8_t marker_guidance_mode; // navMarkerGuidanceMode_e
+ uint8_t marker_guidance_source; // navMarkerGuidanceSource_e
+ uint16_t marker_guidance_max_target_age_ms;
+ uint16_t marker_guidance_max_offset_cm;
+ uint16_t marker_guidance_radius_cm;
+ int16_t marker_containment_hold_north_cm;
+ int16_t marker_containment_hold_east_cm;
+ uint16_t marker_guidance_lost_hold_time_ms;
+ uint8_t marker_guidance_retry_count;
+ uint16_t marker_guidance_retry_altitude_cm;
+ uint16_t marker_guidance_retry_timeout_ms;
+#endif
} general;
struct {
@@ -626,6 +651,14 @@ typedef enum {
MW_NAV_STATE_HOVER_ABOVE_HOME, // Hover/Loitering above home
MW_NAV_STATE_EMERGENCY_LANDING, // Emergency landing
MW_NAV_STATE_RTH_CLIMB, // RTH Climb safe altitude
+#ifdef USE_MARKER_GUIDANCE
+ MW_NAV_STATE_MARKER_GUIDANCE_STANDBY,
+ MW_NAV_STATE_MARKER_GUIDANCE_POSHOLD_CORRECTION,
+ MW_NAV_STATE_MARKER_GUIDANCE_LAND_CORRECTION,
+ MW_NAV_STATE_MARKER_GUIDANCE_TARGET_LOST_HOLD,
+ MW_NAV_STATE_MARKER_GUIDANCE_CLIMB_AND_RETRY,
+ MW_NAV_STATE_MARKER_GUIDANCE_FALLBACK_NORMAL_LAND,
+#endif
} navSystemStatus_State_e;
typedef enum {
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 7da742829c5..5b57fdb5c93 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -50,6 +50,9 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
+#ifdef USE_MARKER_GUIDANCE
+#include "navigation/marker_guidance.h"
+#endif
#include "navigation/sqrt_controller.h"
#include "sensors/battery.h"
@@ -533,6 +536,11 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
const float velExpoFactor = getVelocityExpoAttenuationFactor(neuVelTotal, maxSpeed);
posControl.desiredState.vel.x = neuVelX * velHeadFactor * velExpoFactor;
posControl.desiredState.vel.y = neuVelY * velHeadFactor * velExpoFactor;
+
+ // Optional external marker-guidance correction (MC/VTOL hover contexts only).
+#ifdef USE_MARKER_GUIDANCE
+ markerGuidanceApplyHorizontalVelocityCorrection(&posControl.desiredState.vel.x, &posControl.desiredState.vel.y);
+#endif
}
static float computeNormalizedVelocity(const float value, const float maxValue)
@@ -1019,4 +1027,4 @@ void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlag
if (navStateFlags & NAV_CTL_YAW)
applyMulticopterHeadingController();
}
-}
\ No newline at end of file
+}
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 55ca1653f5a..13cca5898ab 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -209,6 +209,7 @@
#define USE_TELEMETRY_HOTT
#define USE_HOTT_TEXTMODE
#define USE_34CHANNELS
+#define USE_MARKER_GUIDANCE
#define MAX_MIXER_PROFILE_COUNT 2
#define USE_SMARTPORT_MASTER
#ifdef USE_GPS
@@ -228,4 +229,3 @@
#define USE_EZ_TUNE
#define USE_ADAPTIVE_FILTER
-