AP_HAL_ChibiOS: determine presence of disabled channels based on digital channels in a group

This commit is contained in:
Andy Piper 2022-04-19 18:16:25 +01:00 committed by Randy Mackay
parent 8ccc84beab
commit 119cde79a7
2 changed files with 32 additions and 0 deletions

View File

@ -497,6 +497,33 @@ RCOutput::pwm_group *RCOutput::find_chan(uint8_t chan, uint8_t &group_idx)
return nullptr; return nullptr;
} }
/*
* return mask of channels that must be disabled because they share a group with a digital channel
*/
uint16_t RCOutput::get_disabled_channels(uint16_t digital_mask)
{
uint16_t dmask = (digital_mask >> chan_offset);
uint16_t disabled_chan_mask = 0;
for (auto &group : pwm_group_list) {
bool digital_group = false;
for (uint8_t j = 0; j < 4; j++) {
if ((1U << group.chan[j]) & dmask) {
digital_group = true;
}
}
if (digital_group) {
for (uint8_t j = 0; j < 4; j++) {
if (!((1U << group.chan[j]) & dmask)) {
disabled_chan_mask |= (1U << group.chan[j]);
}
}
}
}
disabled_chan_mask <<= chan_offset;
return disabled_chan_mask;
}
uint16_t RCOutput::get_freq(uint8_t chan) uint16_t RCOutput::get_freq(uint8_t chan)
{ {
#if HAL_WITH_IO_MCU #if HAL_WITH_IO_MCU

View File

@ -70,6 +70,11 @@ public:
void set_output_mode(uint16_t mask, const enum output_mode mode) override; void set_output_mode(uint16_t mask, const enum output_mode mode) override;
bool get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) const override; bool get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) const override;
/*
* return mask of channels that must be disabled because they share a group with a digital channel
*/
uint16_t get_disabled_channels(uint16_t digital_mask) override;
float scale_esc_to_unity(uint16_t pwm) override { float scale_esc_to_unity(uint16_t pwm) override {
return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0; return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0;
} }