diff --git a/src/shared/drivers/canbus/CanProtocol.h b/src/shared/drivers/canbus/CanProtocol.h
index 73d751d55c78800e69ed2692eb458e3f3d08da0f..ae2384a0bc15a1bdc64ec462a05b755b7f755c4f 100644
--- a/src/shared/drivers/canbus/CanProtocol.h
+++ b/src/shared/drivers/canbus/CanProtocol.h
@@ -174,7 +174,6 @@ protected:
 
                 packet   = can->getRXBuffer().pop().packet;
                 sourceId = (packet.id & idMask.source) >> 15;
-                // TRACE("source id %d\n", sourceId);
                 if (sourceId >= 0 &&
                     sourceId < NPACKET)  // check for maximum size
                 {
@@ -194,9 +193,6 @@ protected:
                             data[sourceId].canId =
                                 packet.id >> 7;  // discard the sequence number
                         }
-                        /*TRACE("len %d, nrec+1 %d, left to send %d\n",
-                              data[sourceId].len, (data[sourceId].nRec + 1),
-                              leftToSend);*/
                         if ((data[sourceId].len - (data[sourceId].nRec + 1)) ==
                             leftToSend)
                         {
@@ -208,16 +204,16 @@ protected:
                                 tempPayload =
                                     tempPayload | (tempData << (f * 8));
                             }
-                            /*TRACE(
-                                "sourceID %d, Payload index %d, tempPayload "
-                                "%llu\n",
-                                sourceId, data[sourceId].len - leftToSend - 1,
-                                tempPayload);*/
-                            // check for index
-                            data[sourceId]
-                                .payload[data[sourceId].len - leftToSend - 1] =
-                                tempPayload;
-                            data[sourceId].nRec++;
+                            if (data[sourceId].len - leftToSend - 1 >= 0 &&
+                                data[sourceId].len - leftToSend - 1 <
+                                    32)  // check for index
+                            {
+
+                                data[sourceId].payload[data[sourceId].len -
+                                                       leftToSend - 1] =
+                                    tempPayload;
+                                data[sourceId].nRec++;
+                            }
                         }
 
                         if (data[sourceId].nRec == data[sourceId].len &&