node_status_monitor.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
 */
 
#ifndef UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
#define UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
 
#include <uavcan/debug.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/protocol/NodeStatus.hpp>
#include <cassert>
#include <cstdlib>
 
namespace uavcan
{
class UAVCAN_EXPORT NodeStatusMonitor
{
public:
    struct NodeStatus
    {
        uint8_t health   : 2;
        uint8_t mode     : 3;
        uint8_t sub_mode : 3;
 
        NodeStatus() :
            health(protocol::NodeStatus::HEALTH_CRITICAL),
            mode(protocol::NodeStatus::MODE_OFFLINE),
            sub_mode(0)
        {
            StaticAssert<protocol::NodeStatus::FieldTypes::health::BitLen   == 2>::check();
            StaticAssert<protocol::NodeStatus::FieldTypes::mode::BitLen     == 3>::check();
            StaticAssert<protocol::NodeStatus::FieldTypes::sub_mode::BitLen == 3>::check();
        }
 
        bool operator!=(const NodeStatus rhs) const { return !operator==(rhs); }
        bool operator==(const NodeStatus rhs) const
        {
            return (   this->health   == rhs.health 
                    && this->mode     == rhs.mode
                    && this->sub_mode == rhs.sub_mode
                    );
        }
 
#if UAVCAN_TOSTRING
        std::string toString() const
        {
            char buf[40];
            (void)snprintf(buf, sizeof(buf), "health=%d mode=%d sub_mode=%d", int(health), int(mode), int(sub_mode));
            return std::string(buf);
        }
#endif
    };
 
    struct NodeStatusChangeEvent
    {
        NodeID node_id;
        NodeStatus status;
        NodeStatus old_status;
        bool was_known;
 
        NodeStatusChangeEvent() :
            was_known(false)
        { }
    };
 
private:
    enum { TimerPeriodMs100 = 2 };
 
    typedef MethodBinder<NodeStatusMonitor*,
                         void (NodeStatusMonitor::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
            NodeStatusCallback;
 
    typedef MethodBinder<NodeStatusMonitor*, void (NodeStatusMonitor::*)(const TimerEvent&)> TimerCallback;
 
    Subscriber<protocol::NodeStatus, NodeStatusCallback> sub_;
 
    TimerEventForwarder<TimerCallback> timer_;
 
    struct Entry
    {
        NodeStatus status;
        int8_t time_since_last_update_ms100;
        Entry() :
            time_since_last_update_ms100(-1)
        { }
    };
 
    mutable Entry entries_[NodeID::Max];  // [1, NodeID::Max]
 
    Entry& getEntry(NodeID node_id) const
    {
        if (node_id.get() < 1 || node_id.get() > NodeID::Max)
        {
            handleFatalError("NodeStatusMonitor NodeID");
        }
        return entries_[node_id.get() - 1];
    }
 
    void changeNodeStatus(const NodeID node_id, const Entry new_entry_value)
    {
        Entry& entry = getEntry(node_id);
        if (entry.status != new_entry_value.status)
        {
            NodeStatusChangeEvent event;
            event.node_id    = node_id;
            event.old_status = entry.status;
            event.status     = new_entry_value.status;
            event.was_known  = entry.time_since_last_update_ms100 >= 0;
 
            UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: [%s] --> [%s]", int(node_id.get()),
                         (event.was_known ? "known" : "new"),
                         event.old_status.toString().c_str(), event.status.toString().c_str());
 
            handleNodeStatusChange(event);
        }
        entry = new_entry_value;
    }
 
    void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
    {
        Entry new_entry;
        new_entry.time_since_last_update_ms100 = 0;
        new_entry.status.health   = msg.health   & ((1 << protocol::NodeStatus::FieldTypes::health::BitLen) - 1);
        new_entry.status.mode     = msg.mode     & ((1 << protocol::NodeStatus::FieldTypes::mode::BitLen) - 1);
        new_entry.status.sub_mode = msg.sub_mode & ((1 << protocol::NodeStatus::FieldTypes::sub_mode::BitLen) - 1);
 
        changeNodeStatus(msg.getSrcNodeID(), new_entry);
 
        handleNodeStatusMessage(msg);
    }
 
    void handleTimerEvent(const TimerEvent&)
    {
        const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100;
 
        for (uint8_t i = 1; i <= NodeID::Max; i++)
        {
            Entry& entry = getEntry(i);
            if (entry.time_since_last_update_ms100 >= 0 &&
                entry.status.mode != protocol::NodeStatus::MODE_OFFLINE)
            {
                entry.time_since_last_update_ms100 =
                    int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100));
 
                if (entry.time_since_last_update_ms100 > OfflineTimeoutMs100)
                {
                    Entry new_entry_value = entry;
                    new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100;
                    new_entry_value.status.mode = protocol::NodeStatus::MODE_OFFLINE;
                    changeNodeStatus(i, new_entry_value);
                }
            }
        }
    }
 
protected:
    virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
    {
        (void)event;
    }
 
    virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
    {
        (void)msg;
    }
 
public:
    explicit NodeStatusMonitor(INode& node)
        : sub_(node)
        , timer_(node)
    { }
 
    virtual ~NodeStatusMonitor() { }
 
    int start()
    {
        const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus));
        if (res >= 0)
        {
            timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent));
            timer_.startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
        }
        return res;
    }
 
    void forgetNode(NodeID node_id)
    {
        if (node_id.isValid())
        {
            Entry& entry = getEntry(node_id);
            entry = Entry();
        }
        else
        {
            UAVCAN_ASSERT(0);
        }
    }
 
    void forgetAllNodes()
    {
        for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++)
        {
            entries_[i] = Entry();
        }
    }
 
    NodeStatus getNodeStatus(NodeID node_id) const
    {
        if (!node_id.isValid())
        {
            UAVCAN_ASSERT(0);
            return NodeStatus();
        }
 
        const Entry& entry = getEntry(node_id);
        if (entry.time_since_last_update_ms100 >= 0)
        {
            return entry.status;
        }
        else
        {
            return NodeStatus();
        }
    }
 
    bool isNodeKnown(NodeID node_id) const
    {
        if (!node_id.isValid())
        {
            UAVCAN_ASSERT(0);
            return false;
        }
        return getEntry(node_id).time_since_last_update_ms100 >= 0;
    }
 
    NodeID findNodeWithWorstHealth() const
    {
        NodeID nid_with_worst_health;
        uint8_t worst_health = protocol::NodeStatus::HEALTH_OK;
 
        for (uint8_t i = 1; i <= NodeID::Max; i++)
        {
            const NodeID nid(i);
            UAVCAN_ASSERT(nid.isUnicast());
            const Entry& entry = getEntry(nid);
            if (entry.time_since_last_update_ms100 >= 0)
            {
                if (entry.status.health > worst_health || !nid_with_worst_health.isValid())
                {
                    nid_with_worst_health = nid;
                    worst_health = entry.status.health;
                }
            }
        }
 
        return nid_with_worst_health;
    }
 
    template <typename Operator>
    void forEachNode(Operator op) const
    {
        for (uint8_t i = 1; i <= NodeID::Max; i++)
        {
            const NodeID nid(i);
            UAVCAN_ASSERT(nid.isUnicast());
            const Entry& entry = getEntry(nid);
            if (entry.time_since_last_update_ms100 >= 0)
            {
                op(nid, entry.status);
            }
        }
    }
};
 
}
 
#endif // UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED