diff options
| -rw-r--r-- | apps/px4io/comms.c | 5 | ||||
| -rw-r--r-- | apps/px4io/controls.c | 7 | ||||
| -rw-r--r-- | apps/px4io/mixer.c | 19 | ||||
| -rw-r--r-- | apps/px4io/px4io.h | 4 |
4 files changed, 15 insertions, 20 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 0fcf952ab..9a786234e 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -183,7 +183,7 @@ comms_handle_command(const void *buffer, size_t length) system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; system_state.manual_override_ok = cmd->manual_override_ok; system_state.mixer_fmu_available = true; - system_state.fmu_data_received = true; + system_state.fmu_data_received_time = hrt_absolute_time(); /* set PWM update rate if changed (after limiting) */ uint16_t new_servo_rate = cmd->servo_rate; @@ -201,6 +201,9 @@ comms_handle_command(const void *buffer, size_t length) system_state.servo_rate = new_servo_rate; } + /* update servo values immediately */ + mixer_tick(); + /* XXX do relay changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { system_state.relays[i] = cmd->relay_state[i]; diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 9db9a0fa5..4bd6fe1a0 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -61,8 +61,8 @@ #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 1600 -#define RC_CHANNEL_LOW_THRESH 1400 +#define RC_CHANNEL_HIGH_THRESH 1700 +#define RC_CHANNEL_LOW_THRESH 1300 static void ppm_input(void); @@ -133,8 +133,7 @@ controls_main(void) system_state.fmu_report_due = true; } - /* do PWM output updates */ - mixer_tick(); + /* PWM output updates are performed directly on each comms update */ } } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 5a644906b..318183ef5 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -53,10 +53,9 @@ #include "px4io.h" /* - * Count of periodic calls in which we have no FMU input. + * Maximum interval in us before FMU signal is considered lost */ -static unsigned fmu_input_drops; -#define FMU_INPUT_DROP_LIMIT 20 +#define FMU_INPUT_DROP_LIMIT_US 200000 /* * Update a mixer based on the current control signals. @@ -91,17 +90,11 @@ mixer_tick(void) control_values = &system_state.fmu_channel_data[0]; /* check that we are receiving fresh data from the FMU */ - if (!system_state.fmu_data_received) { - fmu_input_drops++; + 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 */ - if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) { - system_state.mixer_manual_override = true; - system_state.mixer_fmu_available = false; - } - } else { - fmu_input_drops = 0; - system_state.fmu_data_received = false; + system_state.mixer_manual_override = true; + system_state.mixer_fmu_available = false; } } else if (system_state.rc_channels > 0 && system_state.manual_override_ok) { @@ -109,7 +102,7 @@ mixer_tick(void) control_count = system_state.rc_channels; control_values = &system_state.rc_channel_data[0]; } else { - /* we have no control input */ + /* we have no control input (no FMU, no RC) */ // XXX builtin failsafe would activate here control_count = 0; diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 84f5ba029..c8699c6c6 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -106,9 +106,9 @@ struct sys_state_s bool fmu_report_due; /** - * If true, new control data from the FMU has been received. + * Last FMU receive time, in microseconds since system boot */ - bool fmu_data_received; + uint64_t fmu_data_received_time; /** * Current serial interface mode, per the serial_rx_mode parameter |