PanicBroadcaster

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

libuavcan/include/uavcan/protocol/panic_broadcaster.hpp

@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/protocol/Panic.hpp>
+
+namespace uavcan
+{
+
+class PanicBroadcaster : private Timer
+{
+    Publisher<protocol::Panic> 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;
+};
+
+}

libuavcan/src/protocol/panic_broadcaster.cpp

@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/protocol/panic_broadcaster.hpp>
+#include <uavcan/debug.hpp>
+
+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;
+}
+
+}

libuavcan/test/protocol/panic_broadcaster.cpp

@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/panic_broadcaster.hpp>
+#include "helpers.hpp"
+
+
+TEST(PanicBroadcaster, Basic)
+{
+    InterlinkedTestNodes nodes;
+
+    uavcan::PanicBroadcaster panicker(nodes.a);
+
+    SubscriberWithCollector<uavcan::protocol::Panic> sub(nodes.b);
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::Panic> _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());
+}