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

AP_RCProtocol: use pulses as clock for frame timeout 

for SRXL and DSM we should judge a frame timeout from the pulses,
otherwise lag in the DMA capture can cause a false frame drop
This commit is contained in:
Andrew Tridgell 2018-11-03 16:53:45 +11:00
parent faf8f73437
commit eca4fec2aa
4 changed files with 30 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
View File

@ -40,6 +40,9 @@ void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1)
uint8_t bit_ofs, byte_ofs;
uint16_t nbits;
// maintain time using pulse widths
clock_us += (width_s0 + width_s1);
if (bits_s0 == 0 || bits_s1 == 0) {
// invalid data
goto reset;
@ -83,7 +86,7 @@ void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1)
}
uint16_t values[8];
uint16_t num_values=0;
if (dsm_decode(AP_HAL::micros64(), bytes, values, &num_values, 8) &&
if (dsm_decode(clock_us, bytes, values, &num_values, 8) &&
num_values >= MIN_RCIN_CHANNELS) {
add_input(num_values, values, false);
}
@ -247,7 +250,7 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]
* Decode the entire dsm frame (all contained channels)
*
*/
bool AP_RCProtocol_DSM::dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16],
bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[16],
uint16_t *values, uint16_t *num_values, uint16_t max_values)
{
#if 0
@ -457,7 +460,7 @@ void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate)
if (byte_input.ofs == 16) {
uint16_t values[8];
uint16_t num_values=0;
if (dsm_decode(AP_HAL::micros64(), byte_input.buf, values, &num_values, 8) &&
if (dsm_decode(AP_HAL::micros(), byte_input.buf, values, &num_values, 8) &&
num_values >= MIN_RCIN_CHANNELS) {
add_input(num_values, values, false);
}

7
libraries/AP_RCProtocol/AP_RCProtocol_DSM.h
View File

@ -31,10 +31,10 @@ private:
void dsm_decode();
bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
void dsm_guess_format(bool reset, const uint8_t dsm_frame[16]);
bool dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16],
bool dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[16],
uint16_t *values, uint16_t *num_values, uint16_t max_values);
uint64_t dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
uint32_t dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */
// state of DSM decoder
struct {
@ -52,6 +52,9 @@ private:
} bind_state;
uint32_t bind_last_ms;
// sum of clock pulses
uint32_t clock_us;
struct {
uint8_t buf[16];
uint8_t ofs;

22
libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp
View File

@ -55,6 +55,9 @@ void AP_RCProtocol_SRXL::process_pulse(uint32_t width_s0, uint32_t width_s1)
uint8_t bit_ofs, byte_ofs;
uint16_t nbits;
// keep a clock based on the pulses
clock_us += (width_s0 + width_s1);
if (bits_s0 == 0 || bits_s1 == 0) {
// invalid data
goto reset;
@ -90,7 +93,7 @@ void AP_RCProtocol_SRXL::process_pulse(uint32_t width_s0, uint32_t width_s1)
break;
}
byte = ((v>>1) & 0xFF);
process_byte(byte, 115200);
_process_byte(clock_us, byte, 115200);
}
memset(&srxl_state, 0, sizeof(srxl_state));
}
@ -249,9 +252,8 @@ int AP_RCProtocol_SRXL::srxl_channels_get_v5(uint16_t max_values, uint8_t *num_v
return 0;
}
void AP_RCProtocol_SRXL::process_byte(uint8_t byte, uint32_t baudrate)
void AP_RCProtocol_SRXL::_process_byte(uint32_t timestamp_us, uint8_t byte, uint32_t baudrate)
{
uint64_t timestamp_us = AP_HAL::micros64();
/*----------------------------------------distinguish different srxl variants at the beginning of each frame---------------------------------------------- */
/* Check if we have a new begin of a frame --> indicators: Time gap in datastream + SRXL header 0xA<VARIANT>*/
if ((timestamp_us - last_data_us) >= SRXL_MIN_FRAMESPACE_US) {
@ -309,8 +311,8 @@ void AP_RCProtocol_SRXL::process_byte(uint8_t byte, uint32_t baudrate)
if (buflen == frame_len_full) {
/* CRC check here */
crc_receiver = ((uint16_t)buffer[buflen-2] << 8U) | ((uint16_t)buffer[buflen-1]);
if (crc_receiver == crc_fmu) {
/* at this point buffer contains all frame data and crc is valid --> extract channel info according to SRXL variant */
if (crc_receiver == crc_fmu) {
/* at this point buffer contains all frame data and crc is valid --> extract channel info according to SRXL variant */
uint16_t values[SRXL_MAX_CHANNELS];
uint8_t num_values;
bool failsafe_state;
@ -330,7 +332,7 @@ void AP_RCProtocol_SRXL::process_byte(uint8_t byte, uint32_t baudrate)
default:
break;
}
}
}
decode_state_next = STATE_IDLE; /* frame data buffering and decoding finished --> statemachine not in use until new header drops is */
} else {
/* frame not completely received --> frame data buffering still ongoing */
@ -345,3 +347,11 @@ void AP_RCProtocol_SRXL::process_byte(uint8_t byte, uint32_t baudrate)
decode_state = decode_state_next;
last_data_us = timestamp_us;
}
/*
process a byte provided by a uart
*/
void AP_RCProtocol_SRXL::process_byte(uint8_t byte, uint32_t baudrate)
{
_process_byte(AP_HAL::micros(), byte, baudrate);
}

5
libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h
View File

@ -41,12 +41,13 @@ public:
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
private:
void _process_byte(uint32_t timestamp_us, uint8_t byte, uint32_t baudrate);
static uint16_t srxl_crc16(uint16_t crc, uint8_t new_byte);
int srxl_channels_get_v1v2(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state);
int srxl_channels_get_v5(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state);
uint8_t buffer[SRXL_FRAMELEN_MAX]; /* buffer for raw srxl frame data in correct order --> buffer[0]=byte0 buffer[1]=byte1 */
uint8_t buflen; /* length in number of bytes of received srxl dataframe in buffer */
uint64_t last_data_us; /* timespan since last received data in us */
uint32_t last_data_us; /* timespan since last received data in us */
uint16_t channels[SRXL_MAX_CHANNELS] = {0}; /* buffer for extracted RC channel data as pulsewidth in microseconds */
uint16_t max_channels = 0;
enum {
@ -60,7 +61,7 @@ private:
uint8_t decode_state_next = STATE_IDLE; /* State of frame decoding thatwill be applied when the next byte from dataframe drops in */
uint16_t crc_fmu = 0U; /* CRC calculated over payload from srxl datastream on this machine */
uint16_t crc_receiver = 0U; /* CRC extracted from srxl datastream */
uint32_t clock_us; /* clock calculated from pulse lengths */
struct {
uint16_t bytes[SRXL_FRAMELEN_MAX];
uint16_t bit_ofs;