From f887c30e86a3d3d5f0477f98bb7bf5277d9fbecc Mon Sep 17 00:00:00 2001
From: Vincent Manoukian <10980775+manoukianv@users.noreply.github.com>
Date: Mon, 30 Aug 2021 09:39:21 +0200
Subject: [PATCH] Add a poll CAN message.

---
 comm_can.c     | 8 ++++++++
 conf_general.h | 2 +-
 datatypes.h    | 3 ++-
 3 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/comm_can.c b/comm_can.c
index 1c5f55d7..daa59178 100644
--- a/comm_can.c
+++ b/comm_can.c
@@ -1774,6 +1774,14 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced)
 		}
 	} break;
 
+	case CAN_PACKET_POLL_ROTOR_POS: {
+		uint8_t buffer[4];
+		int32_t index = 0;
+		buffer_append_int32(buffer, (int32_t)(encoder_read_deg() * 100000.0), &index);
+		comm_can_transmit_eid_replace(data8[0] |
+			((uint32_t)CAN_PACKET_POLL_ROTOR_POS << 8), (uint8_t*)buffer, 4, true);
+	}
+
 	default:
 		break;
 	}
diff --git a/conf_general.h b/conf_general.h
index b031441e..8e100d2a 100755
--- a/conf_general.h
+++ b/conf_general.h
@@ -24,7 +24,7 @@
 #define FW_VERSION_MAJOR			5
 #define FW_VERSION_MINOR			03
 // Set to 0 for building a release and iterate during beta test builds
-#define FW_TEST_VERSION_NUMBER		50
+#define FW_TEST_VERSION_NUMBER		51
 
 #include "datatypes.h"
 
diff --git a/datatypes.h b/datatypes.h
index 43d72199..42595fad 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -1105,7 +1105,8 @@ typedef enum {
 	CAN_PACKET_BMS_AH_WH_CHG_TOTAL,
 	CAN_PACKET_BMS_AH_WH_DIS_TOTAL,
 	CAN_PACKET_UPDATE_PID_POS_OFFSET,
-	CAN_PACKET_MAKE_ENUM_32_BITS = 0xFFFFFFFF
+	CAN_PACKET_POLL_ROTOR_POS,
+	CAN_PACKET_MAKE_ENUM_32_BITS = 0xFFFFFFFF,
 } CAN_PACKET_ID;
 
 // Logged fault data