bcm_sync.h
Go to the documentation of this file.
00001 #ifndef H_BCM_SYNC
00002 #define H_BCM_SYNC
00003 
00004 #include <socketcan_interface/bcm.h>
00005 #include <socketcan_interface/socketcan.h>
00006 #include <canopen_master/canopen.h>
00007 
00008 namespace canopen {
00009 
00010 template<typename T > std::string join(const T &container, const std::string &delim){
00011     if(container.empty()) return std::string();
00012     std::stringstream sstr;
00013     typename T::const_iterator it = container.begin();
00014     sstr << *it;
00015     for(++it; it != container.end(); ++it){
00016         sstr << delim << *it;
00017     }
00018     return sstr.str();
00019 }
00020 
00021 class BCMsync : public Layer {
00022     boost::mutex mutex_;
00023 
00024     std::string device_;
00025 
00026     std::set<int> ignored_nodes_;
00027     std::set<int> monitored_nodes_;
00028     std::set<int> known_nodes_;
00029     std::set<int> started_nodes_;
00030 
00031     bool sync_running_;
00032     can::BCMsocket bcm_;
00033     boost::shared_ptr<can::SocketCANDriver>  driver_;
00034     uint16_t sync_ms_;
00035     can::CommInterface::FrameListener::Ptr handler_;
00036 
00037     std::vector<can::Frame> sync_frames_;
00038 
00039     bool skipNode(uint8_t id){
00040         if(ignored_nodes_.find(id) != ignored_nodes_.end()) return true;
00041         if(!monitored_nodes_.empty() && monitored_nodes_.find(id) == monitored_nodes_.end()) return true;
00042         known_nodes_.insert(id);
00043         return false;
00044     }
00045 
00046     void handleFrame(const can::Frame &frame){
00047         boost::mutex::scoped_lock lock(mutex_);
00048 
00049         if(frame.id == NMT_ID){
00050             if(frame.dlc > 1){
00051                 uint8_t cmd = frame.data[0];
00052                 uint8_t id = frame.data[1];
00053                 if(skipNode(id)) return;
00054 
00055                 if(cmd == 1){ // started
00056                     if(id != 0) started_nodes_.insert(id);
00057                     else started_nodes_.insert(known_nodes_.begin(), known_nodes_.end()); // start all
00058                 }else{
00059                     if(id != 0) started_nodes_.erase(id);
00060                     else started_nodes_.clear(); // stop all
00061                 }
00062             }
00063         }else if((frame.id & ~ALL_NODES_MASK) == HEARTBEAT_ID){
00064             int id = frame.id & ALL_NODES_MASK;
00065             if(skipNode(id)) return;
00066 
00067             if(frame.dlc > 0 && frame.data[0] ==  canopen::Node::Stopped) started_nodes_.erase(id); 
00068         }
00069 
00070         // toggle sync if needed
00071         if(started_nodes_.empty() && sync_running_){
00072             sync_running_ = !bcm_.stopTX(sync_frames_.front());
00073         }else if(!started_nodes_.empty() && !sync_running_){
00074             sync_running_ = bcm_.startTX(boost::chrono::milliseconds(sync_ms_),sync_frames_.front(), sync_frames_.size(), &sync_frames_[0]);
00075         }
00076     }
00077 protected:
00078     virtual void handleRead(LayerStatus &status, const LayerState &current_state) {
00079         if(current_state > Init){
00080         }
00081     }
00082     virtual void handleWrite(LayerStatus &status, const LayerState &current_state) {
00083         if(current_state > Init){
00084         }
00085     }
00086     virtual void handleDiag(LayerReport &report){
00087         boost::mutex::scoped_lock lock(mutex_);
00088         if(!sync_running_) report.warn("sync is not running");
00089 
00090         report.add("sync_running", sync_running_);
00091         report.add("known_nodes", join(known_nodes_, ", "));
00092         report.add("started_nodes", join(started_nodes_, ", "));
00093     }
00094  
00095     virtual void handleInit(LayerStatus &status){
00096         boost::mutex::scoped_lock lock(mutex_);
00097         started_nodes_.clear();
00098 
00099         if(!bcm_.init(device_)){
00100             status.error("BCM_init failed");
00101             return;
00102         }
00103         int sc = driver_->getInternalSocket();
00104 
00105         struct can_filter filter[2];
00106 
00107         filter[0].can_id   = NMT_ID;
00108         filter[0].can_mask = CAN_SFF_MASK;
00109         filter[1].can_id   = HEARTBEAT_ID;
00110         filter[1].can_mask =  ~ALL_NODES_MASK;
00111 
00112         if(setsockopt(sc, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0){
00113             status.warn("could not set filter");
00114             return;
00115         }
00116 
00117         handler_ = driver_->createMsgListener(can::CommInterface::FrameDelegate(this, &BCMsync::handleFrame));
00118     }
00119     virtual void handleShutdown(LayerStatus &status){
00120         boost::mutex::scoped_lock lock(mutex_);
00121         handler_.reset();
00122         bcm_.shutdown();
00123     }
00124 
00125     virtual void handleHalt(LayerStatus &status) {
00126         boost::mutex::scoped_lock lock(mutex_);
00127         if(sync_running_){
00128             bcm_.stopTX(sync_frames_.front());        
00129             sync_running_ = false;
00130             started_nodes_.clear();
00131         }
00132     }
00133 
00134     virtual void handleRecover(LayerStatus &status){
00135         handleShutdown(status);
00136         handleInit(status);
00137     }
00138 public:
00139     static const uint32_t ALL_NODES_MASK = 127;
00140     static const uint32_t HEARTBEAT_ID = 0x700;
00141     static const uint32_t NMT_ID = 0x000;
00142 
00143     BCMsync(const std::string &device, boost::shared_ptr<can::SocketCANDriver>  driver, const SyncProperties &sync_properties)
00144     : Layer(device + " SyncLayer"), device_(device), sync_running_(false), sync_ms_(sync_properties.period_ms_), driver_(driver) {
00145         if(sync_properties.overflow_ == 0){
00146             sync_frames_.resize(1);
00147             sync_frames_[0] = can::Frame(sync_properties.header_,0);
00148         }else{
00149             sync_frames_.resize(sync_properties.overflow_);
00150             for(int i = 0; i < sync_properties.overflow_; ++i){
00151                 sync_frames_[i] = can::Frame(sync_properties.header_,1);
00152                 sync_frames_[i].data[0] = i+1;
00153             }
00154         }
00155     }
00156     template <class T> void setMonitored(const T &other){
00157         monitored_nodes_.clear();
00158         monitored_nodes_.insert(other.begin(), other.end());
00159     }
00160     template <class T> void setIgnored(const T &other){
00161         ignored_nodes_.clear();
00162         ignored_nodes_.insert(other.begin(), other.end());
00163     }
00164 };
00165 
00166 }
00167 #endif


canopen_master
Author(s): Mathias L├╝dtke
autogenerated on Sun Sep 3 2017 03:10:42