Copter: Excluded target velocity from slowdown calculations in FOLLOW mode #26142
Conversation
|
This seems like a slightly different version of #26138? This does have documented testing tho, so points for that ;) |
|
@IamPete1 well, two people tried to solve the same problem at the same day, after years of this code being live. What are the odds! The only comment-worthy difference with @tridge 's code is that in his PR he applies the maximum speed limit (e.g. Other than that, tridge's PR has some nicer vector math. |
I fixed that in my PR this morning :-) |
Georacer
force-pushed
the
bugfix/follow_moving_target
branch
from
232140c
to
5ae53c2
Compare
@peterbarker I re-wrote it. Is this what you expected? I went by this guideline: https://ardupilot.org/dev/docs/submitting-patches-back-to-master.html |
Yep, that's good, thanks. |
ArduCopter/mode_follow.cpp
Outdated
| desired_velocity_neu_cms.z += -vel_of_target.z * 100.0f; | ||
|
|
||
| // scale desired velocity to stay within horizontal speed limit | ||
| float desired_speed_xy = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y)); |
There was a problem hiding this comment.
I think we can replace this block with:
desired_velocity_neu_cms.xy().limit_length(pos_control->get_max_speed_xy_cms());
The slowdown calculations should apply only to the relative velocity, not the absolute one. Thus the target baseline velocity should be added afterwards. Naturally the absolute velocity limits should be applied afterwards.
Georacer
force-pushed
the
bugfix/follow_moving_target
branch
from
5ae53c2
to
6b6dfe6
Compare
|
I tested in SITL, looks good! |
|
Merged, thanks! |
Summary
This PR fixes a bug where Copter would not reach its target in FOLLOW mode.
The issue
Currently, when the Copter would be in FOLLOW mode, following a moving target, it would never actually reach its desired position; instead there would be a steady-state position error.
The reason is that there are slowdown calculations in place in
ArduCopter/mode_follow.cpp, in order to overwrite the position controller velocity setpoint and slow down the UA as it approaches its target, and prevent an overshoot.However, these slowdown calculations would be applied over the absolute velocity, not the velocity relative to the target.
Effectively, the absolute velocity would be required to be zero above the moving target, which is inconsistent with a moving target.
This issue likely blocks the implementation of an effective Copter landing on a moving base.
The changes
Before
ArduCopter/mode_follow.cpp:L65and beyond carries out the calculations for the desired velocity.In order, the steps taken would be:
L67)FOLL_POS_Pgain to calculate approach velocity and add target velocity. (L69-73)L75-85)L94-112).After
With this PR, the order of the steps is slightly changed.
L67)FOLL_POS_Pgain to calculate approach velocity without adding target velocity. (L69-73). This yields the desired relative approach velocity.L75-93)L95-98)L100-110)Testing
A following scenario similar to that of #24720 was simulated.
A truck would drive around at ~6m/s and the Copter was tasked to follow it.
Before, the Copter could get no closer than ~15m.
After, the Copter could effectively nullify the tracking error:
The simulated flight logs are available upon request. Let me know where I should upload them.
ℹ️ A script was running during the tests, but I don't think it alters the presented results.