diff options
Diffstat (limited to 'apps/px4io')
-rw-r--r-- | apps/px4io/comms.c | 10 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 14 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 3 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 2 | ||||
-rw-r--r-- | apps/px4io/safety.c | 4 |
5 files changed, 20 insertions, 13 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 1edff25b1..0a0453b55 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -148,13 +148,19 @@ 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(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.fmu_data_received = true; + /* handle changes signalled by FMU */ - if (!system_state.arm_ok && system_state.armed) - system_state.armed = false; +// if (!system_state.arm_ok && system_state.armed) +// system_state.armed = false; /* XXX do relay changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 471965fd7..28edb2357 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -93,7 +93,7 @@ static void mixer_get_rc_input(void); static void mixer_update(int mixer, uint16_t *inputs, int input_count); /* current servo arm/disarm state */ -bool mixer_servos_armed; +bool mixer_servos_armed = false; /* * Each mixer consumes a set of inputs and produces a single output. @@ -182,10 +182,10 @@ mixer_tick(void *arg) system_state.fmu_data_received = false; } - } else if (system_state.rc_channels > 0) { - /* we have control data from an R/C input */ - control_count = system_state.rc_channels; - control_values = &system_state.rc_channel_data[0]; +// } else if (system_state.rc_channels > 0) { +// /* we have control data from an R/C input */ +// control_count = system_state.rc_channels; +// control_values = &system_state.rc_channel_data[0]; } else { /* we have no control input */ @@ -202,7 +202,7 @@ mixer_tick(void *arg) /* * If we are armed, update the servo output. */ - if (system_state.armed) + if (system_state.armed && system_state.arm_ok) up_pwm_servo_set(i, mixers[i].current_value); } } @@ -210,7 +210,7 @@ mixer_tick(void *arg) /* * Decide whether the servos should be armed right now. */ - should_arm = system_state.armed && (control_count > 0); + 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.c b/apps/px4io/px4io.c index 7039e5d58..7240a80ef 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -133,8 +133,9 @@ int user_start(int argc, char *argv[]) /* print some simple status */ if (timers[TIMER_STATUS_PRINT] == 0) { timers[TIMER_STATUS_PRINT] = 1000; - lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d \r", + lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d \r", cursor[cycle++ & 3], + (system_state.arm_ok ? "FMU_ARMED" : "FMU_SAFE"), (system_state.armed ? "ARMED" : "SAFE"), (system_state.rc_channels ? "RC OK" : "NO RC"), (system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"), diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 274b27ff3..8eb0c53b2 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -68,7 +68,7 @@ struct sys_state_s { - bool armed; /* actually armed */ + bool armed; /* IO armed */ bool arm_ok; /* FMU says OK to arm */ /* diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index f27432664..0ad636c0b 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -76,9 +76,9 @@ safety_check_button(void *arg) /* * Debounce the safety button, change state if it has been held for long enough. * - * Ignore the button if FMU has not said it's OK to arm yet. */ - if (BUTTON_SAFETY && system_state.arm_ok) { + + if (BUTTON_SAFETY) { if (arm_counter < ARM_COUNTER_THRESHOLD) { arm_counter++; } else if (arm_counter == ARM_COUNTER_THRESHOLD) { |