Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: support safety buttons on UAVCAN

Browse Source 
the button works in parallel with any button attached by IOMCU or by a
pin, and obeys all the same BRD_SAFETY* options
This commit is contained in:
Andrew Tridgell 2019-09-03 12:25:39 +10:00
parent 03c1a8bfd9
commit 21cce1385a
2 changed files with 40 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -37,6 +37,7 @@
#include <uavcan/equipment/indication/BeepCommand.hpp> #include <uavcan/equipment/indication/BeepCommand.hpp>
#include <uavcan/equipment/indication/RGB565.hpp> #include <uavcan/equipment/indication/RGB565.hpp>
#include <ardupilot/indication/SafetyState.hpp> #include <ardupilot/indication/SafetyState.hpp>
#include <ardupilot/indication/Button.hpp>
#include <AP_Baro/AP_Baro_UAVCAN.h> #include <AP_Baro/AP_Baro_UAVCAN.h>
#include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h> #include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
@ -103,6 +104,11 @@ static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[
static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[MAX_NUMBER_OF_CAN_DRIVERS];
// subscribers
// handler SafteyButton
UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button);
static uavcan::Subscriber<ardupilot::indication::Button, ButtonCb> *safety_button_listener[MAX_NUMBER_OF_CAN_DRIVERS];
AP_UAVCAN::AP_UAVCAN() : AP_UAVCAN::AP_UAVCAN() :
_node_allocator() _node_allocator()
{ {
@ -234,6 +240,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
safety_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); safety_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
safety_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); safety_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
if (safety_button_listener[driver_index]) {
safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button));
}
_led_conf.devices_count = 0; _led_conf.devices_count = 0;
if (enable_filters) { if (enable_filters) {
configureCanAcceptanceFilters(*_node); configureCanAcceptanceFilters(*_node);
@ -536,4 +547,25 @@ void AP_UAVCAN::safety_state_send()
safety_state[_driver_index]->broadcast(msg); 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 #endif // HAL_WITH_UAVCAN

7
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -46,6 +46,9 @@
#define AP_UAVCAN_MAX_LED_DEVICES 4 #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, 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. 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 uint8_t pending_mask; // mask of interfaces to send to
} _buzzer; } _buzzer;
// safety status send state
uint32_t _last_safety_state_ms; 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_ */ #endif /* AP_UAVCAN_H_ */