From 9cea7031f5093bbceded948d9e3cfd9964b8eaef Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Mon, 23 Sep 2019 13:40:41 -0300
Subject: [PATCH] APMotors_6DOF: Implement motor_is_enabled(),
 get_motor_angular_factors() and set_reversed();

---
 libraries/AP_Motors/AP_Motors6DOF.cpp | 26 ++++++++++++++++++++++++++
 libraries/AP_Motors/AP_Motors6DOF.h   |  8 ++++++++
 2 files changed, 34 insertions(+)

diff --git a/libraries/AP_Motors/AP_Motors6DOF.cpp b/libraries/AP_Motors/AP_Motors6DOF.cpp
index addb5157fd..55fdcd698a 100644
--- a/libraries/AP_Motors/AP_Motors6DOF.cpp
+++ b/libraries/AP_Motors/AP_Motors6DOF.cpp
@@ -548,3 +548,29 @@ void AP_Motors6DOF::output_armed_stabilizing_vectored_6dof()
         }
     }
 }
+
+Vector3f AP_Motors6DOF::get_motor_angular_factors(int motor_number) {
+     if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
+        return Vector3f(0,0,0);
+    }
+    return Vector3f(_roll_factor[motor_number], _pitch_factor[motor_number], _yaw_factor[motor_number]);
+}
+
+bool AP_Motors6DOF::motor_is_enabled(int motor_number) {
+    if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
+        return false;
+    }
+    return motor_enabled[motor_number];
+}
+
+bool AP_Motors6DOF::set_reversed(int motor_number, bool reversed) {
+    if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
+        return false;
+    }
+    if (reversed) {
+        _motor_reverse[motor_number].set_and_save(-1);
+    } else {
+        _motor_reverse[motor_number].set_and_save(1);
+    }
+    return true;
+}
diff --git a/libraries/AP_Motors/AP_Motors6DOF.h b/libraries/AP_Motors/AP_Motors6DOF.h
index 15edfdfc43..ca9d6abc1f 100644
--- a/libraries/AP_Motors/AP_Motors6DOF.h
+++ b/libraries/AP_Motors/AP_Motors6DOF.h
@@ -44,6 +44,14 @@ public:
     // This allows us to read back the output of the altidude controllers
     // The controllers are in charge of the throttle input, so this gives vehicle access/visibility to the output of those controllers
     float get_throttle_in_bidirectional() const { return constrain_float(2*(_throttle_in - 0.5f), -1.0f, 1.0f); }
+    
+    // returns a vector with roll, pitch, and yaw contributions
+    Vector3f get_motor_angular_factors(int motor_number);
+
+    // returns true if motor is enabled
+    bool motor_is_enabled(int motor_number);
+
+    bool set_reversed(int motor_number, bool reversed);
 
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo        var_info[];