static uint8_t ReadSUMD(void) {
int res;
uint8_t resRx;
int i;
uint8_t rssi=0; /* Received Signal Strength Indicator, not used as SUMD does not provide this */
uint8_t rx_count=0; /* counter of received packets, will be incremented for each received packet */
uint16_t channel_count; /* number of received channels in data packet */
uint16_t channels[SUMD_MAX_CHANNELS]; /* here the channel data get stored */
uint8_t ch;
res = 1; /* preset to accumulating */
while(res==1){
resRx=SUMDRx_RecvChar(&ch);
if (resRx==ERR_RXEMPTY) {
break;
}
if (resRx!=ERR_OK) { /* general error */
SUMDRx_ClearRxBuf(); /* clear RX buffer */
break;
}
res = sumd_decode(ch, &rssi, &rx_count, &channel_count, &channels[0], sizeof(channels)/sizeof(channels[0]));
/* @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error */
}
if (res!=0) {
return ERR_FAILED;
}
/* copy data */
FRTOS1_taskENTER_CRITICAL();
memcpy(REMOTE_channels, channels, sizeof(REMOTE_channels));
FRTOS1_taskEXIT_CRITICAL();
return ERR_OK;
}
bool RCTest::sumdTest(void)
{
const char *filepath = TEST_DATA_PATH "sumd_data.txt";
//warnx("loading data from: %s", filepath);
FILE *fp;
fp = fopen(filepath, "rt");
ut_test(fp != nullptr);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
float last_time = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
ut_test(ret > 0);
if (((f - last_time) * 1000 * 1000) > 3000) {
// warnx("FRAME RESET\n\n");
}
uint8_t b = static_cast<uint8_t>(x);
last_time = f;
// Pipe the data into the parser
//hrt_abstime now = hrt_absolute_time();
uint8_t rssi;
uint8_t rx_count;
uint16_t channel_count;
uint16_t channels[32];
bool sumd_failsafe;
if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32, &sumd_failsafe)) {
//PX4_INFO("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) {
//int16_t val = channels[i];
//PX4_INFO("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
}
}
}
ut_test(ret == EOF);
return true;
}
TEST(SUMDTest, SUMD)
{
const char *filepath = "testdata/sumd_data.txt";
warnx("loading data from: %s", filepath);
FILE *fp;
fp = fopen(filepath, "rt");
//ASSERT_TRUE(fp);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
float last_time = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
if (((f - last_time) * 1000 * 1000) > 3000) {
// warnx("FRAME RESET\n\n");
}
uint8_t b = static_cast<uint8_t>(x);
last_time = f;
// Pipe the data into the parser
//hrt_abstime now = hrt_absolute_time();
uint8_t rssi;
uint8_t rx_count;
uint16_t channel_count;
uint16_t channels[32];
if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32)) {
//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
for (unsigned i = 0; i < channel_count; i++) {
//int16_t val = channels[i];
//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
}
}
}
ASSERT_EQ(EOF, ret);
}
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
{
perf_begin(c_gather_dsm);
uint8_t n_bytes = 0;
uint8_t *bytes;
bool dsm_11_bit;
*dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes,
PX4IO_RC_INPUT_CHANNELS);
if (*dsm_updated) {
if (dsm_11_bit) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
} else {
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
}
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_dsm);
/* get data from FD and attempt to parse with DSM and ST24 libs */
uint8_t st24_rssi, lost_count;
uint16_t st24_channel_count = 0;
*st24_updated = false;
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
st24_rssi = RC_INPUT_RSSI_MAX;
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count,
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
}
if (*st24_updated && lost_count == 0) {
/* ensure ADC RSSI is disabled */
r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI);
*rssi = st24_rssi;
r_raw_rc_count = st24_channel_count;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
/* get data from FD and attempt to parse with SUMD libs */
uint8_t sumd_rssi, sumd_rx_count;
uint16_t sumd_channel_count = 0;
*sumd_updated = false;
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
sumd_rssi = RC_INPUT_RSSI_MAX;
*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
}
if (*sumd_updated) {
/* not setting RSSI since SUMD does not provide one */
r_raw_rc_count = sumd_channel_count;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
return (*dsm_updated | *st24_updated | *sumd_updated);
}
