Fix off-track tracking in AutoLineSmoothVel #13321
Conversation
|
@jkflying Awesome! However, something seems quite wrong in one of the waypoint: Compared to before (which was also wrong because not aiming at the next waypoint): |
|
Attached is the test flight plan I used |
|
So this issue is being caused by YAW_MODE=4, it tracks on the outside which then causes it to stop because the yaw setpoint is just a tiny bit higher than on the old implementation, due to pointing at the setpoint instead of along the flight line. Using YAW_MODE=1 I don't see this issue at all. However, I've re-written it to use separated x/y components, since it wasn't too hard and the behavior is more similar to what was there before in terms of separate constraints. Here is a log after the changes, with YAW_MODE=1: |
|
Ok, a small fix, and another comparison to master. Master: notice how the flight lines in each segment are parallel to the line between the waypoints, rather than towards the next waypoint. They are a little offset. This PR: lines between waypoints always point directly to the next waypoint. The small divergences at the end (in both master and this PR) are due to the waypoint acceptance radii overlapping, so it starts going to the next waypoint before reaching this one. |
|
In the "this PR" plot, is there a dent in the setpoint line right around the (37, 20) coordinate? |
There is, I'm honestly not sure where it's from, but it is very small. I've run a bunch of tests in SITL and haven't seen anything else strange related to this. It might be related to doing separate planning in x and y axes? @bresch One other thing I noticed, which is an issue already existing in master, is that we overshoot waypoints which the vehicle is meant to stop at (eg start of survey), because we no longer slow down enough to stop at them. |
This calculates the target velocities better taking into account disturbances along the flight route. Previously entry angles and more were calculated assuming the flight path originates directly from the direction of the previous waypoint. This corrects this assumption to instead make the direction come from the vehicle location.
This is to allow planning to not stop at a waypoint, but instead to reach the waypoint while maintaining a certain velocity
jkflying
force-pushed
the
offtrack-smooth
branch
from
46e4213
to
03a143a
Compare
|
@PX4/testflights Can you please fly that PR on a few multicopters please? Especially auto modes (takeoff, RTL, mission, land). |
Tested on PixRacer V4:Mode Tested: Mission Plan Mode (Automated):
Note Logs: https://review.px4.io/plot_app?log=8674017d-e90c-492a-8a23-6fb889fb39a8 Comparison with v1.10.0 (beta) (5c6d16c) |
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
Outdated
Show resolved
Hide resolved
Describe problem solved by this pull request
Before, if a vehicle was pulled very off-track (eg. by the obstacle avoidance system) the smooth dynamics planning would plan wrong routes, assuming it was still on-track
In addition, there were some cases where code calculating the optimal speed to go through a waypoint had very complicated / hard to follow logic, which was potentially also a source of problems.
Describe your solution
This fixes the angle that is used for the planning, so it uses the angle from the drone instead of from the previous waypoint as the entry angle into the next waypoint acceptance radius.
It also simplifies the logic by adding the capability to 'chain' the constraints from later setpoints' velocities, so that we don't have to override the smoothing to move nicely through waypoints that we don't need to turn for.
Also, I got rid of the MPC_XY_TRAJ_P for the straight line stuff, since it wasn't doing anything in my tests - it is still being used for the acceptance radius calculation. Maybe I'm wrong on this and we should add it back in.
Describe possible alternatives
I used a single speed term instead of separate x/y components, but I didn't see an easy way to do separate x/y without rewriting the whole thing. Not sure if this is an issue, but it is also way simpler this way.
Test data / coverage
I've flown this in SITL a fair amount.
Current master doesn't fix the tracking on the line after an overshoot. What isn't visible here is that the vehicle actually flew sideways, with yaw pointing towards the setpoint but continuing parallel to the flight line.
https://logs.px4.io/plot_app?log=020c2e97-f6db-4b94-a7d3-059876cb0011
This PR tracks the overshoot, correcting it much better, and always flying with yaw straight ahead.
https://logs.px4.io/plot_app?log=0abcbfa8-ffc5-4664-86b1-b8d665f38497
Both the above flown with Iris, without Avoidance running
Additional context
IMO this should be pulled into 1.10, it is a pretty serious fix for Avoidance which pushes the vehicle offtrack as part of normal operation. FYI @dagar @julianoes