|
|
@ -22,6 +22,7 @@ |
|
|
|
|
|
|
|
|
|
|
|
#include "AP_RCProtocol_SRXL.h" |
|
|
|
#include "AP_RCProtocol_SRXL.h" |
|
|
|
#include <AP_Math/crc.h> |
|
|
|
#include <AP_Math/crc.h> |
|
|
|
|
|
|
|
#include <AP_Math/AP_Math.h> |
|
|
|
|
|
|
|
|
|
|
|
// #define SUMD_DEBUG
|
|
|
|
// #define SUMD_DEBUG
|
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
@ -232,28 +233,29 @@ void AP_RCProtocol_SRXL::_process_byte(uint32_t timestamp_us, uint8_t byte) |
|
|
|
/* CRC check here */ |
|
|
|
/* CRC check here */ |
|
|
|
crc_receiver = ((uint16_t)buffer[buflen-2] << 8U) | ((uint16_t)buffer[buflen-1]); |
|
|
|
crc_receiver = ((uint16_t)buffer[buflen-2] << 8U) | ((uint16_t)buffer[buflen-1]); |
|
|
|
if (crc_receiver == crc_fmu) { |
|
|
|
if (crc_receiver == crc_fmu) { |
|
|
|
/* at this point buffer contains all frame data and crc is valid --> extract channel info according to SRXL variant */ |
|
|
|
/* 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]; |
|
|
|
const uint8_t max_values = MIN((unsigned)SRXL_MAX_CHANNELS,(unsigned)MAX_RCIN_CHANNELS); |
|
|
|
uint8_t num_values; |
|
|
|
uint16_t values[max_values]; |
|
|
|
bool failsafe_state; |
|
|
|
uint8_t num_values; |
|
|
|
switch (frame_header) { |
|
|
|
bool failsafe_state; |
|
|
|
case SRXL_HEADER_V1: |
|
|
|
switch (frame_header) { |
|
|
|
srxl_channels_get_v1v2(MAX_RCIN_CHANNELS, &num_values, values, &failsafe_state); |
|
|
|
case SRXL_HEADER_V1: |
|
|
|
add_input(num_values, values, failsafe_state); |
|
|
|
srxl_channels_get_v1v2(max_values, &num_values, values, &failsafe_state); |
|
|
|
break; |
|
|
|
add_input(num_values, values, failsafe_state); |
|
|
|
case SRXL_HEADER_V2: |
|
|
|
break; |
|
|
|
srxl_channels_get_v1v2(MAX_RCIN_CHANNELS, &num_values, values, &failsafe_state); |
|
|
|
case SRXL_HEADER_V2: |
|
|
|
add_input(num_values, values, failsafe_state); |
|
|
|
srxl_channels_get_v1v2(max_values, &num_values, values, &failsafe_state); |
|
|
|
break; |
|
|
|
add_input(num_values, values, failsafe_state); |
|
|
|
case SRXL_HEADER_V5: |
|
|
|
break; |
|
|
|
srxl_channels_get_v5(MAX_RCIN_CHANNELS, &num_values, values, &failsafe_state); |
|
|
|
case SRXL_HEADER_V5: |
|
|
|
add_input(num_values, values, failsafe_state); |
|
|
|
srxl_channels_get_v5(max_values, &num_values, values, &failsafe_state); |
|
|
|
break; |
|
|
|
add_input(num_values, values, failsafe_state); |
|
|
|
default: |
|
|
|
break; |
|
|
|
break; |
|
|
|
default: |
|
|
|
} |
|
|
|
break; |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
decode_state_next = STATE_IDLE; /* frame data buffering and decoding finished --> statemachine not in use until new header drops is */ |
|
|
|
decode_state_next = STATE_IDLE; /* frame data buffering and decoding finished --> statemachine not in use until new header drops is */ |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
/* frame not completely received --> frame data buffering still ongoing */ |
|
|
|
/* frame not completely received --> frame data buffering still ongoing */ |
|
|
|
decode_state_next = STATE_COLLECT; |
|
|
|
decode_state_next = STATE_COLLECT; |
|
|
|