Closes #7
This commit is contained in:
deeaitch committed
1 parent
8d82b13dad
commit
b91cf4aae5
21 files changed
+1165
-1146
No files matched your search
+101
-99
@@ -11,104 +11,105 @@
|
||||
|
||||
#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);
|
||||
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;
|
||||
if (it == database.frameIndexByKey.end())
|
||||
return nullptr;
|
||||
|
||||
const std::size_t index = it->second;
|
||||
if (index >= database.frames.size())
|
||||
return nullptr;
|
||||
const std::size_t index = it->second;
|
||||
if (index >= database.frames.size())
|
||||
return nullptr;
|
||||
|
||||
return &database.frames[index];
|
||||
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);
|
||||
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;
|
||||
if (definition == nullptr)
|
||||
return result;
|
||||
|
||||
result.definition = definition;
|
||||
result.valid = true;
|
||||
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;
|
||||
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;
|
||||
decoded.definition = &signal;
|
||||
|
||||
if (!ExtractUnsigned (frame.data, signal, unsignedValue)) {
|
||||
decoded.valid = false;
|
||||
result.signals.push_back (decoded);
|
||||
continue;
|
||||
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 = unsignedValue;
|
||||
|
||||
decoded.physicalValue =
|
||||
std::visit([&signal](auto rawValue) {
|
||||
return (static_cast<double>(rawValue) * signal.factor) + signal.offset;
|
||||
},
|
||||
decoded.rawValue);
|
||||
decoded.valid = true;
|
||||
|
||||
result.signals.push_back(decoded);
|
||||
}
|
||||
|
||||
if (signal.valueType == ValueType::Signed)
|
||||
decoded.rawValue = SignExtend (unsignedValue, signal.length);
|
||||
else
|
||||
decoded.rawValue = unsignedValue;
|
||||
|
||||
decoded.physicalValue =
|
||||
std::visit ([&signal] (auto rawValue) {
|
||||
return (static_cast<double> (rawValue) * signal.factor) + signal.offset;
|
||||
}, decoded.rawValue);
|
||||
decoded.valid = true;
|
||||
|
||||
result.signals.push_back (decoded);
|
||||
}
|
||||
|
||||
return result;
|
||||
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;
|
||||
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);
|
||||
if (signal.byteOrder == ByteOrder::Intel)
|
||||
return ExtractIntel(data, signal.startBit, signal.length, value);
|
||||
|
||||
return ExtractMotorola (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;
|
||||
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;
|
||||
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;
|
||||
if (byteIndex >= data.size())
|
||||
return false;
|
||||
|
||||
const std::uint64_t bitValue =
|
||||
(static_cast<std::uint64_t> ((data[byteIndex] >> bitInByte) & 0x01U) << bitIndex);
|
||||
const std::uint64_t bitValue =
|
||||
(static_cast<std::uint64_t>((data[byteIndex] >> bitInByte) & 0x01U) << bitIndex);
|
||||
|
||||
value |= bitValue;
|
||||
}
|
||||
value |= bitValue;
|
||||
}
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DbcDecoder::ExtractMotorola (const std::vector<std::uint8_t> &data,
|
||||
std::uint32_t startBit,
|
||||
std::uint32_t length,
|
||||
std::uint64_t &value) {
|
||||
/*
|
||||
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
|
||||
@@ -116,40 +117,41 @@ bool DbcDecoder::ExtractMotorola (const std::vector<std::uint8_t> &data,
|
||||
* Example: a signal starting at 7 follows 7..0, 15..8, 23..16.
|
||||
*/
|
||||
|
||||
value = 0U;
|
||||
value = 0U;
|
||||
|
||||
std::size_t byteIndex = startBit / 8U;
|
||||
std::uint32_t bitInByte = startBit % 8U;
|
||||
std::size_t byteIndex = startBit / 8U;
|
||||
std::uint32_t bitInByte = startBit % 8U;
|
||||
|
||||
for (std::uint32_t bitIndex = 0U; bitIndex < length; ++bitIndex) {
|
||||
if (byteIndex >= data.size())
|
||||
return false;
|
||||
for (std::uint32_t bitIndex = 0U; bitIndex < length; ++bitIndex) {
|
||||
if (byteIndex >= data.size())
|
||||
return false;
|
||||
|
||||
value <<= 1U;
|
||||
value |= static_cast<std::uint64_t> ((data[byteIndex] >> bitInByte) & 0x01U);
|
||||
value <<= 1U;
|
||||
value |= static_cast<std::uint64_t>((data[byteIndex] >> bitInByte) & 0x01U);
|
||||
|
||||
if (bitInByte == 0U) {
|
||||
++byteIndex;
|
||||
bitInByte = 7U;
|
||||
} else
|
||||
--bitInByte;
|
||||
}
|
||||
if (bitInByte == 0U) {
|
||||
++byteIndex;
|
||||
bitInByte = 7U;
|
||||
} else
|
||||
--bitInByte;
|
||||
}
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
std::int64_t DbcDecoder::SignExtend (std::uint64_t value, std::uint32_t bitLength) {
|
||||
if ((bitLength == 0U) || (bitLength > 64U))
|
||||
return 0;
|
||||
std::int64_t DbcDecoder::SignExtend(std::uint64_t value, std::uint32_t bitLength) {
|
||||
if ((bitLength == 0U) || (bitLength > 64U))
|
||||
return 0;
|
||||
|
||||
const std::uint64_t signMask = std::uint64_t (1U) << (bitLength - 1U);
|
||||
const std::uint64_t valueMask = (bitLength == 64U)
|
||||
? ~std::uint64_t (0U) : (std::uint64_t (1U) << bitLength) - 1U;
|
||||
value &= valueMask;
|
||||
const std::uint64_t signMask = std::uint64_t(1U) << (bitLength - 1U);
|
||||
const std::uint64_t valueMask = (bitLength == 64U)
|
||||
? ~std::uint64_t(0U)
|
||||
: (std::uint64_t(1U) << bitLength) - 1U;
|
||||
value &= valueMask;
|
||||
|
||||
if ((value & signMask) == 0U)
|
||||
return static_cast<std::int64_t> (value);
|
||||
if ((value & signMask) == 0U)
|
||||
return static_cast<std::int64_t>(value);
|
||||
|
||||
// The complemented magnitude fits int64_t, including for INT64_MIN.
|
||||
return -1 - static_cast<std::int64_t> ((~value) & valueMask);
|
||||
// The complemented magnitude fits int64_t, including for INT64_MIN.
|
||||
return -1 - static_cast<std::int64_t>((~value) & valueMask);
|
||||
}
|
||||
Reference in new issue
Block a user