Make speed changes during missions obey WPNAV_ACCEL #12821

bnsgeyer · 2019-11-11T15:24:30Z

This PR is following up on what @ChristopherOlson did in PR #11068 which was closed for some reason. It fixes issue #11048. This PR is slightly different than Chris' PR in that it slews the WP speed both for increases in speed and decreases in speed.

Still uncertain why when the max XY accel is being passed to the position controller, it doesn't abide by that max accel parameter. This fixes the issue by slewing the wp speed changes passed to the position controller in accordance with the max wp accel.

bnsgeyer · 2019-11-12T03:25:50Z

@rmackay9 and @peterbarker I have made the requested changes. I did my best to streamline the update wp speed function. I couldn't do a simple if (wp_desired_speed == wp_current_speed) because both variables are floats and the compiler chewed me out for comparing floats. So I put the else statement which I think works just as well.
Randy, do you need me to put a backport in for copter 4.0?

peterbarker · 2019-11-12T03:46:26Z

On Mon, 11 Nov 2019, Bill Geyer wrote: @rmackay9 and @peterbarker I have made the requested changes. I did my best to streamline the update wp speed function. I couldn't do a simple if (wp_desired_speed == wp_current_speed) because both variables are floats and the compiler chewed me out for comparing floats. So I put the else statement

For future reference: if (is_equal(wp_desired_speed, wp_current_speed)) See also "is_zero".

bnsgeyer · 2019-11-12T03:50:21Z

I knew there was a function like that. I tried iszero() and it looks like i got the syntax wrong. thanks for the tips!

libraries/AC_WPNav/AC_WPNav.cpp

bnsgeyer · 2019-11-13T02:57:32Z

@rmackay9 and @peterbarker I made the requested change and sqaushed the changes. I don't understand why I am getting the error in CI.

peterbarker · 2019-11-13T04:16:37Z

@bnsgeyer One failure was just travis failing to bring up a VM.

The second appears to be a result of recent sub tuning changes - that's going to be intermittent, sadly.

I've kicked both of them over.

bnsgeyer · 2019-11-13T11:31:13Z

@peterbarker thanks for the help.
@rmackay9 all checks have passed. Ready for merging.

rmackay9 · 2019-12-03T01:29:52Z

as discussed on the dev call I'll review this..

rmackay9 · 2019-12-03T11:16:12Z

Looking pretty good but I think maybe we need to set _wp_current_speed_cms within wp_and_spline_init().

Here's the situation where I think it could go wrong:

upload a mission with a long-ish leg in it
param set WP_SPEED 200 (a low speed)
Loiter
arm throttle
rc 3 1500
let the vehicle fly for a bit until it's up to 2m/s
setspeed 50 (and let the vehicle get up to maybe 15m/s)
Loiter (and let the vehicle slow to a stop)
Auto

I think what we will see is the vehicle will speed up to something like 10m/s but then slow down again to 2m/s (the default speed).

We can discuss whether when the mission restarts it should go back to the default 2m/s or to the 50m/s but the way master works at the moment it will return to the default... but the way it is it initially climbs up to a high-ish speed and then returns to 2m/s which users will find confusing

bnsgeyer · 2020-02-05T00:36:54Z

@rmackay9 I have made a change that has it retain the desired speed once a mission is started. If the mission is stopped and then started again, it updates the current wp speed to the actual aircraft speed and it will accel to the desired speed. See what you think and I will take it to next weeks dev call if you are happy with that logic.

bnsgeyer · 2020-02-06T00:24:22Z

@rmackay9 not sure why I got an error with Travis CI. Any help would be appreciated.

peterbarker · 2020-02-06T04:21:45Z

@bnsgeyer The Rover tests failing were just travis sucking.

The Copter test may be showing a regression.

The test appears to set the RTL_SPEED to 4m/s and ensure the vehicle sticks to that number. This has been a regression before - thus the explicit test for it :-)

My guess is you've failed to take into account that we have a separate speed when doing an RTL. Just guessing, haven't reviewed this PR :-)

You can run the test locally yourself:

./Tools/autotest/autotest.py build.ArduCopter fly.ArduCopter.RTLSpeed

bnsgeyer · 2020-02-07T00:52:34Z

