From 338ae69f15dec6abbdaaf98c4583bb715e3ded65 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 11 May 2022 13:56:06 +1000 Subject: [PATCH] ArduCopter: support *10 multipler when storing/retrieving radius in NAV_LOITER_TURNS --- ArduCopter/mode_auto.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index f3b084491f..437e38b600 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1363,7 +1363,12 @@ void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) const Location circle_center = loc_from_cmd(cmd, copter.current_loc); // calculate radius - uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 + uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 + if (cmd.id == MAV_CMD_NAV_LOITER_TURNS && + cmd.type_specific_bits & (1U << 0)) { + // special storage handling allows for larger radii + circle_radius_m *= 10; + } // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge circle_movetoedge_start(circle_center, circle_radius_m);