canopen.h

Go to the documentation of this file.

#ifndef H_CANOPEN
#define H_CANOPEN
 
#include <socketcan_interface/interface.h>
#include <socketcan_interface/dispatcher.h>
#include <socketcan_interface/reader.h>
#include "exceptions.h"
#include "layer.h"
#include "objdict.h"
#include "timer.h"
#include <stdexcept>
#include <boost/thread/condition_variable.hpp>
#include <boost/chrono/system_clocks.hpp>
#include <boost/lexical_cast.hpp>
 
namespace canopen{
 
typedef boost::chrono::high_resolution_clock::time_point time_point;
typedef boost::chrono::high_resolution_clock::duration time_duration;
inline time_point get_abs_time(const time_duration& timeout) { return boost::chrono::high_resolution_clock::now() + timeout; }
inline time_point get_abs_time() { return boost::chrono::high_resolution_clock::now(); }
 
 
template<typename T> struct FrameOverlay: public can::Frame{
    T &data;
    FrameOverlay(const Header &h) : can::Frame(h,sizeof(T)), data(* (T*) can::Frame::c_array()) {
        can::Frame::data.fill(0);
    }
    FrameOverlay(const can::Frame &f) : can::Frame(f), data(* (T*) can::Frame::c_array()) { }
};
 
class SDOClient{
 
    can::Header client_id;
 
    boost::timed_mutex mutex;
 
    can::BufferedReader reader_;
    bool processFrame(const can::Frame & msg);
 
    String buffer;
    size_t offset;
    size_t total;
    bool done;
    can::Frame last_msg;
    const canopen::ObjectDict::Entry * current_entry;
 
    void transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data, String *result);
    void abort(uint32_t reason);
 
    const can::CommInterfaceSharedPtr interface_;
protected:
    void read(const canopen::ObjectDict::Entry &entry, String &data);
    void write(const canopen::ObjectDict::Entry &entry, const String &data);
public:
    const ObjectStorageSharedPtr storage_;
 
    void init();
 
    SDOClient(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id)
    : interface_(interface),
      storage_(std::make_shared<ObjectStorage>(dict, node_id,
                                               std::bind(&SDOClient::read, this, std::placeholders::_1, std::placeholders::_2),
                                               std::bind(&SDOClient::write, this, std::placeholders::_1, std::placeholders::_2))
              ),
      reader_(false, 1)
    {
    }
};
 
class PDOMapper{
    boost::mutex mutex_;
 
    class Buffer{
    public:
        bool read(uint8_t* b, const size_t len);
        void write(const uint8_t* b, const size_t len);
        void read(const canopen::ObjectDict::Entry &entry, String &data);
        void write(const canopen::ObjectDict::Entry &, const String &data);
        void clean() { dirty = false; }
        const size_t size;
        Buffer(const size_t sz) : size(sz), dirty(false), empty(true), buffer(sz) {}
 
    private:
        boost::mutex mutex;
        bool dirty;
        bool empty;
        std::vector<char> buffer;
    };
    typedef std::shared_ptr<Buffer> BufferSharedPtr;
 
    class PDO {
    protected:
        void parse_and_set_mapping(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write);
        can::Frame frame;
        uint8_t transmission_type;
        std::vector<BufferSharedPtr>buffers;
    };
 
