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

@@ -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;
+}

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));