Large negative Pitch Setpoint at takeoff, hand launched Fixed Wing

Antiheavy commented 6 years ago

Most of our fixed wing flight logs show a large negative step input to the pitch command right at Takeoff. I think the "minimum pitch" param in the Takeoff Waypoint overrides this, but behind the scenes something is wrong.

I've seen users attempt to auto-takeoff to a non-Takeoff Waypoint and the vehicle immediately noses into the ground.

I suspect some bad interactions between LandDetector and typical ground handling of hand launched fixed wings, but I'm not sure what exactly... (fyi @dagar we discussed previously).

Here are a number of logs that show this issue right at Takeoff: https://review.px4.io/plot_app?log=80301c1d-fac9-4a8a-a5e1-870927233e64 https://review.px4.io/plot_app?log=6f7119a2-c6c3-4d0c-88e5-8debcf59e88f https://review.px4.io/plot_app?log=a8b193bc-f3d7-4cab-ae3e-ba0302c1834b https://review.px4.io/plot_app?log=13dcbb27-7d97-49d4-94d3-0f092d283c7c

Related PR that attempts to improve Fixed Wing takeoff, might need to look into the root cause of this issue here: https://github.com/PX4/Firmware/pull/9243

dagar commented 6 years ago

TODO: look at TECS initialization

LorenzMeier commented 6 years ago

@priseborough Would be awesome if you could have a look at it with Daniel or @RomanBapst.

priseborough commented 6 years ago

TECS relies on the climb_out_setpoint flag being set to true when tecs_update_pitch_throttle is called.

If this is set to true, then the lower pitch limit pitch_min_climbout is applied.

The FixedwingPositionControl::tecs_update_pitch_throttle wrapper function is called in 10 places. It passes the climbout mode flag and minimum climbout pitch angle through tot TECS without modification.

Looking at the first log attached to this issue there are some clues:

1) The pitch setpoint 'dip' coincides with a sudden levelling of the altitude setpoint within TECS that occurs shortly after takeoff. The flight vehicle is not capable of tracking such a sudden change in climb rate, overshoots and pushes the nose down to correct. This levelling of the TECS height demand occurs when the vehicle reaching the takeoff waypoint height. The position controller waits a few seconds before indexing the waypoint and allowing the climb to continue. This is could be because the height target was achieved before the waypoint was reached.

screen shot 2018-07-31 at 7 48 51 am

2) During takeoff controlled by a takeoff waypoint and with launch detection enabled (not a runway takeoff) TECS is called from here:

https://github.com/PX4/Firmware/blob/master/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp#L1252

The attached log has FW_CLIMBOUT_DIFF set to 10m which means that the climbout state is existed 10metres before TECS achieves the takeoff waypoint height. This effect can be seen in the log data. This can cause a number of problems:

It can be a trap for inexperienced users because a takeoff waypoint height of 15 metres when combined with baro errors could easily see this condition triggered immediately after takeoff.
If airspeed is high when climbout state exists , TECS will pull the nose up to the upper pitch limit, then when the height limit is reached, it will push the nose nose down.

3) Until the climbout waypoint is completed, there are unexpected changes to the speed setpoint that should be investigated:

screen shot 2018-07-31 at 8 24 15 am

There are a number of options for improving the transition out of climbout mode and making it more tolerant to mission plan and tuning variations:

During climbout, when within FW_CLIMBOUT_DIFF of the takeoff waypoint, progressively reduce the climb so that the altitude setpoint smotthly arrives at the waypoint height. This could be implemented here: https://github.com/PX4/Firmware/blob/master/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp#L1268 Warn if a takeoff waypoint is less than FW_CLIMBOUT_DIFF+10m Limit the absolute minimum pitch angle to 0 degrees here: https://github.com/PX4/Firmware/blob/master/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp#L1277 If the waypoint after the takeoff waypoint has a higher height, continue the climb.

Antiheavy commented 6 years ago

@priseborough makes a number of good comments about ways the different phases of fixed wing take-off and climb-out should be improved. These, along with some other Takeoff mode improvements should be looked into, maybe as one or more additional issue topics. However, the specific bug(?) I'm posting about is a very large (max?) negative pitch command that lasts <1 second right at the moment of launch/takeoff. Is this related or something different?

priseborough commented 6 years ago

I was looking at the tecs_status data in my analysis which didn't' show the the event. There are two pitch demands in the log. One from tecs_status and the other from the vehicle_attitude_setpoint. Plotting both shows the event you were referring too occurs before tecs logging starts

This would suggest that TECS is not being placed into 'climbout state' soon enough. I will investigate further.

priseborough commented 6 years ago

Looking at the code, the tecs status data is published whenever tecs_update_pitch_throttle is called within FixedwingPositionControl. This indicates that the _att_sp.pitch_body is being set before tecs is running and tecs is not being started until after launch.

priseborough commented 6 years ago

Based on further code and log inspection it looks like the launch detection which uses x acceleration is triggering before _vehicle_land_detected.landed goes true. This results in FixedwingPositionControl::tecs_update_pitch_throttle returning without running TECS.

The simplest solution would appear to be changing the line 1831 of FixedwingPositionControl.cpp to:

// Run TECS if we are in air or a launch has been detected. The launch detection condition
// is required becasue _vehicle_land_detected.landed can be slow to go false
bool run_tecs = !_vehicle_land_detected.landed || (_launch_detection_state != LAUNCHDETECTION_RES_NONE);

tstastny commented 6 years ago

@priseborough @Antiheavy I actually implemented something similar to this already in #9471 @dagar and I were still discussing whether there should be some timeout on disabling the land detected bool during take-off however to avoid motors continuously running in the event of some take-off failure (very possible in hand launches!). Would be happy if you two joined the discussion there as well!