Skip to content

Conversation

henrywarhurst
Copy link
Contributor

@henrywarhurst henrywarhurst commented Aug 10, 2025

Fixes #19028

Yaw the copter on the spot when using a 0 radius LOITER_TURNS instead of using the radius from the previous time a LOITER_TURNS was conducted.

Using zero radius with a LOITER_TURNS falls through to the already implemented panorama mode:

if (_radius_m <= 0) {
_angle_rad = _ahrs.yaw;
return;

Tested before and after in SITL and confirmed this fixes the bug.

if (!is_zero(radius_m)) {
copter.circle_nav->set_radius_cm(radius_m * 100.0f);
}
copter.circle_nav->set_radius_cm(radius_m * 100.0f);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wonder if there's a default radius we should set instead if radius is not > 0?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if radius is 0 it should pirouette in place, IMO

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think pirouette in place feels like the natural limit as loiter radius -> 0

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I agree - and the called code seems to think so too.

Strange thing is that the code has been doing this since 2016....

@henrywarhurst
Copy link
Contributor Author

btw are these test failures flakes @peterbarker?

Copy link
Contributor

@peterbarker peterbarker left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yaw determination code already seems to handle this case.

if (!is_zero(radius_m)) {
copter.circle_nav->set_radius_cm(radius_m * 100.0f);
}
copter.circle_nav->set_radius_cm(radius_m * 100.0f);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I agree - and the called code seems to think so too.

Strange thing is that the code has been doing this since 2016....

@peterbarker
Copy link
Contributor

btw are these test failures flakes @peterbarker?

One was. The other is a serious (pre-existing) problem which I'm now investigating.

@henrywarhurst
Copy link
Contributor Author

friendly ping @rmackay9

@rmackay9 rmackay9 merged commit 62ef39b into ArduPilot:master Aug 19, 2025
94 of 95 checks passed
@rmackay9
Copy link
Contributor

Thanks!

@henrywarhurst henrywarhurst deleted the hwarhurst/fix-19028 branch August 19, 2025 09:20
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

Copter: LOITER_TURNS command with zero radius uses radius of last time LOITER_TURNS command was executed
5 participants