AutoSmoothVel - Compute desired speed at target based on path curvature #12505

bresch · 2019-07-17T15:54:24Z

Describe problem solved by the proposed pull request
Currently, the drone does waypoint navigation while almost stopping at each waypoint. When the waypoints are aligned or that the heading change is small, we should not completely stop the drone but rather decelerate to make a nice turn and continue.

Describe your preferred solution
The solution implemented in that PR assumes that the drone will fly an arc between the two lines connecting to a waypoint and that we want to fly this arc with some desired centripetal acceleration. Given the acceptance radius r, the desired centripetal acceleration ac and the angle between the two lines α, we can easily compute the desired waypoint speed:
$\\ a_c=\frac{v^2}{R} \\ R = r\tan(\alpha/2) \\ v_{in} = \sqrt{a_cr\tan(\alpha/2)}$

Also, to avoid entering too fast into the waypoint if the next waypoint is really close, the maximum speed between current and next is computed to constrain the waypoint speed.

You can interactively estimate the waypoint speed by clicking on the following link:
https://www.desmos.com/calculator/rvx6uybeo8

Tests
I tested quite extensively in SITL and on a F550.
Here an example of a log:
https://logs.px4.io/plot_app?log=2ab24653-a229-4e81-8295-ee7c58c8a373

Related
Fixes #11827

bresch · 2019-07-17T16:03:17Z

@PX4/testflights Can you test this PR in mission (survey and random mission) please? The drone should slow down but not stop if the turn is not too sharp.

Stifael · 2019-07-17T17:05:12Z

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -177,6 +177,50 @@ void FlightTaskAutoLineSmoothVel::_checkEkfResetCounters()
 	}
 }

+float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()


can we move the two new methods into a UtilityClass/Function with more general names? then we can reuse it in other tasks as well

Agreed, it would be great to use the _getMaxSpeedFromDistance in the collision prevention as well, instead of using just the velocity controller P gain.

Stifael · 2019-07-17T17:05:39Z

looks great. will try to test it tomorrow on H520

hamishwillee · 2019-07-18T01:14:16Z

What if the angle is really acute?

bresch · 2019-07-18T07:28:02Z

@hamishwillee The circle with radius R gets smaller and when the angle is 0, the radius is 0 and the desired entrance speed is 0.

Stifael · 2019-07-18T13:18:45Z

I tested it on the H520 and seems to work well. I think it is a bit too conservative at corners and approaches the waypoints a bit too slow. Can that be changed via tuning?

During one particular survey, the vehicle also experienced an abrupt stop at one of the waypoints (see below), where the drone first accelerated before it stopped abruptly.
.
It also has sometimes difficulty to follow the line, but I think that is independent of this PR.

The flight log:
https://review.px4.io/plot_app?log=18956ebf-a86d-4c72-a786-1f97ca318130

In addition, to prevent the vehicle from passing a waypoint if the altitude has not been reached yet, I created this PR.

jorge789 · 2019-07-18T19:12:59Z

Tested on PixRacer V4:

Flight Card 2

Modes Tested
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Takeoff in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

Note:
The flight was good the vehicle followed the points marked in the mission, @Stifael the same behavior as you mentioned. The vehicle slowed down but without stopping. We didn't notice any visual stoppage of the vehicle. Overall no issues were noted, good flight in general.

Logs:
https://review.px4.io/plot_app?log=d1f4a5f9-5c98-4ea2-9e48-12555472fbb4

https://review.px4.io/plot_app?log=8e5b032a-ec31-48e4-9ec6-9ae59b0ded49

https://review.px4.io/plot_app?log=293a137f-56b3-47d0-9a00-46760d04b4e2

Junkim3DR · 2019-07-18T20:33:49Z

Tested on Pixhawk 4 mini v5:

Flight Card 2

Modes Tested
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Take off in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

Note:
Vehicle flew as expected, slowed down instead of stoping at the waypoints.

Logs:

Tested on Pixhawk Pro v4:

Flight Card 2

Modes Tested
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Take off in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

Note:
Vehicle flew as expected, slowed down instead of stoping at the waypoints.

Logs:

