Rover: implement external control

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

Rover: update comment in Rover/AP_ExternalControl_Rover.h

Co-authored-by: Nick E <NExton@sypaq.com.au>

Rover/AP_ExternalControl_Rover.cpp

@@ -0,0 +1,37 @@
+/*
+  external control library for rover
+ */
+
+
+#include "AP_ExternalControl_Rover.h"
+#if AP_EXTERNAL_CONTROL_ENABLED
+
+#include "Rover.h"
+
+/*
+  set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw
+  velocity is in earth frame, NED, m/s
+*/
+bool AP_ExternalControl_Rover::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)
+{
+    if (!ready_for_external_control()) {
+        return false;
+    }
+
+    // rover is commanded in body-frame using FRD convention
+    auto &ahrs = AP::ahrs();
+    Vector3f linear_velocity_body = ahrs.earth_to_body(linear_velocity);
+
+    const float target_speed = linear_velocity_body.x;
+    const float turn_rate_cds = isnan(yaw_rate_rads)? 0: degrees(yaw_rate_rads)*100;
+
+    rover.mode_guided.set_desired_turn_rate_and_speed(turn_rate_cds, target_speed);
+    return true;
+}
+
+bool AP_ExternalControl_Rover::ready_for_external_control()
+{
+    return rover.control_mode->in_guided_mode() && rover.arming.is_armed();
+}
+
+#endif // AP_EXTERNAL_CONTROL_ENABLED

Rover/AP_ExternalControl_Rover.h

@@ -0,0 +1,26 @@
+/*
+  external control library for rover
+ */
+#pragma once
+
+#include <AP_ExternalControl/AP_ExternalControl.h>
+
+#if AP_EXTERNAL_CONTROL_ENABLED
+
+class AP_ExternalControl_Rover : public AP_ExternalControl {
+public:
+    /*
+      Set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw.
+      Velocity is in earth frame, NED [m/s].
+      Yaw is in earth frame, NED [rad/s].
+     */
+    bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) override;
+private:
+    /*
+      Return true if Rover is ready to handle external control data.
+      Currently checks mode and arm states.
+    */
+    bool ready_for_external_control();
+};
+
+#endif // AP_EXTERNAL_CONTROL_ENABLED

Rover/Rover.h

@@ -46,7 +46,7 @@
 #include <AP_Follow/AP_Follow_config.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
 #if AP_EXTERNAL_CONTROL_ENABLED
-#include <AP_ExternalControl/AP_ExternalControl.h>
+#include "AP_ExternalControl_Rover.h"
 #endif
 
 // Configuration
@@ -83,6 +83,9 @@ public:
     friend class AP_Arming_Rover;
 #if ADVANCED_FAILSAFE == ENABLED
     friend class AP_AdvancedFailsafe_Rover;
+#endif
+#if AP_EXTERNAL_CONTROL_ENABLED
+    friend class AP_ExternalControl_Rover;
 #endif
     friend class GCS_Rover;
     friend class Mode;
@@ -147,9 +150,9 @@ private:
     // Arming/Disarming management class
     AP_Arming_Rover arming;
 
-    // dummy external control implementation
+    // external control implementation
 #if AP_EXTERNAL_CONTROL_ENABLED
-    AP_ExternalControl external_control;
+    AP_ExternalControl_Rover external_control;
 #endif
 
 #if AP_OPTICALFLOW_ENABLED

Rover/mode.h

@@ -488,6 +488,9 @@ protected:
 class ModeGuided : public Mode
 {
 public:
+#if AP_EXTERNAL_CONTROL_ENABLED
+    friend class AP_ExternalControl_Rover;
+#endif
 
     Number mode_number() const override { return Number::GUIDED; }
     const char *name4() const override { return "GUID"; }