emcy.cpp
Go to the documentation of this file.
00001 #include <canopen_master/canopen.h>
00002 #include <socketcan_interface/string.h>
00003 
00004 using namespace canopen;
00005 
00006 #pragma pack(push) /* push current alignment to stack */
00007 #pragma pack(1) /* set alignment to 1 byte boundary */
00008 
00009 struct EMCYid{
00010     uint32_t id:29;
00011     uint32_t extended:1;
00012     uint32_t :1;
00013     uint32_t invalid:1;
00014     EMCYid(uint32_t val){
00015         *(uint32_t*) this = val;
00016     }
00017     can::Header header() {
00018         return can::Header(id, extended, false, false);
00019     }
00020     const uint32_t get() const { return *(uint32_t*) this; }
00021 };
00022 
00023 struct EMCYfield{
00024     uint32_t error_code:16;
00025     uint32_t addition_info:16;
00026     EMCYfield(uint32_t val){
00027         *(uint32_t*) this = val;
00028     }
00029 };
00030 
00031 struct EMCYmsg{
00032     uint16_t error_code;
00033     uint8_t error_register;
00034     uint8_t manufacturer_specific_error_field[5];
00035 
00036     struct Frame: public FrameOverlay<EMCYmsg>{
00037         Frame(const can::Frame &f) : FrameOverlay(f){ }
00038     };
00039 };
00040 
00041 #pragma pack(pop) /* pop previous alignment from stack */
00042 
00043 void EMCYHandler::handleEMCY(const can::Frame & msg){
00044     EMCYmsg::Frame em(msg);
00045     LOG("EMCY: " << can::tostring(msg, false));
00046     has_error_ = (em.data.error_register & ~32) != 0;
00047 }
00048 
00049 EMCYHandler::EMCYHandler(const boost::shared_ptr<can::CommInterface> interface, const boost::shared_ptr<ObjectStorage> storage): Layer("EMCY handler"), storage_(storage), has_error_(true){
00050     storage_->entry(error_register_, 0x1001);
00051     try{
00052         storage_->entry(num_errors_, 0x1003,0);
00053     }
00054     catch(...){
00055        // pass, 1003 is optional
00056     }
00057     try{
00058         EMCYid emcy_id(storage_->entry<uint32_t>(0x1014).get_cached());
00059         emcy_listener_ = interface->createMsgListener( emcy_id.header(), can::CommInterface::FrameDelegate(this, &EMCYHandler::handleEMCY));
00060     }
00061     catch(...){
00062        // pass, EMCY is optional
00063     }
00064 }
00065 
00066 void EMCYHandler::handleRead(LayerStatus &status, const LayerState &current_state) {
00067     if(current_state == Ready){
00068         if(has_error_){
00069             status.error("Node has emergency error");
00070         }
00071     }
00072 }
00073 void EMCYHandler::handleWrite(LayerStatus &status, const LayerState &current_state) {
00074     // noithing to do
00075 }
00076 
00077 void EMCYHandler::handleDiag(LayerReport &report){
00078     uint8_t error_register = 0;
00079     if(!error_register_.get(error_register)){
00080         report.error("Could not read error error_register");
00081         return;
00082     }
00083 
00084     if(error_register){
00085         if(error_register & 1){ // first bit should be set on all errors
00086             report.error("Node has emergency error");
00087         }else if(error_register & ~32){ // filter profile-specific bit
00088             report.warn("Error register is not zero");
00089         }
00090         report.add("error_register", (uint32_t) error_register);
00091 
00092         uint8_t num = num_errors_.valid() ? num_errors_.get() : 0;
00093         std::stringstream buf;
00094         for(size_t i = 0; i < num; ++i) {
00095             if( i!= 0){
00096                 buf << ", ";
00097             }
00098             try{
00099                 ObjectStorage::Entry<uint32_t> error;
00100                 storage_->entry(error, 0x1003,i+1);
00101                 EMCYfield field(error.get());
00102                 buf << std::hex << field.error_code << "#" << field.addition_info;
00103             }
00104             catch (const std::out_of_range & e){
00105                 buf << "NOT_IN_DICT!";
00106             }
00107             catch (const TimeoutException & e){
00108                 buf << "LIST_UNDERFLOW!";
00109                 break;
00110             }
00111 
00112         }
00113         report.add("errors", buf.str());
00114 
00115     }
00116 }
00117 void EMCYHandler::handleInit(LayerStatus &status){
00118     uint8_t error_register = 0;
00119     if(!error_register_.get(error_register)){
00120         status.error("Could not read error error_register");
00121         return;
00122     }else if(error_register & 1){
00123         LOG("ER: " << int(error_register));
00124         status.error("Node has emergency error");
00125         return;
00126     }
00127 
00128     resetErrors(status);
00129 }
00130 void EMCYHandler::resetErrors(LayerStatus &status){
00131     if(num_errors_.valid()) num_errors_.set(0);
00132     has_error_ = false;
00133 }
00134 
00135 void EMCYHandler::handleRecover(LayerStatus &status){
00136     handleInit(status);
00137 }
00138 void EMCYHandler::handleShutdown(LayerStatus &status){
00139 }
00140 void EMCYHandler::handleHalt(LayerStatus &status){
00141     // do nothing
00142 }


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