From 2c609c6271383c2343962e20ceea046ac66d2230 Mon Sep 17 00:00:00 2001
From: Rhys Mainwaring <rhys.mainwaring@me.com>
Date: Sat, 7 Oct 2023 11:04:56 +0100
Subject: [PATCH] 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 | 37 ++++++++++++++++++++++++++++++
 Rover/AP_ExternalControl_Rover.h   | 26 +++++++++++++++++++++
 Rover/Rover.h                      |  9 +++++---
 Rover/mode.h                       |  3 +++
 4 files changed, 72 insertions(+), 3 deletions(-)
 create mode 100644 Rover/AP_ExternalControl_Rover.cpp
 create mode 100644 Rover/AP_ExternalControl_Rover.h

diff --git a/Rover/AP_ExternalControl_Rover.cpp b/Rover/AP_ExternalControl_Rover.cpp
new file mode 100644
index 0000000000000..1c08f92a9b5fd
--- /dev/null
+++ b/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
diff --git a/Rover/AP_ExternalControl_Rover.h b/Rover/AP_ExternalControl_Rover.h
new file mode 100644
index 0000000000000..28647968d67d7
--- /dev/null
+++ b/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
diff --git a/Rover/Rover.h b/Rover/Rover.h
index a15d086b4d160..c6aa6db49702c 100644
--- a/Rover/Rover.h
+++ b/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 @@ class Rover : public AP_Vehicle {
     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 @@ class Rover : public AP_Vehicle {
     // 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
diff --git a/Rover/mode.h b/Rover/mode.h
index fb9a57514dbe4..0d7833de99e12 100644
--- a/Rover/mode.h
+++ b/Rover/mode.h
@@ -488,6 +488,9 @@ class ModeCircle : public Mode
 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"; }