Rover: CONDITION_YAW command handling for skid-steering rover

Implements condition_yaw command handling for skid-steering rover only.
Direction specification not yet implemented.
Adds an autotest to rover.py that tests this behaviour.

Co-authored-by: Tiago Santos <tiago.castro.santos@tecnico.ulisboa.pt>

Author: jrcacador

Rover/GCS_MAVLink_Rover.cpp

@@ -500,6 +500,13 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in
 {
     switch (packet.command) {
 
+    case MAV_CMD_CONDITION_YAW:
+        if (rover.g2.motors.have_skid_steering()) {
+            return handle_command_condition_yaw(packet);
+        }
+        // not yet implemented for other vehicle types
+        return MAV_RESULT_UNSUPPORTED;
+
     case MAV_CMD_DO_CHANGE_SPEED:
         // param1 : unused
         // param2 : new speed in m/s
@@ -578,6 +585,33 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_nav_set_yaw_speed(const mavlink_com
 }
 #endif
 
+MAV_RESULT GCS_MAVLINK_Rover::handle_command_condition_yaw(const mavlink_command_int_t &packet)
+{
+        // param1 : target angle [0-360]
+        // param2 : speed during change [deg per second]
+        // param3 : direction (-1:ccw, +1:cw)
+        // param4 : relative offset (1) or absolute angle (0)
+
+        // exit if vehicle is not in Guided mode
+        if (!rover.control_mode->in_guided_mode()) {
+            return MAV_RESULT_FAILED;
+        }
+
+        // get final angle, 1 = Relative, 0 = Absolute
+        switch ((int) packet.param4) {
+        case 0: // absolute angle
+            // absolute angle
+            rover.mode_guided.set_desired_turn_rate_and_heading(packet.param1 * 100.0f, packet.param2 * 100.0f, packet.param3);
+            break;
+        case 1: // relative angle
+            rover.mode_guided.set_desired_turn_rate_and_heading_delta(packet.param1 * 100.0f, packet.param2 * 100.0f, packet.param3);
+            break;
+        default:
+            return MAV_RESULT_DENIED;
+        }
+        return MAV_RESULT_ACCEPTED;
+}
+
 MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
 {
     const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;

Rover/GCS_MAVLink_Rover.h

@@ -19,6 +19,7 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK
 
     MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
     MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
+    MAV_RESULT handle_command_condition_yaw(const mavlink_command_int_t &packet);
     MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);
     MAV_RESULT handle_command_nav_set_yaw_speed(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
 

Rover/mode.h

@@ -551,6 +551,10 @@ class ModeGuided : public Mode
     void set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed);
     void set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed);
 
+    // set desired turn rate and heading
+    void set_desired_turn_rate_and_heading(float yaw_angle_cd, float turn_rate_cds, int8_t direction);
+    void set_desired_turn_rate_and_heading_delta(float yaw_delta_cd, float turn_rate_cds, int8_t direction);
+
     // set steering and throttle (-1 to +1).  Only called from scripts
     void set_steering_and_throttle(float steering, float throttle);
 
@@ -574,6 +578,7 @@ class ModeGuided : public Mode
         TurnRateAndSpeed,
         Loiter,
         SteeringAndThrottle,
+        TurnRateAndHeading,
         Stop
     };
 

Rover/mode_guided.cpp

@@ -101,6 +101,30 @@ void ModeGuided::update()
             break;
         }
 
+        case SubMode::TurnRateAndHeading:
+        {
+            // stop vehicle if target not updated within 3 seconds
+            if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
+                gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
+                have_attitude_target = false;
+                break;
+            }
+            if (have_attitude_target) {
+                // run steering and throttle controllers
+                calc_steering_to_heading(_desired_yaw_cd, _desired_yaw_rate_cds * 0.01f);
+                break;
+            }
+            // we have reached the destination so stay here
+            if (rover.is_boat()) {
+                if (!start_loiter()) {
+                    stop_vehicle();
+                }
+                break;
+            }
+            stop_vehicle();
+            break;
+        }
+
         case SubMode::Loiter:
         {
             rover.mode_loiter.update();
@@ -149,6 +173,7 @@ float ModeGuided::wp_bearing() const
         return g2.wp_nav.wp_bearing_cd() * 0.01f;
     case SubMode::HeadingAndSpeed:
     case SubMode::TurnRateAndSpeed:
+    case SubMode::TurnRateAndHeading:
         return 0.0f;
     case SubMode::Loiter:
         return rover.mode_loiter.wp_bearing();
@@ -169,6 +194,8 @@ float ModeGuided::nav_bearing() const
     case SubMode::HeadingAndSpeed:
     case SubMode::TurnRateAndSpeed:
         return 0.0f;
+    case SubMode::TurnRateAndHeading:
+        return _desired_yaw_cd * 0.01f;
     case SubMode::Loiter:
         return rover.mode_loiter.nav_bearing();
     case SubMode::SteeringAndThrottle:
@@ -187,6 +214,7 @@ float ModeGuided::crosstrack_error() const
         return g2.wp_nav.crosstrack_error();
     case SubMode::HeadingAndSpeed:
     case SubMode::TurnRateAndSpeed:
+    case SubMode::TurnRateAndHeading:
         return 0.0f;
     case SubMode::Loiter:
         return rover.mode_loiter.crosstrack_error();
@@ -206,6 +234,7 @@ float ModeGuided::get_desired_lat_accel() const
         return g2.wp_nav.get_lat_accel();
     case SubMode::HeadingAndSpeed:
     case SubMode::TurnRateAndSpeed:
+    case SubMode::TurnRateAndHeading:
         return 0.0f;
     case SubMode::Loiter:
         return rover.mode_loiter.get_desired_lat_accel();
@@ -226,6 +255,7 @@ float ModeGuided::get_distance_to_destination() const
         return _distance_to_destination;
     case SubMode::HeadingAndSpeed:
     case SubMode::TurnRateAndSpeed:
+    case SubMode::TurnRateAndHeading:
         return 0.0f;
     case SubMode::Loiter:
         return rover.mode_loiter.get_distance_to_destination();
@@ -246,6 +276,7 @@ bool ModeGuided::reached_destination() const
         return g2.wp_nav.reached_destination();
     case SubMode::HeadingAndSpeed:
     case SubMode::TurnRateAndSpeed:
+    case SubMode::TurnRateAndHeading:
     case SubMode::Loiter:
     case SubMode::SteeringAndThrottle:
     case SubMode::Stop:
@@ -269,6 +300,7 @@ bool ModeGuided::set_desired_speed(float speed)
     case SubMode::Loiter:
         return rover.mode_loiter.set_desired_speed(speed);
     case SubMode::SteeringAndThrottle:
+    case SubMode::TurnRateAndHeading:
     case SubMode::Stop:
         // no speed control
         return false;
@@ -288,6 +320,7 @@ bool ModeGuided::get_desired_location(Location& destination) const
         return false;
     case SubMode::HeadingAndSpeed:
     case SubMode::TurnRateAndSpeed:
+    case SubMode::TurnRateAndHeading:
         // not supported in these submodes
         return false;
     case SubMode::Loiter:
@@ -374,6 +407,32 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ
 #endif
 }
 
+// set desired attitude
+void ModeGuided::set_desired_turn_rate_and_heading(float yaw_angle_cd, float turn_rate_cds, int8_t direction)
+{
+    // initialisation and logging
+    _guided_mode = SubMode::TurnRateAndHeading;
+    _des_att_time_ms = AP_HAL::millis();
+
+    // record targets
+    _desired_yaw_cd = yaw_angle_cd;
+    _desired_yaw_rate_cds = turn_rate_cds;
+    have_attitude_target = true;
+}
+
+// set desired attitude
+void ModeGuided::set_desired_turn_rate_and_heading_delta(float yaw_delta_cd, float turn_rate_cds, int8_t direction)
+{
+    /// handle initialisation
+    float desired_yaw_cd = _desired_yaw_cd;
+    if (_guided_mode != SubMode::TurnRateAndHeading) {
+        _guided_mode = SubMode::TurnRateAndHeading;
+        desired_yaw_cd = ahrs.yaw_sensor;
+    }
+
+    set_desired_turn_rate_and_heading(wrap_180_cd(desired_yaw_cd + yaw_delta_cd), turn_rate_cds, direction);
+}
+
 // set steering and throttle (both in the range -1 to +1)
 void ModeGuided::set_steering_and_throttle(float steering, float throttle)
 {

Tools/autotest/rover.py

@@ -6315,6 +6315,27 @@ def MAV_CMD_DO_CHANGE_SPEED(self):
         self.wait_distance_to_home(0, 5, timeout=30)
         self.disarm_vehicle()
 
+    def MAV_CMD_CONDITION_YAW(self):
+        '''simple test for CONDITION_YAW command'''
+        model = "rover-skid"
+        self.customise_SITL_commandline([],
+                                        model=model,
+                                        defaults_filepath=self.model_defaults_filepath(model))
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.change_mode('GUIDED')
+
+        def send_cmd(x, y):
+            self.run_cmd(
+                mavutil.mavlink.MAV_CMD_CONDITION_YAW,
+                p1=60,  # target angle
+                p2=10,  # degrees/second
+                p3=1,   # -1 is counter-clockwise, 1 clockwise
+                p4=0,   # 1 for relative, 0 for absolute
+            )
+        self.wait_heading(60, called_function=send_cmd)
+        self.disarm_vehicle()
+
     def MAV_CMD_MISSION_START(self):
         '''simple test for starting missing using this command'''
         # home and 1 waypoint a long way away:
@@ -6946,6 +6967,7 @@ def tests(self):
             self.SET_POSITION_TARGET_LOCAL_NED,
             self.MAV_CMD_DO_SET_MISSION_CURRENT,
             self.MAV_CMD_DO_CHANGE_SPEED,
+            self.MAV_CMD_CONDITION_YAW,
             self.MAV_CMD_MISSION_START,
             self.MAV_CMD_NAV_SET_YAW_SPEED,
             self.Button,