Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: fixed issue with not regaining RC input 

this fixes an issue where when you lose R/C input on IOMCU that you
may not regain it when R/C comes back.

The issue stems from us still processing the DSM uart when we are
using the SD3 "SBUS" uart for RC input, and still doing the switch of
the SD3 config every 2 seconds.

When we are not searching for a new protocol we should not be changing
UART config
This commit is contained in:
Andrew Tridgell 2023-05-21 18:31:59 +10:00
parent 4d72a86032
commit a25ffa266f
1 changed files with 25 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_IOMCU/iofirmware/rc.cpp
View File

@ -61,6 +61,12 @@ static const SerialConfig dsm_cfg = {
nullptr, // ctx
};
static enum {
RC_SEARCHING,
RC_DSM_PORT,
RC_SBUS_PORT
} rc_state;
/*
init rcin on DSM USART1
*/
@ -101,16 +107,25 @@ void AP_IOMCU_FW::rcin_serial_update(void)
uint8_t b[16];
uint32_t n;
uint32_t now = AP_HAL::millis();
auto &rc = AP::RC();
if (rc.should_search(now)) {
rc_state = RC_SEARCHING;
}
// read from DSM port
if ((n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
if ((rc_state == RC_SEARCHING || rc_state == RC_DSM_PORT) &&
(n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
n = MIN(n, sizeof(b));
// don't mix two 115200 uarts
if (sd3_config == 0) {
rc_stats.num_dsm_bytes += n;
for (uint8_t i=0; i<n; i++) {
if (AP::RC().process_byte(b[i], 115200)) {
if (rc.process_byte(b[i], 115200)) {
rc_stats.last_good_ms = now;
if (!rc.should_search(now)) {
rc_state = RC_DSM_PORT;
}
}
}
}
@ -118,7 +133,8 @@ void AP_IOMCU_FW::rcin_serial_update(void)
}
// read from SBUS port
if ((n = chnReadTimeout(&SD3, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
if ((rc_state == RC_SEARCHING || rc_state == RC_SBUS_PORT) &&
(n = chnReadTimeout(&SD3, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
eventflags_t flags;
if (sd3_config == 0 && ((flags = chEvtGetAndClearFlags(&sd3_listener)) & SD_PARITY_ERROR)) {
rc_stats.sbus_error = flags;
@ -127,8 +143,11 @@ void AP_IOMCU_FW::rcin_serial_update(void)
n = MIN(n, sizeof(b));
rc_stats.num_sbus_bytes += n;
for (uint8_t i=0; i<n; i++) {
if (AP::RC().process_byte(b[i], sd3_config==0?100000:115200)) {
if (rc.process_byte(b[i], sd3_config==0?100000:115200)) {
rc_stats.last_good_ms = now;
if (!rc.should_search(now)) {
rc_state = RC_SBUS_PORT;
}
}
}
}
@ -141,7 +160,8 @@ void AP_IOMCU_FW::rcin_serial_update(void)
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)) {
if (rc_state == RC_SEARCHING &&
now - rc_stats.last_good_ms > 2000 && (sd3_config==1 || !sbus_out_enabled)) {
rc_stats.last_good_ms = now;
sd3_config ^= 1;
sdStop(&SD3);