diff --git a/docs/Navigation.md b/docs/Navigation.md index 4d7b9740d91..e7eaaeda651 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -38,6 +38,74 @@ 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: +* at or below `nav_marker_guidance_retry_min_alt_cm` AGL, FC skips retry and continues normal LAND behavior +* if `nav_marker_guidance_low_alt_lock_xy = ON`, FC locks the current XY position when entering low-altitude fallback +* above that altitude, 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 +* set `nav_marker_guidance_retry_min_alt_cm = 0` to disable the low-altitude retry suppression +* set `nav_marker_guidance_low_alt_lock_xy = OFF` to keep the normal LAND XY target during low-altitude fallback + +### 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..088fdd016d2 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4039,6 +4039,138 @@ Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mod --- +### 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_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_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_guidance_low_alt_lock_xy + +Lock current XY position when marker target is lost at or below nav_marker_guidance_retry_min_alt_cm in PL mode LAND context. OFF keeps normal LAND XY target. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### 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_mode + +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. + +| 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_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_min_alt_cm + +Minimum AGL altitude for climb-and-retry after target loss in PL mode LAND context [cm]. At or below this altitude, retry is skipped and normal LAND continues. Set 0 to disable. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 2000 | + +--- + +### 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_max_altitude Max allowed altitude (above Home Point) that applies to all NAV modes (including Altitude Hold). 0 means limit is disabled @@ -7127,4 +7259,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..a370a9dd97c 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,94 @@ 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_min_alt_cm + description: "Minimum AGL altitude for climb-and-retry after target loss in PL mode LAND context [cm]. At or below this altitude, retry is skipped and normal LAND continues. Set 0 to disable." + default_value: 100 + field: general.marker_guidance_retry_min_alt_cm + condition: USE_MARKER_GUIDANCE + min: 0 + max: 2000 + - name: nav_marker_guidance_low_alt_lock_xy + description: "Lock current XY position when marker target is lost at or below nav_marker_guidance_retry_min_alt_cm in PL mode LAND context. OFF keeps normal LAND XY target." + default_value: OFF + field: general.marker_guidance_low_alt_lock_xy + condition: USE_MARKER_GUIDANCE + type: bool + - 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..605c089e000 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"); + 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..e44d7a7bcd1 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"); + 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..f92841bca65 --- /dev/null +++ b/src/main/navigation/marker_guidance.c @@ -0,0 +1,534 @@ +#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 bool markerGuidanceRetrySuppressedByAltitude(void) +{ + const uint16_t retryMinAltitudeCm = navConfig()->general.marker_guidance_retry_min_alt_cm; + return retryMinAltitudeCm > 0 && + posControl.flags.estAglStatus >= EST_USABLE && + posControl.actualState.agl.pos.z >= 0.0f && + posControl.actualState.agl.pos.z <= retryMinAltitudeCm; +} + +static void setLowAltitudeFallbackNormalLandState(void) +{ + if (markerGuidance.state != MARKER_GUIDANCE_FALLBACK_NORMAL_LAND && + navConfig()->general.marker_guidance_low_alt_lock_xy) { + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); + } + + setMarkerGuidanceState(MARKER_GUIDANCE_FALLBACK_NORMAL_LAND); +} + +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; + } + + if (markerGuidanceRetrySuppressedByAltitude()) { + setLowAltitudeFallbackNormalLandState(); + 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..ddcadbcec9a 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, 10); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -177,6 +180,21 @@ 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_min_alt_cm = SETTING_NAV_MARKER_GUIDANCE_RETRY_MIN_ALT_CM_DEFAULT, + .marker_guidance_low_alt_lock_xy = SETTING_NAV_MARKER_GUIDANCE_LOW_ALT_LOCK_XY_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 +1902,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 +2725,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 +4421,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 +4444,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 +5140,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..9ae90e58a9a 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,21 @@ 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_min_alt_cm; + bool marker_guidance_low_alt_lock_xy; + uint16_t marker_guidance_retry_altitude_cm; + uint16_t marker_guidance_retry_timeout_ms; +#endif } general; struct { @@ -626,6 +653,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..6fa77fb8518 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" @@ -483,6 +486,18 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax) return 1.0f - posControl.posResponseExpo * (1.0f - (velScale * velScale)); // x^3 expo factor } +#ifdef USE_MARKER_GUIDANCE +static void constrainDesiredHorizontalVelocity(float maxSpeed) +{ + const float speed = calc_length_pythagorean_2D(posControl.desiredState.vel.x, posControl.desiredState.vel.y); + if (maxSpeed > 0.0f && speed > maxSpeed) { + const float scale = maxSpeed / speed; + posControl.desiredState.vel.x *= scale; + posControl.desiredState.vel.y *= scale; + } +} +#endif + static void updatePositionVelocityController_MC(const float maxSpeed) { if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { @@ -533,6 +548,12 @@ 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); + constrainDesiredHorizontalVelocity(maxSpeed); +#endif } static float computeNormalizedVelocity(const float value, const float maxValue) @@ -1019,4 +1040,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 -