From 46b452ae396bb996723a29554cfb32119013e20d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Jul 2024 11:15:03 +1000 Subject: [PATCH] SRV_Channel: support 32 rotors in a frame --- libraries/SRV_Channel/SRV_Channel.cpp | 6 +++++- libraries/SRV_Channel/SRV_Channel.h | 25 ++++++++++++++++++++++++- 2 files changed, 29 insertions(+), 2 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index 1cfccbd8bd..f05f389e92 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -306,7 +306,8 @@ uint16_t SRV_Channel::get_limit_pwm(Limit limit) const bool SRV_Channel::is_motor(SRV_Channel::Aux_servo_function_t function) { return ((function >= SRV_Channel::k_motor1 && function <= SRV_Channel::k_motor8) || - (function >= SRV_Channel::k_motor9 && function <= SRV_Channel::k_motor12)); + (function >= SRV_Channel::k_motor9 && function <= SRV_Channel::k_motor12) || + (function >= SRV_Channel::k_motor13 && function <= SRV_Channel::k_motor32)); } // return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous @@ -332,6 +333,7 @@ bool SRV_Channel::should_e_stop(SRV_Channel::Aux_servo_function_t function) case Aux_servo_function_t::k_motor10: case Aux_servo_function_t::k_motor11: case Aux_servo_function_t::k_motor12: + case Aux_servo_function_t::k_motor13 ... Aux_servo_function_t::k_motor32: case Aux_servo_function_t::k_engine_run_enable: return true; default: @@ -380,6 +382,8 @@ int8_t SRV_Channel::get_motor_num(void) const return int8_t(uint16_t(k_function) - k_motor1); case k_motor9 ... k_motor12: return 8 + int8_t(uint16_t(k_function) - k_motor9); + case k_motor13 ... k_motor32: + return 12 + int8_t(uint16_t(k_function) - k_motor13); default: break; } diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index acb6021b89..886fb3b16d 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -189,6 +189,26 @@ public: k_rcin15_mapped = 154, k_rcin16_mapped = 155, k_lift_release = 156, + k_motor13 = 160, + k_motor14 = 161, + k_motor15 = 162, + k_motor16 = 163, + k_motor17 = 164, + k_motor18 = 165, + k_motor19 = 166, + k_motor20 = 167, + k_motor21 = 168, + k_motor22 = 169, + k_motor23 = 170, + k_motor24 = 171, + k_motor25 = 172, + k_motor26 = 173, + k_motor27 = 174, + k_motor28 = 175, + k_motor29 = 176, + k_motor30 = 177, + k_motor31 = 178, + k_motor32 = 179, k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t; @@ -539,7 +559,10 @@ public: if (channel < 8) { return SRV_Channel::Aux_servo_function_t(SRV_Channel::k_motor1+channel); } - return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor9+(channel-8))); + if (channel < 12) { + return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor9+(channel-8))); + } + return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor13+(channel-12))); } void cork();