From 3e35fd99a16fdd96a0cec32dbcb77493694fa3d3 Mon Sep 17 00:00:00 2001
From: Valentin Dabstep <keklolov7@gmail.com>
Date: Wed, 14 May 2025 19:26:10 +0300
Subject: [PATCH] For all types

---
 controller/fw/embed/src/main.cpp | 20 +++++++++++---------
 1 file changed, 11 insertions(+), 9 deletions(-)

diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index cf14e7b..d022e94 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -6,6 +6,7 @@
 #include <AS5045.h>
 #include <DRV8313.h>
 #include <cstring>
+#include <iostream>
 #include <iterator>
 #include "common/base_classes/FOCMotor.h"
 #include "hal_conf_extra.h"
@@ -117,22 +118,23 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
   motor->initFOC();
 }
 
-void send_can_with_id_crc(uint8_t id, uint8_t message_type, const void* data, size_t data_length = 7) {
+template <typename T>
+void send_can_with_id_crc(uint8_t id, uint8_t message_type, const T* data) {
   // Create CAN message
   CAN_message_t msg_l;
   msg_l.id = id;
   // msg_l.len = 8;  // Protocol-defined message length
   memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t));
-  memcpy(&msg_l.buf[1], data, ID_SIZE);
+  memcpy(&msg_l.buf[1], data, sizeof(T));
 
   // Prepare CRC calculation buffer (ID + data)
   uint8_t crc_data[CAN_MSG_MAX_LEN];
 
   // Copy message ID 
-  memcpy(crc_data, (uint8_t*)&msg_l.id, ID_SIZE);
+  memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T));
   // Copy all data bytes
   for(int i = 1;i < CAN_MSG_MAX_LEN;i++)
-    memcpy(crc_data + i,(uint8_t*)&msg_l.buf[i - 1], sizeof(uint8_t));  //As byte variable
+    memcpy(crc_data + i,(uint8_t*)&msg_l.buf[i - 1], sizeof(T));  //As byte variable
   // Calculate CRC
   uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
  
@@ -152,7 +154,7 @@ void send_velocity() {
   }
   uint8_t value = flash_rec[vel].value;
   uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'V',&value,sizeof(value));
+  send_can_with_id_crc(id,'V',&value);
 }
 
 void send_angle() {
@@ -162,7 +164,7 @@ void send_angle() {
       return;
   }
   uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'A',&current_angle,sizeof(current_angle));
+  send_can_with_id_crc(id,'A',&current_angle);
 }
 
 void send_motor_enabled() {
@@ -182,7 +184,7 @@ void send_foc_state() {
   }
   uint8_t value = flash_rec[foc_id].value;
   uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'F',&value,sizeof(value));
+  send_can_with_id_crc(id,'F',&value);
 }
 
 void send_id() {
@@ -193,7 +195,7 @@ void send_id() {
   }
 
   uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'I',&id,sizeof(id));
+  send_can_with_id_crc(id,'I',&id);
   __NOP();
 }
 
@@ -227,7 +229,7 @@ void send_pid(uint8_t param_pid){
   if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp;
   else if(param_pid == pid_i)param_pid = REG_MOTOR_POSPID_Ki;
   else if(param_pid == pid_d)param_pid = REG_MOTOR_POSPID_Kd;
-  send_can_with_id_crc(id,param_pid,&data_send,sizeof(data_send));
+  send_can_with_id_crc(id,param_pid,&data_send);
 }
 
 void setup_id(uint8_t my_id) {