mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_ChibiOS: add support for disabled mask on iomcu
This commit is contained in:
parent
e5d26a579d
commit
7fc61090f2
|
@ -650,6 +650,11 @@ uint32_t RCOutput::get_disabled_channels(uint32_t digital_mask)
|
||||||
}
|
}
|
||||||
|
|
||||||
disabled_chan_mask <<= chan_offset;
|
disabled_chan_mask <<= chan_offset;
|
||||||
|
#if HAL_WITH_IO_MCU
|
||||||
|
if (iomcu_dshot) {
|
||||||
|
disabled_chan_mask |= iomcu.get_disabled_channels(digital_mask);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
return disabled_chan_mask;
|
return disabled_chan_mask;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1463,9 +1468,9 @@ void RCOutput::dshot_send_groups(rcout_timer_t cycle_start_us, rcout_timer_t tim
|
||||||
}
|
}
|
||||||
|
|
||||||
for (auto &group : pwm_group_list) {
|
for (auto &group : pwm_group_list) {
|
||||||
bool pulse_sent;
|
bool pulse_sent = false;
|
||||||
// send a dshot command
|
// send a dshot command
|
||||||
if (is_dshot_protocol(group.current_mode)
|
if (group.can_send_dshot_pulse()
|
||||||
&& dshot_command_is_active(group)) {
|
&& dshot_command_is_active(group)) {
|
||||||
command_sent = dshot_send_command(group, _dshot_current_command.command, _dshot_current_command.chan);
|
command_sent = dshot_send_command(group, _dshot_current_command.command, _dshot_current_command.chan);
|
||||||
pulse_sent = true;
|
pulse_sent = true;
|
||||||
|
|
Loading…
Reference in New Issue