Merge branch 'master' of github.com:jsphuebner/libopeninv

include/canmap.h

@@ -26,6 +26,7 @@
 #define CAN_ERR_INVALID_LEN -3
 #define CAN_ERR_MAXMESSAGES -4
 #define CAN_ERR_MAXITEMS -5
+#define CAN_FORCE_EXTENDED 0x20000000
 
 #ifndef MAX_ITEMS
 #define MAX_ITEMS 50
@@ -35,6 +36,10 @@
 #define MAX_MESSAGES 10
 #endif
 
+#ifndef CAN_SIGNED
+#define CAN_SIGNED 0
+#endif // CAN_SIGNED
+
 #ifdef CAN_EXT
 #define MAX_COB_ID 0x1fffffff
 #else

src/canhardware.cpp

@@ -53,9 +53,11 @@ bool CanHardware::AddCallback(CanCallback* recv)
 }
 
 /** \brief Add CAN Id to user message list
+ * canId can be 0x20000000 + std id to force registering a filter for an extended ID
+ * even if the Id is < 0x7ff
  * \post Receive callback will be called when a message with this Id id received
  * \param canId CAN identifier of message to be user handled
- * \return true: success, false: already 10 messages registered
+ * \return true: success, false: already maximum messages registered
  *
  */
 bool CanHardware::RegisterUserMessage(uint32_t canId, uint32_t mask)

src/canmap.cpp

@@ -34,11 +34,20 @@
 #define ITEM_UNSET            0xff
 #define forEachCanMap(c,m) for (CANIDMAP *c = m; (c - m) < MAX_MESSAGES && c->first != MAX_ITEMS; c++)
 #define forEachPosMap(c,m) for (CANPOS *c = &canPosMap[m->first]; c->next != ITEM_UNSET; c = &canPosMap[c->next])
+#define IS_EXT_FORCE(id)      ((SHIFT_FORCE_FLAG(1) & id) != 0)
+#define MASK_EXT_FORCE(id)    (id & ~SHIFT_FORCE_FLAG(1))
+
+//If we configure the module to only support 11-bit IDs, we only have 16 bits of memory
+//allocated for the ID. In this case we put the "force extended ID filter" flag
+//into the 11th bit.
+//When allowing extended ids we put it into the 29th bit
 
 #ifdef CAN_EXT
 #define IDMAPSIZE 8
+#define SHIFT_FORCE_FLAG(f) (f << 29)
 #else
 #define IDMAPSIZE 4
+#define SHIFT_FORCE_FLAG(f) (f << 11)
 #endif // CAN_EXT
 #if (MAX_ITEMS * 12 + 2 * MAX_MESSAGES * IDMAPSIZE + 4) > FLASH_PAGE_SIZE
 #error CANMAP will not fit in one flash page
@@ -62,7 +71,8 @@ void CanMap::HandleClear()
 {
    forEachCanMap(curMap, canRecvMap)
    {
-      canHardware->RegisterUserMessage(curMap->canId);
+      bool forceExtended = IS_EXT_FORCE(curMap->canId);
+      canHardware->RegisterUserMessage((curMap->canId & ~SHIFT_FORCE_FLAG(1)) + (forceExtended * CAN_FORCE_EXTENDED));
    }
 }
 
@@ -131,12 +141,14 @@ bool CanMap::HandleRx(uint32_t canId, uint32_t data[2], uint8_t)
          // sign-extend our arbitrary sized integer out to 32-bits but only if
          // it is bigger than a single bit
          int32_t ival;
+         #if CAN_SIGNED
          if (numBits > 1)
          {
             uint32_t sign_bit = 1L << (numBits - 1);
             ival = static_cast<int32_t>(((word + sign_bit) & mask)) - sign_bit;
          }
          else
+         #endif
          {
             ival = word;
          }
