Predicting Copter's climb rates and ground speeds during slopes in wp missions with terrain following enabled

Hi @Leonardthall, Can you please let known (or point to a doc if there’s any; I wasn’t able to find one which is in detail enough) how a copter decides on the ground speed and climb/descend rates when it follows a waypoint mission with terrain following enabled. I was not able to judge much from sitl experiments.
Any detailed explanation referring code is also fine. :slight_smile:

Also, is it even really possible to fairly predict what is intended here?

Mate you are opening a can of worms with that question. I am currently looking at ways to improve this. It is actually a very tricky problem because we never know what our required altitude will be from second to second when using a lidar for terrain following.

The simplified answer is that we compare the vertical error with the vertical stopping distance at maximum climb rate. We scale these to the horizontal error and stopping distance. We then apply 3 axis limit to the target velocities.

This basically means as your vertical error approaches the vertical stopping distance your horizontal velocity reduces to zero.

You need to read the code to get the details.

So all we do then is move the vertical target up and down based on the current terrain altitude.

I suspect all this is not very clear but it was a bit of a monster of a problem at the time. There are cases that don’t work well like when you have high vertical accelerations and slow climb rates resulting in very short stopping distances.

While I suspect I didn’t explain how it works I hope this give you a bit of an idea or a starting point when you look at the code.

Thanks man, I am surely getting hints from this. Will look more into it, and probably bother u again to see if i understood it well. Jk.

I have observed that even when terrain following is not happening, and multicopter is following a slope during WP mission, ground speed and climb rate are not same as the WPNAV_SPEED and WPNAV_SPEED_UP (which is the case in mission when there is no change of altitude while going from Waypoint(n) to Waypoint(n+1) with no Alt difference).
So, my question is, if it is possible to predict the ground speed and climb rate when terrain following is not happening but copter is still climbing due to difference of altitude between the two known waypoints of the mission?

There are two issues here. The first is when your aircraft does not have the power to climb at the needed rate to keep up with the forward speed. In this case it slows down as you would expect.

The second is what speed is it trying to go. The current navigation controller treats the velocity request as a 3d limit. So if your vertical max velocity was the same as your horizontal velocity (10 m/s lest say) you would be traveling 10 m/s at 45 degrees up, or 7.07 m/s up and forward. Because the vertical and horizontal limits are not equal we find that the vertical limits the horizontal speed at a lower climb angle.

With the new s-curve stuff this will change to be seperate the xy and z limts.

But if wpnav_speed_up and wpnav_speed are in 1:1 proportion (say 10 and 10 m/s), then, at a 45 deg slope, copter should be able to achieve wpnav_speed_up and wpnav_speed. Isn’t that so? But I noticed the actual speeds are half of that.

I would need to see a log.

I have observed this on SITL.

They will be 1/sqrt(2) * 10. It is a spherical limit.