ArduCopter: implement MAV_CMD_DO_ORBIT in Guided mode

Implements MAV_CMD_DO_ORBIT (command 34) in ArduCopter Guided mode.

Parameters:
- param1: radius in meters (positive=CW, negative=CCW)
- param2: tangential speed in m/s (0=default)
- param3: yaw behaviour (ORBIT_YAW_BEHAVIOUR enum, all 5 values supported)
- param4: number of turns (0=forever, NaN=preserve existing)
- x/y: center latitude/longitude
- z: altitude in meters

Implementation:
- New SubMode::Orbit in ModeGuided
- circle_start(), orbit_run(), orbit_apply_yaw_behaviour() in mode_guided.cpp
- handle_MAV_CMD_DO_ORBIT() in GCS_MAVLink_Copter.cpp
- MAV_CMD_DO_ORBIT in command_long_stores_location() in GCS_Common.cpp
- AC_COPTER_MODEGUIDED_ORBIT_ENABLED flash guard (default: MODE_GUIDED_ENABLED
  && HAL_PROGRAM_SIZE_LIMIT_KB>1024)
- AP_MAVLINK_MAV_CMD_DO_ORBIT_ENABLED in GCS_config.h, build_options.py,
  extract_features.py and test_build_options.py
- Autotest DO_ORBIT in arducopter.py

Fixes:
- Double speed bug: removed duplicate update_ms() call in orbit_run()
- Post-orbit hold: use hold_position() instead of pos_control_start()
- Yaw consistency during approach, orbiting and after completion
- Cross-platform compile: use copter.ahrs.get_yaw_rad()

Addresses #15098

Author: researcher3

ArduCopter/GCS_MAVLink_Copter.cpp

@@ -154,6 +154,19 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
                     POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity & acceleration
         target_accel_ned_mss = copter.mode_guided.get_target_accel_NED_mss();
         break;
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED        
+    case ModeGuided::SubMode::Orbit:
+        if (copter.mode_guided.circle_moving_to_edge()) {
+            // moving to edge - report WP position target
+            type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
+                        POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
+                        POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE;
+            target_pos_ned_m = copter.mode_guided.get_target_pos_NED_m().tofloat();
+            break;
+        }
+        // orbiting - no local position target to report
+        return;
+#endif  // AC_COPTER_MODEGUIDED_ORBIT_ENABLED
     }
 
     mavlink_msg_position_target_local_ned_send(
@@ -552,8 +565,14 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
             return MAV_RESULT_ACCEPTED;
         }
         return MAV_RESULT_FAILED;
+
 #endif
 
+#if AP_MAVLINK_MAV_CMD_DO_ORBIT_ENABLED
+    case MAV_CMD_DO_ORBIT:
+        return handle_MAV_CMD_DO_ORBIT(packet);
+#endif  // AP_MAVLINK_MAV_CMD_DO_ORBIT_ENABLED
+
     default:
         return GCS_MAVLINK::handle_command_int_packet(packet, msg);
     }
@@ -837,6 +856,52 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_comma
     return MAV_RESULT_DENIED;
 }
 
+#if AP_MAVLINK_MAV_CMD_DO_ORBIT_ENABLED
+MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_ORBIT(const mavlink_command_int_t &packet)
+{
+    // reject if not in Guided mode
+    if (!copter.flightmode->in_guided_mode()) {
+        return MAV_RESULT_FAILED;
+    }
+
+    // param1: radius (m) - positive = CW, negative = CCW (MAVLink spec)
+    if (isnan(packet.param1) || fabsf(packet.param1) < 0.1f) {
+        return MAV_RESULT_DENIED;
+    }
+    const float radius_m = fabsf(packet.param1);
+    const bool ccw = (packet.param1 < 0.0f);
+
+    // param2: tangential velocity (m/s) - optional, 0 or NaN = use default
+    const float speed_ms = isnan(packet.param2) ? 0.0f : fabsf(packet.param2);
+
+    // param3: yaw behaviour (ORBIT_YAW_BEHAVIOUR enum), NaN or 0 = face center
+
+    // x/y: center coordinates
+    if (packet.x == INT32_MAX || packet.y == INT32_MAX) {
+        return MAV_RESULT_DENIED;
+    }
+
+    // build center Location from packet
+    if (!check_latlng(packet.x, packet.y)) {
+        return MAV_RESULT_DENIED;
+    }
+    Location circle_center;
+    if (!location_from_command_t(packet, circle_center)) {
+        return MAV_RESULT_DENIED;
+    }
+
+    // param4: Orbit around the centre point for this many radians. 0: Orbit forever
+    // NaN means "do not change" - preserve existing turn count
+    const bool update_turns = !isnan(packet.param4);
+    const float turns = update_turns ? fabsf(packet.param4) : 0.0f;
+    // param3: yaw behaviour (ORBIT_YAW_BEHAVIOUR enum), NaN or 0 = face center
+    const ORBIT_YAW_BEHAVIOUR yaw_behaviour = (isnan(packet.param3) || packet.param3 < 0) ? ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER : (ORBIT_YAW_BEHAVIOUR)(int)packet.param3;
+    copter.mode_guided.orbit_start(circle_center, radius_m, ccw, speed_ms, update_turns, turns, yaw_behaviour);
+
+    return MAV_RESULT_ACCEPTED;
+}
+#endif  // AP_MAVLINK_MAV_CMD_DO_ORBIT_ENABLED
+
 // this is called on receipt of a MANUAL_CONTROL packet and is
 // expected to call manual_override to override RC input on desired
 // axes.

