fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:38:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
internal PX4IO safety_off state is removed and replaced with a normal safety button event. From this 'commit' commander is taking care of the PX4IO safety.
This commit is contained in:
Igor Mišić
2022-06-01 13:15:13 -04:00
committed by Daniel Agar
parent a9cdfff7a3
commit 25488da944
27 changed files with 68 additions and 193 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+30 -104
View File
@@ -74,6 +74,7 @@
#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>
@@ -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("<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");