From e80f04d857448279014560d3ca379c842e748f82 Mon Sep 17 00:00:00 2001
From: Valentin Dabstep <keklolov7@gmail.com>
Date: Sun, 8 Jun 2025 12:29:55 +0300
Subject: [PATCH] Add test SimpleFOC

---
 controller/fw/embed/test/test_simpleFOC.py | 96 ++++++++++++++++++++++
 1 file changed, 96 insertions(+)
 create mode 100644 controller/fw/embed/test/test_simpleFOC.py

diff --git a/controller/fw/embed/test/test_simpleFOC.py b/controller/fw/embed/test/test_simpleFOC.py
new file mode 100644
index 0000000..d87a6e8
--- /dev/null
+++ b/controller/fw/embed/test/test_simpleFOC.py
@@ -0,0 +1,96 @@
+import can
+import time
+import sys
+import struct
+
+# Конфигурация
+CAN_INTERFACE = 'can0'
+DEVICE_ID = 0x69  # Текущий ID устройства
+REG_READ = 0x7
+REG_WRITE = 0x8
+DATA_TYPE_ANGLE = 0x03
+DATA_TYPE_VELOCITY = 0x04
+DATA_TYPE_TORQUE = 0x05
+
+# CRC функция (аналогичная устройству)
+def validate_crc16(data):
+    crc = 0xFFFF
+    for byte in data:
+        crc ^= byte
+        for _ in range(8):
+            if crc & 0x0001:
+                crc = (crc >> 1) ^ 0xA001
+            else:
+                crc >>= 1
+    return crc
+
+def send_can_message(bus, can_id, data):
+    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 test_simplefoc_else_block():
+    bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
+    
+    # 1. Установка типа данных (DATA_TYPE_ANGLE)
+    can_id_write = (DEVICE_ID << 4) | REG_WRITE
+    data_set_type = [DATA_TYPE_ANGLE, 0x00]
+    full_data = list(can_id_write.to_bytes(2, 'little')) + data_set_type
+    crc = validate_crc16(full_data)
+    crc_bytes = list(crc.to_bytes(2, 'little'))
+    packet = data_set_type + crc_bytes
+    send_can_message(bus, can_id_write, packet)
+    time.sleep(0.1)  # Ожидание обработки
+    
+    # 2. Отправка SimpleFOC сообщения (угол)
+    target_angle = 45.0
+    angle_bytes = struct.pack('<f', target_angle)
+    can_id_simplefoc = (DEVICE_ID << 4) | 0x01  # Не REG_READ/REG_WRITE
+    payload = [0x00] + list(angle_bytes) + [0x00]  # [type placeholder, angle, padding]
+    
+    # Расчет CRC
+    full_data_sf = list(can_id_simplefoc.to_bytes(2, 'little')) + payload
+    crc_sf = validate_crc16(full_data_sf)
+    payload += list(crc_sf.to_bytes(2, 'little'))
+    
+    # Отправка
+    print("\nТест SimpleFOC (блок else):")
+    send_can_message(bus, can_id_simplefoc, payload)
+    
+    # 3. Проверка ответа (отправка угла + установка нового угла)
+    response = receive_response(bus)
+    if response:
+        # Проверка структуры ответа
+        if response.data[0] == ord('A'):
+            print("Успех: Отправлен текущий угол")
+        else:
+            print("Ошибка: Неверный тип ответа")
+    else:
+        print("Ошибка: Нет ответа от устройства")
+
+    # 4. Проверка установки нового угла (интеграционно)
+    # ... (может требовать дополнительной проверки на устройстве)
+    
+    bus.shutdown()
+
+if __name__ == "__main__":
+    test_simplefoc_else_block()
\ No newline at end of file