From 5e43370e357af548654cba3dcbdd581038fea4b6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 9 Dec 2021 10:50:32 +1100
Subject: [PATCH] SRV_Channel: added get_motor_num()

map back from a channel to a motor number, for ESC telemetry reporting
---
 libraries/SRV_Channel/SRV_Channel.cpp | 16 ++++++++++++++++
 libraries/SRV_Channel/SRV_Channel.h   |  3 +++
 2 files changed, 19 insertions(+)

diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp
index b1d1fcc55f..75c9625925 100644
--- a/libraries/SRV_Channel/SRV_Channel.cpp
+++ b/libraries/SRV_Channel/SRV_Channel.cpp
@@ -277,3 +277,19 @@ bool SRV_Channel::is_control_surface(SRV_Channel::Aux_servo_function_t function)
 
     return false;
 }
+
+// return the motor number of a channel, or -1 if not a motor
+// return 0 for first motor
+int8_t SRV_Channel::get_motor_num(void) const
+{
+    const auto k_function = get_function();
+    switch (k_function) {
+    case k_motor1 ... k_motor8:
+        return int8_t(uint16_t(k_function) - k_motor1);
+    case k_motor9 ... k_motor12:
+        return 8 + int8_t(uint16_t(k_function) - k_motor9);
+    default:
+        break;
+    }
+    return -1;
+}
diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h
index eb7d8e9102..20ee360b23 100644
--- a/libraries/SRV_Channel/SRV_Channel.h
+++ b/libraries/SRV_Channel/SRV_Channel.h
@@ -240,6 +240,9 @@ public:
         return (SRV_Channel::Aux_servo_function_t)function.get();
     }
 
+    // return the motor number of a channel, or -1 if not a motor
+    int8_t get_motor_num(void) const;
+
     // set and save function for channel. Used in upgrade of parameters in plane
     void function_set_and_save(SRV_Channel::Aux_servo_function_t f) {
         function.set_and_save(int8_t(f));