ADD persistent parameters (PID, ID), request-response model via CAN bus, Position Control

controller/fw/embed/test/python_test_id.py

@@ -0,0 +1,115 @@
+import can
+import time
+
+# Конфигурация
+CAN_INTERFACE = 'can0'
+OLD_DEVICE_ID = 0x00
+NEW_DEVICE_ID = 0x69
+REG_WRITE = 0x8
+REG_READ = 0x7
+REG_ID = 0x1
+
+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=2.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
+
+# Инициализация
+bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
+
+# ======= 1. Отправляем команду изменить ID =======
+
+# Весь буфер: id + команда + параметры
+OLD_WITH_REG = (OLD_DEVICE_ID << 4) | REG_WRITE
+id_bytes = list(OLD_WITH_REG.to_bytes(2, byteorder='little'))
+
+# Важные части сообщения: address (id), команда, параметры
+data_write = [REG_ID, NEW_DEVICE_ID]  # команда изменить ID
+
+# Полностью собираем массив для CRC (включая id и команду)
+full_data_for_crc = id_bytes + data_write
+
+# Расчет CRC по всему пакету
+crc = validate_crc16(full_data_for_crc)
+crc_bytes = list(crc.to_bytes(2, byteorder='little'))
+
+# Итоговый пакет: команда + параметры + CRC
+packet_write = data_write + crc_bytes
+
+print("Отправляем: команда изменить ID + CRC:", packet_write)
+# Отправляем с `OLD_DEVICE_ID` в качестве адреса
+send_can_message(bus, (OLD_DEVICE_ID << 4) | REG_WRITE, packet_write)
+
+time.sleep(0.5)
+
+# ======= 2. Запрашиваем текущий ID (используем новый адрес) =======
+
+# Теперь для запроса используем **уже новый id**
+NEW_WITH_REG = (NEW_DEVICE_ID << 4) | REG_READ
+current_id_bytes = list(NEW_WITH_REG.to_bytes(2, byteorder='little'))
+data_read = [REG_ID, 0x00]
+
+full_data_for_crc = current_id_bytes + data_read
+crc = validate_crc16(full_data_for_crc)
+crc_bytes = list(crc.to_bytes(2, byteorder='little'))
+packet_read = data_read + crc_bytes
+
+print("Запрос на чтение ID + CRC (после смены):", packet_read)
+send_can_message(bus, (NEW_DEVICE_ID << 4) | REG_READ, packet_read)
+
+# ======= 3. Получение и проверка ответа =======
+
+response = receive_response(bus)
+if response:
+    data = response.data
+    if len(data) < 4:
+        print("Ответ слишком короткий")
+    else:
+        received_crc = int.from_bytes(data[-2:], byteorder='little')
+        print("Полученный CRC: ", received_crc)
+        # Расчет CRC по всему пакету без CRC
+        calc_crc = validate_crc16(data[:-2])
+        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}")
+            else:
+                print(f"Некорректный ответ: {list(data)}")
+        else:
+            print("CRC не совпадает, данные повреждены.")
+else:
+    print("Нет ответа от устройства.")
+
+bus.shutdown()