pdo.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 class PDOid{
00009     const uint32_t value_;
00010 public:
00011     static const unsigned int ID_MASK = (1u << 29)-1;
00012     static const unsigned int EXTENDED_MASK = (1u << 29);
00013     static const unsigned int NO_RTR_MASK = (1u << 30);
00014     static const unsigned int INVALID_MASK = (1u << 31);
00015 
00016     PDOid(const uint32_t &val)
00017     : value_(val)
00018     {}
00019     can::Header header(bool fill_rtr = false) const {
00020         return can::Header(value_ & ID_MASK, value_ & EXTENDED_MASK, fill_rtr && !(value_ & NO_RTR_MASK), false);
00021     }
00022     bool isInvalid() const { return value_ & INVALID_MASK; }
00023 };
00024 
00025 struct PDOmap{
00026     uint8_t length;
00027     uint8_t sub_index;
00028     uint16_t index;
00029     PDOmap(uint32_t val)
00030     : length(val & 0xFF),
00031       sub_index((val>>8) & 0xFF),
00032       index(val>>16)
00033     {}
00034 };
00035 
00036 #pragma pack(pop) /* pop previous alignment from stack */
00037 
00038 
00039 const uint8_t SUB_COM_NUM = 0;
00040 const uint8_t SUB_COM_COB_ID = 1;
00041 const uint8_t SUB_COM_TRANSMISSION_TYPE = 2;
00042 const uint8_t SUB_COM_RESERVED = 4;
00043 
00044 const uint8_t SUB_MAP_NUM = 0;
00045 
00046 const uint16_t RPDO_COM_BASE =0x1400;
00047 const uint16_t RPDO_MAP_BASE =0x1600;
00048 const uint16_t TPDO_COM_BASE =0x1800;
00049 const uint16_t TPDO_MAP_BASE =0x1A00;
00050 
00051 bool check_com_changed(const ObjectDict &dict, const uint16_t com_id){
00052     bool com_changed = false;
00053     
00054     // check if com parameter has to be set
00055     for(uint8_t sub = 0; sub <=6 ; ++sub){
00056         try{
00057             if(!dict(com_id,sub).init_val.is_empty()){
00058                 com_changed = true;
00059                 break;
00060             }
00061         }
00062         catch (std::out_of_range) {}
00063     }
00064     return com_changed;
00065 }
00066 
00067 bool check_map_changed(const uint8_t &num, const ObjectDict &dict, const uint16_t &map_index){
00068     bool map_changed = false;
00069 
00070     // check if mapping has to be set
00071     if(num <= 0x40){
00072         for(uint8_t sub = 1; sub <=num ; ++sub){
00073             try{
00074                 if(!dict(map_index,sub).init_val.is_empty()){
00075                     map_changed = true;
00076                     break;
00077                 }
00078             }
00079             catch (std::out_of_range) {}
00080         }
00081     }else{
00082         map_changed = dict( map_index ,0 ).init_val.is_empty();
00083     }
00084     return map_changed;
00085 }
00086 void PDOMapper::PDO::parse_and_set_mapping(const boost::shared_ptr<ObjectStorage> &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write){
00087                             
00088     const canopen::ObjectDict & dict = *storage->dict_;
00089     
00090     ObjectStorage::Entry<uint8_t> num_entry;
00091     storage->entry(num_entry, map_index, SUB_MAP_NUM);
00092 
00093     uint8_t map_num;
00094     
00095     try{
00096         map_num = num_entry.desc().value().get<uint8_t>();
00097     }catch(...){
00098         map_num = 0;
00099     }
00100     
00101     bool map_changed = check_map_changed(map_num, dict, map_index);
00102     
00103     // disable PDO if needed
00104     ObjectStorage::Entry<uint32_t> cob_id;
00105     storage->entry(cob_id, com_index, SUB_COM_COB_ID);
00106     
00107     bool com_changed = check_com_changed(dict, map_index);
00108     if((map_changed || com_changed) && cob_id.desc().writable){
00109         cob_id.set(cob_id.get() | PDOid::INVALID_MASK);
00110     }
00111     if(map_num > 0 && map_num <= 0x40){ // actual mapping 
00112         if(map_changed){
00113             num_entry.set(0);
00114         }
00115         
00116         frame.dlc = 0;
00117         for(uint8_t sub = 1; sub <=map_num; ++sub){
00118             ObjectStorage::Entry<uint32_t> mapentry;
00119             storage->entry(mapentry, map_index, sub);
00120             const HoldAny init = dict(map_index ,sub).init_val;
00121             if(!init.is_empty()) mapentry.set(init.get<uint32_t>());
00122             
00123             PDOmap param(mapentry.get_cached());
00124             boost::shared_ptr<Buffer> b = boost::make_shared<Buffer>(param.length/8);
00125             if(param.index < 0x1000){
00126                 // TODO: check DummyUsage
00127             }else{
00128                 ObjectStorage::ReadDelegate rd;
00129                 ObjectStorage::WriteDelegate wd;
00130                 if(read) rd = ObjectStorage::ReadDelegate(b.get(), &Buffer::read);
00131                 if(read || write) wd = ObjectStorage::WriteDelegate(b.get(), &Buffer::write); // set writer for buffer setup or as write delegate
00132                 size_t l = storage->map(param.index, param.sub_index, rd, wd);
00133                 assert(l  == param.length/8);
00134             }
00135             
00136             frame.dlc += b->size;
00137             assert( frame.dlc <= 8 );
00138             b->clean();
00139             buffers.push_back(b);
00140         }
00141     }
00142     if(com_changed){
00143         uint8_t subs = dict(com_index, SUB_COM_NUM).value().get<uint8_t>();
00144         for(uint8_t i = SUB_COM_NUM+1; i <= subs; ++i){
00145             if(i == SUB_COM_COB_ID || i == SUB_COM_RESERVED) continue;
00146             try{
00147                 storage->init(ObjectDict::Key(com_index, i));
00148             }
00149             catch (const std::out_of_range &){
00150                 // entry was not provided, so skip it
00151             }
00152         }
00153     }
00154     if(map_changed){
00155         num_entry.set(map_num);
00156     }
00157     if((com_changed || map_changed) && cob_id.desc().writable){
00158         storage->init(ObjectDict::Key(com_index, SUB_COM_COB_ID));
00159         
00160         cob_id.set(NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_));
00161     }
00162         
00163     
00164 }
00165 PDOMapper::PDOMapper(const boost::shared_ptr<can::CommInterface> interface)
00166 :interface_(interface)
00167 {
00168 }
00169 bool PDOMapper::init(const boost::shared_ptr<ObjectStorage> storage, LayerStatus &status){
00170     boost::mutex::scoped_lock lock(mutex_);
00171 
00172     try{
00173         rpdos_.clear();
00174 
00175         const canopen::ObjectDict & dict = *storage->dict_;
00176         for(uint16_t i=0; i < 512 && rpdos_.size() < dict.device_info.nr_of_tx_pdo;++i){ // TPDOs of device
00177             if(!dict.has(TPDO_COM_BASE + i,0) && !dict.has(TPDO_MAP_BASE + i,0)) continue;
00178 
00179             boost::shared_ptr<RPDO> rpdo = RPDO::create(interface_,storage, TPDO_COM_BASE + i, TPDO_MAP_BASE + i);
00180             if(rpdo){
00181                 rpdos_.insert(rpdo);
00182             }
00183         }
00184         // LOG("RPDOs: " << rpdos_.size());
00185 
00186         tpdos_.clear();
00187         for(uint16_t i=0; i < 512 && tpdos_.size() <  dict.device_info.nr_of_rx_pdo;++i){ // RPDOs of device
00188             if(!dict.has(RPDO_COM_BASE + i,0) && !dict.has(RPDO_MAP_BASE + i,0)) continue;
00189 
00190             boost::shared_ptr<TPDO> tpdo = TPDO::create(interface_,storage, RPDO_COM_BASE + i, RPDO_MAP_BASE + i);
00191             if(tpdo){
00192                 tpdos_.insert(tpdo);
00193             }
00194         }
00195         // LOG("TPDOs: " << tpdos_.size());
00196 
00197         return true;
00198     }
00199     catch(const std::out_of_range &e){
00200         status.error(std::string("PDO error: ") + e.what());
00201         return false;
00202     }
00203 }
00204 
00205 
00206 bool PDOMapper::RPDO::init(const boost::shared_ptr<ObjectStorage> &storage, const uint16_t &com_index, const uint16_t &map_index){
00207     boost::mutex::scoped_lock lock(mutex);
00208     listener_.reset();
00209     const canopen::ObjectDict & dict = *storage->dict_;
00210     parse_and_set_mapping(storage, com_index, map_index, true, false);
00211     
00212     PDOid pdoid( NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_) );
00213 
00214     if(buffers.empty() || pdoid.isInvalid()){
00215        return false;
00216     }
00217 
00218     frame = pdoid.header(true);
00219 
00220     transmission_type = dict(com_index, SUB_COM_TRANSMISSION_TYPE).value().get<uint8_t>();
00221     
00222     listener_ = interface_->createMsgListener(pdoid.header() ,can::CommInterface::FrameDelegate(this, &RPDO::handleFrame));
00223     
00224     return true;
00225 }
00226 
00227 bool PDOMapper::TPDO::init(const boost::shared_ptr<ObjectStorage> &storage, const uint16_t &com_index, const uint16_t &map_index){
00228     boost::mutex::scoped_lock lock(mutex);
00229     const canopen::ObjectDict & dict = *storage->dict_;
00230 
00231     
00232     PDOid pdoid( NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_) );
00233     frame = pdoid.header();
00234     
00235     parse_and_set_mapping(storage, com_index, map_index, false, true);
00236     if(buffers.empty() || pdoid.isInvalid()){
00237        return false;
00238     }
00239     ObjectStorage::Entry<uint8_t> tt;
00240     storage->entry(tt, com_index, SUB_COM_TRANSMISSION_TYPE);
00241     transmission_type = tt.desc().value().get<uint8_t>();
00242 
00243     if(transmission_type != 1 && transmission_type <=240){
00244         tt.set(1); // enforce 1 for compatibility
00245     }
00246     return true;
00247 }
00248 
00249 void PDOMapper::TPDO::sync(){
00250     boost::mutex::scoped_lock lock(mutex);
00251     
00252     bool updated = false;
00253     size_t len = frame.dlc;
00254     uint8_t * dest = frame.data.c_array();
00255     for(std::vector< boost::shared_ptr<Buffer> >::iterator b_it = buffers.begin(); b_it != buffers.end(); ++b_it){
00256         Buffer &b = **b_it;
00257         if(len >= b.size){
00258             updated = b.read(dest, len) || updated;
00259             len -= b.size;
00260             dest += b.size;
00261         }else{
00262             // ERROR
00263         }
00264     }
00265     
00266     if( len != 0){
00267         // ERROR
00268     }
00269     if(updated){
00270         interface_->send( frame );
00271     }else{
00272         // TODO: Notify 
00273     }
00274 }
00275 
00276 void PDOMapper::RPDO::sync(LayerStatus &status){
00277     boost::mutex::scoped_lock lock(mutex);
00278     if((transmission_type >= 1 && transmission_type <= 240) || transmission_type == 0xFC){ // cyclic
00279         if(timeout > 0){
00280             --timeout;
00281         }else if(timeout == 0) {
00282             status.warn("RPDO timeout");
00283         }
00284     }
00285     if(transmission_type == 0xFC || transmission_type == 0xFD){
00286         if(frame.is_rtr){
00287             interface_->send(frame);
00288         }
00289     }
00290 }
00291 
00292 void PDOMapper::RPDO::handleFrame(const can::Frame & msg){
00293     size_t offset = 0;
00294     const uint8_t * src = msg.data.data();
00295     for(std::vector<boost::shared_ptr<Buffer> >::iterator it = buffers.begin(); it != buffers.end(); ++it){
00296         Buffer &b = **it;
00297         
00298         if( offset + b.size <= msg.dlc ){
00299             b.write(src+offset, b.size);
00300             offset += b.size;
00301         }else{
00302             // ERROR
00303         }
00304     }
00305     if( offset != msg.dlc ){
00306         // ERROR
00307     }
00308     {
00309         boost::mutex::scoped_lock lock(mutex);
00310         if(transmission_type >= 1 && transmission_type <= 240){
00311             timeout = transmission_type + 2;
00312         }else if(transmission_type == 0xFC || transmission_type == 0xFD){
00313             if(frame.is_rtr){
00314                 timeout = 1+2;
00315             }
00316         }
00317     }
00318 }
00319 
00320 void PDOMapper::read(LayerStatus &status){
00321     boost::mutex::scoped_lock lock(mutex_);
00322     for(boost::unordered_set<boost::shared_ptr<RPDO> >::iterator it = rpdos_.begin(); it != rpdos_.end(); ++it){
00323         (*it)->sync(status);
00324     }
00325 }
00326 bool PDOMapper::write(){
00327     boost::mutex::scoped_lock lock(mutex_);
00328     for(boost::unordered_set<boost::shared_ptr<TPDO> >::iterator it = tpdos_.begin(); it != tpdos_.end(); ++it){
00329         (*it)->sync();
00330     }
00331     return true; // TODO: check for errors
00332 }
00333 
00334 bool PDOMapper::Buffer::read(uint8_t* b, const size_t len){
00335     boost::mutex::scoped_lock lock(mutex);
00336     if(size > len){
00337         BOOST_THROW_EXCEPTION( std::bad_cast() );
00338     }
00339     if(empty) return false;
00340     
00341     memcpy(b,&buffer[0], size);
00342     bool was_dirty = dirty;
00343     dirty = false;
00344     return was_dirty;
00345 }
00346 void PDOMapper::Buffer::write(const uint8_t* b, const size_t len){
00347     boost::mutex::scoped_lock lock(mutex);
00348     if(size > len){
00349         BOOST_THROW_EXCEPTION( std::bad_cast() );
00350     }
00351     empty = false;
00352     dirty = true;
00353     memcpy(&buffer[0], b, size);
00354 }
00355 void PDOMapper::Buffer::read(const canopen::ObjectDict::Entry &entry, String &data){
00356     boost::mutex::scoped_lock lock(mutex);
00357     time_point abs_time = get_abs_time(boost::chrono::seconds(1));
00358     if(size != data.size()){
00359         THROW_WITH_KEY(std::bad_cast(), ObjectDict::Key(entry));
00360     }
00361     if(empty){
00362         THROW_WITH_KEY(TimeoutException("PDO data empty"), ObjectDict::Key(entry));
00363     }
00364     if(dirty){
00365         data.assign(buffer.begin(), buffer.end());
00366         dirty = false;
00367     }
00368 }
00369 void PDOMapper::Buffer::write(const canopen::ObjectDict::Entry &entry, const String &data){
00370     boost::mutex::scoped_lock lock(mutex);
00371     if(size != data.size()){
00372         THROW_WITH_KEY(std::bad_cast(), ObjectDict::Key(entry));
00373     }
00374     empty = false;
00375     dirty = true;
00376     buffer.assign(data.begin(),data.end());
00377 }


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