Sub: Guided Mode: Helper function guided_set_yaw_state() to set yaw state and targets when the vehicle is in guided mode.

ArduSub/Sub.h

@@ -295,6 +295,9 @@ private:
     // Navigation Yaw control
     // auto flight mode's yaw mode
     uint8_t auto_yaw_mode;
+    
+    // Parameter to set yaw rate only
+    bool yaw_rate_only;
 
     // Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI
     Vector3f roi_WP;

ArduSub/mode.h

@@ -294,10 +294,10 @@ public:
     bool guided_set_destination(const Vector3f& destination);
     bool guided_set_destination(const Location&);
     void guided_set_velocity(const Vector3f& velocity);
+    void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
     float get_auto_heading();
     void guided_limit_clear();
     void set_auto_yaw_mode(autopilot_yaw_mode yaw_mode);
-    void set_yaw_rate(float turn_rate_dps);
 
 protected:
 
@@ -341,6 +341,7 @@ public:
     void auto_nav_guided_start();
     void set_auto_yaw_roi(const Location &roi_location);
     void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle);
+    void set_yaw_rate(float turn_rate_dps);
     bool auto_terrain_recover_start();
 
 protected:

ArduSub/mode_auto.cpp

@@ -345,7 +345,7 @@ void ModeAuto::auto_loiter_run()
 // set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
 void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
 {
-    // get current yaw target
+    // get current yaw
     int32_t curr_yaw_target = attitude_control->get_att_target_euler_cd().z;
 
     // get final angle, 1 = Relative, 0 = Absolute
@@ -357,18 +357,15 @@ void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps
         if (direction < 0) {
             angle_deg = -angle_deg;
         }
-        sub.yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target));
+        sub.yaw_look_at_heading = wrap_360_cd((angle_deg * 100 + curr_yaw_target));
     }
 
     // get turn speed
-    // TODO actually implement this, right now, yaw_look_at_heading_slew is unused
-    // see AP_Float _slew_yaw in AC_AttitudeControl
     if (is_zero(turn_rate_dps)) {
         // default to regular auto slew rate
         sub.yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
     } else {
-        int32_t turn_rate = (wrap_180_cd(sub.yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps;
-        sub.yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360);    // deg / sec
+        sub.yaw_look_at_heading_slew = MIN(turn_rate_dps, AUTO_YAW_SLEW_RATE);    // deg / sec
     }
 
     // set yaw mode
@@ -379,7 +376,7 @@ void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps
 
 
 // sets the desired yaw rate
-void ModeGuided::set_yaw_rate(float turn_rate_dps)
+void ModeAuto::set_yaw_rate(float turn_rate_dps)
 {    
     // set sub to desired yaw rate
     sub.yaw_look_at_heading_slew = MIN(turn_rate_dps, AUTO_YAW_SLEW_RATE);    // deg / sec

ArduSub/mode_guided.cpp

@@ -277,6 +277,44 @@ void ModeGuided::guided_set_angle(const Quaternion &q, float climb_rate_cms)
     guided_angle_state.update_time_ms = AP_HAL::millis();
 }
 
+// helper function to set yaw state and targets
+void ModeGuided::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
+{    
+    float current_yaw = wrap_2PI(AP::ahrs().get_yaw());
+    float euler_yaw_angle;
+    float yaw_error;
+
+    euler_yaw_angle = wrap_2PI((yaw_cd * 0.01f));
+    yaw_error = wrap_PI(euler_yaw_angle - current_yaw);
+
+    int direction = 0;
+    if (yaw_error < 0){
+        direction = -1;
+    } else {
+        direction = 1;
+    }
+
+    /*
+    case 1: target yaw only
+    case 2: target yaw and yaw rate
+    case 3: target yaw rate only
+    case 4: hold current yaw
+    */
+    if (use_yaw && !use_yaw_rate) {
+        sub.yaw_rate_only = false;
+        sub.mode_auto.set_auto_yaw_look_at_heading(yaw_cd * 0.01f, 0.0f, direction, relative_angle);
+    } else if (use_yaw && use_yaw_rate) { 
+        sub.yaw_rate_only = false;
+        sub.mode_auto.set_auto_yaw_look_at_heading(yaw_cd * 0.01f, yaw_rate_cds * 0.01f, direction, relative_angle);
+    } else if (!use_yaw && use_yaw_rate) {
+        sub.yaw_rate_only = true;
+        sub.mode_auto.set_yaw_rate(yaw_rate_cds * 0.01f);
+    } else{
+        sub.yaw_rate_only = false;
+        set_auto_yaw_mode(AUTO_YAW_HOLD);
+    }
+}
+
 // guided_run - runs the guided controller
 // should be called at 100hz or more
 void ModeGuided::run()
@@ -554,6 +592,10 @@ void ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode)
 
     // perform initialisation
     switch (sub.auto_yaw_mode) {
+    
+    case AUTO_YAW_HOLD:
+        // pilot controls the heading
+        break;
 
     case AUTO_YAW_LOOK_AT_NEXT_WP:
         // wpnav will initialise heading when wpnav's set_destination method is called