Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00022
00023 #include "tCanFilterMessages.h"
00024
00025 #include <map>
00026 #include <boost/filesystem.hpp>
00027
00028 #include "icl_hardware_can/Logging.h"
00029
00030 namespace icl_hardware {
00031 namespace can {
00032
00033 tCanFilterMessages::tCanFilterMessages()
00034 : m_data_file_name(NULL),
00035 is_initialized(false)
00036 {
00037 }
00038
00039 tCanFilterMessages::~tCanFilterMessages()
00040 {
00041 if (m_data_file_name != NULL)
00042 {
00043 delete m_data_file_name;
00044 m_data_file_name = NULL;
00045 }
00046 }
00047
00048 void tCanFilterMessages::Open(const std::string identifier)
00049 {
00050
00051 if (identifier != "")
00052 {
00053 setDataFileName(boost::filesystem::path(identifier));
00054 }
00055 else
00056 {
00057 LOGGING_ERROR(icl_hardware::can::CAN, "No data file provided!" << icl_core::logging::endl);
00058 return;
00059 }
00060
00061 LOGGING_DEBUG(icl_hardware::can::CAN, "Opening canmatrixParser with data from file: " << getDataFileName().string() << icl_core::logging::endl);
00062
00063
00064 int ret=PrepareFiles();
00065
00066 if (ret!=0)
00067 {
00068 LOGGING_ERROR(icl_hardware::can::CAN, "No such file as: " << identifier << icl_core::logging::endl);
00069 return;
00070 }
00071
00072 mapContentOfFile();
00073
00074 is_initialized = true;
00075 }
00076
00077 bool tCanFilterMessages::checkMessage(const icl_hardware::can::tCanMessage& message) const
00078 {
00079 std::map<unsigned int, DataWrapper>::const_iterator iter = m_table_id_to_data.find(message.id);
00080
00081 if (iter != m_table_id_to_data.end())
00082 {
00083 for (unsigned int i=0; i < 8; i++)
00084 {
00086 if (message.data[i] != iter->second.data[i] && !byteIsChangingConstantly(i, message.id))
00087 {
00088 return true;
00089 }
00090 }
00091 }
00092 else
00093 {
00094 return true;
00095 }
00096
00097 return false;
00098 }
00099
00100 bool tCanFilterMessages::byteIsChangingConstantly(unsigned int i, unsigned int id) const
00101 {
00102 std::multimap<unsigned int, unsigned int>::const_iterator iter_multi = m_table_id_to_changing_bytes.find(id);
00103 if (iter_multi != m_table_id_to_changing_bytes.end())
00104 {
00105 while (id == iter_multi->first)
00106 {
00107 if (iter_multi->second == i)
00108 {
00109 return true;
00110 }
00111 iter_multi++;
00112 }
00113 }
00114 return false;
00115 }
00116
00117 void tCanFilterMessages::mapContentOfFile()
00118 {
00119 std::string line;
00120 icl_hardware::can::tCanMessage message;
00121
00122 while (m_data_file.good())
00123 {
00124 getline(m_data_file, line);
00125 if (line.length() == 0)
00126 {
00127
00128 continue;
00129 }
00130
00131
00132 StringToCanMsg(message, line);
00133
00134
00135 if (message.id != 0 || message.dlc != 0 || message.rtr != 0)
00136 {
00137 std::map<unsigned int, DataWrapper>::iterator iter = m_table_id_to_data.find(message.id);
00138
00139 if (iter == m_table_id_to_data.end())
00140 {
00141 LOGGING_TRACE(icl_hardware::can::CAN, "id: " << message.id << " is newly registered" << icl_core::logging::endl);
00142 memcpy(m_message_data.data, message.data, sizeof(message.data));
00143 m_table_id_to_data.insert(std::pair<unsigned int, DataWrapper>(message.id, m_message_data));
00144 }
00145 else
00146 {
00148 for (unsigned int i=0; i < 8; i++)
00149 {
00150 if (iter->second.data[i] != message.data[i])
00151 {
00152 std::multimap<unsigned int, unsigned int>::iterator iter_multi = m_table_id_to_changing_bytes.find(iter->first);
00153 bool known_byte = false;
00154
00155 if (iter_multi != m_table_id_to_changing_bytes.end())
00156 {
00158 while(iter_multi->first == message.id && !known_byte)
00159 {
00160 if (iter_multi->second == i)
00161 {
00162 known_byte = true;
00163 }
00164 iter_multi++;
00165 }
00166 }
00167 if (!known_byte)
00168 {
00169 LOGGING_TRACE(icl_hardware::can::CAN, "id: " << message.id << " - add byte: " << i << icl_core::logging::endl);
00170 m_table_id_to_changing_bytes.insert(std::pair <unsigned int, unsigned int> (message.id, i));
00171 }
00172 }
00173 }
00174 }
00175 }
00176 }
00177 }
00178
00179 void tCanFilterMessages::StringToCanMsg(icl_hardware::can::tCanMessage &message, std::string str)
00180 {
00181 unsigned int pos = 0;
00182 int word_length = 0;
00183
00184 if (str.substr(0,1) == "#")
00185 {
00186
00187 return;
00188 }
00189
00190
00191 for (int i=0; i < 11; i++)
00192 {
00193 size_t position = str.find(' ', pos);
00194 if (position == std::string::npos)
00195 {
00196 word_length = str.size() - pos;
00197 }
00198 else
00199 {
00200 word_length = position - pos;
00201 }
00202
00203 switch(i)
00204 {
00205 case 0:
00206 {
00207
00208 break;
00209 }
00210 case 1:
00211 {
00212 message.id = atoi((str.substr(pos, word_length)).data());
00213 break;
00214 }
00215 case 2:
00216 {
00217 message.dlc = atoi((str.substr(pos, word_length)).data());
00218 break;
00219 }
00220 default:
00221 {
00222
00223 message.data[i-3] = (unsigned char)(strtol((str.substr(pos, word_length)).data(), NULL, 16));
00224 break;
00225 }
00226 }
00227
00228 pos += word_length+1;
00229 }
00230 }
00231
00232 int tCanFilterMessages::PrepareFiles()
00233 {
00234
00235 m_data_file.open(getDataFileName().string().c_str(), std::ios::in);
00236
00237
00238 if (!m_data_file.is_open())
00239 {
00240 return -1;
00241 }
00242
00243 return 0;
00244 }
00245
00246 void tCanFilterMessages::setDataFileName(const boost::filesystem::path &file_name)
00247 {
00248 if (m_data_file_name)
00249 {
00250 delete m_data_file_name;
00251 }
00252 m_data_file_name = new boost::filesystem::path(file_name);
00253 }
00254
00255 const boost::filesystem::path& tCanFilterMessages::getDataFileName() const
00256 {
00257 return *m_data_file_name;
00258 }
00259
00260 bool tCanFilterMessages::getStatus() const
00261 {
00262 return is_initialized;
00263 }
00264
00265 }
00266 }