FW Position Control Landing Glideslope Facing South Causes Vehicle Crash

[AlexanderAurora]

Describe the bug
If you set up a mission with a fixed-wing landing pattern facing south. There is a chance the vehicle will crash itself by nose-diving into the ground. I was using Firmware v1.10.2 but it looks like this is still and issue on v1.11.1 and your master branch.

To Reproduce
Steps to reproduce the behavior:

1. Plan simple fixed-wing mission with landing facing almost directly south.
2. Upload the mission to real vehicle or simulator.
3. Make sure you have a gusty crosswind that blows the vehicle off course during the landing.
4. Fly the mission and allow for autonomous landing.

Screen of the simulated mission I used to reproduce the issue.

Expected behavior
During the glide-slope portion of the landing sequence, the fixed-wing controller will immediately set the altitude command to [terrain_alt + 0] and crash.

Log Files and Screenshots
Plot of altitude setpoint and altitude estimate during a south-facing landing when the bug occurred:

Plot of same data from a non-south-facing landing.

This was caused by a conditional in the getLandingSlopeRelativeAltitudeSave(float, float, float) function.
This function compares the bearing from the previous waypoint to the landing point, and the bearing from the aircraft to the landing point. If the difference between these two bearings is >=90 it returns zero.

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp (where the landing slope function is called)

src/lib/landing_slope/Landingslope.cpp:

I think the idea behind returning zero is that if difference in those bearings is >90, we overshot our landing target and we should get on the ground ASAP.

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp (where the two heading arguments are calculated)

the function get_bearing_to_next_waypoint() is in src/lib/ecl/geo/geo.cpp. It wraps the result from -pi to pi.

In the case where the landing is positioned almost directly south, bearing_lastwp_currwp = +3.13
If the vehicle gets blown slightly off course to the east, bearing_airplane_currwp = -3.13
abs(3.13 - (-3.13)) = +6.26 > 1.57 even though 3.13 and -3.13 are almost the same heading.
This results in: altitude_desired = terrain_alt + 0

The figure below shows the values of bearing_lastwp_currwp (bpc) and bearing_airplane_currwp (bac) and the value of the sloped altitude command. This was recorded during a simulation.

A simple fix for this would be to apply wrap_pi to the result of the heading subtraction in Landingslope.cpp.

I think you should steer clear of subtracting angles from one another, id say use the arccos(dot(A,B)) to find the angle between two unit vectors. this always gives you a value from 0-180 deg.

[bresch]

Thanks for the investigation.

A simple fix for this would be to apply wrap_pi to the result of the heading subtraction in Landingslope.cpp.

Could you make a PR with that wrap_pi and link it to this issue please?

All additions/subtractions of angles should always be in a wrap_pi to be safe. Taking arccos of the dot product of two unit vectors gives the same result but is way more computationally expensive (2 sqrt, 2 divisions, 1 arccos, ...); except if we anyway need the unit vectors for something else.

[ryanjAA]

If this has the ability to bring down a plane that has a seemingly well planned mission, wouldn’t fixing this (and anything else incremental out there) be prudent and version bumping 1.11.1 to 1.11.2? Seems like this is something that could have serious unintended consequences for users that are following the guides and doing things correctly. @dagar @Antiheavy

[AlexanderAurora]

Excuse my novice-ness here but it looks like in order to create a pull request, the branch with the changes needs to be pushed to the repo. I don't have the permissions level to push right now.
Figured it out, I had to remake the merge request since I had branched from v1.10.2 instead of master.

I can open a separate issue for this but it's such a small fix I'm unsure if it warrants it. There was another instance where using an autonomous hand-launch of a FW aircraft caused a nose-dive crash.

In the function TECS::_initialize_states() there is an "IF - ELSE IF" statement where either initialization can happen, or climbout limits set, but both cannot happen at the same time.

As soon as the vehicle is launched, the tecs controller initializes and detects under-speed, but the climbout pitch limit is not set, and the aircraft sets a high negative pitch value for one iteration. This was enough to cause the vehicle to nose-dive.

This was fixed on my end by making the "IF - ELSE IF" into two separate IF-statements so that climbout limits can still be set on initialization.

[ryanjAA]

@AlexanderAurora I can only get this to crash if I plan a landing at exact 180º S. 181 won't let it crash and neither will 179, 359, 360 and zero (same) also don't let it crash in SITL.

Were you seeing something different?