From 05da4d1aeab4e5aec5ccfcccf9128093fb600516 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Wed, 13 May 2026 21:50:45 +0300 Subject: [PATCH 1/4] navigation: add MSP precision landing target consumer for MC/VTOL Implement an INAV-side precision-landing target consumer that accepts external target offsets over MSP and applies correction only in MC/VTOL POSHOLD/LAND contexts, without direct motor/attitude control and without mode switching. - add MSP2_INAV_SET_PRECISION_LANDING_TARGET (0x2231 / 8753) - add precision landing state machine module (cache/validate/use/fallback) - integrate with NAV + multicopter position controller + LAND flow - add nav status exposure for precision states (OSD/DJI mappings) - add CLI settings for source/validation/correction/retry behavior - keep descent profile on standard LAND logic (remove custom PL descent tuning) - add retry timeout auto mode: nav_precision_landing_retry_timeout_ms=0 => 2 * lost_hold_time_ms - document MSP payload/reply and navigation behavior in docs Safety behavior: - no activation in fixed-wing profile - no correction use in failsafe - stale/low-confidence/bad-frame targets are rejected - target loss recovery: HOLD -> CLIMB_AND_RETRY -> NORMAL_LAND fallback --- docs/Navigation.md | 74 +++ docs/development/msp/README.md | 27 +- docs/development/msp/msp_messages.json | 88 +++- src/main/CMakeLists.txt | 2 + src/main/fc/fc_msp.c | 32 ++ src/main/fc/settings.yaml | 66 +++ src/main/io/osd.c | 12 + src/main/io/osd_dji_hd.c | 12 + src/main/msp/msp_protocol_v2_inav.h | 3 +- src/main/navigation/navigation.c | 31 +- src/main/navigation/navigation.h | 21 + src/main/navigation/navigation_multicopter.c | 6 +- src/main/navigation/precision_landing.c | 483 +++++++++++++++++++ src/main/navigation/precision_landing.h | 72 +++ 14 files changed, 922 insertions(+), 7 deletions(-) create mode 100644 src/main/navigation/precision_landing.c create mode 100644 src/main/navigation/precision_landing.h diff --git a/docs/Navigation.md b/docs/Navigation.md index 4d7b9740d91..870123acee1 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -38,6 +38,80 @@ PID meaning: * POS - translated position error to desired velocity, uses P term only * POSR - translates velocity error to desired acceleration +## Precision Landing Target Consumer (MSP) + +INAV can consume externally computed precision-landing target offsets over MSP. +INAV only consumes target updates. + +### Core behavior +* `nav_precision_landing` enables/disables this feature. +* `MSP2_INAV_SET_PRECISION_LANDING_TARGET` updates a target cache and returns acceptance/use status. +* Target updates do **not** arm/disarm, do **not** switch modes, and do **not** start LAND/POSHOLD by themselves. +* Corrections are applied only when: + * active profile is MC/VTOL-hover-capable, and + * current navigation context is POSHOLD-compatible or LAND-compatible. +* In fixed-wing profile, updates may be cached but are not used for correction. + +### POSHOLD behavior +When a fresh/valid target exists, horizontal correction is applied to improve alignment above the target. +Descent is not started automatically in POSHOLD. + +### LAND behavior +When LAND is active and target data is fresh/valid, INAV applies horizontal correction through normal navigation controllers. +Vertical descent profile remains the standard INAV LAND behavior (`nav_land_*` settings). +If target is not available at LAND entry, normal landing behavior is used. + +### Lost target recovery +If target is lost after precision correction already became active: +1. HOLD (`nav_precision_landing_lost_hold_time_ms`) +2. CLIMB_AND_RETRY (`nav_precision_landing_retry_altitude_cm`, `nav_precision_landing_retry_timeout_ms`) +3. Repeat up to `nav_precision_landing_retry_count` +4. Fallback to normal landing + +### Key settings +You do not need to tune all settings on day 1. Start with the minimum set, then tune only if needed. + +Minimum required: +* `nav_precision_landing`: Master enable. + `OFF` keeps legacy behavior unchanged. `ON` allows precision-target consumption in MC/VTOL POSHOLD/LAND contexts. +* `nav_precision_landing_source`: Target source selector (currently `MSP` only). + Keep at `MSP`. +* `nav_precision_landing_min_confidence`: Reject low-confidence target reports. + Example: `60` accepts detections >= 60%; raising to `75` reduces false positives but may drop valid detections in poor light. +* `nav_precision_landing_max_target_age_ms`: Freshness limit for cached targets. + Example: at 20 Hz companion update rate (~50 ms period), `500` ms is tolerant; `200` ms is stricter and avoids stale corrections. + +Recommended for first field tests: +* `nav_precision_landing_max_correction_speed_cm_s`: Horizontal correction clamp. + Example: `80-120` cm/s is conservative for small/medium copters; higher values react faster but can look aggressive near touchdown. +* `nav_precision_landing_align_radius_cm`: Radius treated as close-enough center reference for alignment quality checks. + Example: `60-100` cm for MVP tests. + +Safety and fault-handling settings: +* `nav_precision_landing_max_offset_cm`: Hard bound on accepted offset magnitude. + Example: `1500` cm rejects obviously wrong detections. Set `0` to disable this check. +* `nav_precision_landing_lost_hold_time_ms`: Time to hold after target loss before retry climb. + Example: `1200-2000` ms to allow short vision dropouts to recover without immediate climb. +* `nav_precision_landing_retry_count`: Number of retry attempts before fallback to normal landing. + Example: `2` gives two climb/reacquire attempts. +* `nav_precision_landing_retry_altitude_cm`: Climb distance per retry. + Example: `150-300` cm can improve camera view/FOV. +* `nav_precision_landing_retry_timeout_ms`: Max retry phase duration. + `0` = AUTO mode, computed as `2 x nav_precision_landing_lost_hold_time_ms`. + Example: with `lost_hold=1500`, AUTO timeout becomes `3000` ms. + +Suggested starter profile (conservative): +* `nav_precision_landing = ON` +* `nav_precision_landing_source = MSP` +* `nav_precision_landing_min_confidence = 60` +* `nav_precision_landing_max_target_age_ms = 500` +* `nav_precision_landing_max_correction_speed_cm_s = 100` +* `nav_precision_landing_align_radius_cm = 80` +* `nav_precision_landing_lost_hold_time_ms = 1500` +* `nav_precision_landing_retry_count = 2` +* `nav_precision_landing_retry_altitude_cm = 200` +* `nav_precision_landing_retry_timeout_ms = 0` (AUTO) + ## 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/development/msp/README.md b/docs/development/msp/README.md index a09b622b651..cd046121d83 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_PRECISION_LANDING_TARGET](#msp2_inav_set_precision_landing_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,31 @@ 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_PRECISION_LANDING_TARGET (8753 / 0x2231)` +**Description:** Updates the external precision-landing target cache used by NAV precision landing. 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| +|---|---|---|---|---| +| `valid` | `uint8_t` | 1 | - | 0 clears/invalidates target, non-zero marks update as valid | +| `confidence` | `uint8_t` | 1 | percent | External confidence [0..100] | +| `frame` | `uint8_t` | 1 | enum | Target frame. Current implementation supports `0` = body FRD | +| `offset_forward_cm` | `int16_t` | 2 | cm | Target offset forward from vehicle body origin | +| `offset_right_cm` | `int16_t` | 2 | cm | Target offset right from vehicle body origin | +| `distance_cm` | `uint16_t` | 2 | cm | Optional distance to target (`0` if unknown) | +| `timestamp_ms` | `uint32_t` | 4 | ms | Optional companion timestamp (`0` allowed) | + +**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_precision_state` | `uint8_t` | 1 | Internal precision-landing state | +| `reason` | `uint8_t` | 1 | Result reason (`OK`, `NOT_ENABLED`, `LOW_CONFIDENCE`, `BAD_FRAME`, `STALE`, `OFFSET_TOO_LARGE`, `NOT_MC_PROFILE`, `NOT_IN_POSHOLD_OR_LAND`, etc.) | +| `retry_count` | `uint8_t` | 1 | Active retry attempt count | + +**Notes:** Precision corrections are only applied in MC/VTOL-hover-capable profile and only in POSHOLD/LAND-compatible contexts. Outside those contexts, valid updates may still be cached (`used_now = 0`). + ## `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 +4761,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..5133145a727 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -11260,6 +11260,92 @@ "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_PRECISION_LANDING_TARGET": { + "code": 8753, + "mspv": 2, + "request": { + "payload": [ + { + "name": "valid", + "ctype": "uint8_t", + "desc": "0 clears/invalidates target, non-zero marks update as valid.", + "units": "" + }, + { + "name": "confidence", + "ctype": "uint8_t", + "desc": "External confidence value.", + "units": "percent" + }, + { + "name": "frame", + "ctype": "uint8_t", + "desc": "Target coordinate frame; currently 0 = body FRD.", + "units": "enum" + }, + { + "name": "offset_forward_cm", + "ctype": "int16_t", + "desc": "Target forward offset relative to vehicle body frame.", + "units": "cm" + }, + { + "name": "offset_right_cm", + "ctype": "int16_t", + "desc": "Target right offset relative to vehicle body frame.", + "units": "cm" + }, + { + "name": "distance_cm", + "ctype": "uint16_t", + "desc": "Optional distance to target (0 if unknown).", + "units": "cm" + }, + { + "name": "timestamp_ms", + "ctype": "uint32_t", + "desc": "Optional companion timestamp (0 allowed).", + "units": "ms" + } + ] + }, + "reply": { + "payload": [ + { + "name": "accepted", + "ctype": "uint8_t", + "desc": "Payload accepted by precision-landing target consumer.", + "units": "" + }, + { + "name": "used_now", + "ctype": "uint8_t", + "desc": "Target is currently influencing navigation correction.", + "units": "" + }, + { + "name": "nav_precision_state", + "ctype": "uint8_t", + "desc": "Current internal precision-landing state.", + "units": "enum" + }, + { + "name": "reason", + "ctype": "uint8_t", + "desc": "Processing reason/result code.", + "units": "enum" + }, + { + "name": "retry_count", + "ctype": "uint8_t", + "desc": "Current retry counter value.", + "units": "" + } + ] + }, + "notes": "This message updates precision-landing target cache only. It does not arm/disarm, does not switch modes, and does not trigger landing by itself. Precision correction is applied only in MC/VTOL hover-capable profile and POSHOLD/LAND-compatible navigation contexts.", + "description": "Updates the external precision-landing target cache consumed by NAV precision landing." + }, "MSP2_INAV_SET_CRUISE_HEADING": { "code": 8739, "mspv": 2, @@ -11354,4 +11440,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..84a6aad19b9 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/precision_landing.c + navigation/precision_landing.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..9c58827c80d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -109,6 +109,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" //for MSP_SIMULATOR +#include "navigation/precision_landing.h" #include "navigation/navigation_pos_estimator_private.h" //for MSP_SIMULATOR #include "rx/rx.h" @@ -4367,6 +4368,37 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif + case MSP2_INAV_SET_PRECISION_LANDING_TARGET: + if (dataSize != (3 * sizeof(uint8_t) + 2 * sizeof(int16_t) + sizeof(uint16_t) + sizeof(uint32_t))) { + *ret = MSP_RESULT_ERROR; + break; + } + { + precisionLandingTargetUpdate_t update = { + .valid = sbufReadU8(src), + .confidence = sbufReadU8(src), + .frame = sbufReadU8(src), + .offsetForwardCm = (int16_t)sbufReadU16(src), + .offsetRightCm = (int16_t)sbufReadU16(src), + .distanceCm = sbufReadU16(src), + .timestampMs = sbufReadU32(src), + }; + + precisionLandingMspResponse_t response = { 0 }; + if (!precisionLandingHandleMspTargetUpdate(&update, &response)) { + *ret = MSP_RESULT_ERROR; + break; + } + + sbufWriteU8(dst, response.accepted); + sbufWriteU8(dst, response.usedNow); + sbufWriteU8(dst, response.navPrecisionState); + sbufWriteU8(dst, response.reason); + sbufWriteU8(dst, response.retryCount); + *ret = MSP_RESULT_ACK; + } + break; + 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..205265af9e6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -168,6 +168,8 @@ tables: - name: nav_rth_climb_first enum: navRTHClimbFirst_e values: ["OFF", "ON", "ON_FW_SPIRAL"] + - name: nav_precision_landing_source + values: ["MSP"] - name: nav_wp_mission_restart enum: navMissionRestart_e values: ["START", "RESUME", "SWITCH"] @@ -2759,6 +2761,70 @@ groups: min: 0 max: 1800 field: general.rth_fs_landing_delay + - name: nav_precision_landing + description: "Enable MSP-based precision landing target consumption. This does not trigger mode changes on its own." + default_value: OFF + field: general.precision_landing + type: bool + - name: nav_precision_landing_source + description: "Precision landing target source." + default_value: "MSP" + field: general.precision_landing_source + table: nav_precision_landing_source + - name: nav_precision_landing_min_confidence + description: "Minimum confidence required to accept/use a precision landing target update [0..100]." + default_value: 60 + field: general.precision_landing_min_confidence + min: 0 + max: 100 + - name: nav_precision_landing_max_target_age_ms + description: "Maximum age of a cached precision target that can still be used [ms]." + default_value: 500 + field: general.precision_landing_max_target_age_ms + min: 50 + max: 5000 + - name: nav_precision_landing_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.precision_landing_max_offset_cm + min: 0 + max: 5000 + - name: nav_precision_landing_align_radius_cm + description: "Horizontal radius considered close enough to center for precision-alignment quality checks [cm]." + default_value: 80 + field: general.precision_landing_align_radius_cm + min: 10 + max: 2000 + - name: nav_precision_landing_max_correction_speed_cm_s + description: "Maximum horizontal correction speed generated from target offset [cm/s]." + default_value: 100 + field: general.precision_landing_max_correction_speed_cm_s + min: 10 + max: 1000 + - name: nav_precision_landing_lost_hold_time_ms + description: "Hold duration after target loss before climb-and-retry starts [ms]." + default_value: 1500 + field: general.precision_landing_lost_hold_time_ms + min: 100 + max: 10000 + - name: nav_precision_landing_retry_count + description: "Maximum number of climb-and-retry attempts after target loss." + default_value: 2 + field: general.precision_landing_retry_count + min: 0 + max: 10 + - name: nav_precision_landing_retry_altitude_cm + description: "Climb distance for each retry attempt after target loss [cm]." + default_value: 200 + field: general.precision_landing_retry_altitude_cm + min: 50 + max: 2000 + - name: nav_precision_landing_retry_timeout_ms + description: "Timeout for each climb-and-retry phase [ms]. Set to 0 for AUTO mode (computed as 2 x nav_precision_landing_lost_hold_time_ms)." + default_value: 0 + field: general.precision_landing_retry_timeout_ms + 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..c2fe77d21ae 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1118,6 +1118,18 @@ static const char * navigationStateMessage(void) case MW_NAV_STATE_LAND_START_DESCENT: // Not used break; + case MW_NAV_STATE_PRECISION_LANDING_STANDBY: + return OSD_MESSAGE_STR("PREC LAND STANDBY"); + case MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION: + return OSD_MESSAGE_STR("PREC LAND ALIGN"); + case MW_NAV_STATE_PRECISION_LANDING_LAND_CORRECTION: + return OSD_MESSAGE_STR("PREC LAND DESCENT"); + case MW_NAV_STATE_PRECISION_LANDING_TARGET_LOST_HOLD: + return OSD_MESSAGE_STR("PREC LAND HOLD"); + case MW_NAV_STATE_PRECISION_LANDING_CLIMB_AND_RETRY: + return OSD_MESSAGE_STR("PREC LAND RETRY"); + case MW_NAV_STATE_PRECISION_LANDING_FALLBACK_NORMAL_LAND: + return OSD_MESSAGE_STR("PREC LAND FALLBACK"); } return NULL; diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index a8adf663d57..969038d2b25 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -638,6 +638,18 @@ static char * navigationStateMessage(void) case MW_NAV_STATE_LAND_START_DESCENT: // Not used break; + case MW_NAV_STATE_PRECISION_LANDING_STANDBY: + return OSD_MESSAGE_STR("PREC LAND STANDBY"); + case MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION: + return OSD_MESSAGE_STR("PREC LAND ALIGN"); + case MW_NAV_STATE_PRECISION_LANDING_LAND_CORRECTION: + return OSD_MESSAGE_STR("PREC LAND DESCENT"); + case MW_NAV_STATE_PRECISION_LANDING_TARGET_LOST_HOLD: + return OSD_MESSAGE_STR("PREC LAND HOLD"); + case MW_NAV_STATE_PRECISION_LANDING_CLIMB_AND_RETRY: + return OSD_MESSAGE_STR("PREC LAND RETRY"); + case MW_NAV_STATE_PRECISION_LANDING_FALLBACK_NORMAL_LAND: + return OSD_MESSAGE_STR("PREC LAND FALLBACK"); } return NULL; } diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index b9aeb6b879c..9d1cd47ccae 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_PRECISION_LANDING_TARGET 0x2231 //in message update external precision-landing target cache; does not trigger mode changes #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/navigation.c b/src/main/navigation/navigation.c index 8f60155b68c..6fea2677d87 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -56,6 +56,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "navigation/precision_landing.h" #include "navigation/rth_trackback.h" #include "rx/rx.h" @@ -119,7 +120,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 +178,17 @@ 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 + .precision_landing = SETTING_NAV_PRECISION_LANDING_DEFAULT, + .precision_landing_source = SETTING_NAV_PRECISION_LANDING_SOURCE_DEFAULT, + .precision_landing_min_confidence = SETTING_NAV_PRECISION_LANDING_MIN_CONFIDENCE_DEFAULT, + .precision_landing_max_target_age_ms = SETTING_NAV_PRECISION_LANDING_MAX_TARGET_AGE_MS_DEFAULT, + .precision_landing_max_offset_cm = SETTING_NAV_PRECISION_LANDING_MAX_OFFSET_CM_DEFAULT, + .precision_landing_align_radius_cm = SETTING_NAV_PRECISION_LANDING_ALIGN_RADIUS_CM_DEFAULT, + .precision_landing_max_correction_speed_cm_s = SETTING_NAV_PRECISION_LANDING_MAX_CORRECTION_SPEED_CM_S_DEFAULT, + .precision_landing_lost_hold_time_ms = SETTING_NAV_PRECISION_LANDING_LOST_HOLD_TIME_MS_DEFAULT, + .precision_landing_retry_count = SETTING_NAV_PRECISION_LANDING_RETRY_COUNT_DEFAULT, + .precision_landing_retry_altitude_cm = SETTING_NAV_PRECISION_LANDING_RETRY_ALTITUDE_CM_DEFAULT, + .precision_landing_retry_timeout_ms = SETTING_NAV_PRECISION_LANDING_RETRY_TIMEOUT_MS_DEFAULT, }, // MC-specific @@ -1884,7 +1896,16 @@ 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); + precisionLandingLandControl_t precisionLandControl = { 0 }; + precisionLandingGetLandControl(&precisionLandControl); + + if (precisionLandControl.mode == PREC_LAND_LAND_CTRL_HOLD) { + updateClimbRateToAltitudeController(0.0f, 0.0f, ROC_TO_ALT_CONSTANT); + } else if (precisionLandControl.mode == PREC_LAND_LAND_CTRL_CLIMB) { + updateClimbRateToAltitudeController(precisionLandControl.rateCmS, 0.0f, ROC_TO_ALT_CONSTANT); + } else { + updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT); + } return NAV_FSM_EVENT_NONE; } @@ -2691,7 +2712,7 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) } } - NAV_Status.state = navFSM[posControl.navState].mwState; + NAV_Status.state = precisionLandingOverrideNavStatusState(navFSM[posControl.navState].mwState); NAV_Status.error = navFSM[posControl.navState].mwError; NAV_Status.flags = 0; @@ -4385,6 +4406,7 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback resetRthTrackBack(); + precisionLandingReset(); #ifdef USE_GEOZONE posControl.flags.sendToActive = false; @@ -4405,6 +4427,8 @@ void applyWaypointNavigationAndAltitudeHold(void) /* Process controllers */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); + precisionLandingUpdate(navStateFlags, currentTimeUs); + if (STATE(ROVER) || STATE(BOAT)) { applyRoverBoatNavigationController(navStateFlags, currentTimeUs); } else if (STATE(FIXED_WING_LEGACY)) { @@ -5097,6 +5121,7 @@ void navigationInit(void) /* Reset statistics */ posControl.totalTripDistance = 0.0f; + precisionLandingReset(); /* Use system config */ navigationUsePIDs(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 398781fa596..fa00dd0d589 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -386,6 +386,10 @@ typedef struct positionEstimationConfig_s { PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); +typedef enum { + NAV_PRECISION_LANDING_SOURCE_MSP = 0, +} navPrecisionLandingSource_e; + typedef struct navConfig_s { struct { @@ -441,6 +445,17 @@ 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) + bool precision_landing; // Enable MSP-driven precision landing target consumer + uint8_t precision_landing_source; // navPrecisionLandingSource_e + uint8_t precision_landing_min_confidence; // [0..100] + uint16_t precision_landing_max_target_age_ms; + uint16_t precision_landing_max_offset_cm; + uint16_t precision_landing_align_radius_cm; + uint16_t precision_landing_max_correction_speed_cm_s; + uint16_t precision_landing_lost_hold_time_ms; + uint8_t precision_landing_retry_count; + uint16_t precision_landing_retry_altitude_cm; + uint16_t precision_landing_retry_timeout_ms; } general; struct { @@ -626,6 +641,12 @@ 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 + MW_NAV_STATE_PRECISION_LANDING_STANDBY, + MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION, + MW_NAV_STATE_PRECISION_LANDING_LAND_CORRECTION, + MW_NAV_STATE_PRECISION_LANDING_TARGET_LOST_HOLD, + MW_NAV_STATE_PRECISION_LANDING_CLIMB_AND_RETRY, + MW_NAV_STATE_PRECISION_LANDING_FALLBACK_NORMAL_LAND, } navSystemStatus_State_e; typedef enum { diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 7da742829c5..5c8b7a60650 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -50,6 +50,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "navigation/precision_landing.h" #include "navigation/sqrt_controller.h" #include "sensors/battery.h" @@ -533,6 +534,9 @@ 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 precision landing/alignment correction (MC/VTOL hover contexts only). + precisionLandingApplyHorizontalVelocityCorrection(&posControl.desiredState.vel.x, &posControl.desiredState.vel.y); } static float computeNormalizedVelocity(const float value, const float maxValue) @@ -1019,4 +1023,4 @@ void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlag if (navStateFlags & NAV_CTL_YAW) applyMulticopterHeadingController(); } -} \ No newline at end of file +} diff --git a/src/main/navigation/precision_landing.c b/src/main/navigation/precision_landing.c new file mode 100644 index 00000000000..e9fcda91837 --- /dev/null +++ b/src/main/navigation/precision_landing.c @@ -0,0 +1,483 @@ +#include +#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/precision_landing.h" + +#include "sensors/sensors.h" + +typedef struct { + bool cached; + bool valid; + uint8_t confidence; + uint8_t frame; + int16_t offsetForwardCm; + int16_t offsetRightCm; + uint16_t distanceCm; + uint32_t companionTimestampMs; + timeMs_t receivedAtMs; +} precisionLandingTargetCache_t; + +typedef struct { + precisionLandingState_e state; + precisionLandingTargetCache_t target; + uint8_t retryCount; + bool landCorrectionEverActive; + timeMs_t stateDeadlineMs; + fpVector2_t correctionVelNeu; + precisionLandingLandControl_t landControl; +} precisionLandingRuntime_t; + +static precisionLandingRuntime_t precisionLanding; + +static bool precisionLandingFeatureEnabled(void) +{ + return navConfig()->general.precision_landing && + navConfig()->general.precision_landing_source == NAV_PRECISION_LANDING_SOURCE_MSP; +} + +static bool isMcHoverCapableProfileActive(void) +{ + return STATE(MULTIROTOR) && !STATE(AIRPLANE); +} + +static bool precisionLandingIsPosholdContext(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 precisionLandingIsLandContext(navigationFSMStateFlags_t navStateFlags) +{ + return (navStateFlags & NAV_CTL_LAND) && !STATE(AIRPLANE); +} + +static bool precisionLandingManualTakeoverActive(void) +{ + return posControl.flags.isAdjustingAltitude || + posControl.flags.isAdjustingPosition || + posControl.flags.isAdjustingHeading; +} + +static void setPrecisionState(precisionLandingState_e state) +{ + precisionLanding.state = state; +} + +static float precisionLandingTargetOffsetMagnitudeCm(const precisionLandingTargetCache_t *target) +{ + return calc_length_pythagorean_2D((float)target->offsetForwardCm, (float)target->offsetRightCm); +} + +static bool precisionLandingTargetIsFresh(timeMs_t nowMs, precisionLandingReason_e *reasonOut) +{ + if (!precisionLanding.target.cached || !precisionLanding.target.valid) { + if (reasonOut) { + *reasonOut = PREC_LAND_REASON_INVALID_TARGET; + } + return false; + } + + if (precisionLanding.target.frame != PREC_LAND_FRAME_BODY_FRD) { + if (reasonOut) { + *reasonOut = PREC_LAND_REASON_BAD_FRAME; + } + return false; + } + + if (precisionLanding.target.confidence < navConfig()->general.precision_landing_min_confidence) { + if (reasonOut) { + *reasonOut = PREC_LAND_REASON_LOW_CONFIDENCE; + } + return false; + } + + if (navConfig()->general.precision_landing_max_target_age_ms > 0 && + (nowMs - precisionLanding.target.receivedAtMs) > navConfig()->general.precision_landing_max_target_age_ms) { + if (reasonOut) { + *reasonOut = PREC_LAND_REASON_STALE; + } + return false; + } + + if (navConfig()->general.precision_landing_max_offset_cm > 0 && + precisionLandingTargetOffsetMagnitudeCm(&precisionLanding.target) > navConfig()->general.precision_landing_max_offset_cm) { + if (reasonOut) { + *reasonOut = PREC_LAND_REASON_OFFSET_TOO_LARGE; + } + return false; + } + + if (reasonOut) { + *reasonOut = PREC_LAND_REASON_OK; + } + return true; +} + +static void clearRuntimeOutputs(void) +{ + precisionLanding.correctionVelNeu.x = 0.0f; + precisionLanding.correctionVelNeu.y = 0.0f; + precisionLanding.landControl.mode = PREC_LAND_LAND_CTRL_NONE; + precisionLanding.landControl.rateCmS = 0.0f; +} + +static void clearRuntimeStateKeepCache(void) +{ + precisionLanding.retryCount = 0; + precisionLanding.landCorrectionEverActive = false; + precisionLanding.stateDeadlineMs = 0; + clearRuntimeOutputs(); + setPrecisionState(PREC_LAND_IDLE); +} + +static void clearRuntimeStateAndTarget(void) +{ + precisionLanding.target.cached = false; + precisionLanding.target.valid = false; + clearRuntimeStateKeepCache(); +} + +void precisionLandingReset(void) +{ + clearRuntimeStateAndTarget(); +} + +static void computeHorizontalCorrectionVelocity(void) +{ + const float offsetForward = precisionLanding.target.offsetForwardCm; + const float offsetRight = precisionLanding.target.offsetRightCm; + const float offsetMagnitude = precisionLandingTargetOffsetMagnitudeCm(&precisionLanding.target); + const float alignRadiusCm = navConfig()->general.precision_landing_align_radius_cm; + + if (alignRadiusCm > 0.0f && offsetMagnitude <= alignRadiusCm) { + precisionLanding.correctionVelNeu.x = 0.0f; + precisionLanding.correctionVelNeu.y = 0.0f; + return; + } + + const float cosYaw = posControl.actualState.cosYaw; + const float sinYaw = posControl.actualState.sinYaw; + + float velN = offsetForward * cosYaw - offsetRight * sinYaw; + float velE = offsetForward * sinYaw + offsetRight * cosYaw; + + const float maxCorrectionSpeed = navConfig()->general.precision_landing_max_correction_speed_cm_s; + const float speed = calc_length_pythagorean_2D(velN, velE); + + if (maxCorrectionSpeed > 0.0f && speed > maxCorrectionSpeed) { + const float scale = maxCorrectionSpeed / speed; + velN *= scale; + velE *= scale; + } + + if (alignRadiusCm > 0.0f && offsetMagnitude > alignRadiusCm) { + // Ease corrections just outside alignment radius to reduce jitter around the target center. + const float ease = constrainf((offsetMagnitude - alignRadiusCm) / offsetMagnitude, 0.0f, 1.0f); + velN *= ease; + velE *= ease; + } + + precisionLanding.correctionVelNeu.x = velN; + precisionLanding.correctionVelNeu.y = velE; +} + +static uint16_t getRetryTimeoutMs(void) +{ + const uint16_t configuredRetryTimeoutMs = navConfig()->general.precision_landing_retry_timeout_ms; + if (configuredRetryTimeoutMs > 0) { + return MAX(configuredRetryTimeoutMs, 100); + } + + // Auto mode: derive retry timeout from lost-hold duration to reduce required tuning. + const uint16_t lostHoldMs = MAX(navConfig()->general.precision_landing_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.precision_landing_retry_altitude_cm / retryTimeoutS; + return constrainf(climbRate, 20.0f, navConfig()->mc.max_auto_climb_rate); +} + +static void setLostHoldState(timeMs_t nowMs) +{ + setPrecisionState(PREC_LAND_TARGET_LOST_HOLD); + precisionLanding.stateDeadlineMs = nowMs + MAX(navConfig()->general.precision_landing_lost_hold_time_ms, 100); +} + +static void setClimbRetryState(timeMs_t nowMs) +{ + setPrecisionState(PREC_LAND_CLIMB_AND_RETRY); + precisionLanding.stateDeadlineMs = nowMs + getRetryTimeoutMs(); +} + +void precisionLandingUpdate(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + clearRuntimeOutputs(); + + const timeMs_t nowMs = millis(); + + if (!precisionLandingFeatureEnabled()) { + clearRuntimeStateKeepCache(); + return; + } + + if (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating() || STATE(LANDING_DETECTED)) { + clearRuntimeStateAndTarget(); + return; + } + + const bool mcProfileActive = isMcHoverCapableProfileActive(); + const bool posholdContext = precisionLandingIsPosholdContext(navStateFlags); + const bool landContext = precisionLandingIsLandContext(navStateFlags); + const bool precisionContextActive = mcProfileActive && (posholdContext || landContext); + + if (!precisionContextActive || precisionLandingManualTakeoverActive()) { + precisionLanding.landCorrectionEverActive = false; + precisionLanding.retryCount = 0; + precisionLanding.stateDeadlineMs = 0; + + precisionLandingReason_e reason; + if (precisionLandingTargetIsFresh(nowMs, &reason)) { + setPrecisionState(PREC_LAND_STANDBY); + } else { + setPrecisionState(PREC_LAND_IDLE); + } + return; + } + + precisionLandingReason_e freshnessReason = PREC_LAND_REASON_OK; + const bool targetFresh = precisionLandingTargetIsFresh(nowMs, &freshnessReason); + + if (posholdContext) { + precisionLanding.retryCount = 0; + precisionLanding.landCorrectionEverActive = false; + precisionLanding.stateDeadlineMs = 0; + + if (targetFresh) { + setPrecisionState(PREC_LAND_POSHOLD_CORRECTION); + computeHorizontalCorrectionVelocity(); + } else { + setPrecisionState(PREC_LAND_STANDBY); + } + return; + } + + if (!landContext) { + setPrecisionState(PREC_LAND_STANDBY); + return; + } + + if (precisionLanding.state != PREC_LAND_TARGET_LOST_HOLD && + precisionLanding.state != PREC_LAND_CLIMB_AND_RETRY && + precisionLanding.state != PREC_LAND_FALLBACK_NORMAL_LAND) { + if (targetFresh) { + setPrecisionState(PREC_LAND_LAND_CORRECTION); + } else if (precisionLanding.landCorrectionEverActive) { + setLostHoldState(nowMs); + } else { + setPrecisionState(PREC_LAND_STANDBY); + } + } + + switch (precisionLanding.state) { + case PREC_LAND_LAND_CORRECTION: + if (!targetFresh) { + if (precisionLanding.landCorrectionEverActive) { + setLostHoldState(nowMs); + } else { + setPrecisionState(PREC_LAND_STANDBY); + } + break; + } + + precisionLanding.landCorrectionEverActive = true; + computeHorizontalCorrectionVelocity(); + break; + + case PREC_LAND_TARGET_LOST_HOLD: + precisionLanding.landControl.mode = PREC_LAND_LAND_CTRL_HOLD; + precisionLanding.landControl.rateCmS = 0.0f; + + if (targetFresh) { + setPrecisionState(PREC_LAND_LAND_CORRECTION); + break; + } + + if (nowMs >= precisionLanding.stateDeadlineMs) { + if (precisionLanding.retryCount < navConfig()->general.precision_landing_retry_count) { + setClimbRetryState(nowMs); + } else { + setPrecisionState(PREC_LAND_FALLBACK_NORMAL_LAND); + } + } + break; + + case PREC_LAND_CLIMB_AND_RETRY: + precisionLanding.landControl.mode = PREC_LAND_LAND_CTRL_CLIMB; + precisionLanding.landControl.rateCmS = getRetryClimbRateCmS(); + + if (targetFresh) { + setPrecisionState(PREC_LAND_LAND_CORRECTION); + break; + } + + if (nowMs >= precisionLanding.stateDeadlineMs) { + precisionLanding.retryCount++; + if (precisionLanding.retryCount >= navConfig()->general.precision_landing_retry_count) { + setPrecisionState(PREC_LAND_FALLBACK_NORMAL_LAND); + } else { + setLostHoldState(nowMs); + } + } + break; + + case PREC_LAND_FALLBACK_NORMAL_LAND: + precisionLanding.landControl.mode = PREC_LAND_LAND_CTRL_NONE; + precisionLanding.landControl.rateCmS = 0.0f; + break; + + default: + break; + } +} + +void precisionLandingApplyHorizontalVelocityCorrection(float *velX, float *velY) +{ + if (!velX || !velY) { + return; + } + + if (precisionLanding.state == PREC_LAND_POSHOLD_CORRECTION || precisionLanding.state == PREC_LAND_LAND_CORRECTION) { + *velX += precisionLanding.correctionVelNeu.x; + *velY += precisionLanding.correctionVelNeu.y; + } +} + +void precisionLandingGetLandControl(precisionLandingLandControl_t *controlOut) +{ + if (!controlOut) { + return; + } + + *controlOut = precisionLanding.landControl; +} + +navSystemStatus_State_e precisionLandingOverrideNavStatusState(navSystemStatus_State_e defaultState) +{ + switch (precisionLanding.state) { + case PREC_LAND_STANDBY: + return MW_NAV_STATE_PRECISION_LANDING_STANDBY; + case PREC_LAND_POSHOLD_CORRECTION: + return MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION; + case PREC_LAND_LAND_CORRECTION: + return MW_NAV_STATE_PRECISION_LANDING_LAND_CORRECTION; + case PREC_LAND_TARGET_LOST_HOLD: + return MW_NAV_STATE_PRECISION_LANDING_TARGET_LOST_HOLD; + case PREC_LAND_CLIMB_AND_RETRY: + return MW_NAV_STATE_PRECISION_LANDING_CLIMB_AND_RETRY; + case PREC_LAND_FALLBACK_NORMAL_LAND: + return MW_NAV_STATE_PRECISION_LANDING_FALLBACK_NORMAL_LAND; + default: + return defaultState; + } +} + +bool precisionLandingHandleMspTargetUpdate(const precisionLandingTargetUpdate_t *update, precisionLandingMspResponse_t *responseOut) +{ + if (!update || !responseOut) { + return false; + } + + responseOut->accepted = 0; + responseOut->usedNow = 0; + responseOut->navPrecisionState = (uint8_t)precisionLanding.state; + responseOut->reason = PREC_LAND_REASON_INVALID_TARGET; + responseOut->retryCount = precisionLanding.retryCount; + + if (!precisionLandingFeatureEnabled()) { + responseOut->accepted = 1; + responseOut->reason = PREC_LAND_REASON_NOT_ENABLED; + return true; + } + + if (!update->valid) { + precisionLanding.target.cached = false; + precisionLanding.target.valid = false; + responseOut->accepted = 1; + responseOut->reason = PREC_LAND_REASON_INVALID_TARGET; + return true; + } + + if (update->frame != PREC_LAND_FRAME_BODY_FRD) { + responseOut->reason = PREC_LAND_REASON_BAD_FRAME; + return true; + } + + if (update->confidence < navConfig()->general.precision_landing_min_confidence) { + responseOut->reason = PREC_LAND_REASON_LOW_CONFIDENCE; + return true; + } + + const float offsetMagnitude = calc_length_pythagorean_2D((float)update->offsetForwardCm, (float)update->offsetRightCm); + if (navConfig()->general.precision_landing_max_offset_cm > 0 && + offsetMagnitude > navConfig()->general.precision_landing_max_offset_cm) { + responseOut->reason = PREC_LAND_REASON_OFFSET_TOO_LARGE; + return true; + } + + precisionLanding.target.cached = true; + precisionLanding.target.valid = true; + precisionLanding.target.confidence = update->confidence; + precisionLanding.target.frame = update->frame; + precisionLanding.target.offsetForwardCm = update->offsetForwardCm; + precisionLanding.target.offsetRightCm = update->offsetRightCm; + precisionLanding.target.distanceCm = update->distanceCm; + precisionLanding.target.companionTimestampMs = update->timestampMs; + precisionLanding.target.receivedAtMs = millis(); + + responseOut->accepted = 1; + responseOut->reason = PREC_LAND_REASON_OK; + + const bool mcProfileActive = isMcHoverCapableProfileActive(); + const navigationFSMStateFlags_t navStateFlags = navGetCurrentStateFlags(); + const bool posholdContext = precisionLandingIsPosholdContext(navStateFlags); + const bool landContext = precisionLandingIsLandContext(navStateFlags); + const bool inContext = posholdContext || landContext; + + if (!ARMING_FLAG(ARMED)) { + responseOut->usedNow = 0; + responseOut->reason = PREC_LAND_REASON_NOT_ARMED; + } else if (FLIGHT_MODE(FAILSAFE_MODE)) { + responseOut->usedNow = 0; + responseOut->reason = PREC_LAND_REASON_FAILSAFE; + } else if (!mcProfileActive) { + responseOut->usedNow = 0; + responseOut->reason = PREC_LAND_REASON_NOT_MC_PROFILE; + } else if (!inContext) { + responseOut->usedNow = 0; + responseOut->reason = PREC_LAND_REASON_NOT_IN_POSHOLD_OR_LAND; + } else { + responseOut->usedNow = (precisionLanding.state == PREC_LAND_POSHOLD_CORRECTION || precisionLanding.state == PREC_LAND_LAND_CORRECTION); + responseOut->reason = responseOut->usedNow ? PREC_LAND_REASON_OK : PREC_LAND_REASON_NOT_IN_POSHOLD_OR_LAND; + } + + responseOut->navPrecisionState = (uint8_t)precisionLanding.state; + responseOut->retryCount = precisionLanding.retryCount; + return true; +} diff --git a/src/main/navigation/precision_landing.h b/src/main/navigation/precision_landing.h new file mode 100644 index 00000000000..8a7215319dc --- /dev/null +++ b/src/main/navigation/precision_landing.h @@ -0,0 +1,72 @@ +#pragma once + +#include +#include + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +typedef enum { + PREC_LAND_FRAME_BODY_FRD = 0, +} precisionLandingFrame_e; + +typedef enum { + PREC_LAND_REASON_OK = 0, + PREC_LAND_REASON_NOT_ENABLED, + PREC_LAND_REASON_LOW_CONFIDENCE, + PREC_LAND_REASON_BAD_FRAME, + PREC_LAND_REASON_STALE, + PREC_LAND_REASON_OFFSET_TOO_LARGE, + PREC_LAND_REASON_NOT_MC_PROFILE, + PREC_LAND_REASON_NOT_IN_POSHOLD_OR_LAND, + PREC_LAND_REASON_FAILSAFE, + PREC_LAND_REASON_INVALID_TARGET, + PREC_LAND_REASON_NOT_ARMED, +} precisionLandingReason_e; + +typedef enum { + PREC_LAND_IDLE = 0, + PREC_LAND_STANDBY, + PREC_LAND_POSHOLD_CORRECTION, + PREC_LAND_LAND_CORRECTION, + PREC_LAND_TARGET_LOST_HOLD, + PREC_LAND_CLIMB_AND_RETRY, + PREC_LAND_FALLBACK_NORMAL_LAND, + PREC_LAND_DONE, +} precisionLandingState_e; + +typedef enum { + PREC_LAND_LAND_CTRL_NONE = 0, + PREC_LAND_LAND_CTRL_HOLD, + PREC_LAND_LAND_CTRL_CLIMB, +} precisionLandingLandControlMode_e; + +typedef struct { + uint8_t valid; + uint8_t confidence; + uint8_t frame; + int16_t offsetForwardCm; + int16_t offsetRightCm; + uint16_t distanceCm; + uint32_t timestampMs; +} precisionLandingTargetUpdate_t; + +typedef struct { + uint8_t accepted; + uint8_t usedNow; + uint8_t navPrecisionState; + uint8_t reason; + uint8_t retryCount; +} precisionLandingMspResponse_t; + +typedef struct { + precisionLandingLandControlMode_e mode; + float rateCmS; +} precisionLandingLandControl_t; + +void precisionLandingReset(void); +void precisionLandingUpdate(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs); +void precisionLandingApplyHorizontalVelocityCorrection(float *velX, float *velY); +void precisionLandingGetLandControl(precisionLandingLandControl_t *controlOut); +navSystemStatus_State_e precisionLandingOverrideNavStatusState(navSystemStatus_State_e defaultState); +bool precisionLandingHandleMspTargetUpdate(const precisionLandingTargetUpdate_t *update, precisionLandingMspResponse_t *responseOut); From 48bab1c6fef8a62d610a4f579085f8738c9cd2a4 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 14 May 2026 16:13:27 +0300 Subject: [PATCH 2/4] navigation: gate precision landing with USE_PRECISION_LANDING and clarify MSP/docs - Add USE_PRECISION_LANDING compile-time guard in target common defaults (enabled for larger-flash targets via common policy) - Wrap precision landing API/state/config fields with USE_PRECISION_LANDING in navigation headers and runtime integration paths - Guard precision landing MSP handler (MSP2_INAV_SET_PRECISION_LANDING_TARGET) and related OSD/nav-state mappings - Make nav_precision_landing* CLI settings conditional on USE_PRECISION_LANDING - Update Navigation/MSP docs: - build-time availability note - precision landing scope/limitations (final correction layer) - conservative "Why not SET_WP?" explanation - clarify timestamp semantics (informational; freshness by FC receive time) This keeps behavior unchanged when precision landing is not compiled in and improves compatibility for flash-constrained targets. --- docs/Navigation.md | 20 +++++++++++++++++ docs/development/msp/README.md | 4 ++-- docs/development/msp/msp_messages.json | 4 ++-- src/main/fc/fc_msp.c | 4 ++++ src/main/fc/settings.yaml | 11 ++++++++++ src/main/io/osd.c | 2 ++ src/main/io/osd_dji_hd.c | 2 ++ src/main/navigation/navigation.c | 23 ++++++++++++++++++-- src/main/navigation/navigation.h | 6 +++++ src/main/navigation/navigation_multicopter.c | 4 ++++ src/main/navigation/precision_landing.c | 4 ++++ src/main/navigation/precision_landing.h | 4 ++++ src/main/target/common.h | 2 +- 13 files changed, 83 insertions(+), 7 deletions(-) diff --git a/docs/Navigation.md b/docs/Navigation.md index 870123acee1..667431194cb 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -43,6 +43,10 @@ PID meaning: INAV can consume externally computed precision-landing target offsets over MSP. INAV only consumes target updates. +### Build-time availability +This feature is compiled only when `USE_PRECISION_LANDING` is enabled for the target. +On flash-constrained targets, it can be excluded at build time to preserve headroom. + ### Core behavior * `nav_precision_landing` enables/disables this feature. * `MSP2_INAV_SET_PRECISION_LANDING_TARGET` updates a target cache and returns acceptance/use status. @@ -52,6 +56,22 @@ INAV only consumes target updates. * current navigation context is POSHOLD-compatible or LAND-compatible. * In fixed-wing profile, updates may be cached but are not used for correction. +### Scope and limits +Precision landing here is a **final alignment/correction layer**. +It does not replace normal waypoint/home navigation and it does not solve high-speed approach planning by itself. + +Recommended VTOL flow: +1. transition to MC / hover-capable control +2. controlled low-speed arrival near landing zone +3. precision target alignment in POSHOLD +4. precision correction during LAND + +### Why not `SET_WP`? +`SET_WP` is a navigation target command interface. +Precision landing target updates are external measurement inputs with validity/confidence/age semantics and target-loss handling. + +This feature intentionally keeps those semantics bounded to MC POSHOLD/LAND correction, rather than repeatedly rewriting mission/waypoint targets. + ### POSHOLD behavior When a fresh/valid target exists, horizontal correction is applied to improve alignment above the target. Descent is not started automatically in POSHOLD. diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index cd046121d83..ae47db4b86f 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -4711,7 +4711,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `offset_forward_cm` | `int16_t` | 2 | cm | Target offset forward from vehicle body origin | | `offset_right_cm` | `int16_t` | 2 | cm | Target offset right from vehicle body origin | | `distance_cm` | `uint16_t` | 2 | cm | Optional distance to target (`0` if unknown) | -| `timestamp_ms` | `uint32_t` | 4 | ms | Optional companion timestamp (`0` allowed) | +| `timestamp_ms` | `uint32_t` | 4 | ms | Optional companion timestamp (`0` allowed, informational/debug) | **Reply Payload:** |Field|C Type|Size (Bytes)|Description| @@ -4722,7 +4722,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `reason` | `uint8_t` | 1 | Result reason (`OK`, `NOT_ENABLED`, `LOW_CONFIDENCE`, `BAD_FRAME`, `STALE`, `OFFSET_TOO_LARGE`, `NOT_MC_PROFILE`, `NOT_IN_POSHOLD_OR_LAND`, etc.) | | `retry_count` | `uint8_t` | 1 | Active retry attempt count | -**Notes:** Precision corrections are only applied in MC/VTOL-hover-capable profile and only in POSHOLD/LAND-compatible contexts. Outside those contexts, valid updates may still be cached (`used_now = 0`). +**Notes:** Available only when firmware is built with `USE_PRECISION_LANDING`. Precision corrections are only applied in MC/VTOL-hover-capable profile and only in POSHOLD/LAND-compatible contexts. Outside those contexts, valid updates may still be cached (`used_now = 0`). Freshness is evaluated using FC receive time, not companion clock synchronization. ## `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. diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 5133145a727..5db035dc58d 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -11304,7 +11304,7 @@ { "name": "timestamp_ms", "ctype": "uint32_t", - "desc": "Optional companion timestamp (0 allowed).", + "desc": "Optional companion timestamp (0 allowed, informational/debug).", "units": "ms" } ] @@ -11343,7 +11343,7 @@ } ] }, - "notes": "This message updates precision-landing target cache only. It does not arm/disarm, does not switch modes, and does not trigger landing by itself. Precision correction is applied only in MC/VTOL hover-capable profile and POSHOLD/LAND-compatible navigation contexts.", + "notes": "This message updates precision-landing 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_PRECISION_LANDING. Precision correction is applied only in MC/VTOL hover-capable profile and POSHOLD/LAND-compatible navigation contexts. Target freshness is evaluated using FC receive time.", "description": "Updates the external precision-landing target cache consumed by NAV precision landing." }, "MSP2_INAV_SET_CRUISE_HEADING": { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 9c58827c80d..8763e5dfd01 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -109,7 +109,9 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" //for MSP_SIMULATOR +#ifdef USE_PRECISION_LANDING #include "navigation/precision_landing.h" +#endif #include "navigation/navigation_pos_estimator_private.h" //for MSP_SIMULATOR #include "rx/rx.h" @@ -4368,6 +4370,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif +#ifdef USE_PRECISION_LANDING case MSP2_INAV_SET_PRECISION_LANDING_TARGET: if (dataSize != (3 * sizeof(uint8_t) + 2 * sizeof(int16_t) + sizeof(uint16_t) + sizeof(uint32_t))) { *ret = MSP_RESULT_ERROR; @@ -4398,6 +4401,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ACK; } break; +#endif case MSP2_INAV_SET_LOCAL_TARGET: if (dataSize != 3 * sizeof(int32_t) || !isGCSValid()) { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 205265af9e6..02b0cf19d6d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2766,63 +2766,74 @@ groups: default_value: OFF field: general.precision_landing type: bool + condition: USE_PRECISION_LANDING - name: nav_precision_landing_source description: "Precision landing target source." default_value: "MSP" field: general.precision_landing_source table: nav_precision_landing_source + condition: USE_PRECISION_LANDING - name: nav_precision_landing_min_confidence description: "Minimum confidence required to accept/use a precision landing target update [0..100]." default_value: 60 field: general.precision_landing_min_confidence + condition: USE_PRECISION_LANDING min: 0 max: 100 - name: nav_precision_landing_max_target_age_ms description: "Maximum age of a cached precision target that can still be used [ms]." default_value: 500 field: general.precision_landing_max_target_age_ms + condition: USE_PRECISION_LANDING min: 50 max: 5000 - name: nav_precision_landing_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.precision_landing_max_offset_cm + condition: USE_PRECISION_LANDING min: 0 max: 5000 - name: nav_precision_landing_align_radius_cm description: "Horizontal radius considered close enough to center for precision-alignment quality checks [cm]." default_value: 80 field: general.precision_landing_align_radius_cm + condition: USE_PRECISION_LANDING min: 10 max: 2000 - name: nav_precision_landing_max_correction_speed_cm_s description: "Maximum horizontal correction speed generated from target offset [cm/s]." default_value: 100 field: general.precision_landing_max_correction_speed_cm_s + condition: USE_PRECISION_LANDING min: 10 max: 1000 - name: nav_precision_landing_lost_hold_time_ms description: "Hold duration after target loss before climb-and-retry starts [ms]." default_value: 1500 field: general.precision_landing_lost_hold_time_ms + condition: USE_PRECISION_LANDING min: 100 max: 10000 - name: nav_precision_landing_retry_count description: "Maximum number of climb-and-retry attempts after target loss." default_value: 2 field: general.precision_landing_retry_count + condition: USE_PRECISION_LANDING min: 0 max: 10 - name: nav_precision_landing_retry_altitude_cm description: "Climb distance for each retry attempt after target loss [cm]." default_value: 200 field: general.precision_landing_retry_altitude_cm + condition: USE_PRECISION_LANDING min: 50 max: 2000 - name: nav_precision_landing_retry_timeout_ms description: "Timeout for each climb-and-retry phase [ms]. Set to 0 for AUTO mode (computed as 2 x nav_precision_landing_lost_hold_time_ms)." default_value: 0 field: general.precision_landing_retry_timeout_ms + condition: USE_PRECISION_LANDING min: 0 max: 60000 - name: nav_rth_alt_mode diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c2fe77d21ae..021b7f77791 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1118,6 +1118,7 @@ static const char * navigationStateMessage(void) case MW_NAV_STATE_LAND_START_DESCENT: // Not used break; +#ifdef USE_PRECISION_LANDING case MW_NAV_STATE_PRECISION_LANDING_STANDBY: return OSD_MESSAGE_STR("PREC LAND STANDBY"); case MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION: @@ -1130,6 +1131,7 @@ static const char * navigationStateMessage(void) return OSD_MESSAGE_STR("PREC LAND RETRY"); case MW_NAV_STATE_PRECISION_LANDING_FALLBACK_NORMAL_LAND: return OSD_MESSAGE_STR("PREC LAND FALLBACK"); +#endif } return NULL; diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 969038d2b25..46605c6c237 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -638,6 +638,7 @@ static char * navigationStateMessage(void) case MW_NAV_STATE_LAND_START_DESCENT: // Not used break; +#ifdef USE_PRECISION_LANDING case MW_NAV_STATE_PRECISION_LANDING_STANDBY: return OSD_MESSAGE_STR("PREC LAND STANDBY"); case MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION: @@ -650,6 +651,7 @@ static char * navigationStateMessage(void) return OSD_MESSAGE_STR("PREC LAND RETRY"); case MW_NAV_STATE_PRECISION_LANDING_FALLBACK_NORMAL_LAND: return OSD_MESSAGE_STR("PREC LAND FALLBACK"); +#endif } return NULL; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 6fea2677d87..77cbcfb23ef 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -56,7 +56,9 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#ifdef USE_PRECISION_LANDING #include "navigation/precision_landing.h" +#endif #include "navigation/rth_trackback.h" #include "rx/rx.h" @@ -178,6 +180,7 @@ 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_PRECISION_LANDING .precision_landing = SETTING_NAV_PRECISION_LANDING_DEFAULT, .precision_landing_source = SETTING_NAV_PRECISION_LANDING_SOURCE_DEFAULT, .precision_landing_min_confidence = SETTING_NAV_PRECISION_LANDING_MIN_CONFIDENCE_DEFAULT, @@ -189,6 +192,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .precision_landing_retry_count = SETTING_NAV_PRECISION_LANDING_RETRY_COUNT_DEFAULT, .precision_landing_retry_altitude_cm = SETTING_NAV_PRECISION_LANDING_RETRY_ALTITUDE_CM_DEFAULT, .precision_landing_retry_timeout_ms = SETTING_NAV_PRECISION_LANDING_RETRY_TIMEOUT_MS_DEFAULT, +#endif }, // MC-specific @@ -1896,14 +1900,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); } + bool precisionLandingVerticalOverride = false; +#ifdef USE_PRECISION_LANDING precisionLandingLandControl_t precisionLandControl = { 0 }; precisionLandingGetLandControl(&precisionLandControl); if (precisionLandControl.mode == PREC_LAND_LAND_CTRL_HOLD) { updateClimbRateToAltitudeController(0.0f, 0.0f, ROC_TO_ALT_CONSTANT); + precisionLandingVerticalOverride = true; } else if (precisionLandControl.mode == PREC_LAND_LAND_CTRL_CLIMB) { updateClimbRateToAltitudeController(precisionLandControl.rateCmS, 0.0f, ROC_TO_ALT_CONSTANT); - } else { + precisionLandingVerticalOverride = true; + } +#endif + if (!precisionLandingVerticalOverride) { updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT); } @@ -2712,7 +2722,10 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) } } - NAV_Status.state = precisionLandingOverrideNavStatusState(navFSM[posControl.navState].mwState); + NAV_Status.state = navFSM[posControl.navState].mwState; +#ifdef USE_PRECISION_LANDING + NAV_Status.state = precisionLandingOverrideNavStatusState(NAV_Status.state); +#endif NAV_Status.error = navFSM[posControl.navState].mwError; NAV_Status.flags = 0; @@ -4406,7 +4419,9 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback resetRthTrackBack(); +#ifdef USE_PRECISION_LANDING precisionLandingReset(); +#endif #ifdef USE_GEOZONE posControl.flags.sendToActive = false; @@ -4427,7 +4442,9 @@ void applyWaypointNavigationAndAltitudeHold(void) /* Process controllers */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); +#ifdef USE_PRECISION_LANDING precisionLandingUpdate(navStateFlags, currentTimeUs); +#endif if (STATE(ROVER) || STATE(BOAT)) { applyRoverBoatNavigationController(navStateFlags, currentTimeUs); @@ -5121,7 +5138,9 @@ void navigationInit(void) /* Reset statistics */ posControl.totalTripDistance = 0.0f; +#ifdef USE_PRECISION_LANDING precisionLandingReset(); +#endif /* Use system config */ navigationUsePIDs(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index fa00dd0d589..451512620bb 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -386,9 +386,11 @@ typedef struct positionEstimationConfig_s { PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); +#ifdef USE_PRECISION_LANDING typedef enum { NAV_PRECISION_LANDING_SOURCE_MSP = 0, } navPrecisionLandingSource_e; +#endif typedef struct navConfig_s { @@ -445,6 +447,7 @@ 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_PRECISION_LANDING bool precision_landing; // Enable MSP-driven precision landing target consumer uint8_t precision_landing_source; // navPrecisionLandingSource_e uint8_t precision_landing_min_confidence; // [0..100] @@ -456,6 +459,7 @@ typedef struct navConfig_s { uint8_t precision_landing_retry_count; uint16_t precision_landing_retry_altitude_cm; uint16_t precision_landing_retry_timeout_ms; +#endif } general; struct { @@ -641,12 +645,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_PRECISION_LANDING MW_NAV_STATE_PRECISION_LANDING_STANDBY, MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION, MW_NAV_STATE_PRECISION_LANDING_LAND_CORRECTION, MW_NAV_STATE_PRECISION_LANDING_TARGET_LOST_HOLD, MW_NAV_STATE_PRECISION_LANDING_CLIMB_AND_RETRY, MW_NAV_STATE_PRECISION_LANDING_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 5c8b7a60650..c4b750ed572 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -50,7 +50,9 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#ifdef USE_PRECISION_LANDING #include "navigation/precision_landing.h" +#endif #include "navigation/sqrt_controller.h" #include "sensors/battery.h" @@ -536,7 +538,9 @@ static void updatePositionVelocityController_MC(const float maxSpeed) posControl.desiredState.vel.y = neuVelY * velHeadFactor * velExpoFactor; // Optional external precision landing/alignment correction (MC/VTOL hover contexts only). +#ifdef USE_PRECISION_LANDING precisionLandingApplyHorizontalVelocityCorrection(&posControl.desiredState.vel.x, &posControl.desiredState.vel.y); +#endif } static float computeNormalizedVelocity(const float value, const float maxValue) diff --git a/src/main/navigation/precision_landing.c b/src/main/navigation/precision_landing.c index e9fcda91837..d5dcc0e3675 100644 --- a/src/main/navigation/precision_landing.c +++ b/src/main/navigation/precision_landing.c @@ -15,6 +15,8 @@ #include "sensors/sensors.h" +#ifdef USE_PRECISION_LANDING + typedef struct { bool cached; bool valid; @@ -481,3 +483,5 @@ bool precisionLandingHandleMspTargetUpdate(const precisionLandingTargetUpdate_t responseOut->retryCount = precisionLanding.retryCount; return true; } + +#endif diff --git a/src/main/navigation/precision_landing.h b/src/main/navigation/precision_landing.h index 8a7215319dc..e85fbe865de 100644 --- a/src/main/navigation/precision_landing.h +++ b/src/main/navigation/precision_landing.h @@ -1,5 +1,7 @@ #pragma once +#ifdef USE_PRECISION_LANDING + #include #include @@ -70,3 +72,5 @@ void precisionLandingApplyHorizontalVelocityCorrection(float *velX, float *velY) void precisionLandingGetLandControl(precisionLandingLandControl_t *controlOut); navSystemStatus_State_e precisionLandingOverrideNavStatusState(navSystemStatus_State_e defaultState); bool precisionLandingHandleMspTargetUpdate(const precisionLandingTargetUpdate_t *update, precisionLandingMspResponse_t *responseOut); + +#endif diff --git a/src/main/target/common.h b/src/main/target/common.h index 55ca1653f5a..be9d6a976fb 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_PRECISION_LANDING #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 - From e8bd39b9cbc4beb1b996e7e0877377afc5d55d09 Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Thu, 14 May 2026 17:34:48 +0300 Subject: [PATCH 3/4] docs: regenerate Settings.md --- docs/Settings.md | 110 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 110 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index d49067357bc..c2a0920f046 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4434,6 +4434,116 @@ If GPS fails wait for this much seconds before switching to emergency landing mo --- +### nav_precision_landing + +Enable MSP-based precision landing target consumption. This does not trigger mode changes on its own. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### nav_precision_landing_align_radius_cm + +Horizontal radius considered close enough to center for precision-alignment quality checks [cm]. + +| Default | Min | Max | +| --- | --- | --- | +| 80 | 10 | 2000 | + +--- + +### nav_precision_landing_lost_hold_time_ms + +Hold duration after target loss before climb-and-retry starts [ms]. + +| Default | Min | Max | +| --- | --- | --- | +| 1500 | 100 | 10000 | + +--- + +### nav_precision_landing_max_correction_speed_cm_s + +Maximum horizontal correction speed generated from target offset [cm/s]. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 10 | 1000 | + +--- + +### nav_precision_landing_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_precision_landing_max_target_age_ms + +Maximum age of a cached precision target that can still be used [ms]. + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 50 | 5000 | + +--- + +### nav_precision_landing_min_confidence + +Minimum confidence required to accept/use a precision landing target update [0..100]. + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 100 | + +--- + +### nav_precision_landing_retry_altitude_cm + +Climb distance for each retry attempt after target loss [cm]. + +| Default | Min | Max | +| --- | --- | --- | +| 200 | 50 | 2000 | + +--- + +### nav_precision_landing_retry_count + +Maximum number of climb-and-retry attempts after target loss. + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 10 | + +--- + +### nav_precision_landing_retry_timeout_ms + +Timeout for each climb-and-retry phase [ms]. Set to 0 for AUTO mode (computed as 2 x nav_precision_landing_lost_hold_time_ms). + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 60000 | + +--- + +### nav_precision_landing_source + +Precision landing 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] From b6346d71e20e8fffa4dfb096177e4ffad63a544b Mon Sep 17 00:00:00 2001 From: Martin Petrov Date: Wed, 20 May 2026 11:35:56 +0300 Subject: [PATCH 4/4] feat(nav,msp): unify precision-landing and containment as marker guidance - rename feature guard: USE_PRECISION_LANDING -> USE_MARKER_GUIDANCE - rename MSP command: MSP2_INAV_SET_PRECISION_LANDING_TARGET -> MSP2_INAV_SET_MARKER_GUIDANCE_TARGET - hard-break MSP payload to 4 bytes (offsetForwardCm, offsetRightCm) and drop legacy fields/compat code - switch to mode-based control (OFF/PL/CONTAINMENT) and remove separate enable flag - keep marker guidance state minimal and mode-gated for allowed nav contexts - rename/update marker-related settings and docs for consistent terminology --- docs/Navigation.md | 132 ++--- docs/Settings.md | 63 +-- docs/development/msp/README.md | 23 +- docs/development/msp/msp_messages.json | 50 +- src/main/CMakeLists.txt | 4 +- src/main/fc/fc_msp.c | 23 +- src/main/fc/settings.yaml | 112 ++-- src/main/io/osd.c | 26 +- src/main/io/osd_dji_hd.c | 26 +- src/main/msp/msp_protocol_v2_inav.h | 2 +- src/main/navigation/marker_guidance.c | 510 +++++++++++++++++++ src/main/navigation/marker_guidance.h | 67 +++ src/main/navigation/navigation.c | 64 +-- src/main/navigation/navigation.h | 50 +- src/main/navigation/navigation_multicopter.c | 10 +- src/main/navigation/precision_landing.c | 487 ------------------ src/main/navigation/precision_landing.h | 76 --- src/main/target/common.h | 2 +- 18 files changed, 840 insertions(+), 887 deletions(-) create mode 100644 src/main/navigation/marker_guidance.c create mode 100644 src/main/navigation/marker_guidance.h delete mode 100644 src/main/navigation/precision_landing.c delete mode 100644 src/main/navigation/precision_landing.h diff --git a/docs/Navigation.md b/docs/Navigation.md index 667431194cb..a1d32e7d4d6 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -38,99 +38,69 @@ PID meaning: * POS - translated position error to desired velocity, uses P term only * POSR - translates velocity error to desired acceleration -## Precision Landing Target Consumer (MSP) +## Marker Guidance Target Consumer (MSP) -INAV can consume externally computed precision-landing target offsets over MSP. -INAV only consumes target updates. +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_PRECISION_LANDING` is enabled for the target. +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. -### Core behavior -* `nav_precision_landing` enables/disables this feature. -* `MSP2_INAV_SET_PRECISION_LANDING_TARGET` updates a target cache and returns acceptance/use status. -* Target updates do **not** arm/disarm, do **not** switch modes, and do **not** start LAND/POSHOLD by themselves. -* Corrections are applied only when: - * active profile is MC/VTOL-hover-capable, and - * current navigation context is POSHOLD-compatible or LAND-compatible. -* In fixed-wing profile, updates may be cached but are not used for correction. +### MSP payload +`MSP2_INAV_SET_MARKER_GUIDANCE_TARGET` request payload is 4 bytes: +* `int16_t offsetForwardCm` +* `int16_t offsetRightCm` -### Scope and limits -Precision landing here is a **final alignment/correction layer**. -It does not replace normal waypoint/home navigation and it does not solve high-speed approach planning by itself. +* 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`) -Recommended VTOL flow: -1. transition to MC / hover-capable control -2. controlled low-speed arrival near landing zone -3. precision target alignment in POSHOLD -4. precision correction during LAND +### Mode gating +Marker guidance can influence navigation only when: +* active profile is MC/VTOL-hover-capable +* `nav_marker_guidance_mode` is not `OFF` -### Why not `SET_WP`? -`SET_WP` is a navigation target command interface. -Precision landing target updates are external measurement inputs with validity/confidence/age semantics and target-loss handling. - -This feature intentionally keeps those semantics bounded to MC POSHOLD/LAND correction, rather than repeatedly rewriting mission/waypoint targets. +Outside those contexts, updates may still be cached but do not affect navigation loops. ### POSHOLD behavior -When a fresh/valid target exists, horizontal correction is applied to improve alignment above the target. -Descent is not started automatically in POSHOLD. +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 LAND is active and target data is fresh/valid, INAV applies horizontal correction through normal navigation controllers. -Vertical descent profile remains the standard INAV LAND behavior (`nav_land_*` settings). -If target is not available at LAND entry, normal landing behavior is used. - -### Lost target recovery -If target is lost after precision correction already became active: -1. HOLD (`nav_precision_landing_lost_hold_time_ms`) -2. CLIMB_AND_RETRY (`nav_precision_landing_retry_altitude_cm`, `nav_precision_landing_retry_timeout_ms`) -3. Repeat up to `nav_precision_landing_retry_count` -4. Fallback to normal landing - -### Key settings -You do not need to tune all settings on day 1. Start with the minimum set, then tune only if needed. - -Minimum required: -* `nav_precision_landing`: Master enable. - `OFF` keeps legacy behavior unchanged. `ON` allows precision-target consumption in MC/VTOL POSHOLD/LAND contexts. -* `nav_precision_landing_source`: Target source selector (currently `MSP` only). - Keep at `MSP`. -* `nav_precision_landing_min_confidence`: Reject low-confidence target reports. - Example: `60` accepts detections >= 60%; raising to `75` reduces false positives but may drop valid detections in poor light. -* `nav_precision_landing_max_target_age_ms`: Freshness limit for cached targets. - Example: at 20 Hz companion update rate (~50 ms period), `500` ms is tolerant; `200` ms is stricter and avoids stale corrections. - -Recommended for first field tests: -* `nav_precision_landing_max_correction_speed_cm_s`: Horizontal correction clamp. - Example: `80-120` cm/s is conservative for small/medium copters; higher values react faster but can look aggressive near touchdown. -* `nav_precision_landing_align_radius_cm`: Radius treated as close-enough center reference for alignment quality checks. - Example: `60-100` cm for MVP tests. - -Safety and fault-handling settings: -* `nav_precision_landing_max_offset_cm`: Hard bound on accepted offset magnitude. - Example: `1500` cm rejects obviously wrong detections. Set `0` to disable this check. -* `nav_precision_landing_lost_hold_time_ms`: Time to hold after target loss before retry climb. - Example: `1200-2000` ms to allow short vision dropouts to recover without immediate climb. -* `nav_precision_landing_retry_count`: Number of retry attempts before fallback to normal landing. - Example: `2` gives two climb/reacquire attempts. -* `nav_precision_landing_retry_altitude_cm`: Climb distance per retry. - Example: `150-300` cm can improve camera view/FOV. -* `nav_precision_landing_retry_timeout_ms`: Max retry phase duration. - `0` = AUTO mode, computed as `2 x nav_precision_landing_lost_hold_time_ms`. - Example: with `lost_hold=1500`, AUTO timeout becomes `3000` ms. - -Suggested starter profile (conservative): -* `nav_precision_landing = ON` -* `nav_precision_landing_source = MSP` -* `nav_precision_landing_min_confidence = 60` -* `nav_precision_landing_max_target_age_ms = 500` -* `nav_precision_landing_max_correction_speed_cm_s = 100` -* `nav_precision_landing_align_radius_cm = 80` -* `nav_precision_landing_lost_hold_time_ms = 1500` -* `nav_precision_landing_retry_count = 2` -* `nav_precision_landing_retry_altitude_cm = 200` -* `nav_precision_landing_retry_timeout_ms = 0` (AUTO) +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 diff --git a/docs/Settings.md b/docs/Settings.md index c2a0920f046..0b599d8c3eb 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4434,29 +4434,31 @@ If GPS fails wait for this much seconds before switching to emergency landing mo --- -### nav_precision_landing +### nav_marker_guidance_mode -Enable MSP-based precision landing target consumption. This does not trigger mode changes on its own. +Marker-guidance mode selector (master enable gate). `OFF` disables marker guidance. -| Default | Min | Max | -| --- | --- | --- | -| OFF | OFF | ON | +| Allowed Values | | +| --- | --- | +| OFF | Default | +| PL | | +| CONTAINMENT | | --- -### nav_precision_landing_align_radius_cm +### nav_marker_guidance_radius_cm -Horizontal radius considered close enough to center for precision-alignment quality checks [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 | 10 | 2000 | +| 80 | 0 | 5000 | --- -### nav_precision_landing_lost_hold_time_ms +### nav_marker_guidance_lost_hold_time_ms -Hold duration after target loss before climb-and-retry starts [ms]. +Hold duration after target loss before falling back to normal LAND behavior [ms]. | Default | Min | Max | | --- | --- | --- | @@ -4464,49 +4466,49 @@ Hold duration after target loss before climb-and-retry starts [ms]. --- -### nav_precision_landing_max_correction_speed_cm_s +### nav_marker_containment_hold_north_cm -Maximum horizontal correction speed generated from target offset [cm/s]. +Marker-relative hold target for vehicle North position [cm], relative to marker. Positive means vehicle should stay North of marker. | Default | Min | Max | | --- | --- | --- | -| 100 | 10 | 1000 | +| 0 | -5000 | 5000 | --- -### nav_precision_landing_max_offset_cm +### nav_marker_containment_hold_east_cm -Maximum allowed horizontal target offset magnitude. Larger offsets are rejected [cm]. Set 0 to disable this check. +Marker-relative hold target for vehicle East position [cm], relative to marker. Positive means vehicle should stay East of marker. | Default | Min | Max | | --- | --- | --- | -| 1500 | 0 | 5000 | +| 0 | -5000 | 5000 | --- -### nav_precision_landing_max_target_age_ms +### nav_marker_guidance_max_offset_cm -Maximum age of a cached precision target that can still be used [ms]. +Maximum allowed horizontal target offset magnitude. Larger offsets are rejected [cm]. Set 0 to disable this check. | Default | Min | Max | | --- | --- | --- | -| 500 | 50 | 5000 | +| 1500 | 0 | 5000 | --- -### nav_precision_landing_min_confidence +### nav_marker_guidance_max_target_age_ms -Minimum confidence required to accept/use a precision landing target update [0..100]. +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 | | --- | --- | --- | -| 60 | 0 | 100 | +| 500 | 50 | 5000 | --- -### nav_precision_landing_retry_altitude_cm +### nav_marker_guidance_retry_altitude_cm -Climb distance for each retry attempt after target loss [cm]. +Climb distance for each retry attempt after target loss in PL mode LAND context [cm]. | Default | Min | Max | | --- | --- | --- | @@ -4514,9 +4516,9 @@ Climb distance for each retry attempt after target loss [cm]. --- -### nav_precision_landing_retry_count +### nav_marker_guidance_retry_count -Maximum number of climb-and-retry attempts after target loss. +Maximum number of climb-and-retry attempts after target loss in PL mode LAND context. | Default | Min | Max | | --- | --- | --- | @@ -4524,9 +4526,9 @@ Maximum number of climb-and-retry attempts after target loss. --- -### nav_precision_landing_retry_timeout_ms +### nav_marker_guidance_retry_timeout_ms -Timeout for each climb-and-retry phase [ms]. Set to 0 for AUTO mode (computed as 2 x nav_precision_landing_lost_hold_time_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 | | --- | --- | --- | @@ -4534,9 +4536,9 @@ Timeout for each climb-and-retry phase [ms]. Set to 0 for AUTO mode (computed as --- -### nav_precision_landing_source +### nav_marker_guidance_source -Precision landing target source. +Marker-guidance target source. | Allowed Values | | | --- | --- | @@ -7237,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 ae47db4b86f..291b35b63af 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -449,7 +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_PRECISION_LANDING_TARGET](#msp2_inav_set_precision_landing_target) +[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) @@ -4699,30 +4699,25 @@ 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_PRECISION_LANDING_TARGET (8753 / 0x2231)` -**Description:** Updates the external precision-landing target cache used by NAV precision landing. This message does not arm/disarm, does not switch flight modes, and does not start landing by itself. +## `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| |---|---|---|---|---| -| `valid` | `uint8_t` | 1 | - | 0 clears/invalidates target, non-zero marks update as valid | -| `confidence` | `uint8_t` | 1 | percent | External confidence [0..100] | -| `frame` | `uint8_t` | 1 | enum | Target frame. Current implementation supports `0` = body FRD | -| `offset_forward_cm` | `int16_t` | 2 | cm | Target offset forward from vehicle body origin | -| `offset_right_cm` | `int16_t` | 2 | cm | Target offset right from vehicle body origin | -| `distance_cm` | `uint16_t` | 2 | cm | Optional distance to target (`0` if unknown) | -| `timestamp_ms` | `uint32_t` | 4 | ms | Optional companion timestamp (`0` allowed, informational/debug) | +| `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_precision_state` | `uint8_t` | 1 | Internal precision-landing state | -| `reason` | `uint8_t` | 1 | Result reason (`OK`, `NOT_ENABLED`, `LOW_CONFIDENCE`, `BAD_FRAME`, `STALE`, `OFFSET_TOO_LARGE`, `NOT_MC_PROFILE`, `NOT_IN_POSHOLD_OR_LAND`, etc.) | -| `retry_count` | `uint8_t` | 1 | Active retry attempt count | +| `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:** Available only when firmware is built with `USE_PRECISION_LANDING`. Precision corrections are only applied in MC/VTOL-hover-capable profile and only in POSHOLD/LAND-compatible contexts. Outside those contexts, valid updates may still be cached (`used_now = 0`). Freshness is evaluated using FC receive time, not companion clock synchronization. +**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. diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 5db035dc58d..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,52 +11260,22 @@ "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_PRECISION_LANDING_TARGET": { + "MSP2_INAV_SET_MARKER_GUIDANCE_TARGET": { "code": 8753, "mspv": 2, "request": { "payload": [ - { - "name": "valid", - "ctype": "uint8_t", - "desc": "0 clears/invalidates target, non-zero marks update as valid.", - "units": "" - }, - { - "name": "confidence", - "ctype": "uint8_t", - "desc": "External confidence value.", - "units": "percent" - }, - { - "name": "frame", - "ctype": "uint8_t", - "desc": "Target coordinate frame; currently 0 = body FRD.", - "units": "enum" - }, { "name": "offset_forward_cm", "ctype": "int16_t", - "desc": "Target forward offset relative to vehicle body frame.", + "desc": "Marker forward offset relative to vehicle body frame.", "units": "cm" }, { "name": "offset_right_cm", "ctype": "int16_t", - "desc": "Target right offset relative to vehicle body frame.", - "units": "cm" - }, - { - "name": "distance_cm", - "ctype": "uint16_t", - "desc": "Optional distance to target (0 if unknown).", + "desc": "Marker right offset relative to vehicle body frame.", "units": "cm" - }, - { - "name": "timestamp_ms", - "ctype": "uint32_t", - "desc": "Optional companion timestamp (0 allowed, informational/debug).", - "units": "ms" } ] }, @@ -11314,7 +11284,7 @@ { "name": "accepted", "ctype": "uint8_t", - "desc": "Payload accepted by precision-landing target consumer.", + "desc": "Payload accepted by marker-guidance target consumer.", "units": "" }, { @@ -11324,9 +11294,9 @@ "units": "" }, { - "name": "nav_precision_state", + "name": "nav_guidance_state", "ctype": "uint8_t", - "desc": "Current internal precision-landing state.", + "desc": "Current internal marker-guidance state.", "units": "enum" }, { @@ -11338,13 +11308,13 @@ { "name": "retry_count", "ctype": "uint8_t", - "desc": "Current retry counter value.", + "desc": "Current retry-attempt counter in PL LAND retry flow.", "units": "" } ] }, - "notes": "This message updates precision-landing 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_PRECISION_LANDING. Precision correction is applied only in MC/VTOL hover-capable profile and POSHOLD/LAND-compatible navigation contexts. Target freshness is evaluated using FC receive time.", - "description": "Updates the external precision-landing target cache consumed by NAV precision landing." + "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, diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 84a6aad19b9..00093f41403 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -585,8 +585,8 @@ main_sources(COMMON_SRC navigation/navigation_fw_launch.c navigation/navigation_geo.c navigation/navigation_multicopter.c - navigation/precision_landing.c - navigation/precision_landing.h + 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 8763e5dfd01..230dc8c0c34 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -109,8 +109,8 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" //for MSP_SIMULATOR -#ifdef USE_PRECISION_LANDING -#include "navigation/precision_landing.h" +#ifdef USE_MARKER_GUIDANCE +#include "navigation/marker_guidance.h" #endif #include "navigation/navigation_pos_estimator_private.h" //for MSP_SIMULATOR @@ -4370,32 +4370,27 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif -#ifdef USE_PRECISION_LANDING - case MSP2_INAV_SET_PRECISION_LANDING_TARGET: - if (dataSize != (3 * sizeof(uint8_t) + 2 * sizeof(int16_t) + sizeof(uint16_t) + sizeof(uint32_t))) { +#ifdef USE_MARKER_GUIDANCE + case MSP2_INAV_SET_MARKER_GUIDANCE_TARGET: + if (dataSize != (2 * sizeof(int16_t))) { *ret = MSP_RESULT_ERROR; break; } { - precisionLandingTargetUpdate_t update = { - .valid = sbufReadU8(src), - .confidence = sbufReadU8(src), - .frame = sbufReadU8(src), + markerGuidanceTargetUpdate_t update = { .offsetForwardCm = (int16_t)sbufReadU16(src), .offsetRightCm = (int16_t)sbufReadU16(src), - .distanceCm = sbufReadU16(src), - .timestampMs = sbufReadU32(src), }; - precisionLandingMspResponse_t response = { 0 }; - if (!precisionLandingHandleMspTargetUpdate(&update, &response)) { + markerGuidanceMspResponse_t response = { 0 }; + if (!markerGuidanceHandleMspTargetUpdate(&update, &response)) { *ret = MSP_RESULT_ERROR; break; } sbufWriteU8(dst, response.accepted); sbufWriteU8(dst, response.usedNow); - sbufWriteU8(dst, response.navPrecisionState); + sbufWriteU8(dst, response.navGuidanceState); sbufWriteU8(dst, response.reason); sbufWriteU8(dst, response.retryCount); *ret = MSP_RESULT_ACK; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 02b0cf19d6d..6f59cabe8b8 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -168,8 +168,10 @@ tables: - name: nav_rth_climb_first enum: navRTHClimbFirst_e values: ["OFF", "ON", "ON_FW_SPIRAL"] - - name: nav_precision_landing_source + - 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"] @@ -2761,79 +2763,79 @@ groups: min: 0 max: 1800 field: general.rth_fs_landing_delay - - name: nav_precision_landing - description: "Enable MSP-based precision landing target consumption. This does not trigger mode changes on its own." - default_value: OFF - field: general.precision_landing - type: bool - condition: USE_PRECISION_LANDING - - name: nav_precision_landing_source - description: "Precision landing target source." + - name: nav_marker_guidance_source + description: "Marker-guidance target source." default_value: "MSP" - field: general.precision_landing_source - table: nav_precision_landing_source - condition: USE_PRECISION_LANDING - - name: nav_precision_landing_min_confidence - description: "Minimum confidence required to accept/use a precision landing target update [0..100]." - default_value: 60 - field: general.precision_landing_min_confidence - condition: USE_PRECISION_LANDING - min: 0 - max: 100 - - name: nav_precision_landing_max_target_age_ms - description: "Maximum age of a cached precision target that can still be used [ms]." + 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.precision_landing_max_target_age_ms - condition: USE_PRECISION_LANDING + field: general.marker_guidance_max_target_age_ms + condition: USE_MARKER_GUIDANCE min: 50 max: 5000 - - name: nav_precision_landing_max_offset_cm + - 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.precision_landing_max_offset_cm - condition: USE_PRECISION_LANDING + field: general.marker_guidance_max_offset_cm + condition: USE_MARKER_GUIDANCE min: 0 max: 5000 - - name: nav_precision_landing_align_radius_cm - description: "Horizontal radius considered close enough to center for precision-alignment quality checks [cm]." + - 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.precision_landing_align_radius_cm - condition: USE_PRECISION_LANDING - min: 10 - max: 2000 - - name: nav_precision_landing_max_correction_speed_cm_s - description: "Maximum horizontal correction speed generated from target offset [cm/s]." - default_value: 100 - field: general.precision_landing_max_correction_speed_cm_s - condition: USE_PRECISION_LANDING - min: 10 - max: 1000 - - name: nav_precision_landing_lost_hold_time_ms - description: "Hold duration after target loss before climb-and-retry starts [ms]." + 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.precision_landing_lost_hold_time_ms - condition: USE_PRECISION_LANDING + field: general.marker_guidance_lost_hold_time_ms + condition: USE_MARKER_GUIDANCE min: 100 max: 10000 - - name: nav_precision_landing_retry_count - description: "Maximum number of climb-and-retry attempts after target loss." + - 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.precision_landing_retry_count - condition: USE_PRECISION_LANDING + field: general.marker_guidance_retry_count + condition: USE_MARKER_GUIDANCE min: 0 max: 10 - - name: nav_precision_landing_retry_altitude_cm - description: "Climb distance for each retry attempt after target loss [cm]." + - 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.precision_landing_retry_altitude_cm - condition: USE_PRECISION_LANDING + field: general.marker_guidance_retry_altitude_cm + condition: USE_MARKER_GUIDANCE min: 50 max: 2000 - - name: nav_precision_landing_retry_timeout_ms - description: "Timeout for each climb-and-retry phase [ms]. Set to 0 for AUTO mode (computed as 2 x nav_precision_landing_lost_hold_time_ms)." + - 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.precision_landing_retry_timeout_ms - condition: USE_PRECISION_LANDING + field: general.marker_guidance_retry_timeout_ms + condition: USE_MARKER_GUIDANCE min: 0 max: 60000 - name: nav_rth_alt_mode diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 021b7f77791..cc7baf68b91 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1118,19 +1118,19 @@ static const char * navigationStateMessage(void) case MW_NAV_STATE_LAND_START_DESCENT: // Not used break; -#ifdef USE_PRECISION_LANDING - case MW_NAV_STATE_PRECISION_LANDING_STANDBY: - return OSD_MESSAGE_STR("PREC LAND STANDBY"); - case MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION: - return OSD_MESSAGE_STR("PREC LAND ALIGN"); - case MW_NAV_STATE_PRECISION_LANDING_LAND_CORRECTION: - return OSD_MESSAGE_STR("PREC LAND DESCENT"); - case MW_NAV_STATE_PRECISION_LANDING_TARGET_LOST_HOLD: - return OSD_MESSAGE_STR("PREC LAND HOLD"); - case MW_NAV_STATE_PRECISION_LANDING_CLIMB_AND_RETRY: - return OSD_MESSAGE_STR("PREC LAND RETRY"); - case MW_NAV_STATE_PRECISION_LANDING_FALLBACK_NORMAL_LAND: - return OSD_MESSAGE_STR("PREC LAND FALLBACK"); +#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 } diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 46605c6c237..ffc6bcf697c 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -638,19 +638,19 @@ static char * navigationStateMessage(void) case MW_NAV_STATE_LAND_START_DESCENT: // Not used break; -#ifdef USE_PRECISION_LANDING - case MW_NAV_STATE_PRECISION_LANDING_STANDBY: - return OSD_MESSAGE_STR("PREC LAND STANDBY"); - case MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION: - return OSD_MESSAGE_STR("PREC LAND ALIGN"); - case MW_NAV_STATE_PRECISION_LANDING_LAND_CORRECTION: - return OSD_MESSAGE_STR("PREC LAND DESCENT"); - case MW_NAV_STATE_PRECISION_LANDING_TARGET_LOST_HOLD: - return OSD_MESSAGE_STR("PREC LAND HOLD"); - case MW_NAV_STATE_PRECISION_LANDING_CLIMB_AND_RETRY: - return OSD_MESSAGE_STR("PREC LAND RETRY"); - case MW_NAV_STATE_PRECISION_LANDING_FALLBACK_NORMAL_LAND: - return OSD_MESSAGE_STR("PREC LAND FALLBACK"); +#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 9d1cd47ccae..59b204759b2 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -138,7 +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_PRECISION_LANDING_TARGET 0x2231 //in message update external precision-landing target cache; does not trigger mode changes +#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 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 77cbcfb23ef..5b53ea693e9 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -56,8 +56,8 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" -#ifdef USE_PRECISION_LANDING -#include "navigation/precision_landing.h" +#ifdef USE_MARKER_GUIDANCE +#include "navigation/marker_guidance.h" #endif #include "navigation/rth_trackback.h" @@ -180,18 +180,18 @@ 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_PRECISION_LANDING - .precision_landing = SETTING_NAV_PRECISION_LANDING_DEFAULT, - .precision_landing_source = SETTING_NAV_PRECISION_LANDING_SOURCE_DEFAULT, - .precision_landing_min_confidence = SETTING_NAV_PRECISION_LANDING_MIN_CONFIDENCE_DEFAULT, - .precision_landing_max_target_age_ms = SETTING_NAV_PRECISION_LANDING_MAX_TARGET_AGE_MS_DEFAULT, - .precision_landing_max_offset_cm = SETTING_NAV_PRECISION_LANDING_MAX_OFFSET_CM_DEFAULT, - .precision_landing_align_radius_cm = SETTING_NAV_PRECISION_LANDING_ALIGN_RADIUS_CM_DEFAULT, - .precision_landing_max_correction_speed_cm_s = SETTING_NAV_PRECISION_LANDING_MAX_CORRECTION_SPEED_CM_S_DEFAULT, - .precision_landing_lost_hold_time_ms = SETTING_NAV_PRECISION_LANDING_LOST_HOLD_TIME_MS_DEFAULT, - .precision_landing_retry_count = SETTING_NAV_PRECISION_LANDING_RETRY_COUNT_DEFAULT, - .precision_landing_retry_altitude_cm = SETTING_NAV_PRECISION_LANDING_RETRY_ALTITUDE_CM_DEFAULT, - .precision_landing_retry_timeout_ms = SETTING_NAV_PRECISION_LANDING_RETRY_TIMEOUT_MS_DEFAULT, +#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 }, @@ -1900,20 +1900,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); } - bool precisionLandingVerticalOverride = false; -#ifdef USE_PRECISION_LANDING - precisionLandingLandControl_t precisionLandControl = { 0 }; - precisionLandingGetLandControl(&precisionLandControl); + bool markerGuidanceVerticalOverride = false; +#ifdef USE_MARKER_GUIDANCE + markerGuidanceLandControl_t markerGuidanceControl = { 0 }; + markerGuidanceGetLandControl(&markerGuidanceControl); - if (precisionLandControl.mode == PREC_LAND_LAND_CTRL_HOLD) { + if (markerGuidanceControl.mode == MARKER_GUIDANCE_LAND_CTRL_HOLD) { updateClimbRateToAltitudeController(0.0f, 0.0f, ROC_TO_ALT_CONSTANT); - precisionLandingVerticalOverride = true; - } else if (precisionLandControl.mode == PREC_LAND_LAND_CTRL_CLIMB) { - updateClimbRateToAltitudeController(precisionLandControl.rateCmS, 0.0f, ROC_TO_ALT_CONSTANT); - precisionLandingVerticalOverride = true; + markerGuidanceVerticalOverride = true; + } else if (markerGuidanceControl.mode == MARKER_GUIDANCE_LAND_CTRL_CLIMB) { + updateClimbRateToAltitudeController(markerGuidanceControl.rateCmS, 0.0f, ROC_TO_ALT_CONSTANT); + markerGuidanceVerticalOverride = true; } #endif - if (!precisionLandingVerticalOverride) { + if (!markerGuidanceVerticalOverride) { updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT); } @@ -2723,8 +2723,8 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) } NAV_Status.state = navFSM[posControl.navState].mwState; -#ifdef USE_PRECISION_LANDING - NAV_Status.state = precisionLandingOverrideNavStatusState(NAV_Status.state); +#ifdef USE_MARKER_GUIDANCE + NAV_Status.state = markerGuidanceOverrideNavStatusState(NAV_Status.state); #endif NAV_Status.error = navFSM[posControl.navState].mwError; @@ -4419,8 +4419,8 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback resetRthTrackBack(); -#ifdef USE_PRECISION_LANDING - precisionLandingReset(); +#ifdef USE_MARKER_GUIDANCE + markerGuidanceReset(); #endif #ifdef USE_GEOZONE @@ -4442,8 +4442,8 @@ void applyWaypointNavigationAndAltitudeHold(void) /* Process controllers */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); -#ifdef USE_PRECISION_LANDING - precisionLandingUpdate(navStateFlags, currentTimeUs); +#ifdef USE_MARKER_GUIDANCE + markerGuidanceUpdate(navStateFlags, currentTimeUs); #endif if (STATE(ROVER) || STATE(BOAT)) { @@ -5138,8 +5138,8 @@ void navigationInit(void) /* Reset statistics */ posControl.totalTripDistance = 0.0f; -#ifdef USE_PRECISION_LANDING - precisionLandingReset(); +#ifdef USE_MARKER_GUIDANCE + markerGuidanceReset(); #endif /* Use system config */ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 451512620bb..e45e3086092 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -386,10 +386,16 @@ typedef struct positionEstimationConfig_s { PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); -#ifdef USE_PRECISION_LANDING +#ifdef USE_MARKER_GUIDANCE typedef enum { - NAV_PRECISION_LANDING_SOURCE_MSP = 0, -} navPrecisionLandingSource_e; + 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 { @@ -447,18 +453,18 @@ 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_PRECISION_LANDING - bool precision_landing; // Enable MSP-driven precision landing target consumer - uint8_t precision_landing_source; // navPrecisionLandingSource_e - uint8_t precision_landing_min_confidence; // [0..100] - uint16_t precision_landing_max_target_age_ms; - uint16_t precision_landing_max_offset_cm; - uint16_t precision_landing_align_radius_cm; - uint16_t precision_landing_max_correction_speed_cm_s; - uint16_t precision_landing_lost_hold_time_ms; - uint8_t precision_landing_retry_count; - uint16_t precision_landing_retry_altitude_cm; - uint16_t precision_landing_retry_timeout_ms; +#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; @@ -645,13 +651,13 @@ 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_PRECISION_LANDING - MW_NAV_STATE_PRECISION_LANDING_STANDBY, - MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION, - MW_NAV_STATE_PRECISION_LANDING_LAND_CORRECTION, - MW_NAV_STATE_PRECISION_LANDING_TARGET_LOST_HOLD, - MW_NAV_STATE_PRECISION_LANDING_CLIMB_AND_RETRY, - MW_NAV_STATE_PRECISION_LANDING_FALLBACK_NORMAL_LAND, +#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; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index c4b750ed572..5b57fdb5c93 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -50,8 +50,8 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" -#ifdef USE_PRECISION_LANDING -#include "navigation/precision_landing.h" +#ifdef USE_MARKER_GUIDANCE +#include "navigation/marker_guidance.h" #endif #include "navigation/sqrt_controller.h" @@ -537,9 +537,9 @@ static void updatePositionVelocityController_MC(const float maxSpeed) posControl.desiredState.vel.x = neuVelX * velHeadFactor * velExpoFactor; posControl.desiredState.vel.y = neuVelY * velHeadFactor * velExpoFactor; - // Optional external precision landing/alignment correction (MC/VTOL hover contexts only). -#ifdef USE_PRECISION_LANDING - precisionLandingApplyHorizontalVelocityCorrection(&posControl.desiredState.vel.x, &posControl.desiredState.vel.y); + // Optional external marker-guidance correction (MC/VTOL hover contexts only). +#ifdef USE_MARKER_GUIDANCE + markerGuidanceApplyHorizontalVelocityCorrection(&posControl.desiredState.vel.x, &posControl.desiredState.vel.y); #endif } diff --git a/src/main/navigation/precision_landing.c b/src/main/navigation/precision_landing.c deleted file mode 100644 index d5dcc0e3675..00000000000 --- a/src/main/navigation/precision_landing.c +++ /dev/null @@ -1,487 +0,0 @@ -#include -#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/precision_landing.h" - -#include "sensors/sensors.h" - -#ifdef USE_PRECISION_LANDING - -typedef struct { - bool cached; - bool valid; - uint8_t confidence; - uint8_t frame; - int16_t offsetForwardCm; - int16_t offsetRightCm; - uint16_t distanceCm; - uint32_t companionTimestampMs; - timeMs_t receivedAtMs; -} precisionLandingTargetCache_t; - -typedef struct { - precisionLandingState_e state; - precisionLandingTargetCache_t target; - uint8_t retryCount; - bool landCorrectionEverActive; - timeMs_t stateDeadlineMs; - fpVector2_t correctionVelNeu; - precisionLandingLandControl_t landControl; -} precisionLandingRuntime_t; - -static precisionLandingRuntime_t precisionLanding; - -static bool precisionLandingFeatureEnabled(void) -{ - return navConfig()->general.precision_landing && - navConfig()->general.precision_landing_source == NAV_PRECISION_LANDING_SOURCE_MSP; -} - -static bool isMcHoverCapableProfileActive(void) -{ - return STATE(MULTIROTOR) && !STATE(AIRPLANE); -} - -static bool precisionLandingIsPosholdContext(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 precisionLandingIsLandContext(navigationFSMStateFlags_t navStateFlags) -{ - return (navStateFlags & NAV_CTL_LAND) && !STATE(AIRPLANE); -} - -static bool precisionLandingManualTakeoverActive(void) -{ - return posControl.flags.isAdjustingAltitude || - posControl.flags.isAdjustingPosition || - posControl.flags.isAdjustingHeading; -} - -static void setPrecisionState(precisionLandingState_e state) -{ - precisionLanding.state = state; -} - -static float precisionLandingTargetOffsetMagnitudeCm(const precisionLandingTargetCache_t *target) -{ - return calc_length_pythagorean_2D((float)target->offsetForwardCm, (float)target->offsetRightCm); -} - -static bool precisionLandingTargetIsFresh(timeMs_t nowMs, precisionLandingReason_e *reasonOut) -{ - if (!precisionLanding.target.cached || !precisionLanding.target.valid) { - if (reasonOut) { - *reasonOut = PREC_LAND_REASON_INVALID_TARGET; - } - return false; - } - - if (precisionLanding.target.frame != PREC_LAND_FRAME_BODY_FRD) { - if (reasonOut) { - *reasonOut = PREC_LAND_REASON_BAD_FRAME; - } - return false; - } - - if (precisionLanding.target.confidence < navConfig()->general.precision_landing_min_confidence) { - if (reasonOut) { - *reasonOut = PREC_LAND_REASON_LOW_CONFIDENCE; - } - return false; - } - - if (navConfig()->general.precision_landing_max_target_age_ms > 0 && - (nowMs - precisionLanding.target.receivedAtMs) > navConfig()->general.precision_landing_max_target_age_ms) { - if (reasonOut) { - *reasonOut = PREC_LAND_REASON_STALE; - } - return false; - } - - if (navConfig()->general.precision_landing_max_offset_cm > 0 && - precisionLandingTargetOffsetMagnitudeCm(&precisionLanding.target) > navConfig()->general.precision_landing_max_offset_cm) { - if (reasonOut) { - *reasonOut = PREC_LAND_REASON_OFFSET_TOO_LARGE; - } - return false; - } - - if (reasonOut) { - *reasonOut = PREC_LAND_REASON_OK; - } - return true; -} - -static void clearRuntimeOutputs(void) -{ - precisionLanding.correctionVelNeu.x = 0.0f; - precisionLanding.correctionVelNeu.y = 0.0f; - precisionLanding.landControl.mode = PREC_LAND_LAND_CTRL_NONE; - precisionLanding.landControl.rateCmS = 0.0f; -} - -static void clearRuntimeStateKeepCache(void) -{ - precisionLanding.retryCount = 0; - precisionLanding.landCorrectionEverActive = false; - precisionLanding.stateDeadlineMs = 0; - clearRuntimeOutputs(); - setPrecisionState(PREC_LAND_IDLE); -} - -static void clearRuntimeStateAndTarget(void) -{ - precisionLanding.target.cached = false; - precisionLanding.target.valid = false; - clearRuntimeStateKeepCache(); -} - -void precisionLandingReset(void) -{ - clearRuntimeStateAndTarget(); -} - -static void computeHorizontalCorrectionVelocity(void) -{ - const float offsetForward = precisionLanding.target.offsetForwardCm; - const float offsetRight = precisionLanding.target.offsetRightCm; - const float offsetMagnitude = precisionLandingTargetOffsetMagnitudeCm(&precisionLanding.target); - const float alignRadiusCm = navConfig()->general.precision_landing_align_radius_cm; - - if (alignRadiusCm > 0.0f && offsetMagnitude <= alignRadiusCm) { - precisionLanding.correctionVelNeu.x = 0.0f; - precisionLanding.correctionVelNeu.y = 0.0f; - return; - } - - const float cosYaw = posControl.actualState.cosYaw; - const float sinYaw = posControl.actualState.sinYaw; - - float velN = offsetForward * cosYaw - offsetRight * sinYaw; - float velE = offsetForward * sinYaw + offsetRight * cosYaw; - - const float maxCorrectionSpeed = navConfig()->general.precision_landing_max_correction_speed_cm_s; - const float speed = calc_length_pythagorean_2D(velN, velE); - - if (maxCorrectionSpeed > 0.0f && speed > maxCorrectionSpeed) { - const float scale = maxCorrectionSpeed / speed; - velN *= scale; - velE *= scale; - } - - if (alignRadiusCm > 0.0f && offsetMagnitude > alignRadiusCm) { - // Ease corrections just outside alignment radius to reduce jitter around the target center. - const float ease = constrainf((offsetMagnitude - alignRadiusCm) / offsetMagnitude, 0.0f, 1.0f); - velN *= ease; - velE *= ease; - } - - precisionLanding.correctionVelNeu.x = velN; - precisionLanding.correctionVelNeu.y = velE; -} - -static uint16_t getRetryTimeoutMs(void) -{ - const uint16_t configuredRetryTimeoutMs = navConfig()->general.precision_landing_retry_timeout_ms; - if (configuredRetryTimeoutMs > 0) { - return MAX(configuredRetryTimeoutMs, 100); - } - - // Auto mode: derive retry timeout from lost-hold duration to reduce required tuning. - const uint16_t lostHoldMs = MAX(navConfig()->general.precision_landing_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.precision_landing_retry_altitude_cm / retryTimeoutS; - return constrainf(climbRate, 20.0f, navConfig()->mc.max_auto_climb_rate); -} - -static void setLostHoldState(timeMs_t nowMs) -{ - setPrecisionState(PREC_LAND_TARGET_LOST_HOLD); - precisionLanding.stateDeadlineMs = nowMs + MAX(navConfig()->general.precision_landing_lost_hold_time_ms, 100); -} - -static void setClimbRetryState(timeMs_t nowMs) -{ - setPrecisionState(PREC_LAND_CLIMB_AND_RETRY); - precisionLanding.stateDeadlineMs = nowMs + getRetryTimeoutMs(); -} - -void precisionLandingUpdate(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) -{ - UNUSED(currentTimeUs); - - clearRuntimeOutputs(); - - const timeMs_t nowMs = millis(); - - if (!precisionLandingFeatureEnabled()) { - clearRuntimeStateKeepCache(); - return; - } - - if (!ARMING_FLAG(ARMED) || FLIGHT_MODE(FAILSAFE_MODE) || areSensorsCalibrating() || STATE(LANDING_DETECTED)) { - clearRuntimeStateAndTarget(); - return; - } - - const bool mcProfileActive = isMcHoverCapableProfileActive(); - const bool posholdContext = precisionLandingIsPosholdContext(navStateFlags); - const bool landContext = precisionLandingIsLandContext(navStateFlags); - const bool precisionContextActive = mcProfileActive && (posholdContext || landContext); - - if (!precisionContextActive || precisionLandingManualTakeoverActive()) { - precisionLanding.landCorrectionEverActive = false; - precisionLanding.retryCount = 0; - precisionLanding.stateDeadlineMs = 0; - - precisionLandingReason_e reason; - if (precisionLandingTargetIsFresh(nowMs, &reason)) { - setPrecisionState(PREC_LAND_STANDBY); - } else { - setPrecisionState(PREC_LAND_IDLE); - } - return; - } - - precisionLandingReason_e freshnessReason = PREC_LAND_REASON_OK; - const bool targetFresh = precisionLandingTargetIsFresh(nowMs, &freshnessReason); - - if (posholdContext) { - precisionLanding.retryCount = 0; - precisionLanding.landCorrectionEverActive = false; - precisionLanding.stateDeadlineMs = 0; - - if (targetFresh) { - setPrecisionState(PREC_LAND_POSHOLD_CORRECTION); - computeHorizontalCorrectionVelocity(); - } else { - setPrecisionState(PREC_LAND_STANDBY); - } - return; - } - - if (!landContext) { - setPrecisionState(PREC_LAND_STANDBY); - return; - } - - if (precisionLanding.state != PREC_LAND_TARGET_LOST_HOLD && - precisionLanding.state != PREC_LAND_CLIMB_AND_RETRY && - precisionLanding.state != PREC_LAND_FALLBACK_NORMAL_LAND) { - if (targetFresh) { - setPrecisionState(PREC_LAND_LAND_CORRECTION); - } else if (precisionLanding.landCorrectionEverActive) { - setLostHoldState(nowMs); - } else { - setPrecisionState(PREC_LAND_STANDBY); - } - } - - switch (precisionLanding.state) { - case PREC_LAND_LAND_CORRECTION: - if (!targetFresh) { - if (precisionLanding.landCorrectionEverActive) { - setLostHoldState(nowMs); - } else { - setPrecisionState(PREC_LAND_STANDBY); - } - break; - } - - precisionLanding.landCorrectionEverActive = true; - computeHorizontalCorrectionVelocity(); - break; - - case PREC_LAND_TARGET_LOST_HOLD: - precisionLanding.landControl.mode = PREC_LAND_LAND_CTRL_HOLD; - precisionLanding.landControl.rateCmS = 0.0f; - - if (targetFresh) { - setPrecisionState(PREC_LAND_LAND_CORRECTION); - break; - } - - if (nowMs >= precisionLanding.stateDeadlineMs) { - if (precisionLanding.retryCount < navConfig()->general.precision_landing_retry_count) { - setClimbRetryState(nowMs); - } else { - setPrecisionState(PREC_LAND_FALLBACK_NORMAL_LAND); - } - } - break; - - case PREC_LAND_CLIMB_AND_RETRY: - precisionLanding.landControl.mode = PREC_LAND_LAND_CTRL_CLIMB; - precisionLanding.landControl.rateCmS = getRetryClimbRateCmS(); - - if (targetFresh) { - setPrecisionState(PREC_LAND_LAND_CORRECTION); - break; - } - - if (nowMs >= precisionLanding.stateDeadlineMs) { - precisionLanding.retryCount++; - if (precisionLanding.retryCount >= navConfig()->general.precision_landing_retry_count) { - setPrecisionState(PREC_LAND_FALLBACK_NORMAL_LAND); - } else { - setLostHoldState(nowMs); - } - } - break; - - case PREC_LAND_FALLBACK_NORMAL_LAND: - precisionLanding.landControl.mode = PREC_LAND_LAND_CTRL_NONE; - precisionLanding.landControl.rateCmS = 0.0f; - break; - - default: - break; - } -} - -void precisionLandingApplyHorizontalVelocityCorrection(float *velX, float *velY) -{ - if (!velX || !velY) { - return; - } - - if (precisionLanding.state == PREC_LAND_POSHOLD_CORRECTION || precisionLanding.state == PREC_LAND_LAND_CORRECTION) { - *velX += precisionLanding.correctionVelNeu.x; - *velY += precisionLanding.correctionVelNeu.y; - } -} - -void precisionLandingGetLandControl(precisionLandingLandControl_t *controlOut) -{ - if (!controlOut) { - return; - } - - *controlOut = precisionLanding.landControl; -} - -navSystemStatus_State_e precisionLandingOverrideNavStatusState(navSystemStatus_State_e defaultState) -{ - switch (precisionLanding.state) { - case PREC_LAND_STANDBY: - return MW_NAV_STATE_PRECISION_LANDING_STANDBY; - case PREC_LAND_POSHOLD_CORRECTION: - return MW_NAV_STATE_PRECISION_LANDING_POSHOLD_CORRECTION; - case PREC_LAND_LAND_CORRECTION: - return MW_NAV_STATE_PRECISION_LANDING_LAND_CORRECTION; - case PREC_LAND_TARGET_LOST_HOLD: - return MW_NAV_STATE_PRECISION_LANDING_TARGET_LOST_HOLD; - case PREC_LAND_CLIMB_AND_RETRY: - return MW_NAV_STATE_PRECISION_LANDING_CLIMB_AND_RETRY; - case PREC_LAND_FALLBACK_NORMAL_LAND: - return MW_NAV_STATE_PRECISION_LANDING_FALLBACK_NORMAL_LAND; - default: - return defaultState; - } -} - -bool precisionLandingHandleMspTargetUpdate(const precisionLandingTargetUpdate_t *update, precisionLandingMspResponse_t *responseOut) -{ - if (!update || !responseOut) { - return false; - } - - responseOut->accepted = 0; - responseOut->usedNow = 0; - responseOut->navPrecisionState = (uint8_t)precisionLanding.state; - responseOut->reason = PREC_LAND_REASON_INVALID_TARGET; - responseOut->retryCount = precisionLanding.retryCount; - - if (!precisionLandingFeatureEnabled()) { - responseOut->accepted = 1; - responseOut->reason = PREC_LAND_REASON_NOT_ENABLED; - return true; - } - - if (!update->valid) { - precisionLanding.target.cached = false; - precisionLanding.target.valid = false; - responseOut->accepted = 1; - responseOut->reason = PREC_LAND_REASON_INVALID_TARGET; - return true; - } - - if (update->frame != PREC_LAND_FRAME_BODY_FRD) { - responseOut->reason = PREC_LAND_REASON_BAD_FRAME; - return true; - } - - if (update->confidence < navConfig()->general.precision_landing_min_confidence) { - responseOut->reason = PREC_LAND_REASON_LOW_CONFIDENCE; - return true; - } - - const float offsetMagnitude = calc_length_pythagorean_2D((float)update->offsetForwardCm, (float)update->offsetRightCm); - if (navConfig()->general.precision_landing_max_offset_cm > 0 && - offsetMagnitude > navConfig()->general.precision_landing_max_offset_cm) { - responseOut->reason = PREC_LAND_REASON_OFFSET_TOO_LARGE; - return true; - } - - precisionLanding.target.cached = true; - precisionLanding.target.valid = true; - precisionLanding.target.confidence = update->confidence; - precisionLanding.target.frame = update->frame; - precisionLanding.target.offsetForwardCm = update->offsetForwardCm; - precisionLanding.target.offsetRightCm = update->offsetRightCm; - precisionLanding.target.distanceCm = update->distanceCm; - precisionLanding.target.companionTimestampMs = update->timestampMs; - precisionLanding.target.receivedAtMs = millis(); - - responseOut->accepted = 1; - responseOut->reason = PREC_LAND_REASON_OK; - - const bool mcProfileActive = isMcHoverCapableProfileActive(); - const navigationFSMStateFlags_t navStateFlags = navGetCurrentStateFlags(); - const bool posholdContext = precisionLandingIsPosholdContext(navStateFlags); - const bool landContext = precisionLandingIsLandContext(navStateFlags); - const bool inContext = posholdContext || landContext; - - if (!ARMING_FLAG(ARMED)) { - responseOut->usedNow = 0; - responseOut->reason = PREC_LAND_REASON_NOT_ARMED; - } else if (FLIGHT_MODE(FAILSAFE_MODE)) { - responseOut->usedNow = 0; - responseOut->reason = PREC_LAND_REASON_FAILSAFE; - } else if (!mcProfileActive) { - responseOut->usedNow = 0; - responseOut->reason = PREC_LAND_REASON_NOT_MC_PROFILE; - } else if (!inContext) { - responseOut->usedNow = 0; - responseOut->reason = PREC_LAND_REASON_NOT_IN_POSHOLD_OR_LAND; - } else { - responseOut->usedNow = (precisionLanding.state == PREC_LAND_POSHOLD_CORRECTION || precisionLanding.state == PREC_LAND_LAND_CORRECTION); - responseOut->reason = responseOut->usedNow ? PREC_LAND_REASON_OK : PREC_LAND_REASON_NOT_IN_POSHOLD_OR_LAND; - } - - responseOut->navPrecisionState = (uint8_t)precisionLanding.state; - responseOut->retryCount = precisionLanding.retryCount; - return true; -} - -#endif diff --git a/src/main/navigation/precision_landing.h b/src/main/navigation/precision_landing.h deleted file mode 100644 index e85fbe865de..00000000000 --- a/src/main/navigation/precision_landing.h +++ /dev/null @@ -1,76 +0,0 @@ -#pragma once - -#ifdef USE_PRECISION_LANDING - -#include -#include - -#include "navigation/navigation.h" -#include "navigation/navigation_private.h" - -typedef enum { - PREC_LAND_FRAME_BODY_FRD = 0, -} precisionLandingFrame_e; - -typedef enum { - PREC_LAND_REASON_OK = 0, - PREC_LAND_REASON_NOT_ENABLED, - PREC_LAND_REASON_LOW_CONFIDENCE, - PREC_LAND_REASON_BAD_FRAME, - PREC_LAND_REASON_STALE, - PREC_LAND_REASON_OFFSET_TOO_LARGE, - PREC_LAND_REASON_NOT_MC_PROFILE, - PREC_LAND_REASON_NOT_IN_POSHOLD_OR_LAND, - PREC_LAND_REASON_FAILSAFE, - PREC_LAND_REASON_INVALID_TARGET, - PREC_LAND_REASON_NOT_ARMED, -} precisionLandingReason_e; - -typedef enum { - PREC_LAND_IDLE = 0, - PREC_LAND_STANDBY, - PREC_LAND_POSHOLD_CORRECTION, - PREC_LAND_LAND_CORRECTION, - PREC_LAND_TARGET_LOST_HOLD, - PREC_LAND_CLIMB_AND_RETRY, - PREC_LAND_FALLBACK_NORMAL_LAND, - PREC_LAND_DONE, -} precisionLandingState_e; - -typedef enum { - PREC_LAND_LAND_CTRL_NONE = 0, - PREC_LAND_LAND_CTRL_HOLD, - PREC_LAND_LAND_CTRL_CLIMB, -} precisionLandingLandControlMode_e; - -typedef struct { - uint8_t valid; - uint8_t confidence; - uint8_t frame; - int16_t offsetForwardCm; - int16_t offsetRightCm; - uint16_t distanceCm; - uint32_t timestampMs; -} precisionLandingTargetUpdate_t; - -typedef struct { - uint8_t accepted; - uint8_t usedNow; - uint8_t navPrecisionState; - uint8_t reason; - uint8_t retryCount; -} precisionLandingMspResponse_t; - -typedef struct { - precisionLandingLandControlMode_e mode; - float rateCmS; -} precisionLandingLandControl_t; - -void precisionLandingReset(void); -void precisionLandingUpdate(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs); -void precisionLandingApplyHorizontalVelocityCorrection(float *velX, float *velY); -void precisionLandingGetLandControl(precisionLandingLandControl_t *controlOut); -navSystemStatus_State_e precisionLandingOverrideNavStatusState(navSystemStatus_State_e defaultState); -bool precisionLandingHandleMspTargetUpdate(const precisionLandingTargetUpdate_t *update, precisionLandingMspResponse_t *responseOut); - -#endif diff --git a/src/main/target/common.h b/src/main/target/common.h index be9d6a976fb..13cca5898ab 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -209,7 +209,7 @@ #define USE_TELEMETRY_HOTT #define USE_HOTT_TEXTMODE #define USE_34CHANNELS -#define USE_PRECISION_LANDING +#define USE_MARKER_GUIDANCE #define MAX_MIXER_PROFILE_COUNT 2 #define USE_SMARTPORT_MASTER #ifdef USE_GPS