ArduCopter/GCS_MAVLink_Copter.h

@@ -3,6 +3,7 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Winch/AP_Winch_config.h>
 #include "defines.h"
+#include "config.h"
 
 #ifndef AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED
 #define AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED 1
@@ -123,4 +124,8 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK
     MAV_RESULT handle_MAV_CMD_DO_WINCH(const mavlink_command_int_t &packet);
 #endif
 
+#if AP_MAVLINK_MAV_CMD_DO_ORBIT_ENABLED
+    MAV_RESULT handle_MAV_CMD_DO_ORBIT(const mavlink_command_int_t &packet);
+#endif
+
 };

ArduCopter/config.h

@@ -178,6 +178,10 @@
 # define MODE_GUIDED_ENABLED 1
 #endif
 
+#ifndef AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+# define AC_COPTER_MODEGUIDED_ORBIT_ENABLED (MODE_GUIDED_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB>1024)
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // GuidedNoGPS mode - control vehicle's angles from GCS
 #ifndef MODE_GUIDED_NOGPS_ENABLED

ArduCopter/mode.h

@@ -1153,6 +1153,11 @@ class ModeGuided : public Mode {
     bool set_speed_up_ms(float speed_up_ms) override;
     bool set_speed_down_ms(float speed_down_ms) override;
 
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+    // true while moving to circle edge before orbiting
+    bool _circle_moving_to_edge = false;
+#endif
+
     // initialises position controller to implement take-off
     // takeoff_alt_m is interpreted as alt-above-home (in m) or alt-above-terrain if a rangefinder is available
     bool do_user_takeoff_start_m(float takeoff_alt_m) override;
@@ -1165,12 +1170,21 @@ class ModeGuided : public Mode {
         VelAccel,
         Accel,
         Angle,
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+        Orbit,
+#endif
     };
 
     SubMode submode() const { return guided_mode; }
 
     void angle_control_start();
     void angle_control_run();
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+    void orbit_start(const Location &circle_center, float radius_m, bool ccw, float speed_ms, bool update_turns, float turns, ORBIT_YAW_BEHAVIOUR yaw_behaviour = ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER);
+    void orbit_run();
+    void orbit_apply_yaw_behaviour();
+    bool circle_moving_to_edge() const { return _circle_moving_to_edge; }
+#endif  // AP_MAVLINK_MAV_CMD_DO_ORBIT_ENABLED
 
     // return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rate control
     uint32_t get_timeout_ms() const;
@@ -1228,6 +1242,13 @@ class ModeGuided : public Mode {
     void pause_control_run();
     void posvelaccel_control_run();
     void set_yaw_state_rad(bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_angle);
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+    float _orbit_rate_degs = 20.0f;  // desired orbit rate in deg/s (signed for direction)
+    ORBIT_YAW_BEHAVIOUR _orbit_yaw_behaviour = ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER;
+    float _orbit_turns = 0.0f;
+    float _orbit_angle_total_at_start = 0.0f;
+    bool _orbit_update_turns = true;
+#endif
 
     // controls which controller is run (pos or vel):
     SubMode guided_mode = SubMode::TakeOff;

ArduCopter/mode_guided.cpp

@@ -106,8 +106,18 @@ void ModeGuided::run()
     case SubMode::Angle:
         angle_control_run();
         break;
+
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+    case SubMode::Orbit:
+        if (_circle_moving_to_edge) {
+            wp_control_run();
+        } else {
+            orbit_run();
+        }
+        break;
+#endif  // AC_COPTER_MODEGUIDED_ORBIT_ENABLED
     }
- }
+}
 
 // returns true if the Guided-mode-option is set (see GUID_OPTIONS)
 bool ModeGuided::option_is_enabled(Option option) const
