diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index f65ffefe1c..9d10f1c8ba 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -230,24 +230,6 @@ controls_tick() rssi = 0; } - perf_begin(c_gather_dsm); - bool dsm_updated, st24_updated, sumd_updated; - (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); - - if (dsm_updated) { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_DSM); - } - - if (st24_updated) { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24); - } - - if (sumd_updated) { - PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); - } - - perf_end(c_gather_dsm); - perf_begin(c_gather_sbus); bool sbus_failsafe, sbus_frame_drop; @@ -300,6 +282,26 @@ controls_tick() perf_end(c_gather_ppm); + if (!((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) || (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM))) { + perf_begin(c_gather_dsm); + bool dsm_updated, st24_updated, sumd_updated; + (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); + + if (dsm_updated) { + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_DSM); + } + + if (st24_updated) { + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24); + } + + if (sumd_updated) { + PX4_ATOMIC_MODIFY_OR(r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); + } + + perf_end(c_gather_dsm); + } + /* limit number of channels to allowable data size */ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) { r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;