|
|
|
@ -180,25 +180,36 @@ bool cabana::Signal::operator==(const cabana::Signal &other) const { |
|
|
|
|
// helper functions
|
|
|
|
|
|
|
|
|
|
double get_raw_value(const uint8_t *data, size_t data_size, const cabana::Signal &sig) { |
|
|
|
|
int64_t val = 0; |
|
|
|
|
const int msb_byte = sig.msb / 8; |
|
|
|
|
if (msb_byte >= (int)data_size) return 0; |
|
|
|
|
|
|
|
|
|
int i = sig.msb / 8; |
|
|
|
|
int bits = sig.size; |
|
|
|
|
while (i >= 0 && i < data_size && bits > 0) { |
|
|
|
|
int lsb = (int)(sig.lsb / 8) == i ? sig.lsb : i * 8; |
|
|
|
|
int msb = (int)(sig.msb / 8) == i ? sig.msb : (i + 1) * 8 - 1; |
|
|
|
|
int size = msb - lsb + 1; |
|
|
|
|
const int lsb_byte = sig.lsb / 8; |
|
|
|
|
uint64_t val = 0; |
|
|
|
|
|
|
|
|
|
uint64_t d = (data[i] >> (lsb - (i * 8))) & ((1ULL << size) - 1); |
|
|
|
|
val |= d << (bits - size); |
|
|
|
|
|
|
|
|
|
bits -= size; |
|
|
|
|
i = sig.is_little_endian ? i - 1 : i + 1; |
|
|
|
|
// Fast path: signal fits in a single byte
|
|
|
|
|
if (msb_byte == lsb_byte) { |
|
|
|
|
val = (data[msb_byte] >> (sig.lsb & 7)) & ((1ULL << sig.size) - 1); |
|
|
|
|
} else { |
|
|
|
|
// Multi-byte case: signal spans across multiple bytes
|
|
|
|
|
int bits = sig.size; |
|
|
|
|
int i = msb_byte; |
|
|
|
|
const int step = sig.is_little_endian ? -1 : 1; |
|
|
|
|
while (i >= 0 && i < (int)data_size && bits > 0) { |
|
|
|
|
const int msb = (i == msb_byte) ? sig.msb & 7 : 7; |
|
|
|
|
const int lsb = (i == lsb_byte) ? sig.lsb & 7 : 0; |
|
|
|
|
const int nbits = msb - lsb + 1; |
|
|
|
|
val = (val << nbits) | ((data[i] >> lsb) & ((1ULL << nbits) - 1)); |
|
|
|
|
bits -= nbits; |
|
|
|
|
i += step; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
if (sig.is_signed) { |
|
|
|
|
val -= ((val >> (sig.size - 1)) & 0x1) ? (1ULL << sig.size) : 0; |
|
|
|
|
|
|
|
|
|
// Sign extension (if needed)
|
|
|
|
|
if (sig.is_signed && (val & (1ULL << (sig.size - 1)))) { |
|
|
|
|
val |= ~((1ULL << sig.size) - 1); |
|
|
|
|
} |
|
|
|
|
return val * sig.factor + sig.offset; |
|
|
|
|
|
|
|
|
|
return static_cast<int64_t>(val) * sig.factor + sig.offset; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void updateMsbLsb(cabana::Signal &s) { |
|
|
|
|