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_BLHeli: use UART locking

Browse Source 
this gains exclusive access to the UART while MSP protocol is active to
prevent protocol corruption due to other drivers writing to hal.console
This commit is contained in:
Andrew Tridgell 2018-04-02 16:01:18 +10:00
parent e448b5d069
commit 7c678bd707
2 changed files with 33 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_BLHeli/AP_BLHeli.cpp
View File

@ -16,7 +16,9 @@
implementation of MSP and BLHeli-4way protocols for pass-through ESC
calibration and firmware update
With thanks to betaflight for a great reference implementation
With thanks to betaflight for a great reference
implementation. Several of the functions below are based on
betaflight equivalent functions
*/
#include "AP_BLHeli.h"
@ -40,6 +42,10 @@ extern const AP_HAL::HAL& hal;
#define debug(fmt, args ...)
#endif
// key for locking UART for exclusive use. This prevents any other writes from corrupting
// the MSP protocol on hal.console
#define BLHELI_UART_LOCK_KEY 0x20180402
const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
// @Param: MASK
// @DisplayName: Channel Bitmask
@ -221,7 +227,7 @@ void AP_BLHeli::msp_send_reply(uint8_t cmd, const uint8_t *buf, uint8_t len)
c ^= msp.buf[i+3];
}
*b++ = c;
uart->write(&msp.buf[0], len+6);
uart->write_locked(&msp.buf[0], len+6, BLHELI_UART_LOCK_KEY);
}
void AP_BLHeli::putU16(uint8_t *b, uint16_t v)
@ -294,9 +300,7 @@ void AP_BLHeli::msp_process_command(void)
}
case MSP_REBOOT:
debug("MSP_REBOOT");
debug("MSP: rebooting");
hal.scheduler->reboot(false);
debug("MSP: ignoring reboot command");
break;
case MSP_UID:
@ -440,7 +444,7 @@ void AP_BLHeli::blheli_send_reply(const uint8_t *buf, uint16_t len)
b += len;
*b++ = blheli.ack;
putU16_BE(b, crc_xmodem(&blheli.buf[0], len+6));
uart->write(&blheli.buf[0], len+8);
uart->write_locked(&blheli.buf[0], len+8, BLHELI_UART_LOCK_KEY);
debug("OutB(%u) 0x%02x ack=0x%02x", len+8, (unsigned)blheli.command, blheli.ack);
}
@ -1019,6 +1023,14 @@ bool AP_BLHeli::process_input(uint8_t b)
blheli.state = BLHELI_IDLE;
}
if (valid_packet) {
if (uart->lock_port(BLHELI_UART_LOCK_KEY)) {
uart_locked = true;
} else {
return false;
}
}
return valid_packet;
}
@ -1078,6 +1090,7 @@ void AP_BLHeli::update(void)
{
if (initialised &&
timeout_sec &&
uart_locked &&
AP_HAL::millis() - last_valid_ms > uint32_t(timeout_sec.get())*1000U) {
// we're not processing requests any more, shutdown serial
// output
@ -1089,6 +1102,9 @@ void AP_BLHeli::update(void)
motors_disabled = false;
SRV_Channels::set_disabled_channel_mask(0);
}
uart->lock_port(0);
uart_locked = false;
debug("Unlocked UART");
}
if (initialised || (channel_mask.get() == 0 && channel_auto.get() == 0)) {
if (initialised && run_test.get() > 0) {
@ -1135,16 +1151,19 @@ void AP_BLHeli::update(void)
}
}
static uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
/*
implement the 8 bit CRC used by the BLHeli ESC telemetry protocol
*/
uint8_t AP_BLHeli::telem_crc8(uint8_t crc, uint8_t crc_seed) const
{
uint8_t crc_u = crc;
crc_u ^= crc_seed;
for (int i=0; i<8; i++) {
for (uint8_t i=0; i<8; i++) {
crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
}
return (crc_u);
return crc_u;
}
/*
@ -1165,7 +1184,7 @@ void AP_BLHeli::read_telemetry_packet(void)
// calculate crc
for (uint8_t i=0; i<telem_packet_size-1; i++) {
crc = updateCrc8(buf[i], crc);
crc = telem_crc8(buf[i], crc);
}
if (buf[telem_packet_size-1] != crc) {

4
libraries/AP_BLHeli/AP_BLHeli.h
View File

@ -181,6 +181,9 @@ private:
// have we disabled motor outputs?
bool motors_disabled;
// have we locked the UART?
bool uart_locked;
// mapping from BLHeli motor numbers to RC output channels
uint8_t motor_map[max_motors];
@ -217,6 +220,7 @@ private:
bool BL_VerifyFlash(const uint8_t *buf, uint16_t n);
void blheli_process_command(void);
void run_connection_test(uint8_t chan);
uint8_t telem_crc8(uint8_t crc, uint8_t crc_seed) const;
void read_telemetry_packet(void);
// protocol handler hook