From 28ce1bb5564c44c605417cb2a116219e99af8de0 Mon Sep 17 00:00:00 2001
From: Valentin Dabstep <keklolov7@gmail.com>
Date: Fri, 6 Jun 2025 18:37:48 +0300
Subject: [PATCH] Add save data_type CAN

---
 controller/fw/embed/include/flash.h     |  3 ++-
 controller/fw/embed/include/reg_cah.h   |  7 +++++
 controller/fw/embed/src/process_can.cpp | 34 +++++++++++++++++++++++--
 3 files changed, 41 insertions(+), 3 deletions(-)

diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h
index 01ec21b..6b9b3c3 100644
--- a/controller/fw/embed/include/flash.h
+++ b/controller/fw/embed/include/flash.h
@@ -22,7 +22,8 @@ enum {
   firmw,
   foc_id,
   angl,
-  vel
+  vel,
+  torq
 };
 
 /*  for saved in FLASH float data*/
diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h
index a168e19..7fced2a 100644
--- a/controller/fw/embed/include/reg_cah.h
+++ b/controller/fw/embed/include/reg_cah.h
@@ -1,5 +1,12 @@
 #pragma once
 
+
+enum{
+    error_foc = 0,
+    error_canRX,
+    error_canTX
+};
+
 #define APP_ADDR        0x0800400  // 16KB - Application
 #define ADDR_VAR        0x8040000
 
diff --git a/controller/fw/embed/src/process_can.cpp b/controller/fw/embed/src/process_can.cpp
index 5cf46ff..2cb350d 100644
--- a/controller/fw/embed/src/process_can.cpp
+++ b/controller/fw/embed/src/process_can.cpp
@@ -87,6 +87,11 @@ void send_id() {
   send_can_with_id_crc(id,'I',&id);
 }
 
+void send_error(uint8_t error_code){
+  uint8_t id = flash_rec[addr_id].value;
+  send_can_with_id_crc(id,'P',&error_code);
+}
+
 void send_pid_angle(uint8_t param_pid){
   if (flash_rec == nullptr) {  // Null check
     return;
@@ -137,6 +142,11 @@ void setup_velocity(float target_velocity) {
   motor_control_inputs.target_velocity = target_velocity;
 }
 
+void send_data_type(uint8_t type_d){
+  uint8_t id = flash_rec[addr_id].value;
+  send_can_with_id_crc(id,'D',&type_d);
+}
+
 /**
  * @brief Function for process data from CAN
  * @details Function check your ID deviceю. Compare receive and calculated CRC.
@@ -151,7 +161,7 @@ void listen_can(const CAN_message_t &msg) {
   msg_id = msg.id;
   msg_ch = msg_id & 0xF;        // Extract message channel
   uint16_t id_x = (msg_id >> 4) & 0x7FF; // Extract device address
-
+  uint8_t data_type = 1; //type for work with foc      
   /* CRC Calculation */
   uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
   uint8_t data[10] = {0}; // Message buffer for CRC verification
@@ -210,6 +220,18 @@ void listen_can(const CAN_message_t &msg) {
           write_param(pid_d,conv_float_to_int.i);
           break;
         
+        case DATA_TYPE_ANGLE:
+          data_type = DATA_TYPE_ANGLE;
+          break;
+
+        case DATA_TYPE_VELOCITY:
+          data_type = DATA_TYPE_VELOCITY;
+          break;
+
+        case DATA_TYPE_TORQUE:
+           data_type = DATA_TYPE_TORQUE;
+           break;
+
         case FIRMWARE_UPDATE:
           firmware_update();
           break;
@@ -232,6 +254,9 @@ void listen_can(const CAN_message_t &msg) {
         case MOTOR_VELOCITY: send_velocity(); break;
         case MOTOR_ANGLE: send_angle(); break;
         case MOTOR_ENABLED: send_motor_enabled(); break;
+        case DATA_TYPE_ANGLE: send_data_type(uint8_t(DATA_TYPE_ANGLE)); break;
+        case DATA_TYPE_VELOCITY: send_data_type(uint8_t(DATA_TYPE_VELOCITY)); break;
+        case DATA_TYPE_TORQUE: send_data_type(uint8_t(DATA_TYPE_TORQUE)); break;
         // case MOTOR_TORQUE: send_motor_torque(); break;
         // case FOC_STATE: send_foc_state(); break;
         case REG_MOTOR_POSPID_Kp: send_pid_angle(pid_p); break;
@@ -243,7 +268,7 @@ void listen_can(const CAN_message_t &msg) {
 
     /* If msg_ch != REG_WRITE or REG_READ, then SimpleFOC*/
     else{
-      switch(msg.buf[0]) {
+      switch(data_type) {
         /*  Read after write*/
           case DATA_TYPE_ANGLE: 
             send_angle();  
@@ -262,6 +287,11 @@ void listen_can(const CAN_message_t &msg) {
           case DATA_TYPE_TORQUE:
             send_torque();
             break;
+
+          default:
+            send_error(error_foc);
+            break; 
+            
       }
     }
   }