diff --git a/include/libdbc/dbc.hpp b/include/libdbc/dbc.hpp index 5e0e9e4..099e147 100644 --- a/include/libdbc/dbc.hpp +++ b/include/libdbc/dbc.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include namespace Libdbc { @@ -32,6 +33,7 @@ class DbcParser : public Parser { std::vector get_messages() const; Message::ParseSignalsStatus parse_message(uint32_t message_id, const std::vector& data, std::vector& out_values); + Message::ParseSignalsStatus parse_message(uint32_t message_id, const std::vector& data, std::unordered_map& out_values); std::vector unused_lines() const; diff --git a/include/libdbc/message.hpp b/include/libdbc/message.hpp index 438dc7f..4356563 100644 --- a/include/libdbc/message.hpp +++ b/include/libdbc/message.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include namespace Libdbc { @@ -21,7 +22,11 @@ struct Message { ErrorInvalidConversion, }; + // Decodes the signals present in data, in signal order. For multiplexed messages only the multiplexor, + // the non multiplexed signals and the signals matching the current multiplexer value are present. ParseSignalsStatus parse_signals(const std::vector& data, std::vector& values) const; + // Same as above but keyed by signal name, so multiplexed messages can be decoded without tracking which signals are present. + ParseSignalsStatus parse_signals(const std::vector& data, std::unordered_map& values) const; void append_signal(const Signal& signal); std::vector get_signals() const; diff --git a/include/libdbc/signal.hpp b/include/libdbc/signal.hpp index 18ed3da..49f8963 100644 --- a/include/libdbc/signal.hpp +++ b/include/libdbc/signal.hpp @@ -15,7 +15,9 @@ struct Signal { }; std::string name; - bool is_multiplexed; + bool is_multiplexed; // Signal is only present for a specific multiplexer value (mN) + bool is_multiplexor = false; // Signal is the multiplexer switch of its message (M) + uint32_t multiplexer_value = 0; // Multiplexer value this signal is present for. Only valid if is_multiplexed uint32_t start_bit; uint32_t size; bool is_bigendian; diff --git a/src/dbc.cpp b/src/dbc.cpp index 89e8c24..7b17fac 100644 --- a/src/dbc.cpp +++ b/src/dbc.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include namespace Libdbc { @@ -17,6 +18,7 @@ const auto floatPattern = "(-?\\d+\\.?(\\d+)?)"; // Can be negative const auto signalIdentifierPattern = "(SG_)"; const auto namePattern = "(\\w+)"; +const auto multiplexerPattern = "(?:\\s(M|m\\d+M?))?"; // Optional: M = multiplexor, mN = multiplexed by value N, mNM = both const auto bitStartPattern = "(\\d+)"; // Cannot be negative const auto lengthPattern = "(\\d+)"; // Cannot be negative const auto byteOrderPattern = "([0-1])"; @@ -34,16 +36,17 @@ const auto receiverPattern = "([\\w\\,]+|Vector__XXX)*"; const auto whiteSpace = "\\s"; constexpr unsigned SIGNAL_NAME_GROUP = 2; -constexpr unsigned SIGNAL_START_BIT_GROUP = 3; -constexpr unsigned SIGNAL_SIZE_GROUP = 4; -constexpr unsigned SIGNAL_ENDIAN_GROUP = 5; -constexpr unsigned SIGNAL_SIGNED_GROUP = 6; -constexpr unsigned SIGNAL_FACTOR_GROUP = 7; -constexpr unsigned SIGNAL_OFFSET_GROUP = 9; -constexpr unsigned SIGNAL_MIN_GROUP = 11; -constexpr unsigned SIGNAL_MAX_GROUP = 13; -constexpr unsigned SIGNAL_UNIT_GROUP = 15; -constexpr unsigned SIGNAL_RECIEVER_GROUP = 16; +constexpr unsigned SIGNAL_MULTIPLEXER_GROUP = 3; +constexpr unsigned SIGNAL_START_BIT_GROUP = 4; +constexpr unsigned SIGNAL_SIZE_GROUP = 5; +constexpr unsigned SIGNAL_ENDIAN_GROUP = 6; +constexpr unsigned SIGNAL_SIGNED_GROUP = 7; +constexpr unsigned SIGNAL_FACTOR_GROUP = 8; +constexpr unsigned SIGNAL_OFFSET_GROUP = 10; +constexpr unsigned SIGNAL_MIN_GROUP = 12; +constexpr unsigned SIGNAL_MAX_GROUP = 14; +constexpr unsigned SIGNAL_UNIT_GROUP = 16; +constexpr unsigned SIGNAL_RECIEVER_GROUP = 17; constexpr unsigned MESSAGE_ID_GROUP = 2; constexpr unsigned MESSAGE_NAME_GROUP = 3; @@ -63,11 +66,9 @@ DbcParser::DbcParser() , node_re("^(BU_:)\\s?((?:[\\w]+?\\s?)*)?") , message_re("^(BO_)\\s(\\d+)\\s(\\w+)\\:\\s(\\d+)\\s(\\w+|Vector__XXX)") , value_re("^(VAL_)\\s(\\d+)\\s(\\w+)((?:\\s(\\d+)\\s\"([^\"]*)\")+)\\s;$") - , - // NOTE: No multiplex support yet - signal_re(std::string("^") + whiteSpace + signalIdentifierPattern + whiteSpace + namePattern + whiteSpace + "\\:" + whiteSpace + bitStartPattern + "\\|" - + lengthPattern + "\\@" + byteOrderPattern + signPattern + whiteSpace + offsetScalePattern + whiteSpace + minMaxPattern + whiteSpace + unitPattern - + whiteSpace + receiverPattern) { + , signal_re(std::string("^") + whiteSpace + signalIdentifierPattern + whiteSpace + namePattern + multiplexerPattern + whiteSpace + "\\:" + whiteSpace + + bitStartPattern + "\\|" + lengthPattern + "\\@" + byteOrderPattern + signPattern + whiteSpace + offsetScalePattern + whiteSpace + + minMaxPattern + whiteSpace + unitPattern + whiteSpace + receiverPattern) { } void DbcParser::parse_file(std::istream& stream) { @@ -128,6 +129,16 @@ Message::ParseSignalsStatus DbcParser::parse_message(const uint32_t message_id, return Message::ParseSignalsStatus::ErrorUnknownID; } +Message::ParseSignalsStatus +DbcParser::parse_message(const uint32_t message_id, const std::vector& data, std::unordered_map& out_values) { + for (const auto& message : messages) { + if (message.id() == message_id) { + return message.parse_signals(data, out_values); + } + } + return Message::ParseSignalsStatus::ErrorUnknownID; +} + void DbcParser::parse_dbc_header(std::istream& file_stream) { std::string line; std::smatch match; @@ -183,7 +194,11 @@ void DbcParser::parse_dbc_messages(const std::vector& lines) { if (std::regex_search(line, match, signal_re) && !messages.empty()) { std::string name = match.str(SIGNAL_NAME_GROUP); - bool is_multiplexed = false; // No support yet + // Multiplexer indicator: "M", "mN" or "mNM" (extended multiplexing) + std::string multiplexer = match.str(SIGNAL_MULTIPLEXER_GROUP); + bool is_multiplexed = !multiplexer.empty() && multiplexer.front() == 'm'; + bool is_multiplexor = !multiplexer.empty() && multiplexer.back() == 'M'; + uint32_t multiplexer_value = is_multiplexed ? static_cast(std::stoul(multiplexer.substr(1))) : 0; uint32_t start_bit = static_cast(std::stoul(match.str(SIGNAL_START_BIT_GROUP))); uint32_t size = static_cast(std::stoul(match.str(SIGNAL_SIZE_GROUP))); bool is_bigendian = (std::stoul(match.str(SIGNAL_ENDIAN_GROUP)) == 0); @@ -200,6 +215,8 @@ void DbcParser::parse_dbc_messages(const std::vector& lines) { Utils::String::split(match.str(SIGNAL_RECIEVER_GROUP), receivers, ','); Signal sig(name, is_multiplexed, start_bit, size, is_bigendian, is_signed, factor, offset, min, max, unit, receivers); + sig.is_multiplexor = is_multiplexor; + sig.multiplexer_value = multiplexer_value; messages.back().append_signal(sig); continue; } diff --git a/src/message.cpp b/src/message.cpp index ffe4526..cce3643 100644 --- a/src/message.cpp +++ b/src/message.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include namespace Libdbc { @@ -26,10 +27,55 @@ bool Message::operator==(const Message& rhs) const { return (m_id == rhs.id()) && (m_name == rhs.m_name) && (m_size == rhs.m_size) && (m_node == rhs.m_node); } -Message::ParseSignalsStatus Message::parse_signals(const std::vector& data, std::vector& values) const { +namespace { + +// Extracts the raw (unscaled) bits of a signal. Bits above signal.size are not masked off. +uint64_t extract_raw(const Signal& signal, uint64_t data_little_endian, uint64_t data_big_endian, std::size_t len) { + if (signal.is_bigendian) { + uint32_t start_bit = ONE_BYTE * (signal.start_bit / ONE_BYTE) + (SEVEN_BITS - (signal.start_bit % ONE_BYTE)); // Calculation taken from python CAN + uint64_t value = data_big_endian << start_bit; + return value >> (len - signal.size); + } + return data_little_endian >> signal.start_bit; +} + +double to_physical(const Signal& signal, uint64_t value) { + if (signal.is_signed && signal.size > 1) { + switch (signal.size) { + case ONE_BYTE: + return static_cast(value) * signal.factor + signal.offset; + case TWO_BYTES: + return static_cast(value) * signal.factor + signal.offset; + case FOUR_BYTES: + return static_cast(value) * signal.factor + signal.offset; + case EIGHT_BYTES: + return static_cast(value) * signal.factor + signal.offset; + default: { + // 2 complement -> decimal + const bool is_negative = (value & (1ULL << (signal.size - 1))) != 0; + int64_t nativeInt = 0; + if (is_negative) { + nativeInt = static_cast(value | ~((1ULL << signal.size) - 1)); // invert all bits above signal.size + } else { + nativeInt = static_cast(value & ((1ULL << signal.size) - 1)); // masking + } + return static_cast(nativeInt) * signal.factor + signal.offset; + } + } + } + // use only the relevant bits + value = value & ((1 << signal.size) - 1); // masking + return static_cast(value) * signal.factor + signal.offset; +} + +// Decodes every signal that is present in the frame and hands it to on_value(signal, value). +// Multiplexed signals are only present when the multiplexor's raw value matches their multiplexer value. +// NOTE: Extended multiplexing (SG_MUL_VAL_) is not supported. All multiplexed signals are switched by the top level multiplexor. +template +Message::ParseSignalsStatus decode_signals(const std::vector& signals, const std::vector& data, OnValue on_value) { auto size = data.size(); if (size > ONE_BYTE) { - return ParseSignalsStatus::ErrorMessageToLong; // not supported yet + return Message::ParseSignalsStatus::ErrorMessageToLong; // not supported yet } uint64_t data_little_endian = 0; @@ -42,50 +88,41 @@ Message::ParseSignalsStatus Message::parse_signals(const std::vector& d // TODO: does this also work on a big endian machine? const auto len = size * 8; - uint64_t value = 0; - for (const auto& signal : m_signals) { - if (signal.is_bigendian) { - uint32_t start_bit = ONE_BYTE * (signal.start_bit / ONE_BYTE) + (SEVEN_BITS - (signal.start_bit % ONE_BYTE)); // Calculation taken from python CAN - value = data_big_endian << start_bit; - value = value >> (len - signal.size); - } else { - value = data_little_endian >> signal.start_bit; - } - if (signal.is_signed && signal.size > 1) { - switch (signal.size) { - case ONE_BYTE: - values.push_back(static_cast(value) * signal.factor + signal.offset); - break; - case TWO_BYTES: - values.push_back(static_cast(value) * signal.factor + signal.offset); - break; - case FOUR_BYTES: - values.push_back(static_cast(value) * signal.factor + signal.offset); - break; - case EIGHT_BYTES: - values.push_back(static_cast(value) * signal.factor + signal.offset); - break; - default: { - // 2 complement -> decimal - const bool is_negative = (value & (1ULL << (signal.size - 1))) != 0; - int64_t nativeInt = 0; - if (is_negative) { - nativeInt = static_cast(value | ~((1ULL << signal.size) - 1)); // invert all bits above signal.size - } else { - nativeInt = static_cast(value & ((1ULL << signal.size) - 1)); // masking - } - values.push_back(static_cast(nativeInt) * signal.factor + signal.offset); - break; + bool has_multiplexor = false; + uint64_t multiplexer_value = 0; + for (const auto& signal : signals) { + if (signal.is_multiplexor && !signal.is_multiplexed) { + multiplexer_value = extract_raw(signal, data_little_endian, data_big_endian, len); + if (signal.size < EIGHT_BYTES) { + multiplexer_value &= (1ULL << signal.size) - 1; } - } - } else { - // use only the relevant bits - value = value & ((1 << signal.size) - 1); // masking - values.push_back(static_cast(value) * signal.factor + signal.offset); + has_multiplexor = true; + break; + } + } + + for (const auto& signal : signals) { + if (signal.is_multiplexed && (!has_multiplexor || signal.multiplexer_value != multiplexer_value)) { + continue; } + on_value(signal, to_physical(signal, extract_raw(signal, data_little_endian, data_big_endian, len))); } - return ParseSignalsStatus::Success; + return Message::ParseSignalsStatus::Success; +} + +} + +Message::ParseSignalsStatus Message::parse_signals(const std::vector& data, std::vector& values) const { + return decode_signals(m_signals, data, [&values](const Signal&, double value) { + values.push_back(value); + }); +} + +Message::ParseSignalsStatus Message::parse_signals(const std::vector& data, std::unordered_map& values) const { + return decode_signals(m_signals, data, [&values](const Signal& signal, double value) { + values[signal.name] = value; + }); } void Message::append_signal(const Signal& signal) { diff --git a/src/signal.cpp b/src/signal.cpp index d40bf2f..6082a33 100644 --- a/src/signal.cpp +++ b/src/signal.cpp @@ -32,7 +32,8 @@ Signal::Signal(std::string name, } bool Signal::operator==(const Signal& rhs) const { - return (this->name == rhs.name) && (this->is_multiplexed == rhs.is_multiplexed) && (this->start_bit == rhs.start_bit) && (this->size == rhs.size) + return (this->name == rhs.name) && (this->is_multiplexed == rhs.is_multiplexed) && (this->is_multiplexor == rhs.is_multiplexor) + && (this->multiplexer_value == rhs.multiplexer_value) && (this->start_bit == rhs.start_bit) && (this->size == rhs.size) && (this->is_bigendian == rhs.is_bigendian) && (this->is_signed == rhs.is_signed) && (this->offset == rhs.offset) && (this->min == rhs.min) && (this->max == rhs.max) && (this->unit == rhs.unit) && (this->receivers == rhs.receivers); } @@ -44,6 +45,10 @@ bool Signal::operator<(const Signal& rhs) const { std::ostream& operator<<(std::ostream& out, const Signal& sig) { out << "Signal {name: " << sig.name << ", "; out << "Multiplexed: " << (sig.is_multiplexed ? "True" : "False") << ", "; + if (sig.is_multiplexed) { + out << "Multiplexer value: " << sig.multiplexer_value << ", "; + } + out << "Multiplexor: " << (sig.is_multiplexor ? "True" : "False") << ", "; out << "Start bit: " << sig.start_bit << ", "; out << "Size: " << sig.size << ", "; out << "Endianness: " << (sig.is_bigendian ? "Big endian" : "Little endian") << ", "; diff --git a/test/test_parse_message.cpp b/test/test_parse_message.cpp index 08d68f3..9fbbd72 100644 --- a/test/test_parse_message.cpp +++ b/test/test_parse_message.cpp @@ -2,7 +2,11 @@ #include #include +#include #include +#include +#include +#include #include "testing_utils/common.hpp" #include "testing_utils/defines.hpp" @@ -209,3 +213,126 @@ VAL_ 234 State1 123 "Description 1" 0 "Description 2" 90903489 "Big value and sp REQUIRE(result_values.size() == 1); REQUIRE(Catch::Approx(result_values.at(0)) == 0x1); } + +TEST_CASE("Parse multiplexed signals") { + Libdbc::DbcParser p; + p.parse_file(COMPLEX_DBC_FILE); + + const auto messages = p.get_messages(); + const auto sonars = std::find_if(messages.begin(), messages.end(), [](const Libdbc::Message& msg) { + return msg.id() == 200; + }); + REQUIRE(sonars != messages.end()); + + const auto signals = sonars->get_signals(); + REQUIRE(signals.size() == 10); + + SECTION("Multiplexor") { + CHECK(signals.at(0).name == "SENSOR_SONARS_mux"); + CHECK(signals.at(0).is_multiplexor); + CHECK_FALSE(signals.at(0).is_multiplexed); + } + + SECTION("Non multiplexed signal") { + CHECK(signals.at(1).name == "SENSOR_SONARS_err_count"); + CHECK_FALSE(signals.at(1).is_multiplexor); + CHECK_FALSE(signals.at(1).is_multiplexed); + } + + SECTION("Multiplexed signals") { + CHECK(signals.at(2).name == "SENSOR_SONARS_left"); + CHECK(signals.at(2).is_multiplexed); + CHECK_FALSE(signals.at(2).is_multiplexor); + CHECK(signals.at(2).multiplexer_value == 0); + CHECK(signals.at(2).start_bit == 16); + CHECK(signals.at(2).size == 12); + + CHECK(signals.at(6).name == "SENSOR_SONARS_no_filt_left"); + CHECK(signals.at(6).is_multiplexed); + CHECK(signals.at(6).multiplexer_value == 1); + } + + SECTION("No signal lines are left unparsed") { + for (const auto& line : p.unused_lines()) { + CHECK(line.find("SG_ SENSOR_SONARS") == std::string::npos); + } + } +} + +TEST_CASE("Parse extended multiplexor signal (mNM)") { + std::string dbc_contents = PRIMITIVE_DBC + R"(BO_ 300 EXT_MUX: 8 Vector__XXX + SG_ Mux M : 0|8@1+ (1,0) [0|255] "" Vector__XXX + SG_ SubMux m3M : 8|8@1+ (1,0) [0|255] "" Vector__XXX + SG_ Value m12 : 16|8@1+ (1,0) [0|255] "" Vector__XXX +)"; + const auto filename = create_temporary_dbc_with(dbc_contents.c_str()); + + Libdbc::DbcParser p; + p.parse_file(filename); + + REQUIRE(p.get_messages().size() == 1); + const auto signals = p.get_messages().at(0).get_signals(); + REQUIRE(signals.size() == 3); + + CHECK(signals.at(1).name == "SubMux"); + CHECK(signals.at(1).is_multiplexor); + CHECK(signals.at(1).is_multiplexed); + CHECK(signals.at(1).multiplexer_value == 3); + + CHECK(signals.at(2).name == "Value"); + CHECK(signals.at(2).multiplexer_value == 12); +} + +TEST_CASE("Decode multiplexed message") { + Libdbc::DbcParser p; + p.parse_file(COMPLEX_DBC_FILE); + + // err_count = 5, left = 100, middle = 200, right = 300, rear = 400 (raw) + std::vector mux0{0x50, 0x00, 0x64, 0x80, 0x0C, 0x2C, 0x01, 0x19}; + std::vector mux1{0x51, 0x00, 0x64, 0x80, 0x0C, 0x2C, 0x01, 0x19}; + std::vector mux2{0x52, 0x00, 0x64, 0x80, 0x0C, 0x2C, 0x01, 0x19}; + + SECTION("Values in signal order only contain the active signals") { + std::vector values; + REQUIRE(p.parse_message(200, mux0, values) == Libdbc::Message::ParseSignalsStatus::Success); + std::vector expected{0, 5, 10, 20, 30, 40}; + REQUIRE(values.size() == expected.size()); + for (size_t i = 0; i < expected.size(); i++) { + CHECK(Catch::Approx(values.at(i)) == expected.at(i)); + } + } + + SECTION("Values by name for multiplexer value 0") { + std::unordered_map values; + REQUIRE(p.parse_message(200, mux0, values) == Libdbc::Message::ParseSignalsStatus::Success); + CHECK(values.size() == 6); + CHECK(Catch::Approx(values.at("SENSOR_SONARS_mux")) == 0); + CHECK(Catch::Approx(values.at("SENSOR_SONARS_err_count")) == 5); + CHECK(Catch::Approx(values.at("SENSOR_SONARS_left")) == 10); + CHECK(Catch::Approx(values.at("SENSOR_SONARS_rear")) == 40); + CHECK(values.count("SENSOR_SONARS_no_filt_left") == 0); + } + + SECTION("Values by name for multiplexer value 1") { + std::unordered_map values; + REQUIRE(p.parse_message(200, mux1, values) == Libdbc::Message::ParseSignalsStatus::Success); + CHECK(values.size() == 6); + CHECK(Catch::Approx(values.at("SENSOR_SONARS_mux")) == 1); + CHECK(Catch::Approx(values.at("SENSOR_SONARS_no_filt_left")) == 10); + CHECK(Catch::Approx(values.at("SENSOR_SONARS_no_filt_rear")) == 40); + CHECK(values.count("SENSOR_SONARS_left") == 0); + } + + SECTION("Unknown multiplexer value only yields non multiplexed signals") { + std::unordered_map values; + REQUIRE(p.parse_message(200, mux2, values) == Libdbc::Message::ParseSignalsStatus::Success); + CHECK(values.size() == 2); + CHECK(Catch::Approx(values.at("SENSOR_SONARS_mux")) == 2); + CHECK(Catch::Approx(values.at("SENSOR_SONARS_err_count")) == 5); + } + + SECTION("Unknown message id") { + std::unordered_map values; + CHECK(p.parse_message(9999, mux0, values) == Libdbc::Message::ParseSignalsStatus::ErrorUnknownID); + } +}