@@ -147,6 +157,9 @@ bool ModeGuided::move_vehicle_on_ekf_reset() const
     case SubMode::WP:
     case SubMode::Pos:
     case SubMode::PosVelAccel:
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+    case SubMode::Orbit:
+#endif
         // these submodes have absolute position targets so we smoothly slew the target upon an ekf reset
         return true;
     }
@@ -241,6 +254,20 @@ void ModeGuided::wp_control_run()
     // run waypoint controller
     copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
+    // if moving to circle edge, check if we've arrived
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+    if (guided_mode == SubMode::Orbit && _circle_moving_to_edge && wp_nav->reached_wp_destination()) {
+        // initialise circle controller and switch to circle submode
+        copter.circle_nav->init_NED_m(
+            copter.circle_nav->get_center_NED_m(),
+            copter.circle_nav->center_is_terrain_alt(),
+            _orbit_rate_degs);
+        _circle_moving_to_edge = false;
+        orbit_apply_yaw_behaviour();
+        guided_mode = SubMode::Orbit;
+    }
+#endif  // AP_MAVLINK_MAV_CMD_DO_ORBIT_ENABLED
+
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control->D_update_controller();
 
@@ -449,6 +476,21 @@ bool ModeGuided::get_wp(Location& destination) const
     case SubMode::Pos:
         destination = Location::from_ekf_offset_NED_m(guided_pos_target_ned_m, guided_is_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
         return true;
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+    case SubMode::Orbit:
+        if (_circle_moving_to_edge) {
+            return wp_nav->get_oa_wp_destination(destination);
+        }
+        // already orbiting - return circle center
+        {
+            Location circle_center;
+            if (!AP::ahrs().get_location_from_origin_offset_NED(circle_center, copter.circle_nav->get_center_NED_m())) {
+                return false;
+            }
+            destination = circle_center;
+            return true;
+        }
+#endif  // AP_MAVLINK_MAV_CMD_DO_ORBIT_ENABLED
     case SubMode::Angle:
     case SubMode::TakeOff:
     case SubMode::Accel:
@@ -1147,6 +1189,12 @@ float ModeGuided::wp_distance_m() const
         return get_horizontal_distance(pos_control->get_pos_estimate_NED_m().xy().tofloat(), guided_pos_target_ned_m.xy().tofloat());
     case SubMode::PosVelAccel:
         return pos_control->get_pos_error_NE_m();
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+    case SubMode::Orbit:
+        return _circle_moving_to_edge ? 
+            wp_nav->get_wp_distance_to_destination_m() :
+            copter.circle_nav->get_distance_to_target_m();
+#endif
     default:
         return 0.0f;
     }
@@ -1161,6 +1209,12 @@ float ModeGuided::wp_bearing_deg() const
         return degrees(get_bearing_rad(pos_control->get_pos_estimate_NED_m().xy().tofloat(), guided_pos_target_ned_m.xy().tofloat()));
     case SubMode::PosVelAccel:
         return degrees(pos_control->get_bearing_to_target_rad());
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+    case SubMode::Orbit:
+        return _circle_moving_to_edge ?
+            degrees(wp_nav->get_wp_bearing_to_destination_rad()) :
+            degrees(copter.circle_nav->get_bearing_to_target_rad());
+#endif
     case SubMode::TakeOff:
     case SubMode::Accel:
     case SubMode::VelAccel:
@@ -1183,6 +1237,11 @@ float ModeGuided::crosstrack_error_m() const
     case SubMode::VelAccel:
     case SubMode::PosVelAccel:
         return pos_control->crosstrack_error_m();
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+    case SubMode::Orbit:
+        return _circle_moving_to_edge ?
+            wp_nav->crosstrack_error_m() : 0;
+#endif
     case SubMode::Angle:
         // no track to have a crosstrack to
         return 0;
@@ -1211,4 +1270,110 @@ bool ModeGuided::resume()
     return true;
 }
 