@peterbarker and @rmackay9 So I have changed the logic to always set the wpnav speed to wp_speed_cms when it initializes. That seemed to make the autotest happy. I'm assuming that is how we want it to work. anyway, I also have the wpnav take the current speed and set the variable current speed variable in wpnav so it increases/decreases the speed to the desired at the specified accel. if you are happy with this, I will clean up the code and squash the commits.

bnsgeyer · 2020-02-11T10:48:37Z

@lthall I have finished making changes and feel it is ready to be merged if you think this is the right approach.

lthall

This all checks out and is ready to go in. Sorry for the delay!

rmackay9 · 2020-02-19T02:19:20Z

libraries/AC_WPNav/AC_WPNav.cpp

+    // initialise the current wp speed to current speed along the track
+    const Vector3f &curr_vel = _inav.get_velocity();
+    // get speed along track (note: we convert vertical speed into horizontal speed equivalent)
+    _wp_current_speed_cms = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y;


We don't have a valid _pos_delta_unit at the time that wp_and_spline_init() is called so I think this is using the previous track to initialise the _wp_current_speed_cms...

bnsgeyer · 2020-02-19T04:08:01Z

@rmackay9 check my latest commit. is this what you had in mind? I haven't tested it yet but i think this will behave how we want.

rmackay9 · 2020-02-19T06:41:13Z

@bnsgeyer,

I had a play around with this today and unfortunately my advice didn't work out well (not that the original solution didn't have problems either of course). I found that in a simple back-and-forth mission, as the vehicle came close to the next waypoint (within the leash length's distance of the waypoint) the vehicle would suddenly stop, then it would start again towards the waypoint.

I've rebased this PR on master and then added a couple of changes here: https://github.com/rmackay9/rmackay9-ardupilot/commits/bnsgeyer-obey-wpnav-accel

I think the approach in this new branch is correct but it includes a couple of changes that you may or may not like:

PosControl now accepts very small (less than 1cm/s) changes in the maximum speed. The 1cm minimum change requirement was apparently added 5years ago before we had the float::is_equal() method. This change does eat a bit of extra CPU though as we will recalculate the leash about 4000 times over a few seconds. The leash calculation is not that expensive however but perhaps we should discuss this with @lthall.
I've removed the "_wp_current_speed_cms" because I think it was always the same as the pos controller's maximum speed. My guess is that it was added to get around the issue mentioned above.
I've renamed, "_wp_desired_speed_cms" to "_wp_desired_speed_xy_cms" just to clarify that it's only meant for the xy axis and not z.

bnsgeyer · 2020-02-20T16:12:24Z

@rmackay9 the current waypoint speed was added to track the progression of the waypoint’s speed to the desired waypoint speed. I’ve never thought about tracking out through the position controllers max speed. I am a little concerned that if the position controller max speed is written by another caller then you could see the position controller violate the wp accel but I guess the wp library would catch it on the next loop. Otherwise I like this implementation. I will pull it into my branch and test it tonight. Thanks for the help!!

also small changes in max speed for z-axis

Includes commits by rmackay9 AC_WPNav: fixup max speed acceleration AC_WPNav: simplify the initialisation of poscontrol's max speed Changed at Leonard's request to keep things simpler

bnsgeyer · 2020-02-21T04:09:51Z

@rmackay9 I checked pulled your commits into my branch and did a quick check of the speed changes and moving in and out of auto. I think this will work fine. thanks for the assist. I rebased and squashed the commits.

rmackay9 · 2020-02-21T23:53:50Z

Merged, thanks!

rmackay9 · 2020-05-06T05:22:35Z

this is included in Copter-4.0.4-rc1

bnsgeyer added BUG Copter TradHeli DevCallTopic labels Nov 11, 2019

bnsgeyer requested review from lthall and rmackay9 November 11, 2019 15:24

CraigElder removed the DevCallTopic label Nov 11, 2019

bnsgeyer force-pushed the pr-wpnav-obey-wpaccel branch 3 times, most recently from 6dde95a to b763ba8 Compare November 12, 2019 00:33

rmackay9 reviewed Nov 12, 2019

View reviewed changes

libraries/AC_WPNav/AC_WPNav.cpp Outdated Show resolved Hide resolved

bnsgeyer force-pushed the pr-wpnav-obey-wpaccel branch from b763ba8 to 463a0fe Compare November 13, 2019 00:46

bnsgeyer added the DevCallTopic label Dec 1, 2019

CraigElder removed the DevCallTopic label Dec 3, 2019

bnsgeyer force-pushed the pr-wpnav-obey-wpaccel branch from 463a0fe to e5c07d5 Compare February 4, 2020 23:55

bnsgeyer added the DevCallTopic label Feb 9, 2020

This was referenced Feb 10, 2020

Wp_Nav: an easy way to correct speed reduce issue to avoid twitch #13102

Closed

Bug Reprot: Copter: wp_nav requested speed reduce can cause attitude shock #13251

Closed

CraigElder removed the DevCallTopic label Feb 11, 2020

bnsgeyer force-pushed the pr-wpnav-obey-wpaccel branch from d1bb2e1 to 7cb5519 Compare February 11, 2020 04:05

lthall approved these changes Feb 16, 2020

View reviewed changes

rmackay9 reviewed Feb 19, 2020

View reviewed changes

rmackay9 and others added 2 commits February 20, 2020 23:04

AC_PosControl: allow smaller changes in max speed and accel

94aefe1

also small changes in max speed for z-axis

AC_WPNAV:make speed changes during missions obey WPNAV_ACCEL

9834b02

Includes commits by rmackay9 AC_WPNav: fixup max speed acceleration AC_WPNav: simplify the initialisation of poscontrol's max speed Changed at Leonard's request to keep things simpler

bnsgeyer force-pushed the pr-wpnav-obey-wpaccel branch from 3b12466 to 9834b02 Compare February 21, 2020 04:05

rmackay9 merged commit 593ff68 into ArduPilot:master Feb 21, 2020

lukezhqin mentioned this pull request Feb 24, 2020

AC_WPNav: reduce main load to obey wp_spd change rule. #13657

Closed

lukezhqin referenced this pull request Mar 5, 2020

AC_AttitudeControl: Formatting Changes

32cc642

bnsgeyer deleted the pr-wpnav-obey-wpaccel branch March 30, 2022 02:58

This pull request was closed.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Make speed changes during missions obey WPNAV_ACCEL #12821

Make speed changes during missions obey WPNAV_ACCEL #12821

bnsgeyer commented Nov 11, 2019

bnsgeyer commented Nov 12, 2019

peterbarker commented Nov 12, 2019 via email

bnsgeyer commented Nov 12, 2019

bnsgeyer commented Nov 13, 2019

peterbarker commented Nov 13, 2019

bnsgeyer commented Nov 13, 2019

rmackay9 commented Dec 3, 2019

rmackay9 commented Dec 3, 2019

bnsgeyer commented Feb 5, 2020

bnsgeyer commented Feb 6, 2020

peterbarker commented Feb 6, 2020

bnsgeyer commented Feb 7, 2020

bnsgeyer commented Feb 11, 2020

lthall left a comment

rmackay9 Feb 19, 2020

bnsgeyer commented Feb 19, 2020

rmackay9 commented Feb 19, 2020 •

edited

Loading

bnsgeyer commented Feb 20, 2020 •

edited

Loading

bnsgeyer commented Feb 21, 2020

rmackay9 commented Feb 21, 2020

rmackay9 commented May 6, 2020

Make speed changes during missions obey WPNAV_ACCEL #12821

Make speed changes during missions obey WPNAV_ACCEL #12821

Conversation

bnsgeyer commented Nov 11, 2019

bnsgeyer commented Nov 12, 2019

peterbarker commented Nov 12, 2019 via email

bnsgeyer commented Nov 12, 2019

bnsgeyer commented Nov 13, 2019

peterbarker commented Nov 13, 2019

bnsgeyer commented Nov 13, 2019

rmackay9 commented Dec 3, 2019

rmackay9 commented Dec 3, 2019

bnsgeyer commented Feb 5, 2020

bnsgeyer commented Feb 6, 2020

peterbarker commented Feb 6, 2020

bnsgeyer commented Feb 7, 2020

bnsgeyer commented Feb 11, 2020

lthall left a comment

Choose a reason for hiding this comment

rmackay9 Feb 19, 2020

Choose a reason for hiding this comment

bnsgeyer commented Feb 19, 2020

rmackay9 commented Feb 19, 2020 • edited Loading

bnsgeyer commented Feb 20, 2020 • edited Loading

bnsgeyer commented Feb 21, 2020

rmackay9 commented Feb 21, 2020

rmackay9 commented May 6, 2020

rmackay9 commented Feb 19, 2020 •

edited

Loading

bnsgeyer commented Feb 20, 2020 •

edited

Loading