dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

safety and safety button: refactoring #19413

This commit is contained in:
Igor Misic 2022-04-04 10:50:09 +02:00 committed by Beat Küng
parent 1f17a1470a
commit 80aef942cd
20 changed files with 446 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -52,6 +52,7 @@ set(msg_files
airspeed_wind.msg
autotune_attitude_control_status.msg
battery_status.msg
button_event.msg
camera_capture.msg
camera_status.msg
camera_trigger.msg

6
msg/button_event.msg Normal file
View File

@ -0,0 +1,6 @@
uint64 timestamp # time since system start (microseconds)
bool triggered # Set to true if the event is triggered
# TOPICS button_event safety_button
uint8 ORB_QUEUE_LENGTH = 2

1
src/drivers/px4io/CMakeLists.txt
View File

@ -43,6 +43,7 @@ px4_add_module(
module.yaml
DEPENDS
arch_px4io_serial
button_publisher
circuit_breaker
mixer_module
)

25
src/drivers/px4io/px4io.cpp
View File

@ -61,6 +61,7 @@
#include <lib/perf/perf_counter.h>
#include <lib/rc/dsm.h>
#include <lib/systemlib/mavlink_log.h>
#include <lib/button/ButtonPublisher.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
@ -71,7 +72,6 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/px4io_status.h>
@ -207,10 +207,10 @@ private:
/* advertised topics */
uORB::PublicationMulti<input_rc_s> _to_input_rc{ORB_ID(input_rc)};
uORB::PublicationMulti<safety_s> _to_safety{ORB_ID(safety)};
uORB::Publication<px4io_status_s> _px4io_status_pub{ORB_ID(px4io_status)};
safety_s _safety{};
ButtonPublisher _button_publisher;
bool _previous_safety_off{false};
bool _lockdown_override{false}; ///< override the safety lockdown
@ -477,6 +477,9 @@ int PX4IO::init()
/* 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 */
@ -998,17 +1001,15 @@ int PX4IO::io_handle_status(uint16_t status)
*/
const bool safety_off = status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
// publish immediately on change, otherwise at 1 Hz
if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s)
|| (_safety.safety_off != safety_off)) {
_safety.safety_switch_available = true;
_safety.safety_off = safety_off;
_safety.timestamp = hrt_absolute_time();
_to_safety.publish(_safety);
/* 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();
}
_previous_safety_off = safety_off;
return ret;
}

2
src/drivers/safety_button/CMakeLists.txt
View File

@ -37,6 +37,6 @@ px4_add_module(
SRCS
SafetyButton.cpp
DEPENDS
circuit_breaker
button_publisher
px4_work_queue
)

89
src/drivers/safety_button/SafetyButton.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 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
@ -31,7 +31,6 @@
*
****************************************************************************/
#include <drivers/drv_tone_alarm.h>
#include "SafetyButton.hpp"
#ifndef GPIO_BTN_SAFETY
@ -54,11 +53,7 @@ enum class LED_PATTERN : uint16_t {
SafetyButton::SafetyButton() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
_safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
if (_safety_disabled) {
_safety_btn_off = true;
}
_has_px4io = PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO);
}
SafetyButton::~SafetyButton()
@ -67,33 +62,22 @@ SafetyButton::~SafetyButton()
}
void
SafetyButton::CheckButton()
SafetyButton::CheckSafetyRequest(bool button_pressed)
{
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.
*/
if (safety_button_pressed && !_safety_btn_off) {
/* Keep button pressed for one second to turn off safety */
if (button_pressed) {
if (_button_counter <= CYCLE_COUNT) {
_button_counter++;
}
if (_button_counter == CYCLE_COUNT) {
// switch safety off -> ready to arm state
_safety_btn_off = true;
_button_publisher.safetyButtonTriggerEvent();
}
} else {
_button_counter = 0;
}
CheckPairingRequest(safety_button_pressed);
_safety_btn_prev_sate = safety_button_pressed;
}
void
@ -108,7 +92,7 @@ SafetyButton::CheckPairingRequest(bool button_pressed)
_pairing_button_counter = 0;
}
if (!_safety_btn_prev_sate && button_pressed) {
if (!_button_prev_sate && button_pressed) {
if (_pairing_start == 0) {
_pairing_start = now;
}
@ -116,29 +100,8 @@ SafetyButton::CheckPairingRequest(bool button_pressed)
++_pairing_button_counter;
}
if (_pairing_button_counter == 3) {
vehicle_command_s vcmd{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
vcmd.param1 = 10.f; // GCS pairing request handled by a companion.
vcmd.timestamp = hrt_absolute_time();
_to_command.publish(vcmd);
PX4_DEBUG("Sending GCS pairing request");
led_control_s led_control{};
led_control.led_mask = 0xff;
led_control.mode = led_control_s::MODE_BLINK_FAST;
led_control.color = led_control_s::COLOR_GREEN;
led_control.num_blinks = 1;
led_control.priority = 0;
led_control.timestamp = hrt_absolute_time();
_to_led_control.publish(led_control);
tune_control_s tune_control{};
tune_control.tune_id = tune_control_s::TUNE_ID_NOTIFY_POSITIVE;
tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
tune_control.timestamp = hrt_absolute_time();
_to_tune_control.publish(tune_control);
if (_pairing_button_counter == ButtonPublisher::PAIRING_BUTTON_EVENT_COUNT) {
_button_publisher.pairingButtonTriggerEvent();
// reset state
_pairing_start = 0;
_pairing_button_counter = 0;
@ -156,7 +119,7 @@ SafetyButton::FlashButton()
LED_PATTERN pattern = LED_PATTERN::FMU_REFUSE_TO_ARM;
// cycle the blink state machine
if (_safety_btn_off) {
if (_button_prev_sate) {
if (armed.armed) {
pattern = LED_PATTERN::IO_FMU_ARMED;
@ -190,25 +153,16 @@ SafetyButton::Run()
return;
}
CheckButton();
const bool button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);
// control safety switch LED & publish safety topic
if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) {
// control safety switch LED & safety button
if (!_has_px4io) {
FlashButton();
const bool safety_off = _safety_btn_off || _safety_disabled;
// publish immediately on change, otherwise at 1 Hz
if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s)
|| (_safety.safety_off != safety_off)) {
_safety.safety_switch_available = true;
_safety.safety_off = safety_off;
_safety.timestamp = hrt_absolute_time();
_to_safety.publish(_safety);
}
CheckSafetyRequest(button_pressed);
}
CheckPairingRequest(button_pressed);
_button_prev_sate = button_pressed;
}
int
@ -248,15 +202,6 @@ SafetyButton::custom_command(int argc, char *argv[])
return print_usage("unknown command");
}
int
SafetyButton::print_status()
{
PX4_INFO("Safety Disabled: %s", _safety_disabled ? "yes" : "no");
PX4_INFO("Safety State (from button): %s", _safety_btn_off ? "off" : "on");
return 0;
}
int
SafetyButton::print_usage(const char *reason)
{

43
src/drivers/safety_button/SafetyButton.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 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
@ -35,20 +35,10 @@
#include <float.h>
#include <circuit_breaker/circuit_breaker.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/led_control.h>
#include <uORB/topics/tune_control.h>
#include <button/ButtonPublisher.hpp>
class SafetyButton : public ModuleBase<SafetyButton>, public px4::ScheduledWorkItem
{
@ -65,34 +55,25 @@ public:
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::print_status() */
int print_status() override;
int Start();
private:
void Run() override;
void CheckButton();
void FlashButton();
void CheckSafetyRequest(bool button_pressed);
void CheckPairingRequest(bool button_pressed);
void FlashButton();
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
uORB::Publication<safety_s> _to_safety{ORB_ID(safety)};
uORB::Publication<vehicle_command_s> _to_command{ORB_ID(vehicle_command)};
uORB::Publication<led_control_s> _to_led_control{ORB_ID(led_control)};
uORB::Publication<tune_control_s> _to_tune_control{ORB_ID(tune_control)};
safety_s _safety{};
uint8_t _button_counter{0};
uint8_t _blink_counter{0};
bool _safety_disabled{false}; ///< circuit breaker to disable the safety button
bool _safety_btn_off{false}; ///< State of the safety button read from the HW button
bool _safety_btn_prev_sate{false}; ///< Previous state of the HW button
bool _has_px4io{false};
ButtonPublisher _button_publisher;
uint8_t _button_counter{0};
uint8_t _blink_counter{0};
bool _button_prev_sate{false}; ///< Previous state of the HW button
// Pairing request
hrt_abstime _pairing_start{0};
int _pairing_button_counter{0};
int _pairing_button_counter{0};
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
};

1
src/drivers/uavcan/CMakeLists.txt
View File

@ -174,6 +174,7 @@ px4_add_module(
DEPENDS
px4_uavcan_dsdlc
button_publisher
drivers_rangefinder
led
mixer

37
src/drivers/uavcan/sensors/safety_button.cpp
View File

@ -38,10 +38,10 @@
const char *const UavcanSafetyButtonBridge::NAME = "safety_button";
using namespace time_literals;
UavcanSafetyButtonBridge::UavcanSafetyButtonBridge(uavcan::INode &node) :
UavcanSensorBridgeBase("uavcan_safety_button",
ORB_ID(safety)), // TODO: either multiple publishers or `button_event` uORB topic
_sub_button(node)
UavcanSensorBridgeBase("uavcan_safety_button", ORB_ID(button_event)), _sub_button(node)
{ }
int UavcanSafetyButtonBridge::init()
@ -60,14 +60,33 @@ void UavcanSafetyButtonBridge::button_sub_cb(const
uavcan::ReceivedDataStructure<ardupilot::indication::Button> &msg)
{
bool is_safety = msg.button == ardupilot::indication::Button::BUTTON_SAFETY;
bool pressed = msg.press_time >= 10; // 0.1s increments
bool pressed = msg.press_time >= 10; // 0.1s increments (1s press time for safety button trigger event)
// Detect safety button trigger event
if (is_safety && pressed) {
safety_s safety = {};
safety.timestamp = hrt_absolute_time();
safety.safety_switch_available = true;
safety.safety_off = true;
publish(msg.getSrcNodeID().get(), &safety);
_button_publisher.safetyButtonTriggerEvent();
}
// Detect pairing button trigger event
if (is_safety) {
if (hrt_elapsed_time(&_start_timestamp) > 2_s) {
_start_timestamp = hrt_absolute_time();
_pairing_button_counter = 0u;
}
hrt_abstime press_time_us = (msg.press_time * 100 * 1000);
hrt_abstime elasped_time = hrt_elapsed_time(&_new_press_timestamp);
if (elasped_time > press_time_us) {
_pairing_button_counter++;
_new_press_timestamp = hrt_absolute_time();
}
if (_pairing_button_counter == ButtonPublisher::PAIRING_BUTTON_EVENT_COUNT) {
_button_publisher.pairingButtonTriggerEvent();
_start_timestamp = 0u;
}
}
}

8
src/drivers/uavcan/sensors/safety_button.hpp
View File

@ -34,7 +34,7 @@
#pragma once
#include "sensor_bridge.hpp"
#include <uORB/topics/safety.h>
#include "button/ButtonPublisher.hpp"
#include <ardupilot/indication/Button.hpp>
@ -61,6 +61,8 @@ private:
ButtonCbBinder;
uavcan::Subscriber<ardupilot::indication::Button, ButtonCbBinder> _sub_button;
ButtonPublisher _button_publisher;
uint8_t _pairing_button_counter{0u};
hrt_abstime _start_timestamp{0};
hrt_abstime _new_press_timestamp{0};
};

1
src/lib/CMakeLists.txt
View File

@ -35,6 +35,7 @@ add_subdirectory(airspeed)
add_subdirectory(avoidance)
add_subdirectory(battery)
add_subdirectory(bezier)
add_subdirectory(button)
add_subdirectory(cdev)
add_subdirectory(circuit_breaker)
add_subdirectory(collision_prevention)

84
src/lib/button/ButtonPublisher.cpp Normal file
View File

@ -0,0 +1,84 @@
/****************************************************************************
*
* Copyright (c) 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ButtonPublisher.cpp
*
* Library for button functionality.
*
*/
#include <button/ButtonPublisher.hpp>
using namespace time_literals;
ButtonPublisher::ButtonPublisher()
{
_safety_button_pub.advertise();
}
void ButtonPublisher::safetyButtonTriggerEvent()
{
button_event_s safety_button{};
safety_button.triggered = true;
safety_button.timestamp = hrt_absolute_time();
_safety_button_pub.publish(safety_button);
}
void ButtonPublisher::pairingButtonTriggerEvent()
{
vehicle_command_s vcmd{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
vcmd.param1 = 10.f; // GCS pairing request handled by a companion.
vcmd.timestamp = hrt_absolute_time();
_vehicle_command_pub.publish(vcmd);
PX4_DEBUG("Sending GCS pairing request");
led_control_s led_control{};
led_control.led_mask = 0xff;
led_control.mode = led_control_s::MODE_BLINK_FAST;
led_control.color = led_control_s::COLOR_GREEN;
led_control.num_blinks = 1;
led_control.priority = 0;
led_control.timestamp = hrt_absolute_time();
_led_control_pub.publish(led_control);
tune_control_s tune_control{};
tune_control.tune_id = tune_control_s::TUNE_ID_NOTIFY_POSITIVE;
tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
tune_control.timestamp = hrt_absolute_time();
_tune_control_pub.publish(tune_control);
}

77
src/lib/button/ButtonPublisher.hpp Normal file
View File

@ -0,0 +1,77 @@
/****************************************************************************
*
* Copyright (c) 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ButtonPublisher.hpp
*
* Library for button functionality.
*
*/
#pragma once
#include <drivers/drv_tone_alarm.h>
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/button_event.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/led_control.h>
#include <uORB/topics/tune_control.h>
class ButtonPublisher
{
public:
ButtonPublisher();
~ButtonPublisher() = default;
/**
* Function for publishing safety button trigger event
*/
void safetyButtonTriggerEvent();
/**
* Function for publishing pairing button trigger event
*/
void pairingButtonTriggerEvent();
static constexpr uint8_t PAIRING_BUTTON_EVENT_COUNT{3};
private:
uORB::Publication<button_event_s> _safety_button_pub{ORB_ID(safety_button)};
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<led_control_s> _led_control_pub{ORB_ID(led_control)};
uORB::Publication<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
};

34
src/lib/button/CMakeLists.txt Normal file
View File

@ -0,0 +1,34 @@
############################################################################
#
# Copyright (c) 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
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(button_publisher ButtonPublisher.cpp)

1
src/modules/commander/CMakeLists.txt
View File

@ -52,6 +52,7 @@ px4_add_module(
lm_fit.cpp
mag_calibration.cpp
rc_calibration.cpp
Safety.cpp
state_machine_helper.cpp
worker_thread.cpp
DEPENDS

6
src/modules/commander/Commander.cpp
View File

@ -784,6 +784,10 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_disarmed_by"), events::Log::Info,
"Disarmed by {1}", calling_reason);
if (_param_com_force_safety.get()) {
_safety_handler.enableSafety();
}
_status_changed = true;
} else if (arming_res == TRANSITION_DENIED) {
@ -2196,6 +2200,8 @@ Commander::run()
}
}
_safety_handler.safetyButtonHandler();
/* update safety topic */
const bool safety_updated = _safety_sub.updated();

4
src/modules/commander/Commander.hpp
View File

@ -36,6 +36,7 @@
/* Helper classes */
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
#include "failure_detector/FailureDetector.hpp"
#include "Safety.hpp"
#include "state_machine_helper.h"
#include "worker_thread.hpp"
@ -234,6 +235,7 @@ private:
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
(ParamBool<px4::params::COM_FORCE_SAFETY>) _param_com_force_safety,
(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
@ -399,6 +401,8 @@ private:
vehicle_status_s _status{};
vehicle_status_flags_s _status_flags{};
Safety _safety_handler{};
WorkerThread _worker_thread;
// Subscriptions

80
src/modules/commander/Safety.cpp Normal file
View File

@ -0,0 +1,80 @@
/****************************************************************************
*
* Copyright (c) 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Safety.cpp
*/
#include "Safety.hpp"
#include <circuit_breaker/circuit_breaker.h>
using namespace time_literals;
Safety::Safety()
{
/*
* Safety can be turned off with the CBRK_IO_SAFETY parameter.
*/
_safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
}
void Safety::safetyButtonHandler()
{
if (_safety_disabled) {
_safety.safety_switch_available = true;
_safety.safety_off = true;
} else {
button_event_s button_event;
while (_safety_button_sub.update(&button_event)) {
_safety.safety_switch_available = true;
_safety.safety_off |= button_event.triggered; // triggered safety button activates safety off
}
}
// publish immediately on change, otherwise at 1 Hz for logging
if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s) ||
(_safety.safety_off != _previous_safety_off)) {
_safety.timestamp = hrt_absolute_time();
_safety_pub.publish(_safety);
}
_previous_safety_off = _safety.safety_off;
}
void Safety::enableSafety()
{
_safety.safety_off = false;
}

64
src/modules/commander/Safety.hpp Normal file
View File

@ -0,0 +1,64 @@
/****************************************************************************
*
* Copyright (c) 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Safety.hpp
*/
#pragma once
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/button_event.h>
#include <uORB/topics/safety.h>
class Safety
{
public:
Safety();
~Safety() = default;
void safetyButtonHandler();
void enableSafety();
private:
uORB::Subscription _safety_button_sub{ORB_ID::safety_button};
uORB::Publication<safety_s> _safety_pub{ORB_ID(safety)};
safety_s _safety{};
bool _safety_disabled{false};
bool _previous_safety_off{false};
};

10
src/modules/commander/commander_params.c
View File

@ -978,6 +978,16 @@ PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 0);
*/
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.
*
* @boolean
* @group Commander
*/
PARAM_DEFINE_INT32(COM_FORCE_SAFETY, 0);
/**
* Enable Motor Testing
*