00001 /*********************************************************************************************/ 00021 #ifndef CP1616_IO_CONTROLLER_H 00022 #define CP1616_IO_CONTROLLER_H 00023 00024 #include <ros/ros.h> 00025 00026 //IO base headers 00027 #include "pniousrx.h" 00028 #include "pnioerrx.h" 00029 00030 #define NUM_OF_INPUT_MODULES 4 //Fixed definitions for development purposes 00031 #define NUM_OF_OUTPUT_MODULES 4 00032 00033 namespace cp1616 00034 { 00035 00040 class Cp1616IOController 00041 { 00042 public: 00043 00047 static Cp1616IOController* getControllerInstance(); 00048 00052 ~Cp1616IOController(); 00053 00059 int init(); 00060 00066 int uinit(); 00067 00071 void configureControllerData(); 00072 00081 int addInputModule(unsigned int input_size, unsigned int input_start_address); 00082 00091 int addOutputModule(unsigned int output_size, unsigned int output_start_address); 00092 00098 int updateCyclicInputData(); 00099 00105 int updateCyclicOutputData(); 00106 00114 int changePnioMode(PNIO_MODE_TYPE requested_mode); 00115 00119 std::vector<std::vector<PNIO_UINT8> > input_module_data_; 00120 00124 std::vector<std::vector<PNIO_UINT8> > output_module_data_; 00125 00127 void setSemModChange(int mod_change); 00128 void setCpReady(int cp_ready_value); 00129 int getCpReady(); 00130 00131 void setCpCurrentModeFlag(PNIO_MODE_TYPE mode); 00132 PNIO_MODE_TYPE getCpCurrentModeFlag(); 00133 00135 void printOutputData(unsigned int module); 00136 void printInputData(unsigned int module); 00137 00138 private: 00139 00140 Cp1616IOController(); 00141 static Cp1616IOController *controller_instance_; 00142 00143 PNIO_UINT32 cp_handle_; 00144 PNIO_UINT32 cp_id_; 00145 00146 int cp_ready_; 00147 int sem_mod_change_; 00148 volatile PNIO_MODE_TYPE cp_current_mode_; 00149 volatile PNIO_IOXS cp_local_state_; 00150 00151 PNIO_UINT32 input_module_count_; 00152 unsigned int input_module_total_data_size_; 00153 std::vector<PNIO_UINT32> input_module_data_length_; 00154 std::vector<PNIO_IOXS> input_module_state_; //PNIO_IOXS volatile* volatile device_input_state_; 00155 std::vector<PNIO_ADDR> input_module_address_; 00156 00157 PNIO_UINT32 output_module_count_; 00158 unsigned int output_module_total_data_size_; 00159 std::vector<PNIO_UINT32> output_module_data_length_; 00160 std::vector<PNIO_IOXS> output_module_state_; //PNIO_IOXS volatile* volatile device_output_state_; 00161 std::vector<PNIO_ADDR> output_module_address_; 00162 00163 static const int WAIT_FOR_CALLBACKS_PERIOD = 100000; 00164 static const int MAX_NUM_OF_INIT_ATTEMPTS = 1000; 00165 static const int INIT_DATA_VALUE = 0; 00166 00167 }; //cp1616_io_controller class 00168 } //cp1616 00169 00170 #endif //CP1616_IO_CONTROLLER_H