node.cpp
Go to the documentation of this file.
00001 #include <canopen_master/canopen.h>
00002 
00003 using namespace canopen;
00004 
00005 #pragma pack(push) /* push current alignment to stack */
00006 #pragma pack(1) /* set alignment to 1 byte boundary */
00007 
00008 
00009 struct NMTcommand{
00010     enum Command{
00011         Start = 1,
00012         Stop = 2,
00013         Prepare = 128,
00014         Reset = 129,
00015         Reset_Com = 130
00016     };
00017     uint8_t command;
00018     uint8_t node_id;
00019     
00020     struct Frame: public FrameOverlay<NMTcommand>{
00021         Frame(uint8_t node_id, const Command &c) : FrameOverlay(can::Header()) {
00022             data.command = c;
00023             data.node_id = node_id;
00024         }
00025     };
00026 };
00027 
00028 #pragma pack(pop) /* pop previous alignment from stack */
00029 
00030 Node::Node(const boost::shared_ptr<can::CommInterface> interface, const boost::shared_ptr<ObjectDict> dict, uint8_t node_id, const boost::shared_ptr<SyncCounter> sync)
00031 : Layer("Node 301"), node_id_(node_id), interface_(interface), sync_(sync) , state_(Unknown), sdo_(interface, dict, node_id), pdo_(interface){
00032     try{
00033         getStorage()->entry(heartbeat_, 0x1017);
00034     }
00035     catch(const std::out_of_range){
00036     }
00037 }
00038     
00039 const Node::State Node::getState(){
00040     boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
00041     return state_;
00042 }
00043 
00044 bool Node::reset_com(){
00045     boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
00046     getStorage()->reset();
00047     interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset_Com));
00048     if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){
00049         return false;
00050     }
00051     state_ = PreOperational;
00052     setHeartbeatInterval();
00053     return true;
00054 }
00055 bool Node::reset(){
00056     boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
00057     getStorage()->reset();
00058     
00059     interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset));
00060     if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){
00061         return false;
00062     }
00063     state_ = PreOperational;
00064     setHeartbeatInterval();
00065     return true;
00066 }
00067 
00068 bool Node::prepare(){
00069     boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
00070     if(state_ == BootUp){
00071         // ERROR
00072     }
00073     interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Prepare));
00074     return 0 != wait_for(PreOperational, boost::chrono::seconds(2));
00075 }
00076 bool Node::start(){
00077     boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
00078     if(state_ == BootUp){
00079         // ERROR
00080     }
00081     interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Start));
00082     return 0 != wait_for(Operational, boost::chrono::seconds(2));
00083 }
00084 bool Node::stop(){
00085     boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
00086     if(sync_) sync_->removeNode(this);
00087     if(state_ == BootUp){
00088         // ERROR
00089     }
00090     interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Stop));
00091     return true;
00092 }
00093 
00094 void Node::switchState(const uint8_t &s){
00095     bool changed = state_ != s;
00096     switch(s){
00097         case Operational:
00098             if(changed && sync_) sync_->addNode(this);
00099             break;
00100         case BootUp:
00101         case PreOperational:
00102         case Stopped:
00103             if(changed && sync_) sync_->removeNode(this);
00104             break;
00105         default:
00106             //error
00107             ;
00108     }
00109     if(changed){
00110         state_ = (State) s;
00111         state_dispatcher_.dispatch(state_);
00112         cond.notify_one();
00113     }
00114 }
00115 void Node::handleNMT(const can::Frame & msg){
00116     boost::mutex::scoped_lock cond_lock(cond_mutex);
00117     uint16_t interval = getHeartbeatInterval();
00118     if(interval) heartbeat_timeout_ = get_abs_time(boost::chrono::milliseconds(3*interval));
00119     assert(msg.dlc == 1);
00120     switchState(msg.data[0]);
00121 }
00122 template<typename T> int Node::wait_for(const State &s, const T &timeout){
00123     boost::mutex::scoped_lock cond_lock(cond_mutex);
00124     time_point abs_time = get_abs_time(timeout);
00125 
00126     while(s != state_) {
00127         if(cond.wait_until(cond_lock,abs_time) == boost::cv_status::timeout)
00128         {
00129             break;
00130         }
00131     }
00132     if( s!= state_){
00133         if(getHeartbeatInterval() == 0){
00134             switchState(s);
00135             return -1;
00136         }
00137         return 0;
00138     }
00139     return 1;
00140 }
00141 bool Node::checkHeartbeat(){
00142     if(getHeartbeatInterval() == 0) return true; //disabled
00143     boost::mutex::scoped_lock cond_lock(cond_mutex);
00144     return heartbeat_timeout_ >= boost::chrono::high_resolution_clock::now();
00145 }
00146 
00147 
00148 void Node::handleRead(LayerStatus &status, const LayerState &current_state) {
00149     if(current_state > Init){
00150         if(!checkHeartbeat()){
00151             status.error("heartbeat problem");
00152         } else if(getState() != Operational){
00153             status.error("not operational");
00154         } else{
00155             pdo_.read(status);
00156         }
00157     }
00158 }
00159 void Node::handleWrite(LayerStatus &status, const LayerState &current_state) {
00160     if(current_state > Init){
00161         if(getState() != Operational)  status.error("not operational");
00162         else if(! pdo_.write())  status.error("PDO write problem");
00163     }
00164 }
00165 
00166 
00167 void Node::handleDiag(LayerReport &report){
00168     State state = getState();
00169     if(state != Operational){
00170         report.error("Mode not operational");
00171         report.add("Node state", (int)state);
00172     }else if(!checkHeartbeat()){
00173         report.error("Heartbeat timeout");
00174     }
00175 }
00176 void Node::handleInit(LayerStatus &status){
00177     nmt_listener_ = interface_->createMsgListener( can::MsgHeader(0x700 + node_id_), can::CommInterface::FrameDelegate(this, &Node::handleNMT));
00178 
00179     sdo_.init();
00180     try{
00181         if(!reset_com()) BOOST_THROW_EXCEPTION( TimeoutException("reset_timeout") );
00182     }
00183     catch(const TimeoutException&){
00184         status.error(boost::str(boost::format("could not reset node '%1%'") % (int)node_id_));
00185         return;
00186     }
00187 
00188     if(!pdo_.init(getStorage(), status)){
00189         return;
00190     }
00191     getStorage()->init_all();
00192     sdo_.init(); // reread SDO paramters;
00193     // TODO: set SYNC data
00194 
00195     try{
00196         if(!start()) BOOST_THROW_EXCEPTION( TimeoutException("start timeout") );
00197     }
00198     catch(const TimeoutException&){
00199         status.error(boost::str(boost::format("could not start node '%1%'") %  (int)node_id_));
00200     }
00201 }
00202 void Node::handleRecover(LayerStatus &status){
00203     try{
00204         start();
00205     }
00206     catch(const TimeoutException&){
00207         status.error(boost::str(boost::format("could not start node '%1%'") %  (int)node_id_));
00208     }
00209 }
00210 void Node::handleShutdown(LayerStatus &status){
00211     if(getHeartbeatInterval()> 0) heartbeat_.set(0);
00212     stop();
00213     nmt_listener_.reset();
00214     switchState(Unknown);
00215 }
00216 void Node::handleHalt(LayerStatus &status){
00217     // do nothing
00218 }


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