panic_listener.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED
00006 #define UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED
00007 
00008 #include <cassert>
00009 #include <uavcan/debug.hpp>
00010 #include <uavcan/build_config.hpp>
00011 #include <uavcan/node/subscriber.hpp>
00012 #include <uavcan/util/method_binder.hpp>
00013 #include <uavcan/protocol/Panic.hpp>
00014 
00015 #if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
00016 # error UAVCAN_CPP_VERSION
00017 #endif
00018 
00019 #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
00020 # include <functional>
00021 #endif
00022 
00023 namespace uavcan
00024 {
00025 /**
00026  * This class implements proper panic detector.
00027  * Refer to uavcan.protocol.Panic for details.
00028  * The listener can be stopped from the callback.
00029  *
00030  * @tparam Callback     Possible callback prototypes:
00031  *                      void (const ReceivedDataStructure<protocol::Panic>&)
00032  *                      void (const protocol::Panic&)
00033  */
00034 template <
00035 #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
00036           typename Callback = std::function<void (const ReceivedDataStructure<protocol::Panic>&)>
00037 #else
00038           typename Callback = void (*)(const ReceivedDataStructure<protocol::Panic>&)
00039 #endif
00040           >
00041 class UAVCAN_EXPORT PanicListener : Noncopyable
00042 {
00043     typedef MethodBinder<PanicListener*, void (PanicListener::*)(const ReceivedDataStructure<protocol::Panic>&)>
00044         PanicMsgCallback;
00045 
00046     Subscriber<protocol::Panic, PanicMsgCallback> sub_;
00047     MonotonicTime prev_msg_timestamp_;
00048     Callback callback_;
00049     uint8_t num_subsequent_msgs_;
00050 
00051     void invokeCallback(const ReceivedDataStructure<protocol::Panic>& msg)
00052     {
00053         if (coerceOrFallback<bool>(callback_, true))
00054         {
00055             callback_(msg);
00056         }
00057         else
00058         {
00059             UAVCAN_ASSERT(0);       // This is a logic error because normally we shouldn't start with an invalid callback
00060             sub_.getNode().registerInternalFailure("PanicListener invalid callback");
00061         }
00062     }
00063 
00064     void handleMsg(const ReceivedDataStructure<protocol::Panic>& msg)
00065     {
00066         UAVCAN_TRACE("PanicListener", "Received panic from snid=%i reason=%s",
00067                      int(msg.getSrcNodeID().get()), msg.reason_text.c_str());
00068         if (prev_msg_timestamp_.isZero())
00069         {
00070             num_subsequent_msgs_ = 1;
00071             prev_msg_timestamp_ = msg.getMonotonicTimestamp();
00072         }
00073         else
00074         {
00075             const MonotonicDuration diff = msg.getMonotonicTimestamp() - prev_msg_timestamp_;
00076             UAVCAN_ASSERT(diff.isPositive() || diff.isZero());
00077             if (diff.toMSec() > protocol::Panic::MAX_INTERVAL_MS)
00078             {
00079                 num_subsequent_msgs_ = 1;
00080             }
00081             else
00082             {
00083                 num_subsequent_msgs_++;
00084             }
00085             prev_msg_timestamp_ = msg.getMonotonicTimestamp();
00086             if (num_subsequent_msgs_ >= protocol::Panic::MIN_MESSAGES)
00087             {
00088                 num_subsequent_msgs_ = protocol::Panic::MIN_MESSAGES;
00089                 invokeCallback(msg);                      // The application can stop us from the callback
00090             }
00091         }
00092     }
00093 
00094 public:
00095     explicit PanicListener(INode& node)
00096         : sub_(node)
00097         , callback_()
00098         , num_subsequent_msgs_(0)
00099     { }
00100 
00101     /**
00102      * Start the listener.
00103      * Once started it does not require further attention.
00104      * Returns negative error code.
00105      */
00106     int start(const Callback& callback)
00107     {
00108         stop();
00109         if (!coerceOrFallback<bool>(callback, true))
00110         {
00111             UAVCAN_TRACE("PanicListener", "Invalid callback");
00112             return -ErrInvalidParam;
00113         }
00114         callback_ = callback;
00115         return sub_.start(PanicMsgCallback(this, &PanicListener::handleMsg));
00116     }
00117 
00118     void stop()
00119     {
00120         sub_.stop();
00121         num_subsequent_msgs_ = 0;
00122         prev_msg_timestamp_ = MonotonicTime();
00123     }
00124 };
00125 
00126 }
00127 
00128 #endif // UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED