canopen.h
Go to the documentation of this file.
00001 #ifndef H_CANOPEN
00002 #define H_CANOPEN
00003 
00004 #include <socketcan_interface/interface.h>
00005 #include <socketcan_interface/dispatcher.h>
00006 #include <socketcan_interface/reader.h>
00007 #include "exceptions.h"
00008 #include "layer.h"
00009 #include "objdict.h"
00010 #include "timer.h"
00011 #include <stdexcept>
00012 #include <boost/thread/condition_variable.hpp>
00013 #include <boost/chrono/system_clocks.hpp>
00014 #include <boost/lexical_cast.hpp>
00015 
00016 namespace canopen{
00017 
00018 typedef boost::chrono::high_resolution_clock::time_point time_point;
00019 typedef boost::chrono::high_resolution_clock::duration time_duration;
00020 inline time_point get_abs_time(const time_duration& timeout) { return boost::chrono::high_resolution_clock::now() + timeout; }
00021 inline time_point get_abs_time() { return boost::chrono::high_resolution_clock::now(); }
00022 
00023 
00024 template<typename T> struct FrameOverlay: public can::Frame{
00025     T &data;
00026     FrameOverlay(const Header &h) : can::Frame(h,sizeof(T)), data(*(T*) can::Frame::data.c_array()) {
00027         can::Frame::data.fill(0);
00028     }
00029     FrameOverlay(const can::Frame &f) : can::Frame(f), data(*(T*) can::Frame::data.c_array()) { }
00030 };
00031 
00032 class SDOClient{
00033 
00034     can::Header client_id;
00035 
00036     boost::timed_mutex mutex;
00037 
00038     can::BufferedReader reader_;
00039     bool processFrame(const can::Frame & msg);
00040 
00041     String buffer;
00042     size_t offset;
00043     size_t total;
00044     bool done;
00045     can::Frame last_msg;
00046     const canopen::ObjectDict::Entry * current_entry;
00047 
00048     void transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data, String *result);
00049     void abort(uint32_t reason);
00050 
00051     const boost::shared_ptr<can::CommInterface> interface_;
00052 protected:
00053     void read(const canopen::ObjectDict::Entry &entry, String &data);
00054     void write(const canopen::ObjectDict::Entry &entry, const String &data);
00055 public:
00056     const boost::shared_ptr<ObjectStorage> storage_;
00057 
00058     void init();
00059 
00060     SDOClient(const boost::shared_ptr<can::CommInterface> interface, const boost::shared_ptr<ObjectDict> dict, uint8_t node_id)
00061     : interface_(interface), storage_(boost::make_shared<ObjectStorage>(dict, node_id, ObjectStorage::ReadDelegate(this, &SDOClient::read), ObjectStorage::WriteDelegate(this, &SDOClient::write))), reader_(false, 1)
00062     {
00063     }
00064 };
00065 
00066 class PDOMapper{
00067     boost::mutex mutex_;
00068 
00069     class Buffer{
00070     public:
00071         bool read(uint8_t* b, const size_t len);
00072         void write(const uint8_t* b, const size_t len);
00073         void read(const canopen::ObjectDict::Entry &entry, String &data);
00074         void write(const canopen::ObjectDict::Entry &, const String &data);
00075         void clean() { dirty = false; }
00076         const size_t size;
00077         Buffer(const size_t sz) : size(sz), dirty(false), empty(true), buffer(sz) {}
00078 
00079     private:
00080         boost::mutex mutex;
00081         bool dirty;
00082         bool empty;
00083         std::vector<char> buffer;
00084     };
00085 
00086     class PDO {
00087     protected:
00088         void parse_and_set_mapping(const boost::shared_ptr<ObjectStorage> &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write);
00089         can::Frame frame;
00090         uint8_t transmission_type;
00091         std::vector< boost::shared_ptr<Buffer> >buffers;
00092     };
00093 
00094     struct TPDO: public PDO{
00095         void sync();
00096         static boost::shared_ptr<TPDO> create(const boost::shared_ptr<can::CommInterface> interface, const boost::shared_ptr<ObjectStorage> &storage, const uint16_t &com_index, const uint16_t &map_index){
00097             boost::shared_ptr<TPDO> tpdo(new TPDO(interface));
00098             if(!tpdo->init(storage, com_index, map_index))
00099                 tpdo.reset();
00100             return tpdo;
00101         }
00102     private:
00103         TPDO(const boost::shared_ptr<can::CommInterface> interface) : interface_(interface){}
00104         bool init(const boost::shared_ptr<ObjectStorage> &storage, const uint16_t &com_index, const uint16_t &map_index);
00105         const boost::shared_ptr<can::CommInterface> interface_;
00106         boost::mutex mutex;
00107     };
00108 
00109     struct RPDO : public PDO{
00110         void sync(LayerStatus &status);
00111         static boost::shared_ptr<RPDO> create(const boost::shared_ptr<can::CommInterface> interface, const boost::shared_ptr<ObjectStorage> &storage, const uint16_t &com_index, const uint16_t &map_index){
00112             boost::shared_ptr<RPDO> rpdo(new RPDO(interface));
00113             if(!rpdo->init(storage, com_index, map_index))
00114                 rpdo.reset();
00115             return rpdo;
00116         }
00117     private:
00118         bool init(const boost::shared_ptr<ObjectStorage> &storage, const uint16_t &com_index, const uint16_t &map_index);
00119         RPDO(const boost::shared_ptr<can::CommInterface> interface) : interface_(interface), timeout(-1) {}
00120         boost::mutex mutex;
00121         const boost::shared_ptr<can::CommInterface> interface_;
00122 
00123         can::CommInterface::FrameListener::Ptr listener_;
00124         void handleFrame(const can::Frame & msg);
00125         int timeout;
00126     };
00127 
00128     boost::unordered_set< boost::shared_ptr<RPDO> > rpdos_;
00129     boost::unordered_set< boost::shared_ptr<TPDO> > tpdos_;
00130 
00131     const boost::shared_ptr<can::CommInterface> interface_;
00132 
00133 public:
00134     PDOMapper(const boost::shared_ptr<can::CommInterface> interface);
00135     void read(LayerStatus &status);
00136     bool write();
00137     bool init(const boost::shared_ptr<ObjectStorage> storage, LayerStatus &status);
00138 };
00139 
00140 class EMCYHandler : public Layer {
00141     boost::atomic<bool> has_error_;
00142     ObjectStorage::Entry<uint8_t> error_register_;
00143     ObjectStorage::Entry<uint8_t> num_errors_;
00144     can::CommInterface::FrameListener::Ptr emcy_listener_;
00145     void handleEMCY(const can::Frame & msg);
00146     const boost::shared_ptr<ObjectStorage> storage_;
00147 
00148     virtual void handleDiag(LayerReport &report);
00149 
00150     virtual void handleInit(LayerStatus &status);
00151     virtual void handleRecover(LayerStatus &status);
00152     virtual void handleRead(LayerStatus &status, const LayerState &current_state);
00153     virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
00154     virtual void handleHalt(LayerStatus &status);
00155     virtual void handleShutdown(LayerStatus &status);
00156 
00157 public:
00158     EMCYHandler(const boost::shared_ptr<can::CommInterface> interface, const boost::shared_ptr<ObjectStorage> storage);
00159     void resetErrors(LayerStatus &status);
00160 };
00161 
00162 struct SyncProperties{
00163     const can::Header header_;
00164     const uint16_t period_ms_;
00165     const uint8_t overflow_;
00166     SyncProperties(const can::Header &h, const uint16_t  &p, const uint8_t &o) : header_(h), period_ms_(p), overflow_(o) {}
00167     bool operator==(const SyncProperties &p) const { return p.header_ == (int) header_ && p.overflow_ == overflow_ && p.period_ms_ == period_ms_; }
00168 
00169 };
00170 
00171 class SyncCounter {
00172 public:
00173     const SyncProperties properties;
00174     SyncCounter(const SyncProperties &p) : properties(p) {}
00175     virtual void addNode(void * const ptr)  = 0;
00176     virtual  void removeNode(void * const ptr) = 0;
00177     virtual ~SyncCounter() {}
00178 };
00179 
00180 class Node : public Layer{
00181 public:
00182     enum State{
00183         Unknown = 255, BootUp = 0, Stopped = 4, Operational = 5 , PreOperational = 127
00184     };
00185     const uint8_t node_id_;
00186     Node(const boost::shared_ptr<can::CommInterface> interface, const boost::shared_ptr<ObjectDict> dict, uint8_t node_id, const boost::shared_ptr<SyncCounter> sync = boost::shared_ptr<SyncCounter>());
00187 
00188     const State getState();
00189     void enterState(const State &s);
00190 
00191     const boost::shared_ptr<ObjectStorage> getStorage() { return sdo_.storage_; }
00192 
00193     bool start();
00194     bool stop();
00195     bool reset();
00196     bool reset_com();
00197     bool prepare();
00198 
00199     typedef fastdelegate::FastDelegate1<const State&> StateDelegate;
00200     typedef can::Listener<const StateDelegate, const State&> StateListener;
00201 
00202     StateListener::Ptr addStateListener(const StateDelegate & s){
00203         return state_dispatcher_.createListener(s);
00204     }
00205 
00206     template<typename T> T get(const ObjectDict::Key& k){
00207         return getStorage()->entry<T>(k).get();
00208     }
00209 
00210 private:
00211     virtual void handleDiag(LayerReport &report);
00212 
00213     virtual void handleInit(LayerStatus &status);
00214     virtual void handleRecover(LayerStatus &status);
00215     virtual void handleRead(LayerStatus &status, const LayerState &current_state);
00216     virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
00217     virtual void handleHalt(LayerStatus &status);
00218     virtual void handleShutdown(LayerStatus &status);
00219 
00220     template<typename T> int wait_for(const State &s, const T &timeout);
00221 
00222     boost::timed_mutex mutex;
00223     boost::mutex cond_mutex;
00224     boost::condition_variable cond;
00225 
00226     const boost::shared_ptr<can::CommInterface> interface_;
00227     const boost::shared_ptr<SyncCounter> sync_;
00228     can::CommInterface::FrameListener::Ptr nmt_listener_;
00229 
00230     ObjectStorage::Entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED16>::type> heartbeat_;
00231 
00232     can::SimpleDispatcher<StateListener> state_dispatcher_;
00233 
00234     void handleNMT(const can::Frame & msg);
00235     void switchState(const uint8_t &s);
00236 
00237     State state_;
00238     SDOClient sdo_;
00239     PDOMapper pdo_;
00240 
00241     boost::chrono::high_resolution_clock::time_point heartbeat_timeout_;
00242     uint16_t getHeartbeatInterval() { return heartbeat_.valid()?heartbeat_.get_cached() : 0; }
00243     void setHeartbeatInterval() { if(heartbeat_.valid()) heartbeat_.set(heartbeat_.desc().value().get<uint16_t>()); }
00244     bool checkHeartbeat();
00245 };
00246 
00247 template<typename T> class Chain{
00248 protected:
00249     std::vector<boost::shared_ptr<T> > elements;
00250 public:
00251     void call(void (T::*func)(void)){
00252         typename std::vector<boost::shared_ptr<T> >::iterator it = elements.begin();
00253         while(it != elements.end()){
00254             ((**it).*func)();
00255             ++it;
00256         }
00257     }
00258     template<typename V> void call(void (T::*func)(const V&), const std::vector<V> &vs){
00259         typename std::vector<boost::shared_ptr<T> >::iterator it = elements.begin();
00260         typename std::vector<V>::const_iterator it_v = vs.begin();
00261         while(it_v != vs.end() &&  it != elements.end()){
00262             ((**it).*func)(*it_v);
00263             ++it; ++it_v;
00264         }
00265     }
00266     template<typename V> void call(void (T::*func)(V&), std::vector<V> &vs){
00267         vs.resize(elements.size());
00268 
00269         typename std::vector<boost::shared_ptr<T> >::iterator it = elements.begin();
00270         typename std::vector<V>::iterator it_v = vs.begin();
00271         while(it_v != vs.end() &&  it != elements.end()){
00272             ((**it).*func)(*it_v);
00273             ++it; ++it_v;
00274         }
00275     }
00276     void add(boost::shared_ptr<T> t){
00277         elements.push_back(t);
00278     }
00279 };
00280 
00281 template<typename T> class NodeChain: public Chain<T>{
00282 public:
00283     const std::vector<boost::shared_ptr<T> >& getElements() { return Chain<T>::elements; }
00284     void start() { this->call(&T::start); }
00285     void stop() { this->call(&T::stop); }
00286     void reset() { this->call(&T::reset); }
00287     void reset_com() { this->call(&T::reset_com); }
00288     void prepare() { this->call(&T::prepare); }
00289 };
00290 
00291 class SyncLayer: public Layer, public SyncCounter{
00292 public:
00293     SyncLayer(const SyncProperties &p) : Layer("Sync layer"), SyncCounter(p) {}
00294 };
00295 
00296 class Master: boost::noncopyable {
00297 public:
00298     virtual boost::shared_ptr<SyncLayer> getSync(const SyncProperties &properties) = 0;
00299     virtual ~Master() {}
00300 
00301     class Allocator {
00302     public:
00303         virtual boost::shared_ptr<Master> allocate(const std::string &name, boost::shared_ptr<can::CommInterface> interface) = 0;
00304         virtual ~Allocator() {}
00305     };
00306 };
00307 
00308 class Settings
00309 {
00310 public:
00311     template <typename T> T get_optional(const std::string &n, const T& def) const {
00312         std::string repr;
00313         if(!getRepr(n, repr)){
00314             return def;
00315         }
00316         return boost::lexical_cast<T>(repr);
00317     }
00318     template <typename T> bool get(const std::string &n, T& val) const {
00319         std::string repr;
00320         if(!getRepr(n, repr)) return false;
00321         val =  boost::lexical_cast<T>(repr);
00322         return true;
00323     }
00324     virtual ~Settings() {}
00325 private:
00326     virtual bool getRepr(const std::string &n, std::string & repr) const = 0;
00327 };
00328 
00329 
00330 /*template<typename InterfaceType, typename MasterType, typename NodeType> class Bus: boost::noncopyable{
00331     boost::weak_ptr <InterfaceType> weak_interface_;
00332     boost::weak_ptr <MasterType> weak_master_;
00333 
00334     const String device_;
00335     const unsigned int bitrate_;
00336 public:
00337     Bus(const String &device, unsigned int bitrate) : device_(device), bitrate_(bitrate) {}
00338     boost::shared_ptr<InterfaceType> getInterface(){
00339         boost::shared_ptr<InterfaceType> interface = weak_interface_.lock();
00340         if(!interface){
00341             weak_interface_ = interface = boost::make_shared<InterfaceType>();
00342             interface_
00343         }
00344         return interface;
00345     }
00346     boost::shared_ptr<MasterType> getMaster(){
00347         boost::shared_ptr<MasterType> master = weak_master_.lock();
00348         if(!master){
00349             boost::shared_ptr<InterfaceType> interface = getInterface();
00350             if(interface) weak_master_ = master = boost::make_shared<MasterType>(interface);
00351         }
00352         return master;
00353     }
00354 };*/
00355 
00356 } // canopen
00357 #endif // !H_CANOPEN


canopen_master
Author(s): Mathias Lüdtke
autogenerated on Thu Jun 6 2019 20:43:54