diff options
Diffstat (limited to 'src/modules/px4iofirmware/controls.c')
| -rw-r--r-- | src/modules/px4iofirmware/controls.c | 42 |
1 files changed, 35 insertions, 7 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index d20f776d6..e04ffc940 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -45,6 +45,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> #include <rc/st24.h> +#include <rc/sumd.h> #include "px4io.h" @@ -53,7 +54,7 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); -static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated); +static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -63,7 +64,7 @@ static int _dsm_fd; static uint16_t rc_value_override = 0; -bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) +bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; @@ -106,7 +107,31 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } - return (*dsm_updated | *st24_updated); + + /* 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 */ + st24_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) { + + *rssi = sumd_rssi; + 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); } void @@ -169,14 +194,17 @@ controls_tick() { #endif perf_begin(c_gather_dsm); - bool dsm_updated, st24_updated; - (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); + bool dsm_updated, st24_updated, sumd_updated; + (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); if (dsm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; } if (st24_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; } + if (sumd_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -238,7 +266,7 @@ controls_tick() { /* * If we received a new frame from any of the RC sources, process it. */ - if (dsm_updated || sbus_updated || ppm_updated || st24_updated) { + if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) { /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; @@ -438,7 +466,7 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated || st24_updated) + if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) mixer_tick(); } else { |