From 7c92340b426e1ac9123a1058d735a74d8268f65a Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 18 Feb 2023 19:25:53 +0000
Subject: [PATCH] AP_Motors: add getters for example

---
 libraries/AP_Motors/AP_MotorsMatrix.cpp      | 28 ++++++++++++++++++--
 libraries/AP_Motors/AP_MotorsMatrix.h        |  4 +++
 libraries/AP_Motors/AP_MotorsMulticopter.cpp | 13 +++++++++
 libraries/AP_Motors/AP_MotorsMulticopter.h   |  4 +++
 4 files changed, 47 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp
index e915526d75..790e9be77f 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.cpp
+++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp
@@ -597,7 +597,7 @@ bool AP_MotorsMatrix::setup_quad_matrix(motor_frame_type frame_type)
         add_motors(motors, ARRAY_SIZE(motors));
         break;
     }
-#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
     case MOTOR_FRAME_TYPE_NYT_PLUS: {
         _frame_type_string = "NYT_PLUS";
         static const AP_MotorsMatrix::MotorDef motors[] {
@@ -620,7 +620,7 @@ bool AP_MotorsMatrix::setup_quad_matrix(motor_frame_type frame_type)
         add_motors(motors, ARRAY_SIZE(motors));
         break;
     }
-#endif //APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+#endif //APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
     case MOTOR_FRAME_TYPE_BF_X: {
         // betaflight quad X order
         // see: https://fpvfrenzy.com/betaflight-motor-order/
@@ -1365,5 +1365,29 @@ void AP_MotorsMatrix::disable_yaw_torque(void)
     }
 }
 
+#if APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
+// examples can pull values direct
+float AP_MotorsMatrix::get_thrust_rpyt_out(uint8_t i) const
+{
+    if (i < AP_MOTORS_MAX_NUM_MOTORS) {
+        return _thrust_rpyt_out[i];
+    }
+    return 0.0;
+}
+
+bool AP_MotorsMatrix::get_factors(uint8_t i, float &roll, float &pitch, float &yaw, float &throttle, uint8_t &testing_order) const
+{
+    if ((i < AP_MOTORS_MAX_NUM_MOTORS) && motor_enabled[i]) {
+        roll = _roll_factor[i];
+        pitch = _pitch_factor[i];
+        yaw = _yaw_factor[i];
+        throttle = _throttle_factor[i];
+        testing_order = _test_order[i];
+        return true;
+    }
+    return false;
+}
+#endif
+
 // singleton instance
 AP_MotorsMatrix *AP_MotorsMatrix::_singleton;
diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h
index 5b2061546f..fd5fe322b8 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.h
+++ b/libraries/AP_Motors/AP_MotorsMatrix.h
@@ -101,6 +101,10 @@ public:
     };
     void add_motors_raw(const struct MotorDefRaw *motors, uint8_t num_motors);
 
+    // pull values direct, (examples only)
+    float get_thrust_rpyt_out(uint8_t i) const;
+    bool get_factors(uint8_t i, float &roll, float &pitch, float &yaw, float &throttle, uint8_t &testing_order) const;
+
 protected:
     // output - sends commands to the motors
     void                output_armed_stabilizing() override;
diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp
index 0a861ac6fb..1b07b8a36b 100644
--- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp
+++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp
@@ -873,3 +873,16 @@ bool AP_MotorsMulticopter::arming_checks(size_t buflen, char *buffer) const
 
     return true;
 }
+
+#if APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
+// Getters for AP_Motors example, not used by vehicles
+float AP_MotorsMulticopter::get_throttle_avg_max() const
+{
+    return _throttle_avg_max;
+}
+
+int16_t AP_MotorsMulticopter::get_yaw_headroom() const
+{
+    return _yaw_headroom;
+}
+#endif
diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h
index 5971b6d144..2e4fdcd615 100644
--- a/libraries/AP_Motors/AP_MotorsMulticopter.h
+++ b/libraries/AP_Motors/AP_MotorsMulticopter.h
@@ -105,6 +105,10 @@ public:
     // Run arming checks
     bool arming_checks(size_t buflen, char *buffer) const override;
 
+    // Getters for AP_Motors example, not used by vehicles
+    float get_throttle_avg_max() const;
+    int16_t get_yaw_headroom() const;
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo        var_info[];