mirror of https://github.com/ArduPilot/ardupilot
AP_IOMCU: fixed bug in SBUS output when scanning for FPort input
when we are looking for FPort input, we normally switch UART3 on the IOMCU to 115200 to look for inverted inputs at 115200 baudrate. We need to disable this switching when we have SBUS output enabled to prevent a change in the SBUS output baudrate Many thanks to afishman for finding this bug Fixes #15522
This commit is contained in:
parent
82cf755115
commit
ba8fb3230b
|
@ -127,13 +127,20 @@ void AP_IOMCU_FW::rcin_serial_update(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
if (now - rc_stats.last_good_ms > 2000) {
|
||||
/*
|
||||
when not using SBUS output we switch UART3 between 100000 baud
|
||||
and 115200 baud in order to support RC input protocols that are
|
||||
115200 inverted (such as FPort). If SBUS output is enabled then
|
||||
we need to disable this as the uart is shared between input and
|
||||
output
|
||||
*/
|
||||
const bool sbus_out_enabled = (reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) != 0;
|
||||
if (now - rc_stats.last_good_ms > 2000 && (sd3_config==1 || !sbus_out_enabled)) {
|
||||
rc_stats.last_good_ms = now;
|
||||
sd3_config ^= 1;
|
||||
sdStop(&SD3);
|
||||
sdStart(&SD3, sd3_config==0?&sbus_cfg:&dsm_cfg);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
Loading…
Reference in New Issue