fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 21:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

safety and safety button: refactoring #19413

Browse Source
This commit is contained in:
Igor Misic
2022-04-05 07:57:37 +02:00
committed by Beat Küng
parent 1f17a1470a
commit 80aef942cd
20 changed files with 446 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+13 -12
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;
}