From c93313107bca8cca6da6772bfea1364f9a54f1c2 Mon Sep 17 00:00:00 2001
From: ashvath100 <ashvath100@gmail.com>
Date: Sat, 8 Aug 2020 08:05:11 +0900
Subject: [PATCH] Rover: implement get_control_outputs

---
 Rover/Rover.cpp | 26 ++++++++++++++++++++++++++
 Rover/Rover.h   |  1 +
 2 files changed, 27 insertions(+)

diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp
index b8bf852710..a24a36bcd8 100644
--- a/Rover/Rover.cpp
+++ b/Rover/Rover.cpp
@@ -180,6 +180,32 @@ bool Rover::set_steering_and_throttle(float steering, float throttle)
     return true;
 }
 
+// get control output (for use in scripting)
+// returns true on success and control_value is set to a value in the range -1 to +1
+bool Rover::get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value)
+{
+    switch (control_output) {
+    case AP_Vehicle::ControlOutput::Throttle:
+        control_value = constrain_float(g2.motors.get_throttle() / 100.0f, -1.0f, 1.0f);
+        return true;
+    case AP_Vehicle::ControlOutput::Yaw:
+        control_value = constrain_float(g2.motors.get_steering() / 4500.0f, -1.0f, 1.0f);
+        return true;
+    case AP_Vehicle::ControlOutput::Lateral:
+        control_value = constrain_float(g2.motors.get_lateral() / 100.0f, -1.0f, 1.0f);
+        return true;
+    case AP_Vehicle::ControlOutput::MainSail:
+        control_value = constrain_float(g2.motors.get_mainsail() / 100.0f, -1.0f, 1.0f);
+        return true;
+    case AP_Vehicle::ControlOutput::WingSail:
+        control_value = constrain_float(g2.motors.get_wingsail() / 100.0f, -1.0f, 1.0f);
+        return true;
+    default:
+        return false;
+    }
+    return false;
+}
+
 #if STATS_ENABLED == ENABLED
 /*
   update AP_Stats
diff --git a/Rover/Rover.h b/Rover/Rover.h
index 364721acb7..e4e8ef9add 100644
--- a/Rover/Rover.h
+++ b/Rover/Rover.h
@@ -276,6 +276,7 @@ private:
     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;
+    bool get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value) override;
     void stats_update();
     void ahrs_update();
     void gcs_failsafe_check(void);