AP_IOMCU: fixed handling of RC ignore failsafe option

this allows for ignoring SBUS failsafe on boards using an IOMCU
This commit is contained in:
Andrew Tridgell 2020-08-24 08:48:36 +10:00
parent dbaf4c833b
commit 2f85bd54ea

View File

@ -288,6 +288,9 @@ void AP_IOMCU::read_rc_input()
if (!read_registers(PAGE_RAW_RCIN, 0, sizeof(rc_input)/2, r)) { if (!read_registers(PAGE_RAW_RCIN, 0, sizeof(rc_input)/2, r)) {
return; return;
} }
if (rc_input.flags_failsafe && rc().ignore_rc_failsafe()) {
rc_input.flags_failsafe = false;
}
if (rc_input.flags_rc_ok && !rc_input.flags_failsafe) { if (rc_input.flags_rc_ok && !rc_input.flags_failsafe) {
rc_last_input_ms = AP_HAL::millis(); rc_last_input_ms = AP_HAL::millis();
} }