cp1616_io_device.cpp
Go to the documentation of this file.
00001 /*********************************************************************************************/
00021 #ifndef CP1616_IO_DEVICE_CPP
00022 #define CP1616_IO_DEVICE_CPP
00023 
00024 #include <cp1616/cp1616_io_device.h>
00025 #include <cp1616/cp1616_io_device_callbacks.h>
00026 
00027 namespace cp1616
00028 {
00029 //Define and initialize device_instance_ to zero value;
00030 Cp1616IODevice *Cp1616IODevice::device_instance_ = 0;
00031 
00032 Cp1616IODevice* Cp1616IODevice::getDeviceInstance()
00033 {
00034   if( !device_instance_ )
00035   {
00036     device_instance_ = new Cp1616IODevice();
00037   }
00038   return device_instance_;
00039 }
00040 
00041 Cp1616IODevice::Cp1616IODevice():
00042     cp_id_(1),
00043     cp_handle_(0),
00044     cp_session_key_(0),
00045     cp_ar_number_(0),
00046     ar_info_ind_flag_(0),
00047     prm_end_ind_flag_(0),
00048     indata_ind_flag_(0),
00049     offline_ind_flag_(0),
00050     p_device_data_(NULL)
00051 {
00052 
00053     p_device_data_ = new DeviceData [DEVICE_DATA_ENTRIES * sizeof(DeviceData)];
00054 
00055     //Initialize device_data_ according to STEP7 project setup
00056     p_device_data_[0].slot =  1;
00057     p_device_data_[0].subslot = 1;
00058     p_device_data_[0].modId = 0x19;
00059     p_device_data_[0].subslotId = 0x010001;
00060     p_device_data_[0].api = 0;
00061     p_device_data_[0].maxSubslots = 0;
00062     p_device_data_[0].modState = 0;
00063     p_device_data_[0].subState = 0;
00064     p_device_data_[0].dir = 0;
00065 
00066     p_device_data_[1].slot =  2;
00067     p_device_data_[1].subslot = 1;
00068     p_device_data_[1].modId = 0x23;
00069     p_device_data_[1].subslotId = 0x0001;
00070     p_device_data_[1].api = 0;
00071     p_device_data_[1].maxSubslots = 0;
00072     p_device_data_[1].modState = 0;
00073     p_device_data_[1].subState = 0;
00074     p_device_data_[1].dir = 0;
00075 
00076     //Allocate memory for input/output module variables
00077     cp_number_of_slots_ = NUMOF_SLOTS;
00078     cp_max_number_of_subslots_ = NUMOF_SUBSLOTS;
00079     
00080     unsigned int i,j,k;
00081     std::vector<PNIO_IOXS>   temp_ioxs;
00082     std::vector<PNIO_UINT32> temp_uint32;
00083     
00084     for(i = 0; i < cp_number_of_slots_; i++)
00085     {
00086       for(j = 0; j < cp_max_number_of_subslots_; j++)
00087         temp_ioxs.push_back(PNIO_S_GOOD);
00088         temp_uint32.push_back(0);
00089              
00090       input_data_length_.push_back(temp_uint32);
00091       input_data_iocs_.push_back(temp_ioxs);
00092       input_data_iops_.push_back(temp_ioxs);
00093       
00094       output_data_length_.push_back(temp_uint32);
00095       output_data_iocs_.push_back(temp_ioxs);
00096       output_data_iops_.push_back(temp_ioxs);
00097       
00098       temp_ioxs.clear();
00099       temp_uint32.clear();
00100     }
00101     
00102     std::vector<PNIO_UINT8> temp_data_items;
00103     std::vector<std::vector<PNIO_UINT8> > temp_submodules; 
00104     
00105     for(i = 0; i < cp_number_of_slots_; i++)
00106     {
00107       for(j = 0; j < cp_max_number_of_subslots_; j++)
00108       {
00109         for(k = 0; k < input_data_length_[i][j]; k++)
00110           temp_data_items.push_back(0); 
00111       
00112         temp_submodules.push_back(temp_data_items);
00113       }
00114       input_data_.push_back(temp_submodules);
00115       output_data_.push_back(temp_submodules);
00116     }
00117         
00118 }
00119 
00120 Cp1616IODevice::~Cp1616IODevice()
00121 {
00122   delete p_device_data_;
00123 }
00124 
00125 int Cp1616IODevice::init()
00126 {
00127   PNIO_UINT32 error_code = PNIO_OK;
00128   PNIO_UINT32 ui_max_ar = 1;              //maximum application relationship supported
00129   cp_handle_ = 0;
00130 
00131   PNIO_ANNOTATION struct_pnio_annotation = {
00132     ANNOT_NAME,
00133     ANNOT_ORDERID,
00134     ANNOT_HW_REV,
00135     ANNOT_SW_PREFIX,
00136     ANNOT_SW_REV_1,
00137     ANNOT_SW_REV_2,
00138     ANNOT_SW_REV_3};
00139 
00140 
00141   PNIO_CFB_FUNCTIONS struct_cb_functions;
00142 
00143   //Initialize the callback structure
00144   //Set the callback function pointers
00145   memset(&struct_cb_functions, 0, sizeof(PNIO_CFB_FUNCTIONS));
00146   struct_cb_functions.size                  = sizeof(PNIO_CFB_FUNCTIONS);
00147   struct_cb_functions.cbf_data_write        = pnio_device_callbacks::dataWrite;
00148   struct_cb_functions.cbf_data_read         = pnio_device_callbacks::dataRead;
00149   struct_cb_functions.cbf_rec_read          = pnio_device_callbacks::recordRead;
00150   struct_cb_functions.cbf_rec_write         = pnio_device_callbacks::recordWrite;
00151   struct_cb_functions.cbf_alarm_done        = pnio_device_callbacks::requestDone;
00152   struct_cb_functions.cbf_check_ind         = pnio_device_callbacks::checkIndication;
00153   struct_cb_functions.cbf_ar_check_ind      = pnio_device_callbacks::arCheckIndication;
00154   struct_cb_functions.cbf_ar_info_ind       = pnio_device_callbacks::arInfoIndication;
00155   struct_cb_functions.cbf_ar_indata_ind     = pnio_device_callbacks::arIndataIndication;
00156   struct_cb_functions.cbf_ar_abort_ind      = pnio_device_callbacks::arAbortIndication;
00157   struct_cb_functions.cbf_ar_offline_ind    = pnio_device_callbacks::arOfflineIndication;
00158   struct_cb_functions.cbf_apdu_status_ind   = pnio_device_callbacks::apduStatusIndication;
00159   struct_cb_functions.cbf_prm_end_ind       = pnio_device_callbacks::prmEndIndication;
00160   struct_cb_functions.cbf_cp_stop_req       = pnio_device_callbacks::cpStopRequest;
00161   struct_cb_functions.cbf_device_stopped    = pnio_device_callbacks::deviceStopped;
00162   struct_cb_functions.cbf_start_led_flash   = NULL;
00163   struct_cb_functions.cbf_stop_led_flash    = NULL;
00164 
00165   error_code = PNIO_device_open(
00166                       cp_id_,                      //Communication Module index
00167                       PNIO_CEP_MODE_CTRL,            //permission to change operation mode
00168                       VENDOR_ID,                     //vendor ID
00169                       DEVICE_ID,                     //device ID
00170                       INSTANCE_ID,                   //instance ID
00171                       ui_max_ar,                     //max AR count
00172                       &struct_pnio_annotation,       //annotation
00173                       &struct_cb_functions,          //callback functions information
00174                       &cp_handle_);                  //device handle
00175 
00176   ROS_INFO("Device handle: 0x%x", cp_handle_);
00177 
00178   //Check errors
00179   if(error_code != PNIO_OK)
00180     ROS_ERROR("Not able to open PNIO_device: Error: 0x%x", (int)error_code);
00181    else
00182     ROS_INFO_STREAM("Openning CP1616 in IO_device mode: done");
00183 
00184   return error_code;  //if everything ok, return PNIO_OK
00185 }
00186 
00187 int Cp1616IODevice::uinit()
00188 {
00189   PNIO_UINT32 error_code = PNIO_OK;
00190 
00191   error_code = PNIO_device_close(cp_handle_);
00192 
00193   if(error_code != PNIO_OK)
00194     ROS_ERROR("Not able to uninitialize: Error 0x%x", (int) error_code);
00195 
00196   ROS_INFO_STREAM("Closing PNIO_device: done ");
00197   return (int)error_code;  //if everything ok, return PNIO_OK
00198 }
00199 
00200 int Cp1616IODevice::addApi()
00201 {
00202   int i,j;
00203   int highest_slots_number;
00204   int highest_subslot_number = 0;
00205 
00206   PNIO_UINT32 api;
00207   PNIO_UINT32 error_code = PNIO_OK;
00208 
00209   //for each slot
00210   for(i = j = 0; i < DEVICE_DATA_ENTRIES; i++)
00211   {
00212     //read api from configuration data
00213     api = p_device_data_[i].api;
00214 
00215     //look if api added at a prior position
00216     for(j = 0; j < i; j++)
00217     {
00218       if(api == p_device_data_[j].api){
00219       //api was added
00220       break;
00221     }
00222   }
00223 
00224   if(i == j)   // not added, add a new api
00225   {
00226     // calculate highest slot and subslot number for this api
00227     highest_slots_number   = p_device_data_[j].slot;
00228     highest_subslot_number = p_device_data_[j].subslot;
00229 
00230     //check if the api exists in the slots ahead,
00231     //if yes, then update highest slot/subslot number accordingly
00232 
00233     for(j = i+1; j <  DEVICE_DATA_ENTRIES; j++)
00234     {
00235       if(api == p_device_data_[j].api)
00236       {
00237         if(p_device_data_[j].slot > highest_slots_number) highest_slots_number = p_device_data_[j].slot;
00238         if(p_device_data_[j].subslot > highest_subslot_number) highest_subslot_number = p_device_data_[j].subslot;
00239       }
00240     }
00241 
00242     error_code = PNIO_api_add(
00243       cp_handle_,
00244       api,
00245       (PNIO_UINT16) highest_slots_number,
00246       (PNIO_UINT16) highest_subslot_number);
00247 
00248     if(error_code != PNIO_OK)
00249     {
00250       ROS_ERROR("Not able to add Api profile: Error 0x%x", (int) error_code);
00251       return (int)error_code;  //leave for-loop immediately
00252     }
00253   }
00254  }
00255   ROS_INFO_STREAM("Adding Api profile: done");
00256   return (int)error_code;  //if everything ok, return PNIO_OK
00257 }
00258 
00259 int Cp1616IODevice::removeApi()
00260 {
00261   int i,j;
00262   PNIO_UINT32 api;
00263   PNIO_UINT32 error_code = PNIO_OK;
00264 
00265   //for each slot
00266   for(i = j = 0; i < DEVICE_DATA_ENTRIES && error_code == PNIO_OK; i++)
00267   {
00268     //read api from configuration data
00269     api = p_device_data_[i].api;
00270 
00271     //look if the api has been added at a prior position in our g_device_data structure
00272     for(j = 0; j < i; j++)
00273     {
00274       if(api == p_device_data_[j].api) break; // api added at a prior position, hence it has already been removed
00275     }
00276 
00277     if(i == j) //api not removed yet
00278     {
00279       error_code = PNIO_api_remove(
00280                   cp_handle_,
00281                   api);
00282 
00283       if(error_code != PNIO_OK)
00284       {
00285         ROS_ERROR("Not able to remove Api profile: Error: 0x%x", (int)error_code);
00286         return (int)error_code;  //leave for-loop immediately
00287       }
00288       else
00289         ROS_INFO_STREAM("Removing Api profile: done");
00290     }
00291   }
00292   return (int)error_code;  //if everything ok, return PNIO_OK
00293 }
00294 
00295 int Cp1616IODevice::addModSubMod()
00296 {
00297   PNIO_UINT32 error_code = PNIO_OK;
00298   PNIO_DEV_ADDR addr;     //location (module/submodule)
00299   int slot = 0;
00300   int i;
00301 
00302   addr.AddrType = PNIO_ADDR_GEO;    //must be PNIO_ADDR_GEO
00303 
00304   //Add module 0
00305   
00306   addr.u.Geo.Slot    = p_device_data_[0].slot;    //plug module 0
00307   addr.u.Geo.Subslot = p_device_data_[0].subslot; //get the corresponding sub-slot
00308 
00309   error_code = PNIO_mod_plug(
00310                cp_handle_,                            //device handle
00311                p_device_data_[0].api,             //api number
00312                &addr,                             //location(slot, subslot)
00313                p_device_data_[0].modId);          //module 0 identifier
00314 
00315 
00316   if(error_code != PNIO_OK)
00317   {
00318     ROS_ERROR("Not able to add module 0: Error 0x%x", (int)error_code);
00319     p_device_data_[0].modState = 0;
00320   }
00321   else
00322   {
00323     ROS_INFO_STREAM("Plugging Module 0: done");
00324     p_device_data_[0].modState = 1;
00325   }
00326 
00327   if(!p_device_data_[0].modState)
00328   {
00329     ROS_ERROR_STREAM("ERROR: Failure in plugging module 0 -> no other module / submodule will be plugged...");
00330     return (int)error_code;
00331   }
00332 
00333   //Add submodule corresponding to module 0
00334   error_code = PNIO_sub_plug (
00335                 cp_handle_,                      //device handle
00336                 p_device_data_[0].api,           // api number
00337                 &addr,                           // location (slot, subslot)
00338                 p_device_data_[0].subslotId);    // submodule 0 identifier
00339 
00340 
00341   if(error_code != PNIO_OK)
00342   {
00343     ROS_ERROR("Not able to add submodule 0 to module 0: Error 0x%x", (int) error_code);
00344     p_device_data_[0].subState = 0;
00345   }
00346   else
00347   {
00348     ROS_INFO_STREAM("Pluging submodule 0 to module 0: done ");
00349     p_device_data_[0].subState = 1;
00350   }
00351 
00352   if(!p_device_data_[0].subState)
00353   {
00354     ROS_ERROR_STREAM("ERROR: Failure in plugging the submodule corresponding to module 0"
00355                       << "-> no other module / submodule will be plugged...");
00356 
00357     return (int)error_code;  //if everything ok, return PNIO_OK
00358   }
00359 
00360   //Add all modules
00361   if(NUMOF_SLOTS > 1)
00362   {
00363 
00364     for(i = 1; i < DEVICE_DATA_ENTRIES;)
00365     {
00366       addr.u.Geo.Slot    = p_device_data_[i].slot;           //plug module at correct slot
00367       addr.u.Geo.Subslot = p_device_data_[i].subslot;    //get the corresponding sub-slot
00368 
00369 
00370 
00371       error_code = PNIO_mod_plug(
00372              cp_handle_,                 //device handle
00373              p_device_data_[i].api,      //api number
00374              &addr,                      //location(slot, subslot)
00375              p_device_data_[i].modId);   //module identifier
00376 
00377       if(error_code != PNIO_OK)
00378       {
00379         ROS_ERROR("Not able to plug module: Error 0x%x", (int) error_code);
00380         p_device_data_[i].modState = 0;
00381         return (int)error_code;
00382       }
00383       else
00384       {
00385         ROS_INFO("Plugging module %d: done", i);
00386         p_device_data_[i].modState = 1;
00387       }
00388 
00389       if(error_code == PNIO_OK)
00390       {
00391         //advance in the g_device_data structure jumping over all the submodule entries
00392         //to reach the next module entry in the structure
00393         i += p_device_data_[i].maxSubslots;
00394       }
00395 
00396       else
00397       {
00398         //go to the next entry in p_device_data table
00399         i++;
00400       }
00401 
00402     } //end for
00403 
00404     //Add all submodules
00405     for(i = 1; i < DEVICE_DATA_ENTRIES; i++)
00406     {
00407       if(p_device_data_[i].maxSubslots > 0)
00408       {
00409         //beginning of a new slot
00410         slot = i;   //index of corresponding slot for a given subslot
00411 
00412         p_device_data_[slot].subState = 1;
00413       }
00414 
00415       if(p_device_data_[slot].modState)
00416       {
00417         //add submodule only if the module is added
00418         addr.u.Geo.Slot     = p_device_data_[i].slot;
00419         addr.u.Geo.Subslot  = p_device_data_[i].subslot;
00420 
00421         error_code = PNIO_sub_plug (
00422              cp_handle_,                    //device handle
00423              p_device_data_[i].api,         //api number
00424              &addr,                         //location(slot, subslot)
00425              p_device_data_[i].subslotId);  //submodule identifier
00426 
00427         if(error_code != PNIO_OK)
00428         {
00429           ROS_ERROR("Not able to plug submodule: Error 0x%x", (int) error_code);
00430           p_device_data_[i].subState = 0;
00431           p_device_data_[slot].subState = 0;
00432           return (int)error_code;
00433         }
00434         else
00435         {
00436           ROS_INFO("Plugging submodule to module %d: done", i);
00437           p_device_data_[i].subState = 1;
00438         }
00439       }
00440     }  //end for
00441   }
00442 
00443   //if not all the modules/submodules are plugged correctly, print warning
00444   for(i = 0; i < DEVICE_DATA_ENTRIES; i++)
00445   {
00446     if(p_device_data_[i].subState == 0)
00447       {
00448         ROS_WARN_STREAM("Not all modules or submodules were plugged correctly!!");
00449         break;
00450       }
00451     }
00452   return (int)error_code; //if everything ok, return PNIO_OK
00453 }
00454 
00455 int Cp1616IODevice::removeModSubMod()
00456 {
00457   int i;
00458   PNIO_DEV_ADDR addr;    //location module/submodule
00459   PNIO_UINT32 error_code = PNIO_OK;
00460 
00461   //Remove modules/submodules in reverse order
00462   for(i = DEVICE_DATA_ENTRIES -1; i >= 0 && error_code == PNIO_OK; i--)
00463   {
00464     if(p_device_data_[i].subState == 1)
00465     {
00466       addr.AddrType      = PNIO_ADDR_GEO;          //must be PNIO_ADDR_GEO
00467       addr.u.Geo.Slot    = p_device_data_[i].slot;  //slot number
00468       addr.u.Geo.Subslot = p_device_data_[i].subslot;
00469 
00470       //Remove submodules
00471       error_code = PNIO_sub_pull(
00472                   cp_handle_,
00473                   p_device_data_[i].api, &addr);
00474 
00475       if(error_code != PNIO_OK)
00476         ROS_ERROR("Not able to remove submodule Error 0x%x", (int) error_code);
00477       else
00478       {
00479         ROS_INFO("Removing submodule from module %d: done", i);
00480         p_device_data_[i].subState = 0;
00481         return (int)error_code;
00482       }
00483 
00484       //Notify the controller that the device state is NOT-OK every time after removing a submodule
00485       error_code = PNIO_set_dev_state(cp_handle_, PNIO_DEVSTAT_STATION_PROBLEM);
00486     }
00487 
00488     if(error_code == PNIO_OK && p_device_data_[i].modState == 1)
00489     {
00490       addr.AddrType      = PNIO_ADDR_GEO;                  //must be PNIO_ADDR_GEO
00491       addr.u.Geo.Slot    = p_device_data_[i].slot;         //slot number
00492       addr.u.Geo.Subslot = 1;                              //doesn't matter
00493 
00494       //Remove modules
00495       error_code = PNIO_mod_pull(cp_handle_, p_device_data_[i].api, &addr);
00496 
00497       if(error_code != PNIO_OK)
00498         ROS_ERROR("Not able to remove module: Error 0x%x", (int) error_code);
00499       else
00500       {
00501         ROS_INFO("Removing module %d: done", i);
00502         p_device_data_[i].subState = 0;
00503         return (int)error_code;
00504       }
00505 
00506       //Notify the controller that the device state is NOT-OK every time after removing a module
00507       error_code = PNIO_set_dev_state(cp_handle_, PNIO_DEVSTAT_STATION_PROBLEM);
00508     }
00509   }
00510 }
00511 
00512 int Cp1616IODevice::startOperation()
00513 {
00514   PNIO_UINT32 error_code = PNIO_OK;
00515 
00516   error_code = PNIO_device_start(cp_handle_);
00517   if (error_code != PNIO_OK)
00518     ROS_ERROR("Not able to start IO Device operation: Error 0x%x", (int)error_code);
00519   else
00520       ROS_INFO_STREAM("Starting operation: done");
00521 
00522   if(error_code == PNIO_OK)
00523   {
00524     error_code = PNIO_set_dev_state(cp_handle_, PNIO_DEVSTAT_OK);
00525     if(error_code != PNIO_OK)
00526       ROS_ERROR("Not able to set PNIO device state: Error 0x%x", (int)error_code);
00527     else
00528       ROS_INFO("Setting device state to PNIO_DEVSTAT_OK: done");
00529   }
00530 
00531   //Waiting for initialization callbacks
00532   unsigned int i = 0;
00533 
00534   ROS_INFO_STREAM("Waiting for callbacks...");
00535 
00536   while(i != MAX_PRM_END_COUNT)
00537   {
00538     //prmEndInd() callback already called?
00539     if(prm_end_ind_flag_ == 1)
00540     {
00541       this->doAfterPrmEndIndCbf();
00542       break;
00543     }
00544 
00545     usleep(WAIT_FOR_CALLBACK_PERIOD);
00546     i++;
00547   }
00548 
00549   if(i < MAX_PRM_END_COUNT) //if PRM_END_IND_FLAG == 1
00550   {
00551     i = 0;
00552     while(i != MAX_INDATA_IND_COUNT)
00553     {
00554       //indataInd() callback already called?
00555       if(indata_ind_flag_ == 1)
00556       {
00557         this->doAfterIndataIndCbf();
00558         break;
00559       }
00560 
00561       usleep(WAIT_FOR_CALLBACK_PERIOD);
00562       i++;
00563     }
00564 
00565     if(i == MAX_INDATA_IND_COUNT)
00566     {
00567       ROS_ERROR("PNIOCbfIndataInd callback not recieved within defined period");
00568       error_code = -1;
00569     }
00570   }
00571   else
00572   {
00573     ROS_ERROR("PNIOCbfPrmEndInd callback not recieved within defined period");
00574     error_code = -1;
00575   }
00576 
00577   if(error_code == PNIO_OK)
00578     ROS_INFO_STREAM("All necessary callbacks called....");
00579 
00580   return (int)error_code;
00581 }
00582 
00583 int Cp1616IODevice::stopOperation()
00584 {
00585   PNIO_UINT32 error_code = PNIO_OK;
00586   error_code = PNIO_device_stop(cp_handle_);
00587   if (error_code != PNIO_OK)
00588     ROS_ERROR("Not able to stop IO Device: Error 0x%x", (int)error_code);
00589   else
00590       ROS_INFO_STREAM("Stopping device operation: done");
00591 
00592     //wait for OFFLINE_IND_flag to be set by ar_offline_ind() callback
00593     unsigned int i = 0;
00594     while(i != MAX_OFFLINE_IND_COUNT)
00595     {
00596       if(getOfflineIndFlag() != 0)
00597         break;
00598             
00599       usleep(WAIT_FOR_CALLBACK_PERIOD);
00600       i++;
00601     }
00602 }
00603 
00604 int Cp1616IODevice::GetSubmodNum(PNIO_UINT32 mod, PNIO_UINT32 sub)
00605 {
00606   int i,j;
00607 
00608   for(i = 0; i < DEVICE_DATA_ENTRIES; i++)      //Look for module index
00609   {
00610     if((int)mod == idx_table_[i])
00611     break;
00612   }
00613 
00614   if(i == DEVICE_DATA_ENTRIES)                  //no module means also no submodule
00615     return -1;
00616 
00617   for(j = 0; j < p_device_data_[i].maxSubslots; j++)
00618   {
00619     if(p_device_data_[i+j].subslot == (int)sub)  //find submodule index
00620       return j;
00621   }
00622 
00623   return -1;
00624 }
00625 
00626 void Cp1616IODevice::configureDeviceData()
00627 {
00628   unsigned int i = 0;
00629   unsigned int begin_new_slot = 0;
00630   unsigned int idx = 0;
00631 
00632   for(i = 0; i < DEVICE_DATA_ENTRIES; i++)
00633   {
00634     ROS_INFO("Module: slot %x sub %x mod_id %x sub_id %x",
00635     p_device_data_[i].slot,
00636     p_device_data_[i].subslot,
00637     (unsigned int) p_device_data_[i].modId,
00638     (unsigned int) p_device_data_[i].subslotId);
00639   }
00640 
00641   //fill idxTbl with -1
00642   memset(idx_table_, -1, DEVICE_DATA_ENTRIES * sizeof(int));
00643   idx_table_[idx++] = p_device_data_[0].slot;
00644 
00645   //browsing through the device data structure
00646   for(i = 0; i < DEVICE_DATA_ENTRIES; i++)
00647   {
00648     if(p_device_data_[i].slot == p_device_data_[begin_new_slot].slot)
00649     {
00650       p_device_data_[begin_new_slot].maxSubslots++;
00651       p_device_data_[i].modId = p_device_data_[begin_new_slot].modId;
00652     }
00653     else
00654     {
00655       begin_new_slot = i;                                      //index corresponding to the beginning of the new slots
00656       p_device_data_[begin_new_slot].maxSubslots = 1;         //every new module/slot has min one sub-slot
00657       idx_table_[idx++] = p_device_data_[i].slot;             //store the entry of the new slot in idxTbl
00658     }
00659   }
00660 }
00661 
00662 int Cp1616IODevice::updateCyclicOutputData()
00663 {
00664   PNIO_UINT32 error_code = PNIO_OK;
00665 
00666   //Read data
00667   error_code = PNIO_initiate_data_read(cp_handle_);
00668   if(error_code != PNIO_OK)
00669       ROS_ERROR("Not able to initiate data read: Error 0x%x", (int)error_code);
00670 
00671   return (int)error_code;
00672 }
00673 
00674 int Cp1616IODevice::updateCyclicInputData()
00675 {
00676   PNIO_UINT32 error_code = PNIO_OK;
00677 
00678   //Write data
00679   error_code = PNIO_initiate_data_write(cp_handle_);
00680   if(error_code != PNIO_OK)
00681     ROS_ERROR("Not able to initiate data write: Error 0x%x", (int)error_code);
00682 
00683   return (int)error_code;
00684 }
00685 
00686 
00687 int Cp1616IODevice::doAfterPrmEndIndCbf()
00688 {
00689   PNIO_UINT32 error_code = PNIO_OK;
00690   PNIO_APPL_READY_LIST_TYPE ready_list_type;
00691 
00699   error_code = PNIO_initiate_data_write(cp_handle_);
00700   if(error_code != PNIO_OK)
00701   {
00702     ROS_ERROR("Not able to initiate data write: Error 0x%x", (int)error_code);
00703     return (int)error_code;
00704   }
00711   error_code = PNIO_initiate_data_read(cp_handle_);
00712   if(error_code != PNIO_OK)
00713   {
00714     ROS_ERROR("Not able to initiate data read: Error: 0x%x", (int)error_code);
00715     return (int)error_code;
00716   }
00722    memset(&ready_list_type, 0, sizeof(ready_list_type));
00723    ready_list_type.ap_list.Flink = NULL;
00724    ready_list_type.ap_list.Blink = NULL;
00725 
00726    error_code = PNIO_set_appl_state_ready(
00727          cp_handle_,
00728          cp_ar_number_,
00729          cp_session_key_,
00730          &ready_list_type);
00731 
00732    if(error_code != PNIO_OK)
00733        ROS_ERROR("Not able to set application state ready: Error 0x%x", (int)error_code);
00734 
00735    return (int)error_code;
00736 }
00737 
00738 int Cp1616IODevice::doAfterIndataIndCbf()
00739 {
00740   PNIO_UINT32 error_code = PNIO_OK;
00741 
00749   error_code = PNIO_initiate_data_write(cp_handle_);
00750   if(error_code != PNIO_OK)
00751   {
00752     ROS_ERROR("Not able to initiate data write: Error 0x%x", (int)error_code);
00753     return (int)error_code;
00754   }
00755 
00762    error_code = PNIO_initiate_data_read(cp_handle_);
00763    if(error_code != PNIO_OK)
00764    {
00765      ROS_ERROR("Not able to initiate data read: Error 0x%x", (int)error_code);
00766      return (int)error_code;
00767    }
00768 }
00769 
00770 void Cp1616IODevice::setArInfoIndFlag(int value)
00771 {
00772   ar_info_ind_flag_ = value;
00773 }
00774 
00775 int Cp1616IODevice::getArInfoIndFlag()
00776 {
00777   return ar_info_ind_flag_;
00778 }
00779 
00780 void Cp1616IODevice::setPrmEndIndFlag(int value)
00781 {
00782   prm_end_ind_flag_ = value;
00783 }
00784 
00785 int Cp1616IODevice::getPrmEndIndFlag()
00786 {
00787   return prm_end_ind_flag_;
00788 }
00789 
00790 void Cp1616IODevice::setIndataIndFlag(int value)
00791 {
00792   indata_ind_flag_ = value;
00793 }
00794 
00795 int Cp1616IODevice::getIndataIndFlag()
00796 {
00797   return indata_ind_flag_;
00798 }
00799 
00800 void Cp1616IODevice::setOfflineIndFlag(int value)
00801 {
00802   offline_ind_flag_ = value;
00803 }
00804 
00805 int Cp1616IODevice::getOfflineIndFlag()
00806 {
00807   return offline_ind_flag_;
00808 }
00809 
00810 void Cp1616IODevice::setCpSessionKey(PNIO_UINT16 value)
00811 {
00812   cp_session_key_ = value;
00813 }
00814 
00815 PNIO_UINT16 Cp1616IODevice::getCpSessionKey()
00816 {
00817   return cp_session_key_;
00818 }
00819 
00820 void Cp1616IODevice::setCpArNumber(PNIO_UINT16 value)
00821 {
00822   cp_ar_number_ = value;
00823 }
00824 
00825 
00826 PNIO_UINT16 Cp1616IODevice::getCpArNumber()
00827 {
00828   return cp_ar_number_;
00829 }
00830 
00831 PNIO_UINT32 Cp1616IODevice::getInputDataLength(int slot_number, int subslot_number)
00832 {
00833   return input_data_length_[slot_number][subslot_number];  
00834 }
00835 
00836 void Cp1616IODevice::setInputDataLength(int slot_number, int subslot_number, PNIO_UINT32 value)
00837 {
00838   input_data_length_[slot_number][subslot_number] = value;
00839 }
00840 
00841 PNIO_UINT32 Cp1616IODevice::getOutputDataLength(int slot_number, int subslot_number)
00842 {
00843   return output_data_length_[slot_number][subslot_number];  
00844 }
00845 
00846 void Cp1616IODevice::setOutputDataLength(int slot_number, int subslot_number, PNIO_UINT32 value)
00847 {
00848   output_data_length_[slot_number][subslot_number] = value;
00849 }
00850 
00851 PNIO_IOXS Cp1616IODevice::getInputDataIocs(int slot_number, int subslot_number)
00852 {
00853   return input_data_iocs_[slot_number][subslot_number];  
00854 }
00855 
00856 void Cp1616IODevice::setInputDataIocs(int slot_number, int subslot_number, PNIO_IOXS status)
00857 {
00858   input_data_iocs_[slot_number][subslot_number] = status;
00859 }
00860 
00861 PNIO_IOXS Cp1616IODevice::getOutputDataIocs(int slot_number, int subslot_number)
00862 {
00863   return output_data_iocs_[slot_number][subslot_number];  
00864 }
00865 
00866 void Cp1616IODevice::setOutputDataIocs(int slot_number, int subslot_number, PNIO_IOXS status)
00867 {
00868   output_data_iocs_[slot_number][subslot_number] = status;
00869 }
00870 
00871 PNIO_IOXS Cp1616IODevice::getInputDataIops(int slot_number, int subslot_number)
00872 {
00873   return input_data_iops_[slot_number][subslot_number];  
00874 }
00875 
00876 void Cp1616IODevice::setInputDataIops(int slot_number, int subslot_number, PNIO_IOXS status)
00877 {
00878   input_data_iops_[slot_number][subslot_number] = status;
00879 }
00880 
00881 PNIO_IOXS Cp1616IODevice::getOutputDataIops(int slot_number, int subslot_number)
00882 {
00883   return output_data_iops_[slot_number][subslot_number];  
00884 }
00885 
00886 void Cp1616IODevice::setOutputDataIops(int slot_number, int subslot_number, PNIO_IOXS status)
00887 {
00888   output_data_iops_[slot_number][subslot_number] = status;
00889 }
00890 
00891 } //cp1616
00892 
00893 #endif


cp1616
Author(s):
autogenerated on Fri Aug 28 2015 13:08:36