From e764a9ba121a878b5828189087deef241baf6be0 Mon Sep 17 00:00:00 2001
From: Stefano Calabretti <45517710+cala-br@users.noreply.github.com>
Date: Mon, 7 Mar 2022 00:34:36 +0100
Subject: [PATCH] Update header (#8)

* Fix wrong bit shifts

* Add 'drive::steering_angle' scs
---
 Inc/mmr_can_message_id.h | 11 ++++++++---
 Src/mmr_can_header.c     | 14 +++++++-------
 2 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/Inc/mmr_can_message_id.h b/Inc/mmr_can_message_id.h
index 26844f9..a88accc 100644
--- a/Inc/mmr_can_message_id.h
+++ b/Inc/mmr_can_message_id.h
@@ -27,14 +27,16 @@
  * 
  * Ids scheme:
  *
- *  id  | type | subtype
- * -----|------|---------
- *  SCS | 000  | xxxxxxx
+ *   id   | type | subtype
+ * -------|------|---------
+ *  SCS   | 000  | xxxxxxx
+ *  DRIVE | 001  | xxxxxxx
  */
 typedef enum MmrCanMessageId MmrCanMessageId;
 
 typedef enum {
   MMR_CAN_MESSAGE_ID_TYPE_SCS = 0,
+  MMR_CAN_MESSAGE_ID_TYPE_DRIVE = 1,
 } MmrCanMessageIdType;
 
 
@@ -90,6 +92,9 @@ enum MmrCanMessageId {
   MMR_CAN_MESSAGE_ID_SCS_AM_EBS_TEST,
   MMR_CAN_MESSAGE_ID_SCS_AM_INSPECTION = 0x17,
 // !SCS
+// DRIVE
+  MMR_CAN_MESSAGE_ID_DRIVE_STEERING_ANGLE = 0x8A,
+// !DRIVE
 };
 
 #endif // !INC_MMR_CAN_MESSAGE_ID_H_
diff --git a/Src/mmr_can_header.c b/Src/mmr_can_header.c
index bf05a3e..f3e4d1d 100644
--- a/Src/mmr_can_header.c
+++ b/Src/mmr_can_header.c
@@ -4,18 +4,18 @@
 
 uint32_t MMR_CAN_HeaderToBits(MmrCanHeader header) {
   return 0
-  | ((uint32_t)header.priority << 26)
-  | ((uint32_t)header.messageId << 23)
-  | ((uint32_t)header.senderId << 13)
-  | ((uint32_t)header.seqNumber << 3)
-  | ((uint32_t)header.messageType);
+    | ((uint32_t)header.priority << 26)
+    | ((uint32_t)header.messageId << 16)
+    | ((uint32_t)header.senderId << 6)
+    | ((uint32_t)header.seqNumber << 3)
+    | ((uint32_t)header.messageType);
 }
 
 MmrCanHeader MMR_CAN_HeaderFromBits(uint32_t bits) {
   return (MmrCanHeader) {
     .priority = bits >> 26,
-    .messageId = bits >> 23,
-    .senderId = bits >> 13,
+    .messageId = bits >> 16,
+    .senderId = bits >> 6,
     .seqNumber = bits >> 3,
     .messageType = bits,
   };