From 21cce1385a4d8d405523e50581b984d43e6bfa40 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 3 Sep 2019 12:25:39 +1000 Subject: [PATCH] AP_UAVCAN: support safety buttons on UAVCAN the button works in parallel with any button attached by IOMCU or by a pin, and obeys all the same BRD_SAFETY* options --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 34 ++++++++++++++++++++++++++++++- libraries/AP_UAVCAN/AP_UAVCAN.h | 7 +++++++ 2 files changed, 40 insertions(+), 1 deletion(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index caf9af4b7d..49ca37fffe 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -103,6 +104,11 @@ static uavcan::Publisher* rgb_led[ static uavcan::Publisher* buzzer[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher* safety_state[MAX_NUMBER_OF_CAN_DRIVERS]; +// subscribers +// handler SafteyButton +UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button); +static uavcan::Subscriber *safety_button_listener[MAX_NUMBER_OF_CAN_DRIVERS]; + AP_UAVCAN::AP_UAVCAN() : _node_allocator() { @@ -233,7 +239,12 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) safety_state[driver_index] = new uavcan::Publisher(*_node); safety_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); safety_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); - + + safety_button_listener[driver_index] = new uavcan::Subscriber(*_node); + if (safety_button_listener[driver_index]) { + safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button)); + } + _led_conf.devices_count = 0; if (enable_filters) { configureCanAcceptanceFilters(*_node); @@ -536,4 +547,25 @@ void AP_UAVCAN::safety_state_send() safety_state[_driver_index]->broadcast(msg); } +/* + handle Button message + */ +void AP_UAVCAN::handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb) +{ + switch (cb.msg->button) { + case ardupilot::indication::Button::BUTTON_SAFETY: { + AP_BoardConfig *brdconfig = AP_BoardConfig::get_singleton(); + if (brdconfig && brdconfig->safety_button_handle_pressed(cb.msg->press_time)) { + AP_HAL::Util::safety_state state = hal.util->safety_switch_state(); + if (state == AP_HAL::Util::SAFETY_ARMED) { + hal.rcout->force_safety_on(); + } else { + hal.rcout->force_safety_off(); + } + } + break; + } + } +} + #endif // HAL_WITH_UAVCAN diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 323dd57313..33103bf5e7 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -46,6 +46,9 @@ #define AP_UAVCAN_MAX_LED_DEVICES 4 +// fwd-declare callback classes +class ButtonCb; + /* Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received, the Callback will invoke registery to register the node as separate backend. @@ -207,7 +210,11 @@ private: uint8_t pending_mask; // mask of interfaces to send to } _buzzer; + // safety status send state uint32_t _last_safety_state_ms; + + // safety button handling + static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb); }; #endif /* AP_UAVCAN_H_ */