sdo.cpp
Go to the documentation of this file.
00001 #include <canopen_master/canopen.h>
00002 
00003 using namespace canopen;
00004 
00005 const uint8_t COMMAND_MASK =  (1<<7) | (1<<6) | (1<<5);
00006 const uint8_t INITIATE_DOWNLOAD_REQUEST =  (0 << 5);
00007 const uint8_t INITIATE_DOWNLOAD_RESPONSE =  (1 << 5);
00008 const uint8_t DOWNLOAD_SEGMENT_REQUEST =  (1 << 5);
00009 const uint8_t DOWNLOAD_SEGMENT_RESPONSE =  (3 << 5);
00010 const uint8_t INITIATE_UPLOAD_REQUEST =  (2 << 5);
00011 const uint8_t INITIATE_UPLOAD_RESPONSE =  (2 << 5);
00012 const uint8_t UPLOAD_SEGMENT_REQUEST =  (3 << 5);
00013 const uint8_t UPLOAD_SEGMENT_RESPONSE =  (0 << 5);
00014 const uint8_t ABORT_TRANSFER_REQUEST =  (4 << 5);
00015 
00016 
00017 #pragma pack(push) /* push current alignment to stack */
00018 #pragma pack(1) /* set alignment to 1 byte boundary */
00019 
00020 struct SDOid{
00021     uint32_t id:29;
00022     uint32_t extended:1;
00023     uint32_t dynamic:1;
00024     uint32_t invalid:1;
00025     SDOid(uint32_t val){
00026         *(uint32_t*) this = val;
00027     }
00028     can::Header header() {
00029         return can::Header(id, extended, false, false);
00030     }
00031 };
00032 
00033 struct InitiateShort{
00034     uint8_t :5;
00035     uint8_t command:3;
00036     uint16_t index;
00037     uint8_t sub_index;
00038     uint8_t reserved[4];
00039 };
00040 
00041 struct InitiateLong{
00042     uint8_t size_indicated:1;
00043     uint8_t expedited:1;
00044     uint8_t num:2;
00045     uint8_t :1;
00046     uint8_t command:3;
00047     uint16_t index;
00048     uint8_t sub_index;
00049     uint8_t payload[4];
00050     
00051     size_t data_size(){
00052         if(expedited && size_indicated) return 4-num;
00053         else if(!expedited && size_indicated) return payload[0] | (payload[3]<<8);
00054         else return 0;
00055     }
00056     size_t apply_buffer(const String &buffer){
00057         size_t size = buffer.size();
00058         size_indicated = 1;
00059         if(size > 4){
00060             expedited = 0;
00061             payload[0] = size & 0xFF;
00062             payload[3] = (size >> 8) & 0xFF;
00063             return 0;
00064         }else{
00065             expedited = 1;
00066             size_indicated = 1;
00067             num = 4-size;
00068             memcpy(payload, buffer.data(), size);
00069             return size;
00070         }
00071     }
00072 };
00073 
00074 struct SegmentShort{
00075     uint8_t :4;
00076     uint8_t toggle:1;
00077     uint8_t command:3;
00078     uint8_t reserved[7];
00079 };
00080 
00081 struct SegmentLong{
00082     uint8_t done:1;
00083     uint8_t num:3;
00084     uint8_t toggle:1;
00085     uint8_t command:3;
00086     uint8_t payload[7];
00087     size_t data_size(){
00088         return 7-num;
00089     }
00090     size_t apply_buffer(const String &buffer, const size_t offset){
00091         size_t size = buffer.size() - offset;
00092         if(size > 7) size = 7;
00093         else done = 1;
00094         num = 7 - size;
00095         memcpy(payload, buffer.data() + offset, size);
00096         return offset + size;
00097     }
00098 };
00099 
00100 struct DownloadInitiateRequest: public FrameOverlay<InitiateLong>{
00101     static const uint8_t command = 1;
00102     
00103     DownloadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry, const String &buffer, size_t &offset) : FrameOverlay(h) {
00104         data.command = command;
00105         data.index = entry.index;
00106         data.sub_index = entry.sub_index;
00107         offset = data.apply_buffer(buffer);
00108    }
00109     DownloadInitiateRequest(const can::Frame &f) : FrameOverlay(f){ }
00110 };
00111 
00112 struct DownloadInitiateResponse: public FrameOverlay<InitiateShort>{
00113     static const uint8_t command = 3;
00114     
00115     DownloadInitiateResponse(const can::Frame &f) : FrameOverlay(f){ }
00116     
00117     bool test(const can::Frame  &msg, uint32_t &reason){
00118         DownloadInitiateRequest req(msg);
00119         if(req.data.command ==  DownloadInitiateRequest::command && data.index == req.data.index && data.sub_index == req.data.sub_index){
00120             return true;
00121         }
00122         reason = 0x08000000; // General error
00123         return false;
00124     }
00125 };
00126 
00127 struct DownloadSegmentRequest: public FrameOverlay<SegmentLong>{
00128     static const uint8_t command = 0;
00129     
00130     DownloadSegmentRequest(const can::Frame &f) : FrameOverlay(f){ }
00131     
00132     DownloadSegmentRequest(const Header &h, bool toggle, const String &buffer, size_t& offset) : FrameOverlay(h) {
00133         data.command = command;
00134         data.toggle = toggle?1:0;
00135         offset = data.apply_buffer(buffer, offset);
00136     }
00137 };
00138 
00139 struct DownloadSegmentResponse : public FrameOverlay<SegmentShort>{
00140     static const uint8_t command = 1;
00141     DownloadSegmentResponse(const can::Frame &f) : FrameOverlay(f) {
00142     }
00143     bool test(const can::Frame  &msg, uint32_t &reason){
00144         DownloadSegmentRequest req(msg);
00145         if (req.data.command !=  DownloadSegmentRequest::command){
00146             reason = 0x08000000; // General error
00147             return false;
00148         }else if( data.toggle != req.data.toggle){
00149             reason = 0x05030000; // Toggle bit not alternated
00150             return false;
00151         }
00152         return true;
00153     }
00154 };
00155 
00156 struct UploadInitiateRequest: public FrameOverlay<InitiateShort>{
00157     static const uint8_t command = 2;
00158     UploadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry) : FrameOverlay(h) {
00159         data.command = command;
00160         data.index = entry.index;
00161         data.sub_index = entry.sub_index;
00162    }
00163     UploadInitiateRequest(const can::Frame &f) : FrameOverlay(f){ }
00164 };
00165 
00166 struct UploadInitiateResponse: public FrameOverlay<InitiateLong>{
00167     static const uint8_t command = 2;
00168     UploadInitiateResponse(const can::Frame &f) : FrameOverlay(f) { }
00169     bool test(const can::Frame  &msg, size_t size, uint32_t &reason){
00170         UploadInitiateRequest req(msg);
00171         if(req.data.command ==  UploadInitiateRequest::command && data.index == req.data.index && data.sub_index == req.data.sub_index){
00172                 size_t ds = data.data_size();
00173                 if(ds == 0  || size == 0 || ds >= size) { // should be ==, but >= is needed for Elmo, it responses with more byte than requested
00174                     if(!data.expedited || (ds <= 4 && size <= 4)) return true;
00175                 }else{
00176                     reason = 0x06070010; // Data type does not match, length of service parameter does not match                    
00177                     return false;
00178                 }
00179         }
00180         reason = 0x08000000; // General error
00181         return false;
00182     }
00183     bool read_data(String & buffer, size_t & offset, size_t & total){
00184         if(data.size_indicated && total == 0){
00185             total = data.data_size();
00186             buffer.resize(total);
00187         }
00188         if(data.expedited){
00189             memcpy(&buffer.front(), data.payload, buffer.size());
00190             offset = buffer.size();
00191             return true;
00192         }
00193         return false;
00194     }
00195 };
00196 struct UploadSegmentRequest: public FrameOverlay<SegmentShort>{
00197     static const uint8_t command = 3;
00198     UploadSegmentRequest(const Header &h, bool toggle) : FrameOverlay(h) {
00199         data.command = command;
00200         data.toggle = toggle?1:0;
00201    }
00202     UploadSegmentRequest(const can::Frame &f) : FrameOverlay(f) { }
00203 };
00204 
00205 struct UploadSegmentResponse : public FrameOverlay<SegmentLong>{
00206     static const uint8_t command = 0;
00207     UploadSegmentResponse(const can::Frame &f) : FrameOverlay(f) {
00208     }
00209     bool test(const can::Frame  &msg, uint32_t &reason){
00210         UploadSegmentRequest req(msg);
00211         if(req.data.command !=  UploadSegmentRequest::command){
00212             reason = 0x08000000; // General error
00213             return false;
00214         }else if( data.toggle != req.data.toggle){
00215             reason = 0x05030000; // Toggle bit not alternated
00216             return false;
00217         }
00218         return true;
00219     }
00220     bool read_data(String & buffer, size_t & offset, const size_t & total){
00221         uint32_t n = data.data_size();
00222         if(total == 0){
00223             buffer.resize(offset + n);
00224         }
00225         if(offset +  n <= buffer.size()){
00226             memcpy(&buffer[offset], data.payload, n);
00227             offset +=  n;
00228             return true;
00229         }
00230         return false;
00231     }
00232 };
00233 
00234 struct AbortData{
00235     uint8_t :5;
00236     uint8_t command:3;
00237     uint16_t index;
00238     uint8_t sub_index;
00239     uint32_t reason;
00240     
00241     const char * text(){
00242         switch(reason){
00243         case 0x05030000: return "Toggle bit not alternated.";
00244         case 0x05040000: return "SDO protocol timed out.";
00245         case 0x05040001: return "Client/server command specifier not valid or unknown.";
00246         case 0x05040002: return "Invalid block size (block mode only).";
00247         case 0x05040003: return "Invalid sequence number (block mode only).";
00248         case 0x05040004: return "CRC error (block mode only).";
00249         case 0x05040005: return "Out of memory.";
00250         case 0x06010000: return "Unsupported access to an object.";
00251         case 0x06010001: return "Attempt to read a write only object.";
00252         case 0x06010002: return "Attempt to write a read only object.";
00253         case 0x06020000: return "Object does not exist in the object dictionary.";
00254         case 0x06040041: return "Object cannot be mapped to the PDO.";
00255         case 0x06040042: return "The number and length of the objects to be mapped would exceed PDO length.";
00256         case 0x06040043: return "General parameter incompatibility reason.";
00257         case 0x06040047: return "General internal incompatibility in the device.";
00258         case 0x06060000: return "Access failed due to an hardware error.";
00259         case 0x06070010: return "Data type does not match, length of service parameter does not match";
00260         case 0x06070012: return "Data type does not match, length of service parameter too high";
00261         case 0x06070013: return "Data type does not match, length of service parameter too low";
00262         case 0x06090011: return "Sub-index does not exist.";
00263         case 0x06090030: return "Invalid value for parameter (download only).";
00264         case 0x06090031: return "Value of parameter written too high (download only).";
00265         case 0x06090032: return "Value of parameter written too low (download only).";
00266         case 0x06090036: return "Maximum value is less than minimum value.";
00267         case 0x060A0023: return "Resource not available: SDO connection";
00268         case 0x08000000: return "General error";
00269         case 0x08000020: return "Data cannot be transferred or stored to the application.";
00270         case 0x08000021: return "Data cannot be transferred or stored to the application because of local control.";
00271         case 0x08000022: return "Data cannot be transferred or stored to the application because of the present device state.";
00272         case 0x08000023: return "Object dictionary dynamic generation fails or no object dictionary is present (e.g.object dictionary is generated from file and generation fails because of an file error).";
00273         case 0x08000024: return "No data available";
00274         default: return "Abort code is reserved";
00275         }
00276     }
00277 };
00278 
00279 struct AbortTranserRequest: public FrameOverlay<AbortData>{
00280     static const uint8_t command = 4;
00281     AbortTranserRequest(const can::Frame &f) : FrameOverlay(f) {}
00282     AbortTranserRequest(const Header &h, uint16_t index, uint8_t sub_index, uint32_t reason) : FrameOverlay(h) {
00283         data.command = command;
00284         data.index = index;
00285         data.sub_index = sub_index;
00286         data.reason = reason;
00287    }
00288 };
00289 
00290 #pragma pack(pop) /* pop previous alignment from stack */
00291 
00292 void SDOClient::abort(uint32_t reason){
00293     if(current_entry){
00294         interface_->send(last_msg = AbortTranserRequest(client_id, current_entry->index, current_entry->sub_index, reason));
00295     }
00296 }
00297 
00298 bool SDOClient::processFrame(const can::Frame & msg){
00299     if(msg.dlc != 8) return false;
00300     
00301     uint32_t reason = 0;
00302     switch(msg.data[0] >> 5){
00303         case DownloadInitiateResponse::command:
00304         {
00305             DownloadInitiateResponse resp(msg);
00306             if(resp.test(last_msg, reason) ){
00307                 if(offset < total){
00308                     interface_->send(last_msg = DownloadSegmentRequest(client_id, false, buffer, offset));
00309                 }else{
00310                     done = true;
00311                 }
00312             }
00313             break;
00314         }
00315         case DownloadSegmentResponse::command:
00316         {
00317             DownloadSegmentResponse resp(msg);
00318             if( resp.test(last_msg, reason) ){
00319                 if(offset < total){
00320                     interface_->send(last_msg = DownloadSegmentRequest(client_id, !resp.data.toggle, buffer, offset));
00321                 }else{
00322                     done = true;
00323                 }
00324             }
00325             break;
00326         }
00327             
00328         case UploadInitiateResponse::command:
00329         {
00330             UploadInitiateResponse resp(msg);
00331             if( resp.test(last_msg, total, reason) ){
00332                 if(resp.read_data(buffer, offset, total)){
00333                     done = true;
00334                 }else{
00335                     interface_->send(last_msg = UploadSegmentRequest(client_id, false));
00336                 }
00337             }
00338             break;
00339         }
00340         case UploadSegmentResponse::command:
00341         {
00342             UploadSegmentResponse resp(msg);
00343             if( resp.test(last_msg, reason) ){
00344                 if(resp.read_data(buffer, offset, total)){
00345                     if(resp.data.done || offset == total){
00346                         done = true;
00347                     }else{
00348                         interface_->send(last_msg = UploadSegmentRequest(client_id, !resp.data.toggle));
00349                     }
00350                 }else{
00351                     // abort, size mismatch
00352                     LOG("abort, size mismatch" << buffer.size() << " " << resp.data.data_size());
00353                     reason = 0x06070010; // Data type does not match, length of service parameter does not match
00354                 }
00355             }
00356             break;
00357         }
00358         case AbortTranserRequest::command:
00359             LOG("abort" << std::hex << (uint32_t) AbortTranserRequest(msg).data.index << "#"<< std::dec << (uint32_t) AbortTranserRequest(msg).data.sub_index << ", reason: " << AbortTranserRequest(msg).data.text());
00360             offset = 0;
00361             return false;
00362             break;
00363     }
00364     if(reason){
00365         abort(reason);
00366         offset = 0;
00367         return false;
00368     }
00369     return true;
00370 
00371 }    
00372     
00373 void SDOClient::init(){
00374     assert(storage_);
00375     assert(interface_);
00376     const canopen::ObjectDict & dict = *storage_->dict_;
00377 
00378     try{
00379         client_id = SDOid(NodeIdOffset<uint32_t>::apply(dict(0x1200, 1).value(), storage_->node_id_)).header();
00380     }
00381     catch(...){
00382         client_id = can::MsgHeader(0x600+ storage_->node_id_);
00383     }
00384     
00385     last_msg = AbortTranserRequest(client_id, 0,0,0);
00386     current_entry = 0;
00387 
00388     can::Header server_id;
00389     try{
00390         server_id = SDOid(NodeIdOffset<uint32_t>::apply(dict(0x1200, 2).value(), storage_->node_id_)).header();
00391     }
00392     catch(...){
00393         server_id = can::MsgHeader(0x580+ storage_->node_id_);
00394     }
00395     reader_.listen(interface_, server_id);
00396 }
00397 
00398 void SDOClient::transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data,  String *result){
00399     buffer = data;
00400     offset = 0;
00401     total = buffer.size();
00402     current_entry = &entry;
00403     done = false;
00404 
00405     can::BufferedReader::ScopedEnabler enabler(reader_);
00406 
00407     if(result){
00408         interface_->send(last_msg = UploadInitiateRequest(client_id, entry));
00409     }else{
00410         interface_->send(last_msg = DownloadInitiateRequest(client_id, entry, buffer, offset));
00411     }
00412 
00413     boost::this_thread::disable_interruption di;
00414     can::Frame msg;
00415 
00416     while(!done){
00417         if(!reader_.read(&msg,boost::chrono::seconds(1)))
00418         {
00419             abort(0x05040000); // SDO protocol timed out.
00420             LOG("Did not receive a response message");
00421             break;
00422         }
00423         if(!processFrame(msg)){
00424             LOG("Could not process message");
00425             break;
00426         }
00427     }
00428     if(offset == 0 || offset != total){
00429         THROW_WITH_KEY(TimeoutException("SDO"), ObjectDict::Key(*current_entry));
00430     }
00431 
00432     if(result) *result=buffer;
00433 
00434 }
00435 
00436 void SDOClient::read(const canopen::ObjectDict::Entry &entry, String &data){
00437     boost::timed_mutex::scoped_lock lock(mutex, boost::chrono::seconds(2));
00438     if(lock){
00439         transmitAndWait(entry, data, &data);
00440     }else{
00441         THROW_WITH_KEY(TimeoutException("SDO read"), ObjectDict::Key(entry));
00442     }
00443 }
00444 void SDOClient::write(const canopen::ObjectDict::Entry &entry, const String &data){
00445     boost::timed_mutex::scoped_lock lock(mutex, boost::chrono::seconds(2));
00446     if(lock){
00447         transmitAndWait(entry, data, 0);
00448     }else{
00449         THROW_WITH_KEY(TimeoutException("SDO write"), ObjectDict::Key(entry));
00450     }
00451 }


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