Tested on NXP FMUK66 v3:

Flight Card 2

Modes Tested
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Take off in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

Note:
Vehicle flew as expected, slowed down instead of stoping at the waypoints.

Logs:

…n previous-current and current-next waypoints Also remove crosstrack P controller that produces overshoots when the acceptance radius is large (crosstrack error is suddenly large at waypoint switch).

… been reached yet

Add check for this flag in AutoLine

…utation of the maximum waypoint entrance speed

bresch · 2019-07-30T13:22:11Z

@Stifael I moved the computation in static functions inside the VelocitySmoothing class that contains the jerk-limited optimizer. Is that enough or do you want them in an other file (header library? what would be the scope of the library? name? ...)

Stifael · 2019-07-30T14:14:52Z

Is that enough or do you want them in an other file (header library? what would be the scope of the library? name? ...)

Do you think it makes sense to have computeMaxSpeedFromBrakingDistance and the other function in VelocitySmoothing?

I think if the function has a general implementation and is not necessarily FlightTasks related, then it could be put into the math-library similar to other functions (https://github.com/PX4/Firmware/blob/master/src/lib/mathlib/math/Functions.hpp#L48-L208). If it will anyway just be used by FlightTasks, then I would put it in a file such that it can easily be found. For instance, computeMaxSpeedFromBrakingDistance is kind of smooth-independent and can also be used in FlightTaskAutoLine.
However, I don't want to block you, so sure we can do that in another PR, given that I anyway want to revisit the structure.

bresch · 2019-08-06T13:38:49Z

@Stifael I moved the functions into a TrajMath.hpp header library. It's true it's much cleaner and reusable. Can you review again?

jkflying · 2019-08-07T08:44:41Z

src/lib/FlightTasks/tasks/Utility/TrajMath.hpp

+ * @return maximum speed
+ */
+template<typename T>
+const T computeMaxSpeedFromBrakingDistance(T jerk, T accel, T braking_distance)


Awesome! I'll try to get this into the collision prevention as well.

bresch · 2019-08-07T15:04:01Z

@PX4/testflights Can you test that again please?

dannyfpv · 2019-08-08T05:18:57Z

Tested on Pixhawk4 v5 f-450

Modes Tested
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Takeoff in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

Pr log:
https://review.px4.io/plot_app?log=421760b0-a1cb-4e00-836d-ca085d17b475

https://review.px4.io/plot_app?log=d3b10670-b737-4657-9ea8-9484d79ecbdf

bresch · 2019-08-08T05:47:06Z

@dannyfpv Thanks a lot! Ready to be merged once we have a bit more space on the omnibus target :|

Junkim3DR · 2019-08-08T16:10:02Z

Tested on PixRacer v4:

Modes Tested
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Takeoff in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

Note:
No issue noted, good flight in general.

Logs:

Tested on Pixhawk Pro v4Pro:

Modes Tested
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Takeoff in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

Note:
No issue noted, good flight in general.

Logs:

bresch added the Multirotor 🛸 label Jul 17, 2019

bresch requested a review from Stifael July 17, 2019 15:54

bresch self-assigned this Jul 17, 2019

bresch requested a review from MaEtUgR July 17, 2019 15:58

Stifael reviewed Jul 17, 2019

View reviewed changes

bresch force-pushed the dev-autocontinue branch from cc64cfb to 11e39c2 Compare July 18, 2019 08:05

bresch force-pushed the dev-autocontinue branch from ceea06e to 06a0bf9 Compare July 19, 2019 13:16

weekly-digest bot mentioned this pull request Jul 21, 2019

Weekly Digest (14 July, 2019 - 21 July, 2019) #12528

Closed

bresch and others added 4 commits July 30, 2019 13:31

AutoSmoothVel - Compute desired speed at target based on angle betwee…

515464e

…n previous-current and current-next waypoints Also remove crosstrack P controller that produces overshoots when the acceptance radius is large (crosstrack error is suddenly large at waypoint switch).

FlightTaskAutoLine/SmoothVel: stop at waypoint if altitude is has not…

2576c5a

… been reached yet

MC auto - Modify yaw_sp_aligned to take mis_yaw_error tolerance.

ad6ce7d

Add check for this flag in AutoLine

AutoSmoothVel - scale down acc_hor using traj_p parameter in the comp…

87f4f4f

…utation of the maximum waypoint entrance speed

bresch force-pushed the dev-autocontinue branch from 06a0bf9 to 4b43089 Compare July 30, 2019 13:17

Create TrajMath library and move waypoint speed calculations in it

15dbed2

bresch force-pushed the dev-autocontinue branch from 4b43089 to 15dbed2 Compare August 6, 2019 13:33

Merge branch 'master' into dev-autocontinue

6167ad7

jkflying approved these changes Aug 7, 2019

View reviewed changes

Merge branch 'master' into dev-autocontinue

4ccd16e

Merge branch 'master' into dev-autocontinue

03d6010

Merge branch 'master' into dev-autocontinue

d4953c9

bresch added 2 commits August 8, 2019 18:15

Merge branch 'master' into dev-autocontinue

a6b0cb5

Merge branch 'master' into dev-autocontinue

4342bb2

bresch merged commit 4eb9c7d into master Aug 9, 2019

bresch deleted the dev-autocontinue branch August 9, 2019 10:04

weekly-digest bot mentioned this pull request Aug 11, 2019

Weekly Digest (4 August, 2019 - 11 August, 2019) #12682

Closed

This was referenced Aug 30, 2019

Stopping/Slowing at each Waypoint #12845

Closed

Multicopter mission guidance improvements #12905

Merged

MaEtUgR mentioned this pull request Oct 21, 2019

Adding parameter to scale velocity in flight task get current state #11969

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AutoSmoothVel - Compute desired speed at target based on path curvature #12505

AutoSmoothVel - Compute desired speed at target based on path curvature #12505

bresch commented Jul 17, 2019 •

edited

Loading

bresch commented Jul 17, 2019

Stifael Jul 17, 2019

jkflying Jul 19, 2019

Stifael commented Jul 17, 2019

hamishwillee commented Jul 18, 2019

bresch commented Jul 18, 2019

Stifael commented Jul 18, 2019 •

edited

Loading

jorge789 commented Jul 18, 2019 •

edited by Tony3dr

Loading

Junkim3DR commented Jul 18, 2019

bresch commented Jul 30, 2019

Stifael commented Jul 30, 2019

bresch commented Aug 6, 2019

jkflying Aug 7, 2019

bresch commented Aug 7, 2019

dannyfpv commented Aug 8, 2019

bresch commented Aug 8, 2019

Junkim3DR commented Aug 8, 2019

AutoSmoothVel - Compute desired speed at target based on path curvature #12505

AutoSmoothVel - Compute desired speed at target based on path curvature #12505

Conversation

bresch commented Jul 17, 2019 • edited Loading

bresch commented Jul 17, 2019

Stifael Jul 17, 2019

Choose a reason for hiding this comment

jkflying Jul 19, 2019

Choose a reason for hiding this comment

Stifael commented Jul 17, 2019

hamishwillee commented Jul 18, 2019

bresch commented Jul 18, 2019

Stifael commented Jul 18, 2019 • edited Loading

jorge789 commented Jul 18, 2019 • edited by Tony3dr Loading

Tested on PixRacer V4:

Junkim3DR commented Jul 18, 2019

Tested on Pixhawk 4 mini v5:

Tested on Pixhawk Pro v4:

Tested on NXP FMUK66 v3:

bresch commented Jul 30, 2019

Stifael commented Jul 30, 2019

bresch commented Aug 6, 2019

jkflying Aug 7, 2019

Choose a reason for hiding this comment

bresch commented Aug 7, 2019

dannyfpv commented Aug 8, 2019

bresch commented Aug 8, 2019

Junkim3DR commented Aug 8, 2019

Tested on PixRacer v4:

Tested on Pixhawk Pro v4Pro:

bresch commented Jul 17, 2019 •

edited

Loading

Stifael commented Jul 18, 2019 •

edited

Loading

jorge789 commented Jul 18, 2019 •

edited by Tony3dr

Loading