Copter: additional yaw modes and fixes

ArduCopter/GCS_Mavlink.cpp

@@ -704,11 +704,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t
         // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
         if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) &&
             !copter.camera_mount.has_pan_control()) {
-            copter.flightmode->auto_yaw.set_fixed_yaw(
+            copter.flightmode->auto_yaw.set_yaw_angle_rate(
                 (float)packet.param3 * 0.01f,
-                0.0f,
+                0.0f);
-                0,
-                false);
         }
         break;
 #endif
@@ -974,11 +972,9 @@ void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg)
         // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
         if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) &&
             !copter.camera_mount.has_pan_control()) {
-            copter.flightmode->auto_yaw.set_fixed_yaw(
+            copter.flightmode->auto_yaw.set_yaw_angle_rate(
                 mavlink_msg_mount_control_get_input_c(&msg) * 0.01f,
-                0.0f,
+                0.0f);
-                0,
-                false);
 
             break;
         }

ArduCopter/autoyaw.cpp

@@ -85,7 +85,7 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
 
     case AUTO_YAW_RATE:
         // initialise target yaw rate to zero
-        _rate_cds = 0.0f;
+        _yaw_rate_cds = 0.0f;
         break;
 
     case AUTO_YAW_CIRCLE:
@@ -95,35 +95,52 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
 }
 
 // set_fixed_yaw - sets the yaw look at heading for auto mode
-void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
+void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t direction, bool relative_angle)
 {
-    const int32_t curr_yaw_target = copter.attitude_control->get_att_target_euler_cd().z;
+    fixed_last_update = millis();
+
+    _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z;
+    _yaw_rate_cds = 0.0;
 
     // calculate final angle as relative to vehicle heading or absolute
-    if (!relative_angle) {
+    if (relative_angle) {
-        // absolute angle
+        if (direction == 0) {
-        _fixed_yaw = wrap_360_cd(angle_deg * 100);
+            _fixed_yaw_offset_cd = angle_deg;
-    } else {
+        } else {
-        // relative angle
+            _fixed_yaw_offset_cd = angle_deg * direction;
-        if (direction < 0) {
+        }
-            angle_deg = -angle_deg;
+    } else {
+        // absolute angle
+        _fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd);
+        if ( direction < 0 && is_positive(_fixed_yaw_offset_cd) ) {
+            _fixed_yaw_offset_cd -= 36000.0;
+        } else if ( direction > 0 && is_negative(_fixed_yaw_offset_cd) ) {
+            _fixed_yaw_offset_cd += 36000.0;
         }
-        _fixed_yaw = wrap_360_cd((angle_deg * 100) + curr_yaw_target);
     }
 
     // get turn speed
-    if (is_zero(turn_rate_dps)) {
+    if (!is_positive(turn_rate_ds)) {
-        // default to regular auto slew rate
+        // default to default slew rate
-        _fixed_yaw_slewrate = AUTO_YAW_SLEW_RATE;
+        _fixed_yaw_slewrate_cds = copter.attitude_control->get_slew_yaw_cds();
     } else {
-        const int32_t turn_rate = (wrap_180_cd(_fixed_yaw - curr_yaw_target) / 100) / turn_rate_dps;
+        _fixed_yaw_slewrate_cds = MIN(copter.attitude_control->get_slew_yaw_cds(), turn_rate_ds * 100.0);
-        _fixed_yaw_slewrate = constrain_int32(turn_rate, 1, 360);    // deg / sec
     }
 
     // set yaw mode
     set_mode(AUTO_YAW_FIXED);
+}
 
-    // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction.  1 = clockwise, -1 = counterclockwise
+// 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)
+{
+    fixed_last_update = millis();
+
+    _yaw_angle_cd = yaw_angle_d * 100.0;
+    _yaw_rate_cds = yaw_rate_ds * 100.0;
+
+    // set yaw mode
+    set_mode(AUTO_YAW_ANGLE_RATE);
 }
 
 // set_roi - sets the yaw to look at roi for auto mode
@@ -169,7 +186,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
 void Mode::AutoYaw::set_rate(float turn_rate_cds)
 {
     set_mode(AUTO_YAW_RATE);
-    _rate_cds = turn_rate_cds;
+    _yaw_rate_cds = turn_rate_cds;
 }
 
 // yaw - returns target heading depending upon auto_yaw.mode()
@@ -181,10 +198,16 @@ float Mode::AutoYaw::yaw()
         // point towards a location held in roi
         return roi_yaw();
 
-    case AUTO_YAW_FIXED:
+    case AUTO_YAW_FIXED: {
         // keep heading pointing in the direction held in fixed_yaw
         // with no pilot input allowed
-        return _fixed_yaw;
+        float dt = millis() - fixed_last_update;
+        fixed_last_update = millis();
+        float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds);
+        _fixed_yaw_offset_cd -= yaw_angle_step;
+        _yaw_angle_cd += yaw_angle_step;
+        return _yaw_angle_cd;
+    }
 
     case AUTO_YAW_LOOK_AHEAD:
         // Commanded Yaw to automatically look ahead.
@@ -203,11 +226,18 @@ float Mode::AutoYaw::yaw()
         // return the current attitude target
         return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z);
 
+    case AUTO_YAW_ANGLE_RATE:{
+        float dt = millis() - fixed_last_update;
+        fixed_last_update = millis();
+        _yaw_angle_cd += _yaw_rate_cds * dt;
+        return _yaw_angle_cd;
+    }
+
     case AUTO_YAW_LOOK_AT_NEXT_WP:
     default:
         // point towards next waypoint.
         // we don't use wp_bearing because we don't want the copter to turn too much during flight
-        return copter.wp_nav->get_yaw();
+        return copter.pos_control->get_yaw_cd();
     }
 }
 
@@ -225,11 +255,12 @@ float Mode::AutoYaw::rate_cds() const
     case AUTO_YAW_CIRCLE:
         return 0.0f;
 
+    case AUTO_YAW_ANGLE_RATE:
     case AUTO_YAW_RATE:
-        return _rate_cds;
+        return _yaw_rate_cds;
 
     case AUTO_YAW_LOOK_AT_NEXT_WP:
-        return copter.wp_nav->get_yaw_rate_cds();
+        return copter.pos_control->get_yaw_rate_cds();
     }
 
     // return zero turn rate (this should never happen)

ArduCopter/defines.h

@@ -18,8 +18,9 @@ enum autopilot_yaw_mode {
     AUTO_YAW_FIXED =            3,  // point towards a particular angle (no pilot input accepted)
     AUTO_YAW_LOOK_AHEAD =       4,  // point in the direction the copter is moving
     AUTO_YAW_RESETTOARMEDYAW =  5,  // point towards heading at time motors were armed
-    AUTO_YAW_RATE =             6,  // turn at a specified rate (held in auto_yaw_rate)
+    AUTO_YAW_ANGLE_RATE =       6,  // turn at a specified rate from a starting angle
-    AUTO_YAW_CIRCLE =           7,  // use AC_Circle's provided yaw (used during Loiter-Turns commands)
+    AUTO_YAW_RATE =             7,  // turn at a specified rate (held in auto_yaw_rate)
+    AUTO_YAW_CIRCLE =           8,  // use AC_Circle's provided yaw (used during Loiter-Turns commands)
 };
 
 // Frame types

ArduCopter/mode.h

@@ -207,6 +207,8 @@ public:
                            int8_t direction,
                            bool relative_angle);
 
+        void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
+
     private:
 
         float look_ahead_yaw();
@@ -219,16 +221,19 @@ public:
         Vector3f roi;
 
         // yaw used for YAW_FIXED yaw_mode
-        int32_t _fixed_yaw;
+        float _fixed_yaw_offset_cd;
 
         // Deg/s we should turn
-        int16_t _fixed_yaw_slewrate;
+        float _fixed_yaw_slewrate_cds;
+
+        uint32_t fixed_last_update;
 
         // heading when in yaw_look_ahead_yaw
         float _look_ahead_yaw;
 
         // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
-        float _rate_cds;
+        float _yaw_angle_cd;
+        float _yaw_rate_cds;
     };
     static AutoYaw auto_yaw;
 

ArduCopter/mode_auto.cpp

@@ -1501,7 +1501,7 @@ void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
     // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
     if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) &&
         !copter.camera_mount.has_pan_control()) {
-        auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,false);
+        auto_yaw.set_yaw_angle_rate(cmd.content.mount_control.yaw,0.0f);
     }
     // pass the target angles to the camera mount
     copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);

ArduCopter/mode_guided.cpp

@@ -576,7 +576,7 @@ void ModeGuided::pos_control_run()
         attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
     } else {
         // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
+        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
     }
 }
 
@@ -639,7 +639,7 @@ void ModeGuided::accel_control_run()
         attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
     } else {
         // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
+        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
     }
 }
 
@@ -713,7 +713,7 @@ void ModeGuided::velaccel_control_run()
         attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
     } else {
         // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
+        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
     }
 }
 
@@ -794,7 +794,7 @@ void ModeGuided::posvelaccel_control_run()
         attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
     } else {
         // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
+        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
     }
 }
 
@@ -887,8 +887,12 @@ void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f
 // helper function to set yaw state and targets
 void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
 {
-    if (use_yaw) {
+    if (use_yaw && relative_angle) {
         auto_yaw.set_fixed_yaw(yaw_cd * 0.01f, 0.0f, 0, relative_angle);
+    } else if (use_yaw && use_yaw_rate) {
+        auto_yaw.set_yaw_angle_rate(yaw_cd * 0.01f, yaw_rate_cds * 0.01f);
+    } else if (use_yaw && !use_yaw_rate) {
+        auto_yaw.set_yaw_angle_rate(yaw_cd * 0.01f, 0.0f);
     } else if (use_yaw_rate) {
         auto_yaw.set_rate(yaw_rate_cds);
     }