    struct TPDO: public PDO{
        typedef std::shared_ptr<TPDO> TPDOSharedPtr;
        void sync();
        static TPDOSharedPtr create(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
            TPDOSharedPtr tpdo(new TPDO(interface));
            if(!tpdo->init(storage, com_index, map_index))
                tpdo.reset();
            return tpdo;
        }
    private:
        TPDO(const can::CommInterfaceSharedPtr interface) : interface_(interface){}
        bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index);
        const can::CommInterfaceSharedPtr interface_;
        boost::mutex mutex;
    };
 
    struct RPDO : public PDO{
        void sync(LayerStatus &status);
        typedef std::shared_ptr<RPDO> RPDOSharedPtr;
        static RPDOSharedPtr create(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
            RPDOSharedPtr rpdo(new RPDO(interface));
            if(!rpdo->init(storage, com_index, map_index))
                rpdo.reset();
            return rpdo;
        }
    private:
        bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index);
        RPDO(const can::CommInterfaceSharedPtr interface) : interface_(interface), timeout(-1) {}
        boost::mutex mutex;
        const can::CommInterfaceSharedPtr interface_;
 
        can::FrameListenerConstSharedPtr listener_;
        void handleFrame(const can::Frame & msg);
        int timeout;
    };
 
    std::unordered_set<RPDO::RPDOSharedPtr> rpdos_;
    std::unordered_set<TPDO::TPDOSharedPtr> tpdos_;
 
    const can::CommInterfaceSharedPtr interface_;
 
public:
    PDOMapper(const can::CommInterfaceSharedPtr interface);
    void read(LayerStatus &status);
    bool write();
    bool init(const ObjectStorageSharedPtr storage, LayerStatus &status);
};
 
class EMCYHandler : public Layer {
    std::atomic<bool> has_error_;
    ObjectStorage::Entry<uint8_t> error_register_;
    ObjectStorage::Entry<uint8_t> num_errors_;
    can::FrameListenerConstSharedPtr emcy_listener_;
    void handleEMCY(const can::Frame & msg);
    const ObjectStorageSharedPtr storage_;
 
    virtual void handleDiag(LayerReport &report);
 
    virtual void handleInit(LayerStatus &status);
    virtual void handleRecover(LayerStatus &status);
    virtual void handleRead(LayerStatus &status, const LayerState &current_state);
    virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
    virtual void handleHalt(LayerStatus &status);
    virtual void handleShutdown(LayerStatus &status);
 
public:
    EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage);
    void resetErrors(LayerStatus &status);
};
 
struct SyncProperties{
    const can::Header header_;
    const uint16_t period_ms_;
    const uint8_t overflow_;
    SyncProperties(const can::Header &h, const uint16_t  &p, const uint8_t &o) : header_(h), period_ms_(p), overflow_(o) {}
    bool operator==(const SyncProperties &p) const { return p.header_.key() == header_.key() && p.overflow_ == overflow_ && p.period_ms_ == period_ms_; }
 
};
 
class SyncCounter {
public:
    const SyncProperties properties;
    SyncCounter(const SyncProperties &p) : properties(p) {}
    virtual void addNode(void * const ptr)  = 0;
    virtual  void removeNode(void * const ptr) = 0;
    virtual ~SyncCounter() {}
};
typedef std::shared_ptr<SyncCounter> SyncCounterSharedPtr;
 
class Node : public Layer{
public:
    enum State{
        Unknown = 255, BootUp = 0, Stopped = 4, Operational = 5 , PreOperational = 127
    };
    const uint8_t node_id_;
    Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync = SyncCounterSharedPtr());
 
    const State getState();
    void enterState(const State &s);
 
    const ObjectStorageSharedPtr getStorage() { return sdo_.storage_; }
 
    bool start();
    bool stop();
    bool reset();
    bool reset_com();
    bool prepare();
 
    using StateFunc = std::function<void(const State&)>;
    using StateDelegate [[deprecated("use StateFunc instead")]] = can::DelegateHelper<StateFunc>;
 
    typedef can::Listener<const StateFunc, const State&> StateListener;
    typedef StateListener::ListenerConstSharedPtr StateListenerConstSharedPtr;
 
    StateListenerConstSharedPtr addStateListener(const StateFunc & s){
        return state_dispatcher_.createListener(s);
    }
 
    template<typename T> T get(const ObjectDict::Key& k){
        return getStorage()->entry<T>(k).get();
    }
 
