diff --git a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin index fc9066700f..b91f26a198 100755 Binary files a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin differ diff --git a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin index fc9066700f..b91f26a198 100755 Binary files a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin differ diff --git a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin index 25a00e2357..36128d00f4 100755 Binary files a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin and b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin differ diff --git a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin index 25a00e2357..36128d00f4 100755 Binary files a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin and b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin differ diff --git a/boards/mro/x21-777/extras/px4_io-v2_default.bin b/boards/mro/x21-777/extras/px4_io-v2_default.bin index 25a00e2357..36128d00f4 100755 Binary files a/boards/mro/x21-777/extras/px4_io-v2_default.bin and b/boards/mro/x21-777/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin index 25a00e2357..36128d00f4 100755 Binary files a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin index 25a00e2357..36128d00f4 100755 Binary files a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin index 25a00e2357..36128d00f4 100755 Binary files a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin index 25a00e2357..36128d00f4 100755 Binary files a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin index 25a00e2357..36128d00f4 100755 Binary files a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin index 25a00e2357..36128d00f4 100755 Binary files a/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin index 25a00e2357..36128d00f4 100755 Binary files a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin differ diff --git a/msg/px4io_status.msg b/msg/px4io_status.msg index df072e71c2..295c8fba66 100644 --- a/msg/px4io_status.msg +++ b/msg/px4io_status.msg @@ -19,7 +19,7 @@ bool status_rc_ppm bool status_rc_sbus bool status_rc_st24 bool status_rc_sumd -bool status_safety_off +bool status_safety_button_event # px4io safety button was pressed for longer than 1 second # PX4IO alarms (PX4IO_P_STATUS_ALARMS) bool alarm_pwm_error diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 87fc629f6f..43721f9cb0 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -188,12 +188,6 @@ 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) - /* * * diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 7e230fc567..c748d526e7 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -492,9 +492,7 @@ 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; diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 25b94c70d5..f32f579597 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -717,8 +717,6 @@ 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: diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ceb34bc1cd..bfcb3227da 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include @@ -196,7 +197,8 @@ 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_command{ORB_ID(vehicle_command)}; ///< vehicle command topic + uORB::Subscription _t_vehicle_status{ORB_ID(vehicle_status)}; ///< vehicle status topic uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -443,15 +445,14 @@ 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; } @@ -476,14 +477,6 @@ 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); @@ -621,11 +614,6 @@ 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 */ @@ -974,15 +962,12 @@ 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_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + if (!(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_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, 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)) { @@ -999,18 +984,26 @@ int PX4IO::io_handle_status(uint16_t status) } /** - * Get and handle the safety status + * Get and handle the safety button status */ - const bool safety_off = status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + const bool safety_button_pressed = status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT; - /* 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) { + if (safety_button_pressed) { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0); _button_publisher.safetyButtonTriggerEvent(); } - _previous_safety_off = safety_off; + /** + * 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; + } + } return ret; } @@ -1033,37 +1026,17 @@ 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")); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + int ret = OK; + ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); px4_usleep(500000); - 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); + 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); px4_usleep(72000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4)); + ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4)); px4_usleep(50000); - 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; - } + ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); if (ret != OK) { PX4_INFO("Binding DSM failed"); @@ -1133,7 +1106,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_off = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + status.status_safety_button_event = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT; // PX4IO_P_STATUS_ALARMS status.alarm_rc_lost = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST; @@ -1644,18 +1617,6 @@ 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); @@ -1725,16 +1686,6 @@ 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); @@ -2058,29 +2009,6 @@ 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"); @@ -2162,8 +2090,6 @@ Output driver communicating with the IO co-processor. PRINT_MODULE_USAGE_ARG("", "Firmware file", false); PRINT_MODULE_USAGE_COMMAND_DESCR("update", "Update IO firmware"); PRINT_MODULE_USAGE_ARG("", "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("", "0=disabled, 9=max verbosity", false); PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind"); diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index e1bfc231b0..cc87e8e070 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -951,8 +951,6 @@ 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: diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f1bb34c84c..c22c489987 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -927,7 +927,7 @@ PARAM_DEFINE_INT32(COM_PREARM_MODE, 0); /** * Enable force safety * - * Force safety when the vehicle disarms. Not supported when safety button used over PX4IO board. + * Force safety when the vehicle disarms * * @boolean * @group Commander diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 1f43cc6a4d..865017ac27 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -187,13 +187,6 @@ 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(); @@ -233,9 +226,6 @@ 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"); diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 6e4bcdee66..3df90ffdac 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -39,7 +39,7 @@ add_library(px4iofirmware mixer.cpp px4io.cpp registers.c - safety.cpp + safety_button.cpp serial.cpp ) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 2e0e5bb14f..9fc40dcbde 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -134,13 +134,11 @@ mixer_tick() * FMU or from the mixer. * */ - 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 */ + should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ && (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 */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ + should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ && ((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 */ )); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 73c82dae9d..a0389d78b4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 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,6 +114,7 @@ #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 */ @@ -184,12 +185,8 @@ 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_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_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_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 */ @@ -212,7 +209,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, even when SAFETY_SAFE */ +/* PWM disarmed values that are active */ #define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** diff --git a/src/modules/px4iofirmware/px4io.cpp b/src/modules/px4iofirmware/px4io.cpp index 74f6637f46..69527d7bf2 100644 --- a/src/modules/px4iofirmware/px4io.cpp +++ b/src/modules/px4iofirmware/px4io.cpp @@ -154,8 +154,7 @@ show_debug_messages(void) static void update_mem_usage(void) { - 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)) { + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return; } @@ -183,8 +182,7 @@ ring_blink(void) { #if defined(LED_GREEN) - 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)) { + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { LED_GREEN(true); return; } @@ -314,8 +312,8 @@ extern "C" __EXPORT int user_start(int argc, char *argv[]) ENABLE_SBUS_OUT(false); #endif - /* start the safety switch handler */ - safety_init(); + /* start the safety button handler */ + safety_button_init(); /* initialise the control inputs */ controls_init(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index c8872c8aa0..99cba83e67 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -131,8 +131,6 @@ 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); @@ -145,9 +143,9 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification); extern void mixer_tick(void); /** - * Safety switch/LED. + * Safety button/LED. */ -extern void safety_init(void); +extern void safety_button_init(void); extern void failsafe_led_init(void); /** diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index dfe0a6f8ec..a191577ef3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -413,10 +413,6 @@ 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_SAFETY_OFF) { - // don't allow reboot while armed - break; - } // check the magic value if (value != PX4IO_REBOOT_BL_MAGIC) { @@ -433,22 +429,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; - 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; - } + 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; break; - case PX4IO_P_SETUP_FORCE_SAFETY_OFF: - if (value == PX4IO_FORCE_SAFETY_MAGIC) { + case PX4IO_P_SETUP_SAFETY_OFF: + + if (value) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } else { - return -1; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } break; diff --git a/src/modules/px4iofirmware/safety.cpp b/src/modules/px4iofirmware/safety_button.cpp similarity index 79% rename from src/modules/px4iofirmware/safety.cpp rename to src/modules/px4iofirmware/safety_button.cpp index 1a5eedb7d7..6d4672d5f2 100644 --- a/src/modules/px4iofirmware/safety.cpp +++ b/src/modules/px4iofirmware/safety_button.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file safety.cpp + * @file safety_button.cpp * Safety button logic. * * @author Lorenz Meier @@ -46,7 +46,7 @@ #include "px4io.h" -static struct hrt_call arming_call; +static struct hrt_call safety_button_call; static struct hrt_call failsafe_call; /* @@ -59,30 +59,23 @@ 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; -/* - * 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 +#define SAFETY_SWITCH_THRESHOLD 10 -static void safety_check_button(void *arg); +static void safety_button_check(void *arg); static void failsafe_blink(void *arg); void -safety_init(void) +safety_button_init(void) { /* arrange for the button handler to be called at 10Hz */ - hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); + hrt_call_every(&safety_button_call, 1000, 100000, safety_button_check, NULL); } void @@ -93,26 +86,20 @@ failsafe_led_init(void) } static void -safety_check_button(void *arg) +safety_button_check(void *arg) { - const bool safety_button_pressed = BUTTON_SAFETY; + const bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY); - /* 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. + /* Keep safety button pressed for one second to trigger safety button event. + * The logic to prevent turning on safety again is in the commander. */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { - if (counter <= ARM_COUNTER_THRESHOLD) { - counter++; + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT)) { - } + counter++; - if (counter == ARM_COUNTER_THRESHOLD) { - // switch safety off -> ready to arm state - atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF); + if (counter >= SAFETY_SWITCH_THRESHOLD) { + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT); } } else {