diff options
Diffstat (limited to 'apps/px4io')
-rw-r--r-- | apps/px4io/comms.c | 4 | ||||
-rw-r--r-- | apps/px4io/controls.c | 21 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 5 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 31 | ||||
-rw-r--r-- | apps/px4io/safety.c | 2 |
5 files changed, 49 insertions, 14 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 40ea38cf7..f4eddbdd3 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -177,13 +177,13 @@ comms_handle_command(const void *buffer, size_t length) for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) system_state.fmu_channel_data[i] = cmd->servo_command[i]; - /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ + /* if the IO is armed and FMU gets disarmed, IO must also disarm */ if(system_state.arm_ok && !cmd->arm_ok) { system_state.armed = false; } system_state.arm_ok = cmd->arm_ok; - system_state.mixer_use_fmu = true; + system_state.mixer_fmu_available = true; system_state.fmu_data_received = true; diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index d4eace3df..0e0210373 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -59,6 +59,10 @@ #define DEBUG #include "px4io.h" +#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ +#define RC_CHANNEL_HIGH_THRESH 1600 +#define RC_CHANNEL_LOW_THRESH 1400 + void controls_main(void) { @@ -80,7 +84,22 @@ controls_main(void) if (fds[1].revents & POLLIN) sbus_input(); - /* XXX do ppm processing, bypass mode, etc. here */ + /* force manual input override */ + if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { + system_state.mixer_use_fmu = false; + } else { + /* override not engaged, use FMU */ + system_state.mixer_use_fmu = true; + } + + /* detect rc loss event */ + if (hrt_absolute_time() - system_state.rc_channels_timestamp > RC_FAILSAFE_TIMEOUT) { + system_state.rc_lost = true; + } + + /* XXX detect fmu loss event */ + + /* XXX handle failsave events - RC loss and FMU loss - here */ /* do PWM output updates */ mixer_tick(); diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 483e9fe4d..d9626c1e3 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -97,7 +97,7 @@ mixer_tick(void) /* * Decide which set of inputs we're using. */ - if (system_state.mixer_use_fmu) { + if (system_state.mixer_use_fmu && 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]; @@ -141,9 +141,10 @@ mixer_tick(void) /* * Decide whether the servos should be armed right now. + * A sufficient reason is armed state and either FMU or RC control inputs */ - should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu; + should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 483b9bcc8..9bfe3b1e4 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -76,43 +76,58 @@ struct sys_state_s bool dsm_input_ok; /* valid Spektrum DSM data */ bool sbus_input_ok; /* valid Futaba S.Bus data */ - /* + /** * Data from the remote control input(s) */ int rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; - /* + /** * Control signals from FMU. */ uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS]; - /* + /** * Relay controls */ bool relays[PX4IO_RELAY_CHANNELS]; - /* - * If true, we are using the FMU controls. + /** + * If true, we are using the FMU controls, else RC input if available. */ bool mixer_use_fmu; - /* + /** + * If true, FMU input is available. + */ + bool mixer_fmu_available; + + /** * If true, state that should be reported to FMU has been updated. */ bool fmu_report_due; - /* + /** * If true, new control data from the FMU has been received. */ bool fmu_data_received; - /* + /** * Current serial interface mode, per the serial_rx_mode parameter * in the config packet. */ uint8_t serial_rx_mode; + + /** + * If true, the RC signal has been lost for more than a timeout interval + */ + bool rc_lost; + + /** + * If true, the connection to FMU has been lost for more than a timeout interval + */ + bool fmu_lost; }; extern struct sys_state_s system_state; diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index d5bd103c1..916988af1 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -58,7 +58,7 @@ static struct hrt_call failsafe_call; * Count the number of times in a row that we see the arming button * held down. */ -static unsigned counter; +static unsigned counter = 0; #define ARM_COUNTER_THRESHOLD 10 #define DISARM_COUNTER_THRESHOLD 2 |