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 "HardwareCanFilterInterpretMessage.h"
00024
00025 #include <icl_core_config/Config.h>
00026 #include <icl_sourcesink/SimpleURI.h>
00027 #include "icl_hardware_can/CanMessageHelper.h"
00028 #include "icl_hardware_can/Logging.h"
00029
00030 namespace icl_hardware {
00031 namespace can {
00032
00033 HardwareCanFilterInterpretMessage::HardwareCanFilterInterpretMessage(const std::string &uri, const std::string &name)
00034 : icl_sourcesink::DataFilter<tCanMessage, InterpretedCanMessages>(uri, name),
00035 m_can_mask()
00036 {
00037 icl_sourcesink::SimpleURI parsed_uri(uri);
00038
00039 std::string can_mask = "";
00040 boost::optional<std::string> uri_can_mask = parsed_uri.getQuery<std::string>("can_mask");
00041 if (uri_can_mask)
00042 {
00043 can_mask = *uri_can_mask;
00044 }
00045 else
00046 {
00047 can_mask = icl_core::config::getDefault<std::string>("/icl_hardware_can/can_mask", "");
00048 }
00049
00050
00051 if (can_mask != "")
00052 {
00053 tCanMatrixParser parser(can_mask);
00054 if (parser.isActive())
00055 {
00056 m_can_mask = parser.getCanMatrix();
00057 LOGGING_DEBUG(CAN, "Using CAN Mask: " << can_mask << endl);
00058 }
00059 else
00060 {
00061 LOGGING_WARNING(CAN, "Could not use provided CAN mask " << can_mask << endl);
00062 }
00063 }
00064 }
00065
00066 HardwareCanFilterInterpretMessage::~HardwareCanFilterInterpretMessage()
00067 { }
00068
00069 bool HardwareCanFilterInterpretMessage::filter(const icl_core::Stamped<tCanMessage>& input_msg,
00070 icl_core::Stamped<InterpretedCanMessages>& output_msg)
00071 {
00072 if (input_msg->id == 0 && input_msg->dlc == 0 && input_msg->rtr == 0)
00073 {
00074 LOGGING_WARNING(CAN, "Received empty message." << endl);
00075 return false;
00076 }
00077
00078 output_msg.header() = input_msg.header();
00079 output_msg->clear();
00080
00081 CanMatrix::const_iterator mask_iter = m_can_mask.find(input_msg->id);
00082 if (mask_iter != m_can_mask.end())
00083 {
00084 for (std::size_t i = 0; i < mask_iter->second.size(); ++i)
00085 {
00086 double msg_value;
00087 parseCanMessage<double>(*input_msg,
00088 mask_iter->second.at(i).start_bit,
00089 mask_iter->second.at(i).signal_length,
00090 mask_iter->second.at(i).conversion,
00091 mask_iter->second.at(i).offset,
00092 mask_iter->second.at(i).lower_border,
00093 mask_iter->second.at(i).upper_border,
00094 mask_iter->second.at(i).little_endian,
00095 mask_iter->second.at(i).signedness,
00096 msg_value);
00097
00098 output_msg->push_back(InterpretedCanMessage(msg_value,
00099 mask_iter->second.at(i).description,
00100 mask_iter->second.at(i).unit));
00101 }
00102 }
00103
00104 return true;
00105 }
00106
00107 }
00108 }