@@ -249,15 +261,18 @@ void CanMap::SendAll()
  */
 int CanMap::AddSend(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;
    return Add(canSendMap, param, canId, offsetBits, length, gain, offset);
 }
 
 int CanMap::AddSend(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, int8_t length, float gain)
 {
+   if (canId > MAX_COB_ID) return CAN_ERR_INVALID_ID;
    return Add(canSendMap, param, canId, offsetBits, length, gain, 0);
 }
 
 /** \brief Map data from CAN bus to parameter
+ * To force registering an extended filter for a standard ID, add 0x20000000 to canId
  *
  * \param param Parameter index of parameter to be received
  * \param canId CAN identifier of consumed message
@@ -274,7 +289,13 @@ int CanMap::AddSend(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits,
  */
 int CanMap::AddRecv(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, int8_t length, float gain, int8_t offset)
 {
-   int res = Add(canRecvMap, param, canId, offsetBits, length, gain, offset);
+   bool forceExtended = (canId & CAN_FORCE_EXTENDED) != 0;
+   uint32_t moddedId = canId & ~CAN_FORCE_EXTENDED; //mask out force flag
+   if (moddedId > MAX_COB_ID) return CAN_ERR_INVALID_ID;
+   //Put force flag either in bit 11 (when mapping restricted to std IDs or bit 29 when allowing ext ids
+   moddedId |= SHIFT_FORCE_FLAG(forceExtended);
+
+   int res = Add(canRecvMap, param, moddedId, offsetBits, length, gain, offset);
    canHardware->RegisterUserMessage(canId);
    return res;
 }
@@ -432,7 +453,10 @@ bool CanMap::FindMap(Param::PARAM_NUM param, uint32_t& canId, uint8_t& start, in
          {
             if (curPos->mapParam == param)
             {
+               bool forceExt = IS_EXT_FORCE(curMap->canId);
                canId = curMap->canId;
+               canId = MASK_EXT_FORCE(canId);
+               canId |= forceExt * CAN_FORCE_EXTENDED;
                start = curPos->offsetBits;
                length = curPos->numBits;
                gain = curPos->gain;
@@ -457,7 +481,10 @@ const CanMap::CANPOS* CanMap::GetMap(bool rx, uint8_t ididx, uint8_t itemidx, ui
    {
       if (itemidx == 0)
       {
+         bool forceExt = IS_EXT_FORCE(map->canId);
          canId = map->canId;
+         canId = MASK_EXT_FORCE(canId);
+         canId |= forceExt * CAN_FORCE_EXTENDED;
          return curPos;
       }
       itemidx--;
@@ -475,7 +502,11 @@ void CanMap::IterateCanMap(void (*callback)(Param::PARAM_NUM, uint32_t, uint8_t,
       {
          forEachPosMap(curPos, curMap)
          {
-            callback((Param::PARAM_NUM)curPos->mapParam, curMap->canId, curPos->offsetBits, curPos->numBits, curPos->gain, curPos->offset, rx);
+            bool forceExt = IS_EXT_FORCE(curMap->canId);
+            uint32_t canId = curMap->canId;
+            canId = MASK_EXT_FORCE(canId);
+            canId |= forceExt * CAN_FORCE_EXTENDED;
+            callback((Param::PARAM_NUM)curPos->mapParam, canId, curPos->offsetBits, curPos->numBits, curPos->gain, curPos->offset, rx);
          }
       }
       done = rx;
@@ -502,7 +533,7 @@ 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 (canId > MAX_COB_ID) return CAN_ERR_INVALID_ID;
    if (length == 0 || ABS(length) > 32) return CAN_ERR_INVALID_LEN;
    if (length > 0)
    {
@@ -672,7 +703,7 @@ CanMap::CANIDMAP* CanMap::FindById(CANIDMAP *canMap, uint32_t canId)
 {
    forEachCanMap(curMap, canMap)
    {
-      if (curMap->canId == canId)
+      if ((curMap->canId & ~SHIFT_FORCE_FLAG(1)) == (canId & ~SHIFT_FORCE_FLAG(1)))
          return curMap;
    }
    return 0;

src/cansdo.cpp

@@ -358,7 +358,7 @@ void CanSdo::AddCanMap(CAN_SDO* sdo, bool rx)
 
       if (sdo->subIndex == 0)
       {
-         if (sdo->data <= MAX_COB_ID)
+         if (sdo->data < 0x20000000 || (sdo->data & ~CAN_FORCE_EXTENDED) < 0x800)
          {
             mapId = sdo->data;
             result = 0;

src/stm32_can.cpp

@@ -305,7 +305,7 @@ void Stm32Can::ConfigureFilters()
    {
       if (userIds[i] > 0x7ff)
       {
-         extIdList[extIdIndex] = userIds[i];
+         extIdList[extIdIndex] = userIds[i] & 0x1FFFFFFF;
          extIdIndex++;
       }
       else if (userMasks[i] != 0)