Copter: add support for CONDITION_YAW-relative-to-vehicle-heading

ArduCopter/GCS_Mavlink.cpp

@@ -934,6 +934,13 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_CONDITION_YAW(const mavlink_comman
             direction,
             true);
         return MAV_RESULT_ACCEPTED;
+    case 2: {
+        // relative-to-vehicle-heading
+        // start by turning angle and direction into a relative yaw:
+        const float relative_angle = wrap_180(direction * target_angle);
+        copter.flightmode->auto_yaw.set_vehicle_heading_yaw(relative_angle, rate_degs);
+        return MAV_RESULT_ACCEPTED;
+    }
     default:
         return MAV_RESULT_FAILED;
     }

ArduCopter/autoyaw.cpp

@@ -16,6 +16,7 @@ float Mode::AutoYaw::look_ahead_yaw()
     // Commanded Yaw to automatically look ahead.
     if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {
         _look_ahead_yaw = degrees(atan2f(vel.y,vel.x));
+        _look_ahead_yaw += _look_ahead_yaw_offset;
     }
     return _look_ahead_yaw;
 }
@@ -82,6 +83,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode)
     case Mode::LOOK_AHEAD:
         // Commanded Yaw to automatically look ahead.
         _look_ahead_yaw = copter.ahrs.yaw_sensor * 0.01;  // cdeg -> deg
+        _look_ahead_yaw_offset = 0.0;
         break;
 
     case Mode::RESETTOARMEDYAW:
@@ -134,6 +136,14 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di
     set_mode(Mode::FIXED);
 }
 
+// set_vehicle_heading_yaw - sets the yaw look at heading to be relative to vehicle travel
+void Mode::AutoYaw::set_vehicle_heading_yaw(float relative_angle, float turn_rate_ds)
+{
+    // currently ignores turn rate
+    _look_ahead_yaw_offset = relative_angle;
+    set_mode(Mode::LOOK_AHEAD);
+}
+
 // set_fixed_yaw - sets the yaw look at heading for auto mode
 void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds)
 {

ArduCopter/mode.h

@@ -325,6 +325,7 @@ class Mode {
                            bool relative_angle);
 
         void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
+        void set_vehicle_heading_yaw(float relative_angle, float turn_rate_ds);
 
         bool reached_fixed_yaw_target();
 
@@ -365,6 +366,7 @@ class Mode {
 
         // heading when in yaw_look_ahead_yaw
         float _look_ahead_yaw;
+        float _look_ahead_yaw_offset;  // added to vehicle CoG to produce _look_ahead_yaw
 
         // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
         float _yaw_angle_cd;
@@ -1135,7 +1137,8 @@ class ModeGuided : public Mode {
         DoNotStabilizePositionXY = (1U << 4),
         DoNotStabilizeVelocityXY = (1U << 5),
         WPNavUsedForPosControl = (1U << 6),
-        AllowWeatherVaning = (1U << 7)
+        AllowWeatherVaning = (1U << 7),
+        PreserveYaw = (1U << 8),
     };
 
     bool option_is_enabled(Option option) const;

ArduCopter/mode_guided.cpp

@@ -343,12 +343,16 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
     // if configured to use wpnav for position control
     if (use_wpnav_for_position_control()) {
         // ensure we are in position control mode
+        bool init_yaw = !option_is_enabled(Option::PreserveYaw);
         if (guided_mode != SubMode::WP) {
             wp_control_start();
+            init_yaw = true;
         }
 
         // set yaw state
-        set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+        if (init_yaw) {
+            set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+        }
 
         // no need to check return status because terrain data is not used
         wp_nav->set_wp_destination(destination, terrain_alt);
@@ -361,10 +365,13 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
         return true;
     }
 
+    bool init_yaw = !option_is_enabled(Option::PreserveYaw);
+
     // if configured to use position controller for position control
     // ensure we are in position control mode
     if (guided_mode != SubMode::Pos) {
         pos_control_start();
+        init_yaw = true;
     }
 
     // initialise terrain following if needed
@@ -385,8 +392,10 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
         pos_control->set_pos_offset_z_cm(0.0);
     }
 
-    // set yaw state
-    set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+    if (init_yaw) {
+        // set yaw state
+        set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+    }
 
     // set position target and zero velocity and acceleration
     guided_pos_target_cm = destination.topostype();
@@ -438,8 +447,10 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
 
     // if using wpnav for position control
     if (use_wpnav_for_position_control()) {
+        bool init_yaw = !option_is_enabled(Option::PreserveYaw);
         if (guided_mode != SubMode::WP) {
             wp_control_start();
+            init_yaw = true;
         }
 
         if (!wp_nav->set_wp_destination_loc(dest_loc)) {
@@ -449,8 +460,10 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
             return false;
         }
 
-        // set yaw state
-        set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+        if (init_yaw) {
+            // set yaw state
+            set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+        }
 
 #if HAL_LOGGING_ENABLED
         // log target
@@ -468,14 +481,19 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
         return false;
     }
 
+    bool init_yaw = !option_is_enabled(Option::PreserveYaw);
+
     // if configured to use position controller for position control
     // ensure we are in position control mode
     if (guided_mode != SubMode::Pos) {
         pos_control_start();
+        init_yaw = true;
     }
 
-    // set yaw state
-    set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+    if (init_yaw) {
+        // set yaw state
+        set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+    }
 
     // initialise terrain following if needed
     if (terrain_alt) {
@@ -588,13 +606,18 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const
     }
 #endif
 
+    bool init_yaw = !option_is_enabled(Option::PreserveYaw);
+
     // check we are in velocity control mode
     if (guided_mode != SubMode::PosVelAccel) {
         posvelaccel_control_start();
+        init_yaw = true;
     }
 
-    // set yaw state
-    set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+    if (init_yaw) {
+        // set yaw state
+        set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+    }
 
     update_time_ms = millis();
     guided_pos_target_cm = destination.topostype();