diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5422101bcc..9e668de4c2 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -133,11 +133,13 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *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 (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SUMD))) { + 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) { @@ -161,11 +163,13 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *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, &sumd_failsafe_state)); + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24))) { + 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, &sumd_failsafe_state)); + } } if (*sumd_updated) { @@ -263,35 +267,39 @@ controls_tick() perf_begin(c_gather_sbus); #endif - bool sbus_failsafe, sbus_frame_drop; - bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, - PX4IO_RC_INPUT_CHANNELS); + bool sbus_updated = false; - if (sbus_updated) { - atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS); + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24 | PX4IO_P_STATUS_FLAGS_RC_SUMD))) { + bool sbus_failsafe, sbus_frame_drop; + sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + PX4IO_RC_INPUT_CHANNELS); - unsigned sbus_rssi = RC_INPUT_RSSI_MAX; + if (sbus_updated) { + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS); - if (sbus_frame_drop) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; - sbus_rssi = RC_INPUT_RSSI_MAX / 2; + unsigned sbus_rssi = RC_INPUT_RSSI_MAX; + + if (sbus_frame_drop) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; + sbus_rssi = RC_INPUT_RSSI_MAX / 2; + + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + } + + if (sbus_failsafe) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + + /* set RSSI to an emulated value if ADC RSSI is off */ + if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + _rssi = sbus_rssi; + } - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } - - if (sbus_failsafe) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; - - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - } - - /* set RSSI to an emulated value if ADC RSSI is off */ - if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { - _rssi = sbus_rssi; - } - } #if defined(PX4IO_PERF) @@ -321,7 +329,7 @@ controls_tick() bool dsm_updated = false, st24_updated = false, sumd_updated = false; - if (!((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) || (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM))) { + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_SBUS | PX4IO_P_STATUS_FLAGS_RC_PPM))) { #if defined(PX4IO_PERF) perf_begin(c_gather_dsm); #endif