Stop Missions blowing past desired speeds

[peterbarker]

SCurve navigation plans its segments based off the current vehicle speed, not taking into account that the speed can be changed between the next waypoint and the one after it.

This has led to a crash where the vehicle accelerated up to its waypoint navigation speed and was travelling too fast for the rise in terrain it was traveling over. i.e. the mission was well-planned but the unexpected velocity caused a crash.

The included autotest reproduces this in SITL (assuming you comment-out the actual test in the test, ensure_low_gps_speed you can get the log which produced the pre-fix graphs below):

Pre-fix:

Post-fix:

This will be a positive change for users who set a speed at the start of their missions and stick to it.

This will potentially be a negative change for users who are changing speeds many times as part of their missions.

A better fix might be to pass the expected speed for the next segment in.

However, there may be (a) a delay before we DO_CHANGE_SPEED and (b) there may be multiple DO_CHANGE_SPEEDs.

Example of pathological case which makes correct SCurve planning difficult: MAV_CMD_DO_CONDITION_YAW followed by DO_CHANGE_SPEED. Not actually an unreasonable demand in a mission, but planning your vel/accel profile becomes problematic when you don't really know how long that condition might take to become true!

This work sponsored by Harris Aerial

[IamPete1]

I did some testing with a simple mission and a do change speed after way point 3.

Master we get lovely SCurve corners:

This PR we stop and start again:

This is exactly the behavior you describe just the map view makes the change in behavior clearer I think.

I think we have to find a way to do it smoothly. This would be a unexpected change for many.

This has led to a crash where the vehicle accelerated up to its waypoint navigation speed and was travelling too fast for the rise in terrain it was traveling over. i.e. the mission was well-planned but the unexpected velocity caused a crash.

I don't understand this, I thought we will slowdown horizontal speed if the vertical speed is not sufficient to keep up with the mission. Maybe this is the thing we need to fix?

[lthall]

I think this is the better of two imperfect options. At least this option won't do something the user didn't ask for so should be less scary.

[rmackay9]

I'm not sure about this, we get complaints on the forums when SCurves don't cut the corners and so I worry we will just see more of these.

I wonder if perhaps the the cause of the crash was that the WPNAV_TER_MARGIN was set too high

[tridge]

@peterbarker should we set the terrain margin to the min of the parameter and the target alt AGL?

[rmackay9]

We discussed this at length and I think we decided:

1. enhance WPNav to look for the do-change-speed immediately after the next waypoint and use that as the maximum desired start speed of the next leg
2. create a new PR as per Tridge's suggestion to set the terrain margin to the minimum of the parameter value and the vehicle's current target above terrain

Thanks @peterbarker for your work on this!

[peterbarker]

Closes #22466

[peterbarker]

This is the SCurve test:

        self.upload_simple_relhome_mission([
            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt),
            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 40, 0, alt),
            self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=1.5),
            (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
        ])

SCurve test was doing bad things (the last hump is the RTL:

It's going too fast for that segment.

After these patches:

And the "Next" test prior to this patch:

And after this patch:

Slight difference in shape as it's planning the slowdown curve with the slower speed.

[peterbarker]

(@IamPete1 - pretty corners are back ;-) )

[lthall]

Well done @peterbarker !!!!

[lthall]

That is an extremely tight change. Well done!!!

This is perfect!

[IamPete1]

This is better, but I don't think its quite right. This is effectively taking the change speed one waypoint early. If there is a big slowdown in speed and a long leg it could mean that the vehicle can no longer fly a mission it could previously due to the extra time (battery) taken.

For example:

The change speed is after waypoint 3. Marked in blue. We come it waypoint 2 at 10m/s. We take the conner at 10m/s. Then slow down to 5m/s for the steady state portion of the leg.

I think we should keep the steady state portion at the original speed. So the it would look like this:

Basically we should do the change speed at the end of the steady state section not the start.

As it stands this PR doesn't really fix the underlying problem, it just moves it one waypoint earlier. It would have fixed the original complaint but I think it introduces other issues. Slowing down one waypoint before the user was expecting it could result in running out of battery and speeding up one waypoint before could introduce safety issues.

[lthall]

This is better, but I don't think its quite right. This is effectively taking the change speed one waypoint early.

Ok, I missed that. Maybe the Do Change Speed is being called on both segments instead of only the second one.

Yes, this approach can't work because update_track_with_speed_accel_limits() changes both the current and next segment speed.

Can we get the do_change_speed and feed that speed into the call for the next_leg as a speed overide parameter. Or maybe always feed the speed in from the higher layer. I have been thinking for a while now that we should be doing something like this anyway.