Merge branch 'scs' of https://github.com/mmr-driverless/mmr_can into scs

Inc/mmr_can.h

@@ -95,4 +95,6 @@ MmrCanFilterSettings MMR_CAN_GetDefaultFilterSettings();
 HalStatus MMR_CAN_Send(CanHandle *hcan, MmrCanPacket packet);
 HalStatus MMR_CAN_Receive(CanHandle *hcan, MmrCanMessage *result);
 
+HalStatus MMR_CAN_SendSCS(CanHandle *hcan, MmrCanMessageId scsId, CanMailbox *mailbox, CanId senderId);
+
 #endif /* INC_MMR_CAN_H_ */

Inc/mmr_can_scs_dict.h

@@ -0,0 +1,27 @@
+#ifndef INC_MMR_CAN_SCS_H_
+#define INC_MMR_CAN_SCS_H_
+
+#ifndef MMR_CAN_SCS_DICTIONARY_SIZE
+#define MMR_CAN_SCS_DICTIONARY_SIZE 5
+#endif
+
+#include <stdint.h>
+#include "mmr_can.h"
+
+
+typedef struct {
+  uint32_t key;
+  uint32_t delayStart;
+} MmrCanScsDictEntry;
+
+typedef MmrCanScsDictEntry MmrCanScsDictionary[MMR_SCS_DICTIONARY_SIZE];
+typedef void(*MmrCanScsDictAction)(MmrCanScsDictEntry *entry);
+
+
+uint32_t MMR_CAN_ScsDictPut(MmrCanScsDictEntry entry);
+void MMR_CAN_ScsDictRemove(uint32_t key);
+void MMR_CAN_ScsDictForeach(MmrCanScsDictAction action);
+
+uint32_t MMR_CAN_ScsGetKey(MmrCanMessage *message);
+
+#endif // !INC_MMR_CAN_SCS_H_

Src/mmr_can_scs_dict.c

@@ -0,0 +1,30 @@
+#include "mmr_can_scs.h"
+
+static MmrCanScsDictionary _dictionary;
+
+
+uint32_t MMR_CAN_ScsDictPut(MmrCanScsDictEntry entry) {
+  _dictionary[entry.key % MMR_CAN_SCS_DICTIONARY_SIZE] = entry.delayStart;
+}
+
+void MMR_CAN_ScsDictRemove(uint32_t key) {
+  _dictionary[key] = {};
+}
+
+void MMR_CAN_ScsDictForeach(MmrCanScsDictAction action) {
+  int i = 0;
+  for (; i < MMR_CAN_SCS_DICTIONARY_SIZE; i++) {
+    MmrCanScsDictEntry entry = _dictionary[i];
+    if (entry.key != 0) {
+      action(&entry);
+    }
+  }
+}
+
+
+uint32_t MMR_CAN_ScsMessageAsKey(MmrCanMessage *message) {
+  uint16_t upperHalf = message->header.messageId;
+  uint16_t lowerHalf = message->header.messageType;
+
+  return (upperHalf << 16) | lowerHalf;
+}

Src/mmr_can_send_scs.c

@@ -0,0 +1,21 @@
+#include "mmr_can.h"
+
+
+HalStatus MMR_CAN_SendSCS(
+  CanHandle *hcan,
+  MmrCanMessageId scsId,
+  CanMailbox *mailbox,
+  CanId senderId
+) {
+  MmrCanPacket packet = {
+    .header = {
+      .senderId = senderId,
+      .messageId = scsId,
+      .priority = MMR_CAN_MESSAGE_PRIORITY_HIGH,
+    },
+    .mailbox = mailbox,
+    .length = 0,
+  };
+  
+  return MMR_CAN_Send(hcan, packet);
+}