Added decoding support to runtime decode.

dbc_decoder.cpp

@@ -0,0 +1,146 @@
+#include "dbc_decoder.h"
+
+const DecodeFrame *DbcDecoder::FindFrame (const DecodeDatabase &database,
+    std::uint32_t canId,
+    bool isExtended) const {
+  const FrameKey key (canId, isExtended);
+  const std::unordered_map<FrameKey, std::size_t, FrameKeyHasher>::const_iterator it =
+    database.frameIndexByKey.find (key);
+
+  if (it == database.frameIndexByKey.end())
+    return nullptr;
+
+  const std::size_t index = it->second;
+  if (index >= database.frames.size())
+    return nullptr;
+
+  return &database.frames[index];
+}
+
+DecodedFrameValue DbcDecoder::Decode (const DecodeDatabase &database,
+                                      const RawCanFrame &frame) const {
+  DecodedFrameValue result;
+  const DecodeFrame *definition = FindFrame (database, frame.canId, frame.isExtended);
+
+  if (definition == nullptr)
+    return result;
+
+  result.definition = definition;
+  result.valid = true;
+
+  for (std::size_t signalIndex = 0U; signalIndex < definition->signals.size(); ++signalIndex) {
+    const DecodeSignal &signal = definition->signals[signalIndex];
+    DecodedSignalValue decoded;
+    std::uint64_t unsignedValue = 0U;
+
+    decoded.definition = &signal;
+
+    if (!ExtractUnsigned (frame.data, signal, unsignedValue)) {
+      decoded.valid = false;
+      result.signals.push_back (decoded);
+      continue;
+    }
+
+    if (signal.valueType == ValueType::Signed)
+      decoded.rawValue = SignExtend (unsignedValue, signal.length);
+    else
+      decoded.rawValue = static_cast<std::int64_t> (unsignedValue);
+
+    decoded.physicalValue =
+      (static_cast<double> (decoded.rawValue) * signal.factor) + signal.offset;
+    decoded.valid = true;
+
+    result.signals.push_back (decoded);
+  }
+
+  return result;
+}
+
+bool DbcDecoder::ExtractUnsigned (const std::vector<std::uint8_t> &data,
+                                  const DecodeSignal &signal,
+                                  std::uint64_t &value) {
+  if ((signal.length == 0U) || (signal.length > 64U))
+    return false;
+
+  if (signal.byteOrder == ByteOrder::Intel)
+    return ExtractIntel (data, signal.startBit, signal.length, value);
+
+  return ExtractMotorola (data, signal.startBit, signal.length, value);
+}
+
+bool DbcDecoder::ExtractIntel (const std::vector<std::uint8_t> &data,
+                               std::uint32_t startBit,
+                               std::uint32_t length,
+                               std::uint64_t &value) {
+  value = 0U;
+
+  for (std::uint32_t bitIndex = 0U; bitIndex < length; ++bitIndex) {
+    const std::uint32_t absoluteBit = startBit + bitIndex;
+    const std::uint32_t byteIndex = absoluteBit / 8U;
+    const std::uint32_t bitInByte = absoluteBit % 8U;
+
+    if (byteIndex >= data.size())
+      return false;
+
+    const std::uint64_t bitValue =
+      (static_cast<std::uint64_t> ((data[byteIndex] >> bitInByte) & 0x01U) << bitIndex);
+
+    value |= bitValue;
+  }
+
+  return true;
+}
+
+bool DbcDecoder::ExtractMotorola (const std::vector<std::uint8_t> &data,
+                                  std::uint32_t startBit,
+                                  std::uint32_t length,
+                                  std::uint64_t &value) {
+  /*
+   * DBC Motorola bit numbering:
+   * - startBit points to the most significant bit of the signal
+   * - inside a byte, bit numbering goes 7..0
+   * - crossing byte boundary moves to the next byte, bit 7
+   */
+
+  value = 0U;
+
+  std::int32_t currentBit = static_cast<std::int32_t> (startBit);
+
+  for (std::uint32_t bitIndex = 0U; bitIndex < length; ++bitIndex) {
+    if (currentBit < 0)
+      return false;
+
+    const std::uint32_t absoluteBit = static_cast<std::uint32_t> (currentBit);
+    const std::uint32_t byteIndex = absoluteBit / 8U;
+    const std::uint32_t bitFromMsb = absoluteBit % 8U;
+    const std::uint32_t bitInByte = 7U - bitFromMsb;
+
+    if (byteIndex >= data.size())
+      return false;
+
+    value <<= 1U;
+    value |= static_cast<std::uint64_t> ((data[byteIndex] >> bitInByte) & 0x01U);
+
+    if ((absoluteBit % 8U) == 7U)
+      currentBit = static_cast<std::int32_t> ((byteIndex + 1U) * 8U);
+    else
+      --currentBit;
+  }
+
+  return true;
+}
+
+std::int64_t DbcDecoder::SignExtend (std::uint64_t value, std::uint32_t bitLength) {
+  if ((bitLength == 0U) || (bitLength >= 64U))
+    return static_cast<std::int64_t> (value);
+
+  const std::uint64_t signMask = (static_cast<std::uint64_t> (1U) << (bitLength - 1U));
+  const std::uint64_t valueMask = (static_cast<std::uint64_t> (1U) << bitLength) - 1U;
+
+  value &= valueMask;
+
+  if ((value & signMask) == 0U)
+    return static_cast<std::int64_t> (value);
+
+  return static_cast<std::int64_t> (value | (~valueMask));
+}