ArduCopter: MAV_CMD_DO_ORBIT improvements

- Fix orbit completion: use hold_position() instead of pos_control_start()
  to prevent vehicle flying back to start position and descending
- Fix yaw after orbit completion: maintain yaw behaviour instead of
  always pointing to circle center
- Implement param3 (ORBIT_YAW_BEHAVIOUR):
  0=face center, 1=hold initial heading, 2=uncontrolled,
  3=tangent to circle, 4=RC controlled, 5=unchanged
- Fix approach yaw: use set_mode_to_default() during edge approach
  so vehicle looks in direction of travel

Author: researcher3

ArduCopter/GCS_MAVLink_Copter.cpp

@@ -899,10 +899,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_ORBIT(const mavlink_command_int
     // 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 - center-facing is default, others are future work
-    // const ORBIT_YAW_BEHAVIOUR yaw_behaviour = isnan(packet.param3)
-    //     ? ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER
-    //     : (ORBIT_YAW_BEHAVIOUR)(int)packet.param3;
+    // 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) {
@@ -922,10 +919,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_ORBIT(const mavlink_command_int
     // 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;
-    copter.mode_guided.circle_start(circle_center, radius_m, ccw, speed_ms, update_turns, turns);
-
-    // start orbit
-    copter.mode_guided.circle_start(circle_center, radius_m, ccw, speed_ms, update_turns, turns);
+    // param3: yaw behaviour (ORBIT_YAW_BEHAVIOUR enum), NaN or 0 = face center
+    const uint8_t yaw_behaviour = (isnan(packet.param3) || packet.param3 < 0) ? 0 : (uint8_t)packet.param3;
+    copter.mode_guided.circle_start(circle_center, radius_m, ccw, speed_ms, update_turns, turns, yaw_behaviour);
 
     return MAV_RESULT_ACCEPTED;
 }

ArduCopter/mode.h

@@ -1180,8 +1180,9 @@ class ModeGuided : public Mode {
     void angle_control_start();
     void angle_control_run();
 #if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
-    void circle_start(const Location &circle_center, float radius_m, bool ccw, float speed_ms, bool update_turns, float turns);
+    void circle_start(const Location &circle_center, float radius_m, bool ccw, float speed_ms, bool update_turns, float turns, uint8_t yaw_behaviour = 0);
     void orbit_run();
+    void orbit_apply_yaw_behaviour();
     bool circle_moving_to_edge() const { return _circle_moving_to_edge; }
 #endif
 
@@ -1243,6 +1244,7 @@ class ModeGuided : public Mode {
     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)
+    uint8_t _orbit_yaw_behaviour = 0; // yaw behaviour during orbit (ORBIT_YAW_BEHAVIOUR enum)
     float _orbit_turns = 0.0f;
     float _orbit_angle_total_at_start = 0.0f;
     bool _orbit_update_turns = true;

ArduCopter/mode_guided.cpp

@@ -263,7 +263,7 @@ void ModeGuided::wp_control_run()
             copter.circle_nav->center_is_terrain_alt(),
             _orbit_rate_degs);
         _circle_moving_to_edge = false;
-        auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
+        orbit_apply_yaw_behaviour();
         guided_mode = SubMode::Orbit;
     }
 #endif
@@ -1272,7 +1272,7 @@ bool ModeGuided::resume()
 
 #if AC_COPTER_MODEGUIDED_ORBIT_ENABLED
 // circle_start - initialise guided controller to fly a circle around a specified location
-void ModeGuided::circle_start(const Location &circle_center, float radius_m, bool ccw, float speed_ms, bool update_turns, float turns)
+void ModeGuided::circle_start(const Location &circle_center, float radius_m, bool ccw, float speed_ms, bool update_turns, float turns, uint8_t yaw_behaviour)
 {
     // set circle center, radius and direction
     copter.circle_nav->set_center(circle_center);
@@ -1296,6 +1296,7 @@ void ModeGuided::circle_start(const Location &circle_center, float radius_m, boo
         _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;
@@ -1311,6 +1312,8 @@ void ModeGuided::circle_start(const Location &circle_center, float radius_m, boo
             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 {
@@ -1319,12 +1322,38 @@ void ModeGuided::circle_start(const Location &circle_center, float radius_m, boo
             copter.circle_nav->get_center_NED_m(),
             copter.circle_nav->center_is_terrain_alt(),
             _orbit_rate_degs);
-        // point camera/yaw toward circle center
-        auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
+        // 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) {
+    case 0: // ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER
+    default:
+        auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
+        break;
+    case 1: // ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING
+        auto_yaw.set_fixed_yaw_rad(AP::ahrs().get_yaw(), 0.0f, 0, false);
+        break;
+    case 2: // ORBIT_YAW_BEHAVIOUR_UNCONTROLLED
+        auto_yaw.set_mode(AutoYaw::Mode::HOLD);
+        break;
+    case 3: // ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE
+        auto_yaw.set_mode(AutoYaw::Mode::LOOK_AHEAD);
+        break;
+    case 4: // ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED
+        auto_yaw.set_mode(AutoYaw::Mode::PILOT_RATE);
+        break;
+    case 5: // ORBIT_YAW_BEHAVIOUR_UNCHANGED
+        // do not change yaw mode
+        break;
+    }
+}
+
 // orbit_run - run circle controller
 void ModeGuided::orbit_run()
 {
@@ -1335,8 +1364,9 @@ void ModeGuided::orbit_run()
     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
-            pos_control_start();
+            // orbit complete - hold position and maintain yaw behaviour
+            hold_position();
+            orbit_apply_yaw_behaviour();
             return;
         }
     }