Rover: added a new form of GUIDED mode

Rover now accepts a new message MAV_CMD_NAV_SET_YAW_SPEED
which has an angle in centidegrees and a speed scale and the rover
will drive based on these inputs.

APMrover2/APMrover2.cpp

@@ -412,21 +412,34 @@ void Rover::update_current_mode(void)
         calc_throttle(g.speed_cruise);
         break;
 
-    case GUIDED:
-        if (rtl_complete || verify_RTL()) {
-            // we have reached destination so stop where we are
-            if (channel_throttle->get_servo_out() != g.throttle_min.get()) {
-                gcs_send_mission_item_reached_message(0);
+    case GUIDED: {
+        switch (guided_mode){
+        case Guided_Angle:
+            nav_set_yaw_speed();
+            break;
+
+        case Guided_WP:
+            if (rtl_complete || verify_RTL()) {
+                // we have reached destination so stop where we are
+                if (channel_throttle->get_servo_out() != g.throttle_min.get()) {
+                    gcs_send_mission_item_reached_message(0);
+                }
+                channel_throttle->set_servo_out(g.throttle_min.get());
+                channel_steer->set_servo_out(0);
+                lateral_acceleration = 0;
+            } else {
+                calc_lateral_acceleration();
+                calc_nav_steer();
+                calc_throttle(g.speed_cruise);
             }
-            channel_throttle->set_servo_out(g.throttle_min.get());
-            channel_steer->set_servo_out(0);
-            lateral_acceleration = 0;
-        } else {
-            calc_lateral_acceleration();
-            calc_nav_steer();
-            calc_throttle(g.speed_cruise);
+            break;
+
+        default:
+            gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
+            break;
         }
         break;
+    }
 
     case STEERING: {
         /*
@@ -509,17 +522,29 @@ void Rover::update_navigation()
         break;
 
     case GUIDED:
-        // no loitering around the wp with the rover, goes direct to the wp position
-        calc_lateral_acceleration();
-        calc_nav_steer();
-        if (rtl_complete || verify_RTL()) {
-            // we have reached destination so stop where we are
-            channel_throttle->set_servo_out(g.throttle_min.get());
-            channel_steer->set_servo_out(0);
-            lateral_acceleration = 0;
+        switch (guided_mode){
+        case Guided_Angle:
+            nav_set_yaw_speed();
+            break;
+
+        case Guided_WP:
+            // no loitering around the wp with the rover, goes direct to the wp position
+            calc_lateral_acceleration();
+            calc_nav_steer();
+            if (rtl_complete || verify_RTL()) {
+                // we have reached destination so stop where we are
+                channel_throttle->set_servo_out(g.throttle_min.get());
+                channel_steer->set_servo_out(0);
+                lateral_acceleration = 0;
+            }
+            break;
+
+        default:
+            gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
+            break;
         }
         break;
     }
 }
- 
+
 AP_HAL_MAIN_CALLBACKS(&rover);

APMrover2/GCS_Mavlink.cpp

@@ -796,9 +796,11 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
         // only accept position updates when in GUIDED mode
         return false;
     }
-        
     rover.guided_WP = cmd.content.location;
 
+    // This method is only called when we are in Guided WP GUIDED mode
+    rover.guided_mode = Guided_WP;
+
     // make any new wp uploaded instant (in case we are already in Guided mode)
     rover.rtl_complete = false;
     rover.set_guided_WP();
@@ -1166,6 +1168,25 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             result = rover.compass.handle_mag_cal_command(packet);
             break;
 
+        case MAV_CMD_NAV_SET_YAW_SPEED:
+        {
+            // param1 : yaw angle to adjust direction by in centidegress
+            // param2 : Speed - normalized to 0 .. 1
+
+            // exit if vehicle is not in Guided mode
+            if (rover.control_mode != GUIDED) {
+                break;
+            }
+
+            rover.guided_mode = Guided_Angle;
+            rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
+            rover.guided_yaw_speed.turn_angle = packet.param1;
+            rover.guided_yaw_speed.target_speed = constrain_float(packet.param2, 0.0f, 1.0f);
+            rover.nav_set_yaw_speed();
+            result = MAV_RESULT_ACCEPTED;
+            break;
+        }
+
         default:
                 break;
             }

APMrover2/Rover.h

@@ -369,7 +369,18 @@ private:
     bool auto_throttle_mode;
 
     // Store the time the last GPS message was received.
-    uint32_t last_gps_msg_ms{0}; 
+    uint32_t last_gps_msg_ms{0};
+
+    // Store parameters from NAV_SET_YAW_SPEED
+    struct {
+        float turn_angle;
+        float target_speed;
+        uint32_t msg_time_ms;
+    } guided_yaw_speed;
+
+    // Guided
+    GuidedMode guided_mode;  // stores which GUIDED mode the vehicle is in
+
 
 private:
     // private member functions
@@ -520,6 +531,7 @@ private:
     bool motor_active();
     void update_home();
     void accel_cal_update(void);
+    void nav_set_yaw_speed();
 public:
     bool print_log_menu(void);
     int8_t dump_log(uint8_t argc, const Menu::arg *argv);

APMrover2/commands_logic.cpp

@@ -307,6 +307,29 @@ bool Rover::verify_loiter_unlim()
     return false;
 }
 
+void Rover::nav_set_yaw_speed()
+{
+    // if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt
+    if ((millis() - guided_yaw_speed.msg_time_ms) > 3000)
+    {
+        gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
+        channel_throttle->set_servo_out(g.throttle_min.get());
+        channel_steer->set_servo_out(0);
+        lateral_acceleration = 0;
+        return;
+    }
+
+    channel_steer->set_servo_out(steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle));
+
+    // speed param in the message gives speed as a proportion of cruise speed.
+    // 0.5 would set speed to the cruise speed
+    // 1 is double the cruise speed.
+    float target_speed = g.speed_cruise * guided_yaw_speed.target_speed * 2;
+    calc_throttle(target_speed);
+
+    return;
+}
+
 /********************************************************************************/
 //  Condition (May) commands
 /********************************************************************************/

APMrover2/defines.h

@@ -40,6 +40,12 @@ enum mode {
     INITIALISING=16
 };
 
+enum GuidedMode {
+    Guided_WP,
+    Guided_Angle
+};
+
+
 // types of failsafe events
 #define FAILSAFE_EVENT_THROTTLE (1<<0)
 #define FAILSAFE_EVENT_GCS      (1<<1)