AC_Circle: Always restore the radius from parameters

[giacomo892]

Allow the pilot to reload the circle radius from the parameters every time CIRCLE mode is enabled.

It is nice from a safety point of view. Had my copter heading towards a tree because I supposed the circle to be 10m in radius, instead it was much higher due to me having adjusted it with the pitch stick in a previous use of CIRCLE mode.

[rmackay9]

nice work. I'd like to hear from other devs but I think the circle radius should always be restored by re-entering circle mode. This would avoid the need for the new option or at least it should be defaulted to 1. The equivalent in Auto mode is the WPNAV_SPEED. If the user exits Auto and enters again the target speed will be reset to the parameter value.

So if we go with this then we probably need to change AC_CIRCLE so it doesn't directly use the _radius variable but instead has another radius parameter which is used. set_radius() should only update this temporary value leaving the _radius parameter value unchanged.

[giacomo892]

Thanks @rmackay9 . Let's wait for other opinions. In the meanwhile i've flight tested this and it works as expected.

I'm not quite sure we should reset the radius every time, since if you start from the perimeter then it might be actually good to keep the user set radius and just continue the circle from where it was stopped.

I guess a good compromise could be to reset the radius by default when the user choose to start the circle from the centre of it since if you start at the perimeter you expect the moves of the machine (going CW or CCW), while if you start from the center of the circle, the machine has to reposition itself and it might head very quickly toward and obstacle. What do you think?

[IamPete1]

Just need to make sure were still using the right radius and not the param value for circles via auto and guided.

[giacomo892]

@IamPete1 looks like Loiter with turns is broken on master, but this PR will let it use the radius it needs.

@tridge @rmackay9 I've reworked this as discussed. Good for you?

[shellixyz]

Tested with SITL, LGTM 👍

[IamPete1]

This breaks auto circles, they are done at the param radius rather than the radius given in the mission. I suspect your not testing it correctly because they also seem to be fine on master for me.

To fix I think you just need to set the param value not the internal variable in the set rad call.

ardupilot/libraries/AC_WPNav/AC_Circle.cpp

Line 150 in f7da853

_radius = constrain_float(radius_cm, 0, AC_CIRCLE_RADIUS_MAX);

[IamPete1]

Actually my suggestion will break updating the radius from sticks in a mission. You probably need a set_radius call that sets the param and a update_radius call to change the internal variable.

Of course this change also means that you cant just change the radius param and have it update in real time. You would have to exit and re-enter circle mode. Not a complaint, just a observation.

[giacomo892]

Thanks for the review @IamPete1
I've addressed what you have pointed out and now the radius from AUTO mode should be set correctly. Only when CIRCLE is called as a mode we restore the radius.
What mission element is CIRCLE? Loiter Turns? If I try it in SITL, the copter arrives to the WP and waits there forever.

[IamPete1]

@giacomo892 Yeah, Loiter turns, you need to give it a number of turns and a radius.

[giacomo892]

Tested on SITL, working as expected beside #17126

[rmackay9]

I thought there was an issue with the get_radius() needing to check if init() had been called or not because there is a code path that calls get_radius() before init is called. I think it's when the Loiter-turns command has a lat,lon specified.

[giacomo892]

I thought there was an issue with the get_radius() needing to check if init() had been called or not because there is a code path that calls get_radius() before init is called. I think it's when the Loiter-turns command has a lat,lon specified.

Thanks for the review. Looks like it is not the case, it seems all good to me.

[tridge]

add _last_radius? @IamPete1 will do a new PR

[IamPete1]

Re-tested in SITL, works great, Thank you