AP_TECS: scale velRateMin correctly with airspeed

[coleschon]

This PR improves TECS deceleration rates by allowing to decrease speed more rapidly, the faster a plane is going.

Description

The current calculation for velRateMin scales incorrectly with airspeed, where currently, the max allowable velocity rate decreases as velocity increases. i.e. the faster an aircraft is going, the less-quickly it's able to slow down. This is particularly problematic on aircraft which operate over a wide range of speeds or which have a large difference between TECS_SINK_MIN and TECS_SINK_MAX, since they are empirically determined.

This change introduces discrete calculations for:

• _STEdot_neg_max — Specific total energy rate at max speed (using TECS_SINK_MAX)
• velRateNegCruise — Velocity rate at cruise speed (existing velocity rate calc at TRIM_ARSPD_CM)
• velRateNegMax — Velocity rate at max speed (existing velocity rate calc at ARSPD_FBW_MAX)

The change then replaces the velRateMin calculation with a linear interpolation between the velocity rates at cruise and max speed, which is capped at those values outside the bounds of cruise and max speed. The deceleration margin is also decreased from 50% to 10% to improve deceleration rate performance.

Testing

This change was tested in ArduPilot SITL (Plane-4.3.3) with the following sequence:

1. Takeoff the plane
2. Send guided mode command to a distant point or for a large loiter
3. Issue MAV_CMD_DO_CHANGE_SPEED commands to increase and decrease speed setpoint
4. Issue MAV_CMD_DO_CHANGE_ALTITUDE commands to increase and decrease altitude setpoint

Before

Notice the blue at line at the top of graph (behind the green) representing TECS.sp, this is the plane's airspeed. Before this change, it required just over 6 minutes for the plane to decelerate from 75 m/s to 40 m/s. Extremely slow, and not by limitation of the aircraft — throttle is being modulated to maintain a gradual deceleration.

After

Notice again the blue line representing TECS.sp. After this change, the plane requires only 40 seconds to decelerate from 75 m/s to 40 m/s.

Note: all TECS parameters are identical between runs.

Summary

Here we can see the rates produced from our capped linear interpolation between velocity rate at cruise and max speeds compared against what currently exists.

X 40 m/s = TRIM_ARSPD_CM (cruise airspeed)
X 80 m/s = ARSPD_FBW_MAX (max airspeed)

[coleschon]

What is the repo's guidance for backporting improvements like this? My team is currently running Plane-4.3.3 — would we have to own a fork with the changes applied on top of this branch? I'd love to get this merged into master in either case.

[IamPete1]

@coleschon You explain the testing that was done, but not issue that this fixes. What are the symptoms and how does the fix improve those?

RE backporting, we would get it into master first. I don't think this is a critical fix it would not be back-ported but it would be included in the next stable release, 4.5. If you need it on 4.3 you would have to maintain your own branch.

[Hwurzburg]

squash and proper commit message needed

[tridge]

@priseborough can you check this?

[coleschon]

@IamPete1 I agree that this should use the built-in linear_interpolate() function and am testing now to verify it behaves as expected. Also, I added a before/after example in Testing to illustrate the issues with the current deceleration rates. Please let me know if you think this is sufficient.

@Hwurzburg squashed commits and corrected message.

stale

[IamPete1]

@priseborough Can you take a look at this please, Thanks.

[IamPete1]

@coleschon Sorry we were so slow getting this in, Thanks for sticking with us.