diff --git a/src/canmap.cpp b/src/canmap.cpp index 89f82fe..d3fe749 100644 --- a/src/canmap.cpp +++ b/src/canmap.cpp @@ -80,7 +80,6 @@ bool CanMap::HandleRx(uint32_t canId, uint32_t data[2], uint8_t) uint32_t word; uint8_t pos = curPos->offsetBits; uint8_t numBits = ABS(curPos->numBits); - uint32_t mask = (1 << numBits) - 1; if (curPos->numBits < 0) //negative length is big endian { @@ -102,7 +101,7 @@ bool CanMap::HandleRx(uint32_t canId, uint32_t data[2], uint8_t) } //Swap byte order - uint8_t* bptr = (uint8_t*)&word; + const uint8_t* bptr = (uint8_t*)&word; word = (bptr[0] << 24) | (bptr[1] << 16) | (bptr[2] << 8) | bptr[3]; pos = 31 - pos; } @@ -113,7 +112,7 @@ bool CanMap::HandleRx(uint32_t canId, uint32_t data[2], uint8_t) word = data[1]; pos -= 32; //position in second word } - else if ((curPos->offsetBits + curPos->numBits) < 32) //all data in first word + else if ((curPos->offsetBits + curPos->numBits) <= 32) //all data in first word { word = data[0]; } @@ -125,7 +124,24 @@ bool CanMap::HandleRx(uint32_t canId, uint32_t data[2], uint8_t) } } - val = (word >> pos) & mask; + // extract the bits we are looking for + uint32_t mask = (1L << numBits) - 1; + word = (word >> pos) & mask; + + // sign-extend our arbitrary sized integer out to 32-bits but only if + // it is bigger than a single bit + int32_t ival; + if (numBits > 1) + { + uint32_t sign_bit = 1L << (numBits - 1); + ival = static_cast(((word + sign_bit) & mask)) - sign_bit; + } + else + { + ival = word; + } + + val = ival; val += curPos->offset; val *= curPos->gain; @@ -165,16 +181,50 @@ void CanMap::SendAll() val *= curPos->gain; val += curPos->offset; - int ival = val; - ival &= ((1 << curPos->numBits) - 1); + // convert to a signed integer value before storing in an unsigned to + // avoid sign-extension problems when we start shifting and masking + uint32_t ival = (int32_t)val; + uint8_t numBits = ABS(curPos->numBits); + ival &= (1UL << numBits) - 1; - if (curPos->offsetBits > 31) + if (curPos->numBits < 0) // big-endian { - data[1] |= ival << (curPos->offsetBits - 32); + //Swap byte order + const uint8_t* bptr = (uint8_t*)&ival; + ival = (bptr[0] << 24) | (bptr[1] << 16) | (bptr[2] << 8) | bptr[3]; + + if (curPos->offsetBits < 32) //all data in first word + { + data[0] |= ival >> (31 - curPos->offsetBits); + } + else if ((curPos->offsetBits + curPos->numBits) >= 31) //all data in second word + { + data[1] |= ival >> (63 - curPos->offsetBits); + } + else //data spans across both words + { + data[0] |= ival << (curPos->offsetBits - 31); + data[1] |= ival >> (63 - curPos->offsetBits); + } } - else + else // little-endian { - data[0] |= ival << curPos->offsetBits; + if (curPos->offsetBits > 31) + { + // data entirely in the second word + data[1] |= ival << (curPos->offsetBits - 32); + } + else if ((curPos->offsetBits + curPos->numBits) <= 32) + { + // data entirely in the first word + data[0] |= ival << curPos->offsetBits; + } + else + { + // data spans both words + data[0] |= ival << curPos->offsetBits; + data[1] |= ival >> (32 - curPos->offsetBits); + } } } @@ -453,8 +503,18 @@ void CanMap::ClearMap(CANIDMAP *canMap) int CanMap::Add(CANIDMAP *canMap, Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, int8_t length, float gain, int8_t offset) { if (canId > MAX_COB_ID) return CAN_ERR_INVALID_ID; - if (offsetBits > 63) return CAN_ERR_INVALID_OFS; - if (length > 32 || length < -32) return CAN_ERR_INVALID_LEN; + if (length == 0 || ABS(length) > 32) return CAN_ERR_INVALID_LEN; + if (length > 0) + { + // little-endian mapping + if (offsetBits + length - 1 > 63) return CAN_ERR_INVALID_OFS; + } + else + { + // big-endian mapping + if (offsetBits > 63) return CAN_ERR_INVALID_OFS; + if (static_cast(offsetBits) + length + 1 < 0) return CAN_ERR_INVALID_OFS; + } CANIDMAP *existingMap = FindById(canMap, canId); diff --git a/src/cansdo.cpp b/src/cansdo.cpp index 08fcc01..9c444a6 100644 --- a/src/cansdo.cpp +++ b/src/cansdo.cpp @@ -330,7 +330,7 @@ void CanSdo::ReadOrDeleteCanMap(CAN_SDO* sdo) else if (sdo->subIndex & 1) //odd sub indexes have data id, position and length sdo->data = id | (canPos->offsetBits << 16) | (canPos->numBits << 24); else //even sub indexes except 0 have gain and offset - sdo->data = (((uint32_t)(canPos->gain * 1000)) & 0xFFFFFF) | (canPos->offset << 24); + sdo->data = (uint32_t)(((int32_t)(canPos->gain * 1000)) & 0xFFFFFF) | (canPos->offset << 24); sdo->cmd = SDO_READ_REPLY; } else @@ -379,7 +379,11 @@ void CanSdo::AddCanMap(CAN_SDO* sdo, bool rx) else if (mapInfo.numBits != 0 && sdo->subIndex == 2) //This sort of verifies that we received subindex 1 { //Now we receive gain and offset and add the map - mapInfo.gain = (sdo->data & 0xFFFFFF) / 1000.0f; + + // sign extend the 24-bit integer to a 32-bit integer + int32_t gainFixedPoint = (sdo->data & 0xFFFFFF) << (32-24); + gainFixedPoint >>= (32-24); + mapInfo.gain = gainFixedPoint / 1000.0f; mapInfo.offset = sdo->data >> 24; if (rx) //RX map