WP_RADIUS has no effect in AUTO mode

Youssof · July 11, 2023, 2:13pm

Hello,

We have been using ArduRover 4.2.3 for quite some time now with our vectored thrust boat and we’re satisfied with the autopilot’s performance after tuning.

A few days ago we decided to make the upgrade to 4.3 to experiment with S-Curves and also utilise other features like the SIYI camera gimbal control. The boat seems to follow lines pretty well, however it appeared to overshoot the waypoints by quite a bit when there is a 90 degree (or sharper) turn. I can see that this happening because the WP_RADIUS is set too low so it’s taking longer for the boat to get to a “waypoint reached” state before it starts to turn to the next one.

Here’s the problem: Increasing the WP_RADIUS parameter doesn’t appear to have any affect on the boat’s pathing. The boat follows a basic square pattern with the same degree of overshoot with WP_RADIUS set to 15m as it does when it’s set to 100m and everything in between. The parameter is simply being ignored.

What could be going on here? Is there another parameter that limits the effects of WP_RADIUS?

Yuri_Rage · July 11, 2023, 2:48pm

Known issue in Rover 4.3 and higher. Expecting a fix in Rover 4.4 when it goes stable.

github.com/ArduPilot/ardupilot

AR_WPNav: Rover 4.3+ does not honor WP_RADIUS

opened 03:57PM - 12 Apr 23 UTC

yuri-rage

Rover

https://github.com/ArduPilot/ardupilot/blob/db7fad0acaee98dc6e00d725a0535ef184f0…3819/libraries/AR_WPNav/AR_WPNav.cpp#L459 The above referenced line is only ever reached when the vehicle has passed the destination waypoint (a result of `s_finished` remaining false until enough time has elapsed to have arrived at the destination). Thus, there is always an overshoot of the desired waypoint, which necessitates a course correction upon pivot completion. If I change the logic to ignore `s_finished` except during fast waypoints, then `_reached_destination` becomes true in a timely manner, but it seems the controller is still awaiting time elapsed on that leg before initiating a pivot, and the resulting behavior is actually worse: ``` // check if we've reached the waypoint if (!_reached_destination) { // "fast" waypoints are complete once the intermediate point reaches the destination if (_fast_waypoint && s_finished) { _reached_destination = true; } else { // regular waypoints also require the vehicle to be within the waypoint radius or past the "finish line" const bool near_wp = current_loc.get_distance(_destination) <= _radius; const bool past_wp = current_loc.past_interval_finish_line(_origin, _destination); _reached_destination = near_wp || past_wp; } } ``` On the other hand, if I use a Lua script (workaround) to force the WP ahead when WP_RADIUS is satisfied, the vehicle behaves extremely well, so I'm certain that I'm on the right track. I suspect this is among the chief reasons that the community has had a hard time tuning the new nav controller. Any vehicle that is slightly "loose" in deceleration will overshoot nearly every waypoint, sometimes by a significant margin, causing path control issues thereafter.

Youssof · July 13, 2023, 1:56pm

Thanks! It’s good to know that I’m not crazy haha. I see in your Github issue that you mentioned using a Lua script to “force the WP ahead”. Would you mind sharing how you did this? We would ideally like to use 4.3+, but would have to revert back to 4.2.3 if we’re unable to add some kind of temporary workaround.