Fix CRC from read data

controller/fw/embed/include/reg_cah.h

@@ -37,4 +37,11 @@
 #define MOTOR_ANGLE             0x72
 #define MOTOR_TORQUE            0x73
 
+
+//For send
+#define CAN_MSG_MAX_LEN 7
+#define CRC_SIZE        2
+#define ID_SIZE         sizeof(uint8_t)
+
+
 #endif  // REG_CAH_H_

controller/fw/embed/src/main.cpp

@@ -123,19 +123,18 @@ void send_can_with_id_crc(uint8_t id, uint8_t message_type, const void* data, si
   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, 1);
+  memcpy(&msg_l.buf[1], data, ID_SIZE);
 
   // Prepare CRC calculation buffer (ID + data)
-  size_t crc_data_size = sizeof((uint8_t)msg_l.id) +  2 * sizeof(uint8_t);
-  uint8_t crc_data[7];
+  uint8_t crc_data[CAN_MSG_MAX_LEN];
 
   // Copy message ID 
-  memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(uint8_t));
+  memcpy(crc_data, (uint8_t*)&msg_l.id, ID_SIZE);
   // Copy all data bytes
-  for(int i = 1;i < 7;i++)
-    memcpy(crc_data + i,(uint8_t*)&msg_l.buf[i - 1],1);
+  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
   // Calculate CRC
-  uint16_t crc_value = validate_crc16(crc_data, 7);
+  uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
  
   // Insert CRC into buffer
   msg_l.buf[6] = crc_value & 0xFF;

controller/fw/embed/test/python_read_id.py

@@ -0,0 +1,102 @@
+import can
+import time
+
+# Конфигурация
+CAN_INTERFACE = 'can0'
+OLD_DEVICE_ID = 0x03  # Текущий ID устройства (по умолчанию)
+REG_READ = 0x7        # Код команды чтения
+REG_ID = 0x1          # Адрес регистра с ID устройства
+
+def send_can_message(bus, can_id, data):
+    """Отправка CAN-сообщения"""
+    try:
+        msg = can.Message(
+            arbitration_id=can_id,
+            data=data,
+            is_extended_id=False
+        )
+        bus.send(msg)
+        print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
+        return True
+    except can.CanError as e:
+        print(f"Ошибка CAN: {e}")
+        return False
+
+def receive_response(bus, timeout=1.0):
+    """Ожидание ответа от устройства"""
+    start_time = time.time()
+    while time.time() - start_time < timeout:
+        msg = bus.recv(timeout=0.1)
+        if msg:
+            print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
+            return msg
+    print("[Ошибка] Таймаут")
+    return None
+
+def validate_crc16(data):
+    """Расчет CRC16 (MODBUS) для проверки целостности данных"""
+    crc = 0xFFFF
+    for byte in data:
+        crc ^= byte
+        for _ in range(8):
+            if crc & 0x0001:
+                crc = (crc >> 1) ^ 0xA001
+            else:
+                crc >>= 1
+    return crc
+
+# Инициализация CAN-интерфейса
+bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
+
+# ======= 1. Запрос текущего ID устройства =======
+
+# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ
+can_id_read = (OLD_DEVICE_ID << 4) | REG_READ
+
+# Данные для запроса: [регистр, резервный байт]
+data_read = [REG_ID, 0x00]
+
+# Формируем полные данные для расчета CRC:
+# - CAN ID разбивается на 2 байта (little-endian)
+# - Добавляем данные запроса
+full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
+
+# Рассчитываем CRC и разбиваем на байты (little-endian)
+crc = validate_crc16(full_data_for_crc)
+crc_bytes = list(crc.to_bytes(2, 'little'))
+
+# Собираем итоговый пакет: данные + CRC
+packet_read = data_read + crc_bytes
+
+print("Запрос на чтение ID:", packet_read)
+send_can_message(bus, can_id_read, packet_read)
+
+# ======= 2. Получение и проверка ответа =======
+response = receive_response(bus)     
+if response:
+    data = response.data
+
+    if len(data) < 4:
+        print("Слишком короткий ответ")
+
+    # Проверяем минимальную длину ответа (данные + CRC)
+    else:
+        id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
+        #buff with id and data without CRC
+        full_data = list(id_bytes) + list(data[:-2])
+        print(f"Received full_data: {list(full_data)}")
+        received_crc = int.from_bytes(data[-2:], byteorder='little')
+        #calc CRC
+        calc_crc = validate_crc16(full_data)
+        
+        print(f"Расчитанный CRC PYTHON : 0x{calc_crc:02X}")
+        if received_crc == calc_crc:
+            # Если CRC совпадает, проверяем структуру ответа:
+            print(f"Текущий ID устройства: 0x{data[1]:02X}")
+        else:
+            print("Ошибка: CRC не совпадает")
+else:
+    print("Устройство не ответило")
+
+# Завершаем работу с шиной
+bus.shutdown()

controller/fw/embed/test/python_test_id.py

@@ -3,7 +3,7 @@ import time
 
 # Конфигурация
 CAN_INTERFACE = 'can0'
-OLD_DEVICE_ID = 0x00
+OLD_DEVICE_ID = 0x03
 NEW_DEVICE_ID = 0x69
 REG_WRITE = 0x8
 REG_READ = 0x7
@@ -24,7 +24,7 @@ def send_can_message(bus, can_id, data):
         print(f"Ошибка CAN: {e}")
         return False
 
-def receive_response(bus, timeout=2.0):
+def receive_response(bus, timeout=1.0):
     """Ожидание ответа"""
     start_time = time.time()
     while time.time() - start_time < timeout:
@@ -73,7 +73,7 @@ print("Отправляем: команда изменить ID + CRC:", packet_
 # Отправляем с `OLD_DEVICE_ID` в качестве адреса
 send_can_message(bus, (OLD_DEVICE_ID << 4) | REG_WRITE, packet_write)
 
-time.sleep(0.5)
+time.sleep(1.0)
 
 # ======= 2. Запрашиваем текущий ID (используем новый адрес) =======
 
@@ -98,10 +98,13 @@ if response:
     if len(data) < 4:
         print("Ответ слишком короткий")
     else:
+        id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
+        #buff with id and data without CRC
+        full_data = list(id_bytes) + list(data[:-2])
+        print(f"Received full_data: {list(full_data)}")
         received_crc = int.from_bytes(data[-2:], byteorder='little')
-        print("Полученный CRC: ", received_crc)
-        # Расчет CRC по всему пакету без CRC
-        calc_crc = validate_crc16(data[:-2])
+        #calc CRC
+        calc_crc = validate_crc16(full_data)
         if received_crc == calc_crc:
             if data[0] == ord('I') and data[1] == NEW_DEVICE_ID:
                 print(f"\nУСПЕХ! ID устройства изменен на 0x{NEW_DEVICE_ID:02X}")