Revert "px4io: replace safety_off state with safety button event (#19558)"

This reverts commit 12a81979a8.
This commit is contained in:
Igor Misic 2022-05-19 15:27:08 +02:00 committed by Daniel Agar
parent 6aefcbb6cf
commit 554283655c
26 changed files with 190 additions and 69 deletions

View File

@ -19,7 +19,7 @@ bool status_rc_ppm
bool status_rc_sbus
bool status_rc_st24
bool status_rc_sumd
bool status_safety_button_event # px4io safety button was pressed for longer than 1 second
bool status_safety_off
# PX4IO alarms (PX4IO_P_STATUS_ALARMS)
bool alarm_pwm_error

View File

@ -188,6 +188,12 @@ typedef uint16_t servo_position_t;
/** get the maximum PWM value the output will send */
#define PWM_SERVO_GET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 19)
/** force safety switch off (to disable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _PX4_IOC(_PWM_SERVO_BASE, 25)
/** force safety switch on (to enable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 28)
/*
*
*

View File

@ -492,7 +492,9 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_ON:
case PWM_SERVO_ARM:
case PWM_SERVO_DISARM:
break;

View File

@ -717,6 +717,8 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
case PWM_SERVO_SET_FORCE_SAFETY_ON:
break;
case PWM_SERVO_DISARM:

View File

@ -74,7 +74,6 @@
#include <uORB/topics/input_rc.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/px4io_status.h>
#include <uORB/topics/parameter_update.h>
@ -197,8 +196,7 @@ private:
uint16_t _last_written_arming_c{0}; ///< the last written arming state reg
uORB::Subscription _t_actuator_armed{ORB_ID(actuator_armed)}; ///< system armed control topic
uORB::Subscription _t_vehicle_command{ORB_ID(vehicle_command)}; ///< vehicle command topic
uORB::Subscription _t_vehicle_status{ORB_ID(vehicle_status)}; ///< vehicle status topic
uORB::Subscription _t_vehicle_command{ORB_ID(vehicle_command)}; ///< vehicle command topic
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@ -445,14 +443,15 @@ int PX4IO::init()
// the startup script to be able to load a new IO
// firmware
// If IO has already safety off it won't accept going into bootloader mode,
// therefore we need to set safety on first.
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
// Now the reboot into bootloader mode should succeed.
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, PX4IO_REBOOT_BL_MAGIC);
return -1;
}
/* Set safety_off to false when FMU boot*/
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, 0);
if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) {
_max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS;
}
@ -477,6 +476,14 @@ int PX4IO::init()
return ret;
}
/* set safety to off if circuit breaker enabled */
if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
} else {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
}
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
@ -614,6 +621,11 @@ void PX4IO::Run()
update_params();
/* Check if the IO safety circuit breaker has been updated */
bool circuit_breaker_io_safety_enabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
/* Bypass IO safety switch logic by setting FORCE_SAFETY_OFF */
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, circuit_breaker_io_safety_enabled);
/* Check if the flight termination circuit breaker has been updated */
bool disable_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
/* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */
@ -962,12 +974,15 @@ int PX4IO::io_handle_status(uint16_t status)
*/
/* check for IO reset - force it back to armed if necessary */
if (!(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the arming flag */
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0,
PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
/* set new status */
_status = status;
_status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
@ -984,26 +999,18 @@ int PX4IO::io_handle_status(uint16_t status)
}
/**
* Get and handle the safety button status
* Get and handle the safety status
*/
const bool safety_button_pressed = status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
const bool safety_off = status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
if (safety_button_pressed) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0);
/* px4io board will change safety_off from false to true and stay like that until the vehicle is rebooted
* where safety will change back to false. Here we are triggering the safety button event once.
* TODO: change px4io firmware to act on the event. This will enable the "force safety on disarming" feature. */
if (_previous_safety_off != safety_off) {
_button_publisher.safetyButtonTriggerEvent();
}
/**
* Inform PX4IO board about safety_off state for LED control
*/
vehicle_status_s vehicle_status;
if (_t_vehicle_status.update(&vehicle_status)) {
if (_previous_safety_off != vehicle_status.safety_off) {
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off);
_previous_safety_off = vehicle_status.safety_off;
}
}
_previous_safety_off = safety_off;
return ret;
}
@ -1026,17 +1033,37 @@ int PX4IO::dsm_bind_ioctl(int dsmMode)
return -EINVAL;
}
// Check if safety was off
bool safety_off = (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
int ret = -1;
// Put safety on
if (safety_off) {
// re-enable safety
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, PX4IO_P_STATUS_FLAGS_SAFETY_OFF, 0);
// set new status
_status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
}
PX4_INFO("Binding DSM%s RX", (dsmMode == DSM2_BIND_PULSES) ? "2" : ((dsmMode == DSMX_BIND_PULSES) ? "-X" : "-X8"));
int ret = OK;
ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
px4_usleep(500000);
ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
px4_usleep(72000);
ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4));
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4));
px4_usleep(50000);
ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
ret = OK;
// Put safety back off
if (safety_off) {
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0,
PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
_status |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
}
if (ret != OK) {
PX4_INFO("Binding DSM failed");
@ -1106,7 +1133,7 @@ int PX4IO::io_get_status()
status.status_arm_sync = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_ARM_SYNC;
status.status_init_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_INIT_OK;
status.status_failsafe = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FAILSAFE;
status.status_safety_button_event = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
status.status_safety_off = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
// PX4IO_P_STATUS_ALARMS
status.alarm_rc_lost = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST;
@ -1617,6 +1644,18 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
*(unsigned *)arg = _max_actuators;
break;
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_OFF");
/* force safety swith off */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_SAFETY_ON:
PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_ON");
/* force safety switch on */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
break;
case DSM_BIND_START:
/* bind a DSM receiver */
ret = dsm_bind_ioctl(arg);
@ -1686,6 +1725,16 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
// re-enable safety
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
if (ret != PX4_OK) {
PX4_WARN("IO refused to re-enable safety");
}
// set new status
_status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
usleep(1);
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);
@ -2009,6 +2058,29 @@ int PX4IO::custom_command(int argc, char *argv[])
return 1;
}
if (!strcmp(verb, "safety_off")) {
int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
if (ret != OK) {
PX4_ERR("failed to disable safety (%i)", ret);
return 1;
}
return 0;
}
if (!strcmp(verb, "safety_on")) {
int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
if (ret != OK) {
PX4_ERR("failed to enable safety (%i)", ret);
return 1;
}
return 0;
}
if (!strcmp(verb, "debug")) {
if (argc <= 1) {
PX4_ERR("usage: px4io debug LEVEL");
@ -2090,6 +2162,8 @@ Output driver communicating with the IO co-processor.
PRINT_MODULE_USAGE_ARG("<filename>", "Firmware file", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("update", "Update IO firmware");
PRINT_MODULE_USAGE_ARG("<filename>", "Firmware file", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("safety_off", "Turn off safety (force)");
PRINT_MODULE_USAGE_COMMAND_DESCR("safety_on", "Turn on safety (force)");
PRINT_MODULE_USAGE_COMMAND_DESCR("debug", "set IO debug level");
PRINT_MODULE_USAGE_ARG("<debug_level>", "0=disabled, 9=max verbosity", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind");

View File

@ -951,6 +951,8 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
switch (cmd) {
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
// these are no-ops, as no safety switch
break;
case MIXERIOCRESET:

View File

@ -927,7 +927,7 @@ PARAM_DEFINE_INT32(COM_PREARM_MODE, 0);
/**
* Enable force safety
*
* Force safety when the vehicle disarms
* Force safety when the vehicle disarms. Not supported when safety button used over PX4IO board.
*
* @boolean
* @group Commander

View File

@ -187,6 +187,13 @@ static int do_esc_calibration_ioctl(orb_advert_t *mavlink_log_pub)
goto Out;
}
/* tell IO to switch off safety without using the safety switch */
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
return_code = PX4_ERROR;
goto Out;
}
calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");
timeout_start = hrt_absolute_time();
@ -226,6 +233,9 @@ static int do_esc_calibration_ioctl(orb_advert_t *mavlink_log_pub)
Out:
if (fd != -1) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off");
}
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed");

View File

@ -39,7 +39,7 @@ add_library(px4iofirmware
mixer.cpp
px4io.cpp
registers.c
safety_button.cpp
safety.cpp
serial.cpp
)

View File

@ -134,11 +134,13 @@ mixer_tick()
* FMU or from the mixer.
*
*/
should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
should_arm = (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_ARMED) /* and FMU is armed */
;
should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
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_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */
|| (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
));

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -114,7 +114,6 @@
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 11) /* FMU was initialized and OK once */
#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 12) /* ST24 input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 13) /* SUMD input is valid */
#define PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT (1 << 14) /* px4io safety button was pressed for longer than 1 second */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 0) /* timed out waiting for RC input */
@ -185,8 +184,12 @@ enum { /* DSM bind states */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
/* storage space of 12 occupied by CRC */
#define PX4IO_P_SETUP_SAFETY_BUTTON_ACK 14 /**< ACK from FMU when it gets safety button pressed status */
#define PX4IO_P_SETUP_SAFETY_OFF 15 /**< FMU inform PX4IO about safety_off for LED indication*/
#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into
'armed' (PWM enabled) state - this is a non-data write and
hence index 12 can safely be used. */
#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */
#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
#define PX4IO_P_SETUP_SBUS_RATE 16 /**< frame rate of SBUS1 output in Hz */
#define PX4IO_P_SETUP_THERMAL 17 /**< thermal management */
#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 18 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
@ -209,7 +212,7 @@ enum { /* DSM bind states */
#define PX4IO_PAGE_TEST 127
#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */
/* PWM disarmed values that are active */
/* PWM disarmed values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**

View File

@ -154,7 +154,8 @@ show_debug_messages(void)
static void
update_mem_usage(void)
{
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
return;
}
@ -182,7 +183,8 @@ ring_blink(void)
{
#if defined(LED_GREEN)
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
LED_GREEN(true);
return;
}
@ -312,8 +314,8 @@ extern "C" __EXPORT int user_start(int argc, char *argv[])
ENABLE_SBUS_OUT(false);
#endif
/* start the safety button handler */
safety_button_init();
/* start the safety switch handler */
safety_init();
/* initialise the control inputs */
controls_init();

View File

@ -131,6 +131,8 @@ extern struct sys_state_s system_state;
# define ADC_VSERVO 4
# define ADC_RSSI 5
#define BUTTON_SAFETY px4_arch_gpioread(GPIO_BTN_SAFETY)
#define PX4_CRITICAL_SECTION(cmd) { irqstate_t flags = px4_enter_critical_section(); cmd; px4_leave_critical_section(flags); }
void atomic_modify_or(volatile uint16_t *target, uint16_t modification);
@ -143,9 +145,9 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification);
extern void mixer_tick(void);
/**
* Safety button/LED.
* Safety switch/LED.
*/
extern void safety_button_init(void);
extern void safety_init(void);
extern void failsafe_led_init(void);
/**

View File

@ -413,7 +413,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break;
case PX4IO_P_SETUP_REBOOT_BL:
if (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) {
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
// don't allow reboot while armed
break;
}
@ -433,19 +433,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
dsm_bind(value & 0x0f, (value >> 4) & 0xF);
break;
case PX4IO_P_SETUP_SAFETY_BUTTON_ACK:
// clear safety button pressed flag so it can be used again
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
case PX4IO_P_SETUP_FORCE_SAFETY_ON:
if (value == PX4IO_FORCE_SAFETY_MAGIC) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
} else {
return -1;
}
break;
case PX4IO_P_SETUP_SAFETY_OFF:
if (value) {
case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
if (value == PX4IO_FORCE_SAFETY_MAGIC) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
return -1;
}
break;

View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file safety_button.cpp
* @file safety.cpp
* Safety button logic.
*
* @author Lorenz Meier <lorenz@px4.io>
@ -46,7 +46,7 @@
#include "px4io.h"
static struct hrt_call safety_button_call;
static struct hrt_call arming_call;
static struct hrt_call failsafe_call;
/*
@ -59,23 +59,30 @@ static unsigned counter = 0;
* Define the various LED flash sequences for each system state.
*/
#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */
#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */
#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */
#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */
#define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */
#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */
static unsigned blink_counter = 0;
#define SAFETY_SWITCH_THRESHOLD 10
/*
* IMPORTANT: The arming state machine critically
* depends on using the same threshold
* for arming and disarming. Since disarming
* is quite deadly for the system, a similar
* length can be justified.
*/
#define ARM_COUNTER_THRESHOLD 10
static void safety_button_check(void *arg);
static void safety_check_button(void *arg);
static void failsafe_blink(void *arg);
void
safety_button_init(void)
safety_init(void)
{
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&safety_button_call, 1000, 100000, safety_button_check, NULL);
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
}
void
@ -86,20 +93,26 @@ failsafe_led_init(void)
}
static void
safety_button_check(void *arg)
safety_check_button(void *arg)
{
const bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);
const bool safety_button_pressed = BUTTON_SAFETY;
/* Keep safety button pressed for one second to trigger safety button event.
* The logic to prevent turning on safety again is in the commander.
/* Keep safety button pressed for one second to turn off safety
*
* Note that safety cannot be turned on again by button because a button
* hardware problem could accidentally disable it in flight.
*/
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT)) {
if (counter <= ARM_COUNTER_THRESHOLD) {
counter++;
counter++;
}
if (counter >= SAFETY_SWITCH_THRESHOLD) {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT);
if (counter == ARM_COUNTER_THRESHOLD) {
// switch safety off -> ready to arm state
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
}
} else {