Go to the documentation of this file.00001 #include <canopen_master/canopen.h>
00002
00003 using namespace canopen;
00004
00005 #pragma pack(push)
00006 #pragma pack(1)
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)
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);
00041 return state_;
00042 }
00043
00044 bool Node::reset_com(){
00045 boost::timed_mutex::scoped_lock lock(mutex);
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);
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);
00070 if(state_ == BootUp){
00071
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);
00078 if(state_ == BootUp){
00079
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);
00086 if(sync_) sync_->removeNode(this);
00087 if(state_ == BootUp){
00088
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
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;
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 ¤t_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 ¤t_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();
00193
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
00218 }