-#endif
+#if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+// orbit_start - initialise guided controller to fly a circle around a specified location
+void ModeGuided::orbit_start(const Location &circle_center, float radius_m, bool ccw, float speed_ms, bool update_turns, float turns, ORBIT_YAW_BEHAVIOUR yaw_behaviour)
+{
+    // set circle center, radius and direction
+    copter.circle_nav->set_center(circle_center);
+    // configure position controller speeds and accelerations
+    pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
+    pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
+    pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
+    pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
+    copter.circle_nav->set_radius_m(radius_m);
+
+    // convert tangential speed to angular rate, or use default rate with correct direction
+    if (is_positive(speed_ms) && is_positive(radius_m)) {
+        _orbit_rate_degs = degrees(speed_ms / radius_m);
+    } else {
+        _orbit_rate_degs = fabsf(copter.circle_nav->get_rate_degs());
+    }
+    if (ccw) {
+        _orbit_rate_degs = -_orbit_rate_degs;
+    }
+    copter.circle_nav->set_rate_degs(_orbit_rate_degs);
+    // store requested number of turns (0 = orbit forever)
+    if (update_turns) {
+        _orbit_turns = turns;
+        _orbit_angle_total_at_start = copter.circle_nav->get_angle_total_rad();
+    }
+    _orbit_yaw_behaviour = yaw_behaviour;
+
+    // check distance to edge of circle
+    Vector3p circle_edge_ned_m;
+    float dist_to_edge_m;
+    copter.circle_nav->get_closest_point_on_circle_NED_m(circle_edge_ned_m, dist_to_edge_m);
+
+    // if more than 3m away, fly to edge first
+    if (dist_to_edge_m > 3.0f) {
+        Location circle_edge = Location::from_ekf_offset_NED_m(circle_edge_ned_m, Location::AltFrame::ABOVE_ORIGIN);
+        circle_edge.copy_alt_from(circle_center);
+        if (!wp_nav->set_wp_destination_loc(circle_edge)) {
+            // terrain data missing
+            copter.failsafe_terrain_on_event();
+            return;
+        }
+        // during approach look in direction of travel (default WP yaw behaviour)
+        auto_yaw.set_mode_to_default(false);
+        guided_mode = SubMode::Orbit;
+        _circle_moving_to_edge = true;
+    } else {
+        // already at edge - start circling immediately
+        copter.circle_nav->init_NED_m(
+            copter.circle_nav->get_center_NED_m(),
+            copter.circle_nav->center_is_terrain_alt(),
+            _orbit_rate_degs);
+        // set yaw behaviour
+        orbit_apply_yaw_behaviour();
+        guided_mode = SubMode::Orbit;
+    }
+}
+
+// orbit_apply_yaw_behaviour - set yaw mode based on ORBIT_YAW_BEHAVIOUR parameter
+void ModeGuided::orbit_apply_yaw_behaviour()
+{
+    switch ((ORBIT_YAW_BEHAVIOUR)_orbit_yaw_behaviour) {
+    case ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER:
+    default:
+        auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
+        break;
+    case ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING:
+        auto_yaw.set_fixed_yaw_rad(copter.ahrs.get_yaw_rad(), 0.0f, 0, false);
+        break;
+    case ORBIT_YAW_BEHAVIOUR_UNCONTROLLED:
+        auto_yaw.set_mode(AutoYaw::Mode::HOLD);
+        break;
+    case ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
+        auto_yaw.set_mode(AutoYaw::Mode::LOOK_AHEAD);
+        break;
+    case ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:
+        auto_yaw.set_mode(AutoYaw::Mode::PILOT_RATE);
+        break;
+    case ORBIT_YAW_BEHAVIOUR_UNCHANGED:
+        // do not change yaw mode
+        break;
+    }
+}
+
+// orbit_run - run circle controller
+void ModeGuided::orbit_run()
+{
+    // call circle controller
+    copter.failsafe_terrain_set_status(copter.circle_nav->update_ms(0.0f));
+    // check if we've completed the requested number of turns
+    if (is_positive(_orbit_turns)) {
+        const float turns_completed = fabsf(copter.circle_nav->get_angle_total_rad() / float(M_2PI));
+        if (turns_completed >= _orbit_turns) {
+            // orbit complete - hold position and maintain yaw behaviour
+            hold_position();
+            orbit_apply_yaw_behaviour();
+            return;
+        }
+    }
+    // call attitude, position and yaw controllers
+    pos_control->D_update_controller();
+    attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
+}
+#endif  // AC_COPTER_MODEGUIDED_ORBIT_ENABLED
+#endif  // MODE_GUIDED_ENABLED