From 3829506368804ea45d4647f0d4bb0d819b3ac2f2 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Wed, 19 Mar 2014 14:27:37 +0400
Subject: [PATCH] PanicListener

---
 .../uavcan/protocol/panic_listener.hpp        | 101 ++++++++++++++++++
 libuavcan/test/protocol/panic_listener.cpp    |  52 +++++++++
 2 files changed, 153 insertions(+)
 create mode 100644 libuavcan/include/uavcan/protocol/panic_listener.hpp
 create mode 100644 libuavcan/test/protocol/panic_listener.cpp

diff --git a/libuavcan/include/uavcan/protocol/panic_listener.hpp b/libuavcan/include/uavcan/protocol/panic_listener.hpp
new file mode 100644
index 0000000000..7cef91292a
--- /dev/null
+++ b/libuavcan/include/uavcan/protocol/panic_listener.hpp
@@ -0,0 +1,101 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/protocol/Panic.hpp>
+
+namespace uavcan
+{
+/**
+ * Callback prototype:
+ *   void (const ReceivedDataStructure<protocol::Panic>&)
+ * Or:
+ *   void (const protocol::Panic&)
+ * The listener can be stopped from the callback.
+ */
+template <typename Callback = void(*)(const protocol::Panic&)>
+class PanicListener : Noncopyable
+{
+    typedef MethodBinder<PanicListener*, void(PanicListener::*)(const ReceivedDataStructure<protocol::Panic>&)>
+        PanicMsgCallback;
+
+    Subscriber<protocol::Panic, PanicMsgCallback> sub_;
+    MonotonicTime prev_msg_timestamp_;
+    Callback callback_;
+    uint8_t num_subsequent_msgs_;
+
+    void invokeCallback(const ReceivedDataStructure<protocol::Panic>& msg)
+    {
+        if (try_implicit_cast<bool>(callback_, true))
+        {
+            callback_(msg);
+        }
+        else
+        {
+            assert(0);       // This is a logic error because normally we shouldn't start with an invalid callback
+            sub_.getNode().registerInternalFailure("PanicListener invalid callback");
+        }
+    }
+
+    void handleMsg(const ReceivedDataStructure<protocol::Panic>& msg)
+    {
+        UAVCAN_TRACE("PanicListener", "Received panic from snid=%i reason=%s",
+                     int(msg.getSrcNodeID().get()), msg.reason_text.c_str());
+        if (prev_msg_timestamp_.isZero())
+        {
+            num_subsequent_msgs_ = 1;
+            prev_msg_timestamp_ = msg.getMonotonicTimestamp();
+        }
+        else
+        {
+            const MonotonicDuration diff = msg.getMonotonicTimestamp() - prev_msg_timestamp_;
+            assert(diff.isPositive() || diff.isZero());
+            if (diff.toMSec() > protocol::Panic::MAX_INTERVAL_MS)
+            {
+                num_subsequent_msgs_ = 1;
+            }
+            else
+            {
+                num_subsequent_msgs_++;
+            }
+            prev_msg_timestamp_ = msg.getMonotonicTimestamp();
+            if (num_subsequent_msgs_ >= protocol::Panic::MIN_MESSAGES)
+            {
+                num_subsequent_msgs_ = protocol::Panic::MIN_MESSAGES;
+                invokeCallback(msg);                      // The application can stop us from the callback
+            }
+        }
+    }
+
+public:
+    PanicListener(INode& node)
+    : sub_(node)
+    , callback_()
+    , num_subsequent_msgs_(0)
+    { }
+
+    int start(const Callback& callback)
+    {
+        stop();
+        if (!try_implicit_cast<bool>(callback, true))
+        {
+            UAVCAN_TRACE("PanicListener", "Invalid callback");
+            return -1;
+        }
+        callback_ = callback;
+        return sub_.start(PanicMsgCallback(this, &PanicListener::handleMsg));
+    }
+
+    void stop()
+    {
+        sub_.stop();
+        num_subsequent_msgs_ = 0;
+        prev_msg_timestamp_ = MonotonicTime();
+    }
+};
+
+}
diff --git a/libuavcan/test/protocol/panic_listener.cpp b/libuavcan/test/protocol/panic_listener.cpp
new file mode 100644
index 0000000000..8f2962713f
--- /dev/null
+++ b/libuavcan/test/protocol/panic_listener.cpp
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/panic_listener.hpp>
+#include <uavcan/protocol/panic_broadcaster.hpp>
+#include "helpers.hpp"
+
+
+struct PanicHandler
+{
+    uavcan::protocol::Panic msg;
+
+    PanicHandler() : msg() { }
+
+    void handle(const uavcan::protocol::Panic& msg)
+    {
+        std::cout << msg << std::endl;
+        this->msg = msg;
+    }
+
+    typedef uavcan::MethodBinder<PanicHandler*, void (PanicHandler::*)(const uavcan::protocol::Panic& msg)> Binder;
+
+    Binder bind() { return Binder(this, &PanicHandler::handle); }
+};
+
+
+TEST(PanicListener, Basic)
+{
+    InterlinkedTestNodes nodes;
+
+    uavcan::PanicListener<PanicHandler::Binder> pl(nodes.a);
+    uavcan::PanicBroadcaster pbr(nodes.b);
+    PanicHandler handler;
+    ASSERT_LE(0, pl.start(handler.bind()));
+
+    pbr.panic("PANIC!!!");
+    ASSERT_TRUE(handler.msg == uavcan::protocol::Panic()); // One message published, panic is not registered yet
+
+    pbr.dontPanic();
+    ASSERT_FALSE(pbr.isPanicking());
+    std::cout << "Not panicking" << std::endl;
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); // Will reset
+    ASSERT_TRUE(handler.msg == uavcan::protocol::Panic());
+
+    pbr.panic("PANIC2!!!");     // Message text doesn't matter
+    ASSERT_TRUE(pbr.isPanicking());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+    ASSERT_STREQ("PANIC2!", handler.msg.reason_text.c_str()); // Registered, only 7 chars preserved
+}