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

AP_IOMCU: fixed error handling for short packets 

this prevents short packets from the IOMCU being processed. A short
enough read could be processed as a valid status packet, which would
lead to invalid values for safety flag and servo voltage

fixes issue #12030
This commit is contained in:
Andrew Tridgell 2019-08-13 17:59:18 +10:00
parent afc8a70ce4
commit 984542fa53
1 changed files with 23 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -286,7 +286,9 @@ void AP_IOMCU::read_rc_input()
{
// read a min of 9 channels and max of IOMCU_MAX_CHANNELS
uint8_t n = MIN(MAX(9, rc_input.count), IOMCU_MAX_CHANNELS);
read_registers(PAGE_RAW_RCIN, 0, 6+n, (uint16_t *)&rc_input);
if (!read_registers(PAGE_RAW_RCIN, 0, 6+n, (uint16_t *)&rc_input)) {
return;
}
if (rc_input.flags_rc_ok && !rc_input.flags_failsafe) {
rc_input.last_input_ms = AP_HAL::millis();
}
@ -298,7 +300,9 @@ void AP_IOMCU::read_rc_input()
void AP_IOMCU::read_status()
{
uint16_t *r = (uint16_t *)&reg_status;
read_registers(PAGE_STATUS, 0, sizeof(reg_status)/2, r);
if (!read_registers(PAGE_STATUS, 0, sizeof(reg_status)/2, r)) {
return;
}
check_iomcu_reset();
@ -391,11 +395,23 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
// wait for the expected number of reply bytes or timeout
if (!uart.wait_timeout(count*2+4, 10)) {
debug("t=%u timeout read page=%u offset=%u count=%u\n",
AP_HAL::millis(), page, offset, count);
return false;
}
uint8_t *b = (uint8_t *)&pkt;
uint8_t n = uart.available();
if (n < offsetof(struct IOPacket, regs)) {
debug("t=%u small pkt %u\n", AP_HAL::millis(), n);
protocol_fail_count++;
return false;
}
if (pkt.get_size() != n) {
debug("t=%u bad len %u %u\n", AP_HAL::millis(), n, pkt.get_size());
protocol_fail_count++;
return false;
}
for (uint8_t i=0; i<n; i++) {
if (i < sizeof(pkt)) {
b[i] = uart.read();
@ -406,8 +422,8 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
pkt.crc = 0;
uint8_t expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
if (got_crc != expected_crc) {
debug("bad crc %02x should be %02x n=%u %u/%u/%u\n",
got_crc, expected_crc,
debug("t=%u bad crc %02x should be %02x n=%u %u/%u/%u\n",
AP_HAL::millis(), got_crc, expected_crc,
n, page, offset, count);
protocol_fail_count++;
return false;
@ -919,9 +935,11 @@ void AP_IOMCU::check_iomcu_reset(void)
if (last_iocmu_timestamp_ms == 0) {
// initialisation
last_iocmu_timestamp_ms = reg_status.timestamp_ms;
hal.console->printf("IOMCU startup\n");
return;
}
uint32_t dt_ms = reg_status.timestamp_ms - last_iocmu_timestamp_ms;
uint32_t ts1 = last_iocmu_timestamp_ms;
last_iocmu_timestamp_ms = reg_status.timestamp_ms;
if (dt_ms < 500) {
// all OK
@ -929,7 +947,7 @@ void AP_IOMCU::check_iomcu_reset(void)
}
detected_io_reset = true;
AP::internalerror().error(AP_InternalError::error_t::iomcu_reset);
hal.console->printf("IOMCU reset\n");
hal.console->printf("IOMCU reset t=%u %u %u dt=%u\n", AP_HAL::millis(), ts1, reg_status.timestamp_ms, dt_ms);
// we need to ensure the mixer data and the rates are sent over to
// the IOMCU
if (mixing.enabled) {