From e03aed3aa7efea7af2fb7b58e9b1b33449ea5c80 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 10 Dec 2021 16:49:58 +0000 Subject: [PATCH] AP_BLHeli: support upto 32 servo outputs --- libraries/AP_BLHeli/AP_BLHeli.cpp | 22 +++++++++++----------- libraries/AP_BLHeli/AP_BLHeli.h | 2 +- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index 779e8a68e3..814d7be3c1 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -58,7 +58,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Param: MASK // @DisplayName: BLHeli Channel Bitmask // @Description: Enable of BLHeli pass-thru servo protocol support to specific channels. This mask is in addition to motors enabled using SERVO_BLH_AUTO (if any) - // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 + // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("MASK", 1, AP_BLHeli, channel_mask, 0), @@ -129,7 +129,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Param: 3DMASK // @DisplayName: BLHeli bitmask of 3D channels // @Description: Mask of channels which are dynamically reversible. This is used to configure ESCs in '3D' mode, allowing for the motor to spin in either direction - // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 + // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("3DMASK", 10, AP_BLHeli, channel_reversible_mask, 0), @@ -138,7 +138,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Param: BDMASK // @DisplayName: BLHeli bitmask of bi-directional dshot channels // @Description: Mask of channels which support bi-directional dshot. This is used for ESCs which have firmware that supports bi-directional dshot allowing fast rpm telemetry values to be returned for the harmonic notch. - // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 + // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("BDMASK", 11, AP_BLHeli, channel_bidir_dshot_mask, 0), @@ -146,7 +146,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Param: RVMASK // @DisplayName: BLHeli bitmask of reversed channels // @Description: Mask of channels which are reversed. This is used to configure ESCs in reversed mode - // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 + // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32 // @User: Advanced // @RebootRequired: True AP_GROUPINFO("RVMASK", 12, AP_BLHeli, channel_reversed_mask, 0), @@ -1333,7 +1333,7 @@ void AP_BLHeli::init(void) } #endif - uint16_t mask = uint16_t(channel_mask.get()); + uint32_t mask = uint32_t(channel_mask.get()); /* allow mode override - this makes it possible to use DShot for @@ -1359,7 +1359,7 @@ void AP_BLHeli::init(void) break; } - uint16_t digital_mask = 0; + uint32_t digital_mask = 0; // setting the digital mask changes the min/max PWM values // it's important that this is NOT done for non-digital channels as otherwise // PWM min can result in motors turning. set for individual overrides first @@ -1386,15 +1386,15 @@ void AP_BLHeli::init(void) } #endif // tell SRV_Channels about ESC capabilities - SRV_Channels::set_digital_outputs(digital_mask, uint16_t(channel_reversible_mask.get()) & digital_mask); + SRV_Channels::set_digital_outputs(digital_mask, uint32_t(channel_reversible_mask.get()) & digital_mask); // the dshot ESC type is required in order to send the reversed/reversible dshot command correctly hal.rcout->set_dshot_esc_type(SRV_Channels::get_dshot_esc_type()); - hal.rcout->set_reversible_mask(uint16_t(channel_reversible_mask.get()) & digital_mask); - hal.rcout->set_reversed_mask(uint16_t(channel_reversed_mask.get()) & digital_mask); + hal.rcout->set_reversible_mask(uint32_t(channel_reversible_mask.get()) & digital_mask); + hal.rcout->set_reversed_mask(uint32_t(channel_reversed_mask.get()) & digital_mask); #ifdef HAL_WITH_BIDIR_DSHOT // possibly enable bi-directional dshot hal.rcout->set_motor_poles(motor_poles); - hal.rcout->set_bidir_dshot_mask(uint16_t(channel_bidir_dshot_mask.get()) & digital_mask); + hal.rcout->set_bidir_dshot_mask(uint32_t(channel_bidir_dshot_mask.get()) & digital_mask); #endif // add motors from channel mask for (uint8_t i=0; i<16 && num_motors < max_motors; i++) { @@ -1404,7 +1404,7 @@ void AP_BLHeli::init(void) } } motor_mask = mask; - debug("ESC: %u motors mask=0x%04x", num_motors, mask); + debug("ESC: %u motors mask=0x%08lx", num_motors, mask); // check if we have a combination of reversable and normal mixed_type = (mask != (mask & channel_reversible_mask.get())) && (channel_reversible_mask.get() != 0); diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h index 9af197e835..53a0a0b9ff 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.h +++ b/libraries/AP_BLHeli/AP_BLHeli.h @@ -242,7 +242,7 @@ private: // mapping from BLHeli motor numbers to RC output channels uint8_t motor_map[max_motors]; - uint16_t motor_mask; + uint32_t motor_mask; // convert between servo number and FMU channel number for ESC telemetry uint8_t chan_offset;