mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-02 03:52:11 +08:00
cabana: optimize get_raw_value() function for CAN signal extraction (#35137)
optimize get_raw_value() for CAN signal extraction
This commit is contained in:
+26
-15
@@ -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) {
|
||||
|
||||
Reference in New Issue
Block a user