diff --git a/dsdl/uavcan/protocol/1.Panic.uavcan b/dsdl/uavcan/protocol/1.Panic.uavcan index 1cd934a579..297985f9b4 100644 --- a/dsdl/uavcan/protocol/1.Panic.uavcan +++ b/dsdl/uavcan/protocol/1.Panic.uavcan @@ -1,7 +1,7 @@ # # This message must never be published during normal operation. This message may # be published with high frequency (~10..100 Hz) to inform the network participants -# that the system has entered unrecoverable fault condition (e.g. mission critical +# that the system has encountered an unrecoverable fault (e.g. mission critical # node failure) and is not capable of further operation. # # Typical reaction to this message may include emergency motor/power shutdown, @@ -10,8 +10,9 @@ # interval no higher than MAX_INTERVAL_MS before undertaking any emergency actions. # -uint8 MIN_MESSAGES = 5 +uint8 MIN_MESSAGES = 3 uint16 MAX_INTERVAL_MS = 500 +uint16 BROADCASTING_INTERVAL_MS = 100 # Short description that would fit into single CAN frame -uint8[<=8] reason_text +uint8[<=7] reason_text diff --git a/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp b/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp new file mode 100644 index 0000000000..793517fcee --- /dev/null +++ b/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +namespace uavcan +{ + +class PanicBroadcaster : private Timer +{ + Publisher pub_; + protocol::Panic msg_; + + void publishOnce(); + + void handleTimerEvent(const TimerEvent&); + +public: + PanicBroadcaster(INode& node) + : Timer(node) + , pub_(node) + { + pub_.setTxTimeout(MonotonicDuration::fromMSec(protocol::Panic::BROADCASTING_INTERVAL_MS - 10)); + } + + void panic(const char* short_reason); + + void dontPanic(); // Where's my towel + + bool isPanicking() const; + + const typename protocol::Panic::FieldTypes::reason_text& getReason() const; +}; + +} diff --git a/libuavcan/src/protocol/panic_broadcaster.cpp b/libuavcan/src/protocol/panic_broadcaster.cpp new file mode 100644 index 0000000000..1437ab14a3 --- /dev/null +++ b/libuavcan/src/protocol/panic_broadcaster.cpp @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ + +void PanicBroadcaster::publishOnce() +{ + const int res = pub_.broadcast(msg_); + if (res < 0) + { + pub_.getNode().registerInternalFailure("Panic broadcasting failure"); + } +} + +void PanicBroadcaster::handleTimerEvent(const TimerEvent&) +{ + publishOnce(); +} + +void PanicBroadcaster::panic(const char* short_reason) +{ + msg_.reason_text.clear(); + const char* p = short_reason; + while (p && *p) + { + if (msg_.reason_text.size() == msg_.reason_text.capacity()) + break; + msg_.reason_text.push_back(*p); + p++; + } + + UAVCAN_TRACE("PanicBroadcaster", "Panicking with reason '%s'", getReason().c_str()); + + publishOnce(); + startPeriodic(MonotonicDuration::fromMSec(protocol::Panic::BROADCASTING_INTERVAL_MS)); +} + +void PanicBroadcaster::dontPanic() +{ + stop(); + msg_.reason_text.clear(); +} + +bool PanicBroadcaster::isPanicking() const +{ + return isRunning(); +} + +const typename protocol::Panic::FieldTypes::reason_text& PanicBroadcaster::getReason() const +{ + return msg_.reason_text; +} + +} diff --git a/libuavcan/test/protocol/panic_broadcaster.cpp b/libuavcan/test/protocol/panic_broadcaster.cpp new file mode 100644 index 0000000000..ca32440764 --- /dev/null +++ b/libuavcan/test/protocol/panic_broadcaster.cpp @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(PanicBroadcaster, Basic) +{ + InterlinkedTestNodes nodes; + + uavcan::PanicBroadcaster panicker(nodes.a); + + SubscriberWithCollector sub(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + ASSERT_LE(0, sub.start()); + + panicker.panic("I lost my towel!"); // Only the first 7 chars allowed + + ASSERT_STREQ("I lost ", panicker.getReason().c_str()); + ASSERT_TRUE(panicker.isPanicking()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str()); + sub.collector.msg.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str()); + sub.collector.msg.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str()); + sub.collector.msg.reset(); + + panicker.dontPanic(); + ASSERT_FALSE(panicker.isPanicking()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300)); + ASSERT_FALSE(sub.collector.msg.get()); +}