private:
    virtual void handleDiag(LayerReport &report);
 
    virtual void handleInit(LayerStatus &status);
    virtual void handleRecover(LayerStatus &status);
    virtual void handleRead(LayerStatus &status, const LayerState &current_state);
    virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
    virtual void handleHalt(LayerStatus &status);
    virtual void handleShutdown(LayerStatus &status);
 
    template<typename T> int wait_for(const State &s, const T &timeout);
 
    boost::timed_mutex mutex;
    boost::mutex cond_mutex;
    boost::condition_variable cond;
 
    const can::CommInterfaceSharedPtr interface_;
    const SyncCounterSharedPtr sync_;
    can::FrameListenerConstSharedPtr nmt_listener_;
 
    ObjectStorage::Entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED16>::type> heartbeat_;
 
    can::SimpleDispatcher<StateListener> state_dispatcher_;
 
    void handleNMT(const can::Frame & msg);
    void switchState(const uint8_t &s);
 
    State state_;
    SDOClient sdo_;
    PDOMapper pdo_;
 
    boost::chrono::high_resolution_clock::time_point heartbeat_timeout_;
    uint16_t getHeartbeatInterval() { return heartbeat_.valid()?heartbeat_.get_cached() : 0; }
    void setHeartbeatInterval() { if(heartbeat_.valid()) heartbeat_.set(heartbeat_.desc().value().get<uint16_t>()); }
    bool checkHeartbeat();
};
typedef std::shared_ptr<Node> NodeSharedPtr;
 
template<typename T> class Chain{
public:
    typedef std::shared_ptr<T> MemberSharedPtr;
    void call(void (T::*func)(void)){
        typename std::vector<MemberSharedPtr>::iterator it = elements.begin();
        while(it != elements.end()){
            ((**it).*func)();
            ++it;
        }
    }
    template<typename V> void call(void (T::*func)(const V&), const std::vector<V> &vs){
        typename std::vector<MemberSharedPtr>::iterator it = elements.begin();
        typename std::vector<V>::const_iterator it_v = vs.begin();
        while(it_v != vs.end() &&  it != elements.end()){
            ((**it).*func)(*it_v);
            ++it; ++it_v;
        }
    }
    template<typename V> void call(void (T::*func)(V&), std::vector<V> &vs){
        vs.resize(elements.size());
 
        typename std::vector<MemberSharedPtr>::iterator it = elements.begin();
        typename std::vector<V>::iterator it_v = vs.begin();
        while(it_v != vs.end() &&  it != elements.end()){
            ((**it).*func)(*it_v);
            ++it; ++it_v;
        }
    }
    void add(MemberSharedPtr t){
        elements.push_back(t);
    }
protected:
    std::vector<MemberSharedPtr> elements;
};
 
template<typename T> class NodeChain: public Chain<T>{
public:
    const std::vector<typename Chain<T>::MemberSharedPtr>& getElements() { return Chain<T>::elements; }
    void start() { this->call(&T::start); }
    void stop() { this->call(&T::stop); }
    void reset() { this->call(&T::reset); }
    void reset_com() { this->call(&T::reset_com); }
    void prepare() { this->call(&T::prepare); }
};
 
class SyncLayer: public Layer, public SyncCounter{
public:
    SyncLayer(const SyncProperties &p) : Layer("Sync layer"), SyncCounter(p) {}
};
typedef std::shared_ptr<SyncLayer> SyncLayerSharedPtr;
 
class Master{
    Master(const Master&) = delete; // prevent copies
    Master& operator=(const Master&) = delete;
public:
    Master() = default;
    virtual SyncLayerSharedPtr getSync(const SyncProperties &properties) = 0;
    virtual ~Master() {}
 
    typedef std::shared_ptr<Master> MasterSharedPtr;
    class Allocator {
    public:
        virtual MasterSharedPtr allocate(const std::string &name, can::CommInterfaceSharedPtr interface) = 0;
        virtual ~Allocator() {}
    };
};
typedef Master::MasterSharedPtr MasterSharedPtr;
 
using can::Settings;
 
} // canopen
#endif // !H_CANOPEN