From 54c8296622dd2fe662e7a475f64c50870f4d910e Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Sat, 19 Apr 2025 13:42:10 +0300
Subject: [PATCH] fix: typo in header name and change buf0 flags and fix tests

---
 .../fw/embed/include/{reg_cah.h => reg_can.h} |   7 +-
 controller/fw/embed/src/main.cpp              |  29 ++---
 controller/fw/embed/test/python_can.py        | 106 +++++++++++++-----
 controller/fw/embed/test/python_send_angle.py |  66 ++++++++---
 .../fw/embed/test/python_send_velocity.py     |   2 +-
 5 files changed, 149 insertions(+), 61 deletions(-)
 rename controller/fw/embed/include/{reg_cah.h => reg_can.h} (90%)

diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_can.h
similarity index 90%
rename from controller/fw/embed/include/reg_cah.h
rename to controller/fw/embed/include/reg_can.h
index dbbfdc7..5ed1513 100644
--- a/controller/fw/embed/include/reg_cah.h
+++ b/controller/fw/embed/include/reg_can.h
@@ -1,5 +1,5 @@
-#ifndef REG_CAH_H_
-#define REG_CAH_H_
+#ifndef REG_CAN_H_
+#define REG_CAN_H_
 
 #define APP_ADDR        0x0800400  // 16KB - Application
 #define ADDR_VAR        0x8040000
@@ -36,5 +36,6 @@
 #define MOTOR_ENABLED           0x71
 #define MOTOR_ANGLE             0x72
 #define MOTOR_TORQUE            0x73
+#define MOTOR_ID                0x74
 
-#endif  // REG_CAH_H_
+#endif  // REG_CAN_H_
diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index 8dd8e66..9dddab2 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -1,18 +1,16 @@
 // clang-format off
 #include "Arduino.h"
+#include "reg_can.h"
 #include "stm32f446xx.h"
 #include <SimpleFOC.h>
 #include <STM32_CAN.h>
 #include <AS5045.h>
 #include <DRV8313.h>
 #include <cstring>
-#include <iterator>
 #include "common/base_classes/FOCMotor.h"
 #include "hal_conf_extra.h"
-#include "wiring_analog.h"
 #include "wiring_constants.h"
 // clang-format on
-#include "reg_cah.h"
 #include "flash.h"
 
 
@@ -145,7 +143,7 @@ void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, s
 
 
 void send_velocity() {
-  float current_velocity = motor.shaftVelocity();
+  float current_velocity = motor.shaft_velocity;
   flash_rec = load_params();
   if (flash_rec == nullptr) { // Проверка на NULL
       // Обработка ошибки: запись в лог, сигнализация и т.д.
@@ -153,11 +151,11 @@ 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, MOTOR_VELOCITY, &value,sizeof(value));
 }
 
 void send_angle() {
-  float current_angle = motor.shaftAngle();
+  float current_angle = motor.shaft_angle;
 
   flash_rec = load_params();
   if (flash_rec == nullptr) { // Проверка на NULL
@@ -166,13 +164,13 @@ void send_angle() {
   }
   // uint8_t value = flash_rec[angl].value;
   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, MOTOR_ANGLE, &current_angle,sizeof(current_angle));
 }
 
 void send_motor_enabled() {
   uint8_t id = *(volatile uint8_t*)ADDR_VAR;
   CAN_TX_msg.id = id;
-  CAN_TX_msg.buf[0] = 'E';
+  CAN_TX_msg.buf[0] = MOTOR_ENABLED;
   memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled,
          sizeof(motor_control_inputs.motor_enabled));
   Can.write(CAN_TX_msg);
@@ -188,7 +186,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, FOC_STATE, &value,sizeof(value));
 }
 
 void send_id() {
@@ -199,7 +197,7 @@ void send_id() {
       return;
   }
   uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'I',&id,sizeof(id));
+  send_can_with_id_crc(id, MOTOR_ID, &id,sizeof(id));
   __NOP();
 }
 
@@ -209,7 +207,7 @@ void send_motor_torque() {
   torque *= 100;
   flash_rec = load_params();
   CAN_TX_msg.id = flash_rec->value;
-  CAN_TX_msg.buf[0] = 'T';
+  CAN_TX_msg.buf[0] = MOTOR_TORQUE;
   CAN_TX_msg.len = 5;
   memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
   Can.write(CAN_TX_msg);
@@ -247,9 +245,12 @@ void setup_id(uint8_t my_id) {
 }
 
 void setup_angle(float target_angle) {
-  // float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы
-  motor.enable();  // Включаем мотор если он отключен
-  motor.controller = MotionControlType::angle;
+  if (!motor_control_inputs.motor_enabled) {
+    motor.enable();
+  }
+  if (motor.controller != MotionControlType::angle) {
+    motor.controller = MotionControlType::angle;
+  }
   motor.move(target_angle);
 }
 
diff --git a/controller/fw/embed/test/python_can.py b/controller/fw/embed/test/python_can.py
index 7a54a29..61d8132 100644
--- a/controller/fw/embed/test/python_can.py
+++ b/controller/fw/embed/test/python_can.py
@@ -2,29 +2,68 @@ import can
 import struct
 import time
 
-def process_can_message(msg):
-    if msg.dlc == 5:  # Check the message length
-        print(f"Received message with ID: {msg.arbitration_id}")
-        print(f"Data: {msg.data}")
 
-        # The first byte determines the data type (flag)
-        flag = chr(msg.data[0])
-        
-        if flag == 'A':  # Angle
-            angle_bytes = msg.data[1:5]
-            angle = struct.unpack('<f', bytes(angle_bytes))[0]
-            print(f"Angle: {angle} degrees")
-        elif flag == 'V':  # Velocity
-            velocity_bytes = msg.data[1:5]
-            velocity = struct.unpack('<f', bytes(velocity_bytes))[0]
-            print(f"Velocity: {velocity} rad/s")
-        elif flag == 'E' and msg.dlc >= 2:  # Enable/Disable
-            enabled = msg.data[1]  # Expecting 1 byte (0 or 1)
-            print(f"Enabled: {bool(enabled)}")
-        else:
-            print(f"Unknown flag: {flag}")
-    else:
-        print(f"Received message with unexpected length: {msg.dlc}")
+REG_READ = 0x7
+DEVICE_ID = 0x00          # Старый ID устройства
+REG_POS = 0x72             # Предположим, что команда записи позиции — 0x2
+CAN_INTERFACE = 'can0'
+
+
+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
+
+def process_can_message(bus):
+    # ID и команда
+    arbitration_id = (DEVICE_ID << 4) | REG_READ
+    id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
+
+    # Команда и параметры
+    data_read = [REG_POS] + list(struct.pack('<f', 0))
+
+    # Для CRC — весь пакет: id + команда + параметры
+    full_data_for_crc = id_bytes + data_read
+    crc = validate_crc16(full_data_for_crc)
+    crc_bytes = list(crc.to_bytes(2, byteorder='little'))
+
+    # Итоговый пакет
+    packet = data_read + crc_bytes
+
+    # Создание и отправка сообщения
+    msg = can.Message(
+        arbitration_id=arbitration_id,
+        is_extended_id=False,
+        data=packet
+    )
+
+    try:
+        bus.send(msg)
+    except can.CanError:
+        print("Ошибка отправки сообщения")
+
+
+def receive_response(bus, timeout=2.0):
+    """Ожидание ответа"""
+    start_time = time.time()
+    while time.time() - start_time < timeout:
+        msg = bus.recv(timeout=timeout)
+        data_bytes = msg.data[1:5]  # берём первые 4 байта
+        print(data_bytes)
+        angle = struct.unpack('<f', bytes(data_bytes))[0]  # '<f' = little-endian float
+        # angle = int.from_bytes(data_bytes, byteorder='little', signed=True)  # или 'big'
+        if msg:
+            print(f"[Прием] Angle: 0x{msg.arbitration_id:03X}, Данные: {angle}, RAW: {bytes(msg.data)}")
+            return msg
+    print("[Ошибка] Таймаут")
+    return None
 
 def receive_can_messages():
     try:
@@ -34,9 +73,26 @@ def receive_can_messages():
         print("Waiting for messages on the CAN bus...")
 
         while True:
-            msg = bus.recv()
-            if msg:
-                process_can_message(msg)
+            process_can_message(bus)
+            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] == REG_POS:
+                            print("\nУСПЕХ!")
+                        else:
+                            print(f"Некорректный ответ: {list(data)}")
+                    else:
+                        print("CRC не совпадает, данные повреждены.")
+            else:
+                print("Нет ответа от устройства.")
 
     except KeyboardInterrupt:
         print("\nExiting program...")
diff --git a/controller/fw/embed/test/python_send_angle.py b/controller/fw/embed/test/python_send_angle.py
index dc23d9b..94e4f14 100644
--- a/controller/fw/embed/test/python_send_angle.py
+++ b/controller/fw/embed/test/python_send_angle.py
@@ -3,35 +3,65 @@ import struct
 import time
 import argparse
 
-# Function to send the target angle
+# Константы
+CAN_INTERFACE = 'can0'
+DEVICE_ID = 0x00          # Старый ID устройства
+REG_WRITE = 0x8
+REG_POS = 0x72             # Предположим, что команда записи позиции — 0x2
+
+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
+
 def send_target_angle(bus, target_angle):
-    msg = can.Message()
-    msg.arbitration_id = 1  # Message ID
-    msg.is_extended_id = False
-    msg.dlc = 5  # Message length
-    msg.data = [ord('A')] + list(struct.pack('<f', target_angle))  # 'A' for the command identifier, followed by the angle in float format
+    # ID и команда
+    arbitration_id = (DEVICE_ID << 4) | REG_WRITE
+    id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
+
+    # Команда и параметры
+    data_write = [REG_POS] + list(struct.pack('<f', target_angle))  # Позиция как float (4 байта)
+
+    # Для CRC — весь пакет: id + команда + параметры
+    full_data_for_crc = id_bytes + data_write
+    crc = validate_crc16(full_data_for_crc)
+    crc_bytes = list(crc.to_bytes(2, byteorder='little'))
+
+    # Итоговый пакет
+    packet = data_write + crc_bytes
+
+    # Создание и отправка сообщения
+    msg = can.Message(
+        arbitration_id=arbitration_id,
+        is_extended_id=False,
+        data=packet
+    )
 
     try:
         bus.send(msg)
-        print(f"Sent message with target angle: {target_angle} degrees")
-        print(f"Message data: {msg.data}")
+        print(f"[Отправка] CAN ID: 0x{arbitration_id:03X}, Угол: {target_angle} rad, Данные: {list(msg.data)}")
     except can.CanError:
-        print("Message failed to send")
+        print("Ошибка отправки сообщения")
 
-# Main function
 def main():
-    parser = argparse.ArgumentParser(description="Send target angles over CAN bus.")
-    parser.add_argument("--angle", type=float, required=True, help="Target angle to send over the CAN bus")
+    parser = argparse.ArgumentParser(description="Отправка угла позиции по CAN.")
+    parser.add_argument("--angle", type=float, required=True, help="Угол (в градусах)")
     args = parser.parse_args()
 
-    target_angle = args.angle
+    # Инициализация CAN
+    bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
+    print("CAN шина инициализирована.")
 
-    # CAN interface setup
-    bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000)  # Ensure the bitrate matches the microcontroller settings
-    print("CAN bus initialized, sending target angles...")
+    send_target_angle(bus, args.angle)
 
-    # Loop to send messages
-    send_target_angle(bus, target_angle)
+    bus.shutdown()
 
 if __name__ == '__main__':
     main()
diff --git a/controller/fw/embed/test/python_send_velocity.py b/controller/fw/embed/test/python_send_velocity.py
index dd46c6c..13ce0d3 100644
--- a/controller/fw/embed/test/python_send_velocity.py
+++ b/controller/fw/embed/test/python_send_velocity.py
@@ -45,7 +45,7 @@ def main():
     # CAN interface setup
     bus = None  # Define outside the try block for proper shutdown
     try:
-        bus = can.interface.Bus(channel='COM4', bustype='slcan', bitrate=1000000)  # Ensure the bitrate matches the microcontroller settings
+        bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000)  # Ensure the bitrate matches the microcontroller settings
         print("CAN bus initialized.")
 
         while True: