Plane: Fix FBWB/CRUISE missing zero crossing of elevator input

[pieniacy]

Summary

In FBWB/CRUISE Plane can drastically overshoot altitude and continue to climb/descent long after pilot gives opposite elevator stick input, resulting in crashing into the ground or drastically overshooting the ceiling. This fixes the issue.

Scenarios

CRUISE_ALT_FLOOR 0
FBWB_CLIMB_RATE 5
TECS_SINK_MIN 2.5
TECS_SINK_MAX 5

Descent

Imagine you are at 120 meters in CRUISE and want to do your max descent with lowest airspeed. You lower your throttle to zero and command full nose down. You requested slowest airspeed, so your plane will not be able to sink 5 m/s as directed, more like 2.5 m/s. You descent to 20 m and want to pull up, so you move your elevator stick for full nose up, you expect your airplane to pull up and climb, but it does not, it continues down and will hit the ground.

Climb

Imagine you want to climb from 20 to 120 meters in CRUISE but you want a fairly high airspeed. You raise your throttle and command a full nose up. You requested faster airspeed, so your plane will not be able to climb 5 m/s as directed. At 120 meters you realize you reached your maximum legal altitude, so you move your elevator stick for full nose down and expect your airplane to level off and descent, but it does not, it continues up and will probably reach an altitude of over 150 meters.

Reproducing

Both scenarios can be easily reproduced in SITL. I can confirm it is also a case with a physical airplane on ArduPlane 4.5.1 with ELRS (at 50 Hz).

Reason

ardupilot/ArduPlane/navigation.cpp

Lines 400 to 404 in 233f344

	if (is_zero(elevator_input) && !is_zero(target_altitude.last_elevator_input)) {
	// the user has just released the elevator, lock in
	// the current altitude
	set_target_altitude_current();
	}

In Plane::update_fbwb_speed_height(void) a condition to reset target altitude only checks if elevator input became zero, but does not check for zero crossing (pilot moving the stick through zero). So the behaviour is different if pilot moves the stick slowly, in which case a zero will be registered and altitude will be reset and moves the stick fast in which case it will not be registered and the height setpoint has been wound up by as much as 100 meters.

What makes the issue even worse is that the loop to update height target is running at only 10 Hz:

ardupilot/ArduPlane/navigation.cpp

Line 383 in 233f344

if (now - target_altitude.last_elev_check_us >= 100000) {

This makes it even easier to jump over the deadzone with a realistic stick movement.

Solution

The only change in this PR is to replace the condition to check for the input becoming zero to also check zero crossings. I believe the most efficient way to do this is to check if the input is no longer positive or no longer negative. It preserves the previous behaviour and also triggers if the input jumped over zero. In terms of implementation one could alternatively multiply previous and current input and check for negative (indicating different signs), but this way it should be more efficient as it does not require floating point multiplication, which can take a few more mpu cycles.

Resources

Here you can see before and after the fix from SITL. Notice that on the left image the aircraft crashes into the ground despite full pull up being commanded for 10 long and terrifying seconds.

Here are the logs to produce above figures:
FBWB_BUG_BEFORE_FIX.BIN
FBWB_BUG_AFTER_FIX.BIN

Remarks

Documentation of FBWB is not very clear on the somewhat hidden altitude target, which is not always intuitive in FBWB/CRUISE. Most users will not be aware that such target altitude windup can occur. The fact of the target winding up in some configurations of commanded airspeed/vspeeds is disputable, one could argue that the stick should indeed set the hidden altitude target strictly according to the FBWB_CLIMB_RATE without taking into account real conditions like slow airspeed or randomly encountered thermal lift. On the other hand flying in FBWB/CRUISE tends to be rather interactive (often FPV) and the pilot probably expects that elevator input will control current vertical speed, not an altitude target that is not known when flying VLOS or FPV. This can lead to crashes or (subjectively) uncommanded climbs.

Severity

In my opinion the bug is severe and should be fixed asap. The fact that an airplane will behave differently whether you rise your elevator stick slow or fast is in my opinion not acceptable. What's worse, slow stick movement will register zero and reset the altitude target. However, faster stick movement, which might occur suddenly in response to approaching ground, obstacles, or other aircraft, will not reset the altitude target. Instead, it will continue the climb or descent until the setpoint unwinds, which for perfectly realistic scenarios can easily take 10-15 seconds, resulting in crashes.

[IamPete1]

We really should not need this stuff at all, but for now we do, so it makes sense to fix.

I also note that we don't constrain flybywire_climb_rate to be within the bounds of TECS max rates.

The longer term fix would be to pass TECS the desired climb rate not the altitude.

[tridge]

@pieniacy very good catch, thank you!

[IamPete1]

LGTM

[tridge]

@pieniacy it would be nice to follow up with a PR to do a constrain on the FBWB/CRUISE climb/sink rate to the TECS limits

[pieniacy]

@tridge Sure, I can do that, but it seems like

• at this point the critical misbehaviour is fixed
• limiting to TECS limits will not prevent windups at all
  • it will just help a little when FBWB_CLIMB_RATE is misconfigured above limits
  • I believe the user should be responsible for using achievable FBWB_CLIMB_RATE (the default definitely is)
• not sure if setpoint should be clipped or scaled to TECS limit
  • it might indroduce asymmetry (TECS_CLMB_MAX vs TECS_SINK_MAX) in the elevator stick making it feel weird
• better windup prevention is probably needed anyway
  • btw imo passing wanted dh to TECS is not a good solution here, because we want the airplane to firmly hold the altitude (for example to correct if passed through a thermal bubble)

[IamPete1]

We were just thinking to prevent windup due to misconfiguration, just a constrain rather than a scale so there is no change for most users who have configured FBWB_CLIMB_RATE appropriately.

[pieniacy]

To complete the above discussion the limits were merged with #26856

[rmackay9]

This is included in 4.5.2-beta1