diff --git a/src/drivers/uavcannode/Publishers/SafetyButton.hpp b/src/drivers/uavcannode/Publishers/SafetyButton.hpp index 69fad19e2a..136cd27c45 100644 --- a/src/drivers/uavcannode/Publishers/SafetyButton.hpp +++ b/src/drivers/uavcannode/Publishers/SafetyButton.hpp @@ -38,7 +38,7 @@ #include #include -#include +#include namespace uavcannode { @@ -51,7 +51,7 @@ class SafetyButton : public: SafetyButton(px4::WorkItem *work_item, uavcan::INode &node) : UavcanPublisherBase(ardupilot::indication::Button::DefaultDataTypeID), - uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(safety)), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(safety_button)), uavcan::Publisher(node) { this->setPriority(uavcan::TransferPriority::Default); @@ -69,14 +69,13 @@ public: void BroadcastAnyUpdates() override { - // safety -> standard::indication::button - safety_s safety; + button_event_s safety_button; - if (uORB::SubscriptionCallbackWorkItem::update(&safety)) { - if (safety.safety_switch_available) { + if (uORB::SubscriptionCallbackWorkItem::update(&safety_button)) { + if (safety_button.triggered) { ardupilot::indication::Button Button{}; Button.button = ardupilot::indication::Button::BUTTON_SAFETY; - Button.press_time = safety.safety_off ? UINT8_MAX : 0; + Button.press_time = UINT8_MAX; uavcan::Publisher::broadcast(Button); } }