From 79bf9dd05d538cfa06c60b70b907315155e8045e Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Tue, 25 Jul 2023 18:05:26 -0700
Subject: [PATCH] Sub: Guided Mode: Improved guided_set_velocity() and fixed
 Z-controller in guided_vel_control_run()

---
 ArduSub/mode_guided.cpp | 37 +++++++++++++++++++++++++++++++++++++
 1 file changed, 37 insertions(+)

diff --git a/ArduSub/mode_guided.cpp b/ArduSub/mode_guided.cpp
index 490ea0d22e..7c92150036 100644
--- a/ArduSub/mode_guided.cpp
+++ b/ArduSub/mode_guided.cpp
@@ -109,6 +109,9 @@ void ModeGuided::guided_vel_control_start()
     // initialise velocity controller
     position_control->init_z_controller();
     position_control->init_xy_controller();
+
+    // pilot always controls yaw
+    set_auto_yaw_mode(AUTO_YAW_HOLD);
 }
 
 // initialise guided mode's posvel controller
@@ -260,6 +263,24 @@ void ModeGuided::guided_set_velocity(const Vector3f& velocity)
     position_control->set_vel_desired_cms(velocity);
 }
 
+// guided_set_velocity - sets guided mode's target velocity
+void ModeGuided::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
+{
+   // check we are in velocity control mode
+    if (sub.guided_mode != Guided_Velocity) {
+        guided_vel_control_start();
+    }
+
+    // set yaw state
+    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+
+    update_time_ms = AP_HAL::millis();
+
+    // set position controller velocity target
+    position_control->set_vel_desired_cms(velocity);
+
+}
+
 // set guided mode posvel target
 bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
 {
@@ -465,6 +486,12 @@ void ModeGuided::guided_vel_control_run()
         target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
             set_auto_yaw_mode(AUTO_YAW_HOLD);
+        } else{
+            if (sub.yaw_rate_only){
+                set_auto_yaw_mode(AUTO_YAW_RATE);
+            } else{
+                set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
+            }
         }
     }
 
@@ -480,6 +507,8 @@ void ModeGuided::guided_vel_control_run()
     position_control->stop_pos_xy_stabilisation();
     // call velocity controller which includes z axis controller
     position_control->update_xy_controller();
+
+    position_control->set_pos_target_z_from_climb_rate_cm(position_control->get_vel_desired_cms().z);
     position_control->update_z_controller();
 
     float lateral_out, forward_out;
@@ -493,6 +522,14 @@ void ModeGuided::guided_vel_control_run()
     if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch & yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
+    } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) {
+        // roll, pitch from pilot, yaw & yaw_rate from auto_control
+        target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
+        attitude_control->input_euler_angle_roll_pitch_slew_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate);
+    } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) {
+        // roll, pitch from pilot, yaw_rate from auto_control
+        target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
     } else {
         // roll, pitch from pilot, yaw heading from auto_heading()
         attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);