diff options
Diffstat (limited to 'apps/px4io')
-rw-r--r-- | apps/px4io/mixer.c | 53 |
1 files changed, 33 insertions, 20 deletions
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 135d97bb3..9cdb98e23 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -84,32 +84,45 @@ mixer_tick(void) int i; bool should_arm; + /* check that we are receiving fresh data from the FMU */ + if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + /* too many frames without FMU input, time to go to failsafe */ + system_state.mixer_manual_override = true; + system_state.mixer_fmu_available = false; + lib_lowprintf("\nRX timeout\n"); + } + /* * Decide which set of inputs we're using. */ - if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { - /* we have recent control data from the FMU */ - control_count = PX4IO_OUTPUT_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - - /* check that we are receiving fresh data from the FMU */ - if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { - - /* too many frames without FMU input, time to go to failsafe */ - system_state.mixer_manual_override = true; - system_state.mixer_fmu_available = false; - lib_lowprintf("\nRX timeout\n"); + /* this is for planes, where manual override makes sense */ + if(system_state.manual_override_ok) { + /* if everything is ok */ + if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { + /* we have recent control data from the FMU */ + control_count = PX4IO_OUTPUT_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* when override is on or the fmu is not available */ + } else if (system_state.rc_channels > 0) { + control_count = system_state.rc_channels; + control_values = &system_state.rc_channel_data[0]; + } else { + /* we have no control input (no FMU, no RC) */ + + // XXX builtin failsafe would activate here + control_count = 0; } - } else if (system_state.rc_channels > 0 && system_state.manual_override_ok) { - /* we have control data from an R/C input */ - control_count = system_state.rc_channels; - control_values = &system_state.rc_channel_data[0]; + /* this is for multicopters, etc. where manual override does not make sense */ } else { - /* we have no control input (no FMU, no RC) */ - - // XXX builtin failsafe would activate here - control_count = 0; + /* if the fmu is available whe are good */ + if(system_state.mixer_fmu_available) { + control_count = PX4IO_OUTPUT_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* we better shut everything off */ + } else { + control_count = 0; + } } /* |