From 0b51053881bc3eaf7937e30287c8932487580307 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Wed, 21 Aug 2019 20:38:08 +0200 Subject: [PATCH 1/3] Add parameter COM_PREARM_MODE Condition to enter the prearmed state, an intermediate state between disarmed and armed * in which non-throttling actuators are active. * * @value 0 Disabled * @value 1 Safety button * @value 2 Always --- src/modules/commander/Commander.cpp | 35 ++++++++++++++++++------ src/modules/commander/Commander.hpp | 10 ++++++- src/modules/commander/commander_params.c | 14 ++++++++++ 3 files changed, 49 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e0689e105283..99d2637194c8 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2333,18 +2333,35 @@ Commander::run() status.timestamp = hrt_absolute_time(); _status_pub.publish(status); - /* set prearmed state if safety is off, or safety is not present and 5 seconds passed */ - if (safety.safety_switch_available) { - - /* safety is off, go into prearmed */ - armed.prearmed = safety.safety_off; + switch ((PrearmedMode)_param_com_prearm_mode.get()) { + case PrearmedMode::DISABLED: + /* skip prearmed state */ + armed.prearmed = false; + break; - } else { + case PrearmedMode::ALWAYS: /* safety is not present, go into prearmed - * (all output drivers should be started / unlocked last in the boot process - * when the rest of the system is fully initialized) - */ + * (all output drivers should be started / unlocked last in the boot process + * when the rest of the system is fully initialized) + */ armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5_s); + break; + + case PrearmedMode::SAFETY_BUTTON: + if (safety.safety_switch_available) { + + /* safety is off, go into prearmed */ + armed.prearmed = safety.safety_off; + + } else { + /* safety is not present, do not go into prearmed */ + armed.prearmed = false; + } + break; + + default: + armed.prearmed = false; + break; } armed.timestamp = hrt_absolute_time(); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index b4f8901dba9e..5fad5ce8084b 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -140,10 +140,18 @@ class Commander : public ModuleBase, public ModuleParams (ParamFloat) _param_com_of_loss_t, (ParamInt) _param_com_obl_act, - (ParamInt) _param_com_obl_rc_act + (ParamInt) _param_com_obl_rc_act, + + (ParamInt) _param_com_prearm_mode ) + enum class PrearmedMode { + DISABLED = 0, + SAFETY_BUTTON = 1, + ALWAYS = 2 + }; + const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */ const int64_t POSVEL_PROBATION_MAX = 100_s; /**< maximum probation duration (usec) */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index c8656f7c56a2..4f884f4edcf5 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -953,3 +953,17 @@ PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0); * @boolean */ PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 1); + +/** + * Condition to enter prearmed mode + * + * Condition to enter the prearmed state, an intermediate state between disarmed and armed + * in which non-throttling actuators are active. + * + * @value 0 Disabled + * @value 1 Safety button + * @value 2 Always + * + * @group Commander + */ +PARAM_DEFINE_INT32(COM_PREARM_MODE, 1); From 755f76425ef441f51a7516d5243a35fb570ed6a2 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sun, 25 Aug 2019 13:44:45 +0200 Subject: [PATCH 2/3] IO: respect actuator_armed.prearmed flag --- src/drivers/px4io/px4io.cpp | 8 ++++++++ src/modules/px4iofirmware/mixer.cpp | 9 ++++++--- src/modules/px4iofirmware/protocol.h | 19 ++++++++++--------- src/modules/px4iofirmware/registers.c | 1 + 4 files changed, 25 insertions(+), 12 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b66c4a68c768..e61f291f58b7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1344,6 +1344,13 @@ PX4IO::io_set_arming_state() _armed = armed.armed; + if (armed.prearmed) { + set |= PX4IO_P_SETUP_ARMING_FMU_PREARMED; + + } else { + clear |= PX4IO_P_SETUP_ARMING_FMU_PREARMED; + } + if ((armed.lockdown || armed.manual_lockdown) && !_lockdown_override) { set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; _lockdown_override = true; @@ -2284,6 +2291,7 @@ PX4IO::print_status(bool extended_status) printf("arming 0x%04hx%s%s%s%s%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), + ((arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) ? " FMU_PREARMED" : " FMU_NOT_PREARMED"), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 117763586021..fa27df27ec37 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -211,9 +211,12 @@ mixer_tick(void) ); should_arm_nothrottle = ( - (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) /* and there is valid input via or mixer */ + (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ + && (((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */ + || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */ + ) ); should_always_enable_pwm = ( diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 08092bd3382e..3dde775ef617 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -182,15 +182,16 @@ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ #define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ -#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ -#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ -#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ -#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ -#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ -#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ -#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ -#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ -#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ +#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 3) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* use custom failsafe values, not 0 values of mixer */ +#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 7) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 8) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 9) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8cd9d909e115..1e43a5fb80ed 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -191,6 +191,7 @@ volatile uint16_t r_page_setup[] = { PX4IO_P_SETUP_FEATURES_PWM_RSSI) #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ + PX4IO_P_SETUP_ARMING_FMU_PREARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ From f881c476882bc4e45a58902b326a9e4bee33701a Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 26 Aug 2019 15:02:22 +0200 Subject: [PATCH 3/3] Clarify comment --- src/modules/commander/Commander.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 99d2637194c8..09cda66d14ef 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2349,12 +2349,11 @@ Commander::run() case PrearmedMode::SAFETY_BUTTON: if (safety.safety_switch_available) { - - /* safety is off, go into prearmed */ + /* safety switch is present, go into prearmed if safety is off */ armed.prearmed = safety.safety_off; } else { - /* safety is not present, do not go into prearmed */ + /* safety switch is not present, do not go into prearmed */ armed.prearmed = false; } break;