Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
94 changes: 94 additions & 0 deletions docs/Navigation.md
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,100 @@ 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.

### 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.
* 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.

### 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.

### 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.
Expand Down
110 changes: 110 additions & 0 deletions docs/Settings.md
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
27 changes: 26 additions & 1 deletion docs/development/msp/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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.

## <a id="msp2_inav_set_precision_landing_target"></a>`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, informational/debug) |

**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:** 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.

## <a id="msp2_inav_set_cruise_heading"></a>`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.

Expand Down Expand Up @@ -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.

88 changes: 87 additions & 1 deletion docs/development/msp/msp_messages.json
Original file line number Diff line number Diff line change
Expand Up @@ -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, informational/debug).",
"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. 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": {
"code": 8739,
"mspv": 2,
Expand Down Expand Up @@ -11354,4 +11440,4 @@
"description": "Initiates binding for MSP receivers (mLRS)."
}
}
}
}
2 changes: 2 additions & 0 deletions src/main/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading