Rover: Guided mode supports direct steering and throttle control

Rover/Rover.cpp

@@ -167,6 +167,19 @@ bool Rover::set_target_velocity_NED(const Vector3f& vel_ned)
     return true;
 }
 
+// set steering and throttle (-1 to +1) (for use by scripting)
+bool Rover::set_steering_and_throttle(float steering, float throttle)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!control_mode->in_guided_mode()) {
+        return false;
+    }
+
+    // set steering and throttle
+    mode_guided.set_steering_and_throttle(steering, throttle);
+    return true;
+}
+
 #if STATS_ENABLED == ENABLED
 /*
   update AP_Stats

Rover/Rover.h

@@ -278,6 +278,7 @@ private:
     // Rover.cpp
     bool set_target_location(const Location& target_loc) override;
     bool set_target_velocity_NED(const Vector3f& vel_ned) override;
+    bool set_steering_and_throttle(float steering, float throttle) override;
     void stats_update();
     void ahrs_update();
     void gcs_failsafe_check(void);

Rover/mode.h

@@ -387,6 +387,9 @@ public:
     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 steering and throttle (-1 to +1).  Only called from scripts
+    void set_steering_and_throttle(float steering, float throttle);
+
     // vehicle start loiter
     bool start_loiter();
 
@@ -402,7 +405,8 @@ protected:
         Guided_WP,
         Guided_HeadingAndSpeed,
         Guided_TurnRateAndSpeed,
-        Guided_Loiter
+        Guided_Loiter,
+        Guided_SteeringAndThrottle
     };
 
     bool _enter() override;
@@ -416,6 +420,12 @@ protected:
     bool sent_notification;     // used to send one time notification to ground station
     float _desired_speed;       // desired speed used only in HeadingAndSpeed submode
 
+    // direct steering and throttle control
+    bool _have_strthr;          // true if we have a valid direct steering and throttle inputs
+    uint32_t _strthr_time_ms;   // system time last call to set_steering_and_throttle was made (used for timeout)
+    float _strthr_steering;     // direct steering input in the range -1 to +1
+    float _strthr_throttle;     // direct throttle input in the range -1 to +1
+
     // limits
     struct {
         uint32_t timeout_ms;// timeout from the time that guided is invoked

Rover/mode_guided.cpp

@@ -105,6 +105,30 @@ void ModeGuided::update()
             break;
         }
 
+        case Guided_SteeringAndThrottle:
+        {
+            // handle timeout
+            if (_have_strthr && (AP_HAL::millis() - _strthr_time_ms) > 3000) {
+                _have_strthr = false;
+                gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
+            }
+            if (_have_strthr) {
+                // pass latest steering and throttle directly to motors library
+                g2.motors.set_steering(_strthr_steering * 4500.0f, false);
+                g2.motors.set_throttle(_strthr_throttle * 100.0f);
+            } else {
+                // loiter or stop vehicle
+                if (rover.is_boat()) {
+                    if (!start_loiter()) {
+                        stop_vehicle();
+                    }
+                } else {
+                    stop_vehicle();
+                }
+            }
+            break;
+        }
+
         default:
             gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
             break;
@@ -122,6 +146,8 @@ float ModeGuided::get_distance_to_destination() const
         return 0.0f;
     case Guided_Loiter:
         return rover.mode_loiter.get_distance_to_destination();
+    case Guided_SteeringAndThrottle:
+        return 0.0f;
     }
 
     // we should never reach here but just in case, return 0
@@ -137,6 +163,7 @@ bool ModeGuided::reached_destination() const
     case Guided_HeadingAndSpeed:
     case Guided_TurnRateAndSpeed:
     case Guided_Loiter:
+    case Guided_SteeringAndThrottle:
         return true;
     }
 
@@ -160,6 +187,9 @@ bool ModeGuided::set_desired_speed(float speed)
         return false;
     case Guided_Loiter:
         return rover.mode_loiter.set_desired_speed(speed);
+    case Guided_SteeringAndThrottle:
+        // no speed control
+        return false;
     }
     return false;
 }
@@ -181,6 +211,9 @@ bool ModeGuided::get_desired_location(Location& destination) const
     case Guided_Loiter:
         // get destination from loiter
         return rover.mode_loiter.get_desired_location(destination);
+    case Guided_SteeringAndThrottle:
+        // no desired location in this submode
+        break;
     }
 
     // should never get here but just in case
@@ -246,6 +279,16 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ
     rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
 }
 
+// set steering and throttle (both in the range -1 to +1)
+void ModeGuided::set_steering_and_throttle(float steering, float throttle)
+{
+    _guided_mode = ModeGuided::Guided_SteeringAndThrottle;
+    _strthr_time_ms = AP_HAL::millis();
+    _strthr_steering = constrain_float(steering, -1.0f, 1.0f);
+    _strthr_throttle = constrain_float(throttle, -1.0f, 1.0f);
+    _have_strthr = true;
+}
+
 bool ModeGuided::start_loiter()
 {
     if (rover.mode_loiter.enter()) {