Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: don't use pulse input for FPORT

Browse Source
This commit is contained in:
Andrew Tridgell 2019-12-02 16:15:56 +11:00
parent 9332c08c46
commit 6599fd49a2
1 changed files with 34 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -28,7 +28,7 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// usart3 is for SBUS input and output // usart3 is for SBUS input and output
static const SerialConfig uart3_cfg = { static const SerialConfig sbus_cfg = {
100000, // speed 100000, // speed
USART_CR1_PCE | USART_CR1_M, // cr1, enable even parity USART_CR1_PCE | USART_CR1_M, // cr1, enable even parity
USART_CR2_STOP_1, // cr2, two stop bits USART_CR2_STOP_1, // cr2, two stop bits
@ -40,15 +40,19 @@ static const SerialConfig uart3_cfg = {
// listen for parity errors on sd3 input // listen for parity errors on sd3 input
static event_listener_t sd3_listener; static event_listener_t sd3_listener;
static uint8_t sd3_config;
void sbus_out_write(uint16_t *channels, uint8_t nchannels) void sbus_out_write(uint16_t *channels, uint8_t nchannels)
{ {
uint8_t buffer[25]; if (sd3_config == 0) {
AP_SBusOut::sbus_format_frame(channels, nchannels, buffer); uint8_t buffer[25];
chnWrite(&SD3, buffer, sizeof(buffer)); AP_SBusOut::sbus_format_frame(channels, nchannels, buffer);
chnWrite(&SD3, buffer, sizeof(buffer));
}
} }
// usart1 is for DSM input and (optionally) debug to FMU // usart1 is for DSM input and (optionally) debug to FMU
static const SerialConfig uart1_cfg = { static const SerialConfig dsm_cfg = {
115200, // speed 115200, // speed
0, // cr1 0, // cr1
0, // cr2 0, // cr2
@ -62,16 +66,17 @@ static const SerialConfig uart1_cfg = {
*/ */
void AP_IOMCU_FW::rcin_serial_init(void) void AP_IOMCU_FW::rcin_serial_init(void)
{ {
sdStart(&SD1, &uart1_cfg); sdStart(&SD1, &dsm_cfg);
sdStart(&SD3, &uart3_cfg); sdStart(&SD3, &sbus_cfg);
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD3), chEvtRegisterMaskWithFlags(chnGetEventSource(&SD3),
&sd3_listener, &sd3_listener,
EVENT_MASK(1), EVENT_MASK(1),
SD_PARITY_ERROR); SD_PARITY_ERROR);
// disable input for SBUS with pulses, we will use the UART for // disable input for SBUS with pulses, we will use the UART for
// SBUS. // SBUS and FPORT
AP::RC().disable_for_pulses(AP_RCProtocol::SBUS); AP::RC().disable_for_pulses(AP_RCProtocol::SBUS);
AP::RC().disable_for_pulses(AP_RCProtocol::SBUS_NI); AP::RC().disable_for_pulses(AP_RCProtocol::SBUS_NI);
AP::RC().disable_for_pulses(AP_RCProtocol::FPORT);
} }
static struct { static struct {
@ -79,6 +84,7 @@ static struct {
uint32_t num_sbus_bytes; uint32_t num_sbus_bytes;
uint32_t num_sbus_errors; uint32_t num_sbus_errors;
eventflags_t sbus_error; eventflags_t sbus_error;
uint32_t last_good_ms;
} rc_stats; } rc_stats;
/* /*
@ -88,13 +94,19 @@ void AP_IOMCU_FW::rcin_serial_update(void)
{ {
uint8_t b[16]; uint8_t b[16];
uint32_t n; uint32_t n;
uint32_t now = AP_HAL::millis();
// read from DSM port // read from DSM port
if ((n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) { if ((n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
n = MIN(n, sizeof(b)); n = MIN(n, sizeof(b));
rc_stats.num_dsm_bytes += n; // don't mix two 115200 uarts
for (uint8_t i=0; i<n; i++) { if (sd3_config == 0) {
AP::RC().process_byte(b[i], 115200); rc_stats.num_dsm_bytes += n;
for (uint8_t i=0; i<n; i++) {
if (AP::RC().process_byte(b[i], 115200)) {
rc_stats.last_good_ms = now;
}
}
} }
//BLUE_TOGGLE(); //BLUE_TOGGLE();
} }
@ -102,17 +114,26 @@ void AP_IOMCU_FW::rcin_serial_update(void)
// read from SBUS port // read from SBUS port
if ((n = chnReadTimeout(&SD3, b, sizeof(b), TIME_IMMEDIATE)) > 0) { if ((n = chnReadTimeout(&SD3, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
eventflags_t flags; eventflags_t flags;
if ((flags = chEvtGetAndClearFlags(&sd3_listener)) & SD_PARITY_ERROR) { if (sd3_config == 0 && ((flags = chEvtGetAndClearFlags(&sd3_listener)) & SD_PARITY_ERROR)) {
rc_stats.sbus_error = flags; rc_stats.sbus_error = flags;
rc_stats.num_sbus_errors++; rc_stats.num_sbus_errors++;
} else { } else {
n = MIN(n, sizeof(b)); n = MIN(n, sizeof(b));
rc_stats.num_sbus_bytes += n; rc_stats.num_sbus_bytes += n;
for (uint8_t i=0; i<n; i++) { for (uint8_t i=0; i<n; i++) {
AP::RC().process_byte(b[i], 100000); if (AP::RC().process_byte(b[i], sd3_config==0?100000:115200)) {
rc_stats.last_good_ms = now;
}
} }
} }
} }
if (now - rc_stats.last_good_ms > 2000) {
rc_stats.last_good_ms = now;
sd3_config ^= 1;
sdStop(&SD3);
sdStart(&SD3, sd3_config==0?&sbus_cfg:&dsm_cfg);
}
} }
/* /*