wboayue / stable-flight / 38482244415

92%

Build Type

push

github

Committed by web-flow

Commit Message

Waypoint acceptance needs altitude (#359) (#369)

A waypoint is now reached only when the vehicle is at its altitude as
well as at its position, so a waypoint straight above the vehicle is no
longer reached on the ground. `RouteEnd::Land` with one waypoint over
the start now climbs, lands, and disarms; before, it stayed armed on the
ground. `RouteEnd::Hold` completes at the waypoint's altitude, not at t
= 0.

## Changes

**Core: `WaypointFollower`**
- A waypoint is accepted when the vehicle is inside the horizontal
radius (or has passed the waypoint along its leg) **and** within an
altitude band. This is the PX4 rule,
`src/modules/navigator/mission_block.cpp:380` (PX4 `c4e4ef98e9`). A NaN
altitude never counts as within the band.
- `DEFAULT_ALTITUDE_ACCEPTANCE_M` = 0.8 m, PX4's multicopter band
(`NAV_MC_ALT_RAD`, `navigator_params.yaml:56-64`).
`PLANE_ALTITUDE_ACCEPTANCE_M` = 10 m, PX4's fixed-wing band
(`NAV_FW_ALT_RAD`).
- New `with_altitude_acceptance` and `set_altitude_acceptance`.
- Plan decision 2a: this is a cylinder, not ArduPilot's sphere (3D
distance ≤ `WPNAV_RADIUS`, `libraries/AC_WPNav/AC_WPNav.cpp:645-648`).
With the 3 m radius, a sphere would accept a 5 m hover waypoint at 2 m.

**sitl: `ScriptCommander`**
- Picks the band for each leg from the script's regime column: copter
legs use 0.8 m, plane legs 10 m. Override with
`with_altitude_acceptance(regime, m)`. It sets the band on the follower
each tick.
- A single 10 m band in `tailsitter-auto` would have accepted
`flights/explora_hover.csv`'s 5 m waypoint over the spawn on the ground,
which is this bug again.

## Tests

- **Core table tests:**
- a waypoint overhead isn't reached on the ground or mid-climb, and is
at the band's edge;
  - a waypoint above the band isn't reached;
  - a waypoint passed low isn't passed, and passed at altitude is;
  - a NaN altitude never reaches a waypoint;
  - a configurable band;
- with one waypoint over the start, `Hold` and `Land` wait for the
climb.
- *... (continued)

Coverage Stats

48 of 53 new or added lines in 4 files covered. (90.57%)

3 existing lines in 1 file now uncovered.

5099 of 5672 relevant lines covered (89.9%)

4.5 hits per line