Add interrupt

controller/fw/embed/include/flash.h

@@ -22,7 +22,8 @@ enum {
   firmw,
   foc_id,
   angl,
-  vel
+  vel,
+  torq
 };
 
 /*  for saved in FLASH float data*/

controller/fw/embed/include/process_can.h

@@ -4,13 +4,19 @@
 #include "flash.h"
 #include "reg_cah.h"
 
+#define CAN_TIMEOUT 5  
+#define CAN_SILENCE_TIMEOUT 5
+#define MAX_CAN_READS 2
 extern FLASH_RECORD *flash_rec;
 extern volatile uint16_t msg_id;
 extern volatile uint16_t id_x;
 extern volatile uint8_t msg_ch;
 extern volatile uint8_t crc_h;
 extern volatile uint8_t crc_l;
+extern volatile bool send_blocked;
 
+static volatile bool need_send_angle = false;
+static volatile bool need_send_velocity = false;
 
 void send_velocity();
 void send_angle();
@@ -19,9 +25,12 @@ void send_motor_enabled();
 void send_id();
 void firmware_update();
 void send_pid_angle(uint8_t param_pid);
-// void send_motor_torque();
+void send_with_confirmation(void (*send_func)(void));
+// void send_torque();
 void send_pid(uint8_t param_pid);
 void setup_id(uint8_t my_id);
 void setup_angle(float target_angle);
-void setup_pid_angle(uint8_t param_pid, float data);
+void setup_velocity(float target_velocity);
+void process_can_messages();
+
 void listen_can(const CAN_message_t &msg);

controller/fw/embed/include/reg_cah.h

@@ -1,5 +1,11 @@
-#ifndef REG_CAH_H_
+#pragma once
-#define REG_CAH_H_
+
+
+enum{
+    error_foc = 0,
+    error_canRX,
+    error_canTX
+};
 
 #define APP_ADDR        0x0800400  // 16KB - Application
 #define ADDR_VAR        0x8040000
@@ -16,6 +22,10 @@
 #define REG_ID                  0x01
 #define REG_BAUDRATE            0x02
 
+#define DATA_TYPE_ANGLE         0x03
+#define DATA_TYPE_VELOCITY      0x04
+#define DATA_TYPE_TORQUE        0x05
+
 #define REG_MOTOR_POSPID_Kp     0x30
 #define REG_MOTOR_POSPID_Ki     0x31
 #define REG_MOTOR_POSPID_Kd     0x32
@@ -43,6 +53,3 @@
 #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

@@ -12,12 +12,13 @@
 #include "wiring_analog.h"
 #include "wiring_constants.h"
 // clang-format on
-
+#include "hal_conf_extra.h"
 #include "reg_cah.h"
 #include "flash.h"
 #include "config.h"
 #include "process_can.h"
 
+
 void SysTick_Handler(void) {
   HAL_IncTick();
 }
@@ -29,7 +30,7 @@ uint8_t flag_can = 0;
 uint32_t flash_error;
 FLASH_EraseInitTypeDef pEraseInit;
 uint32_t SectorError;
-
+uint32_t timeout;
 
 /* bool for test CAN    */
 volatile bool CAN_GET = false;
@@ -60,9 +61,11 @@ MotorControlInputs motor_control_inputs;
  volatile uint8_t crc_h;
  volatile uint8_t crc_l;
 
+volatile bool rx_can = false;
+CAN_message_t msg;
+
   void setup(){
   SCB->VTOR = (volatile uint32_t)0x08008004;
-  
   Serial.setRx(HARDWARE_SERIAL_RX_PIN);
   Serial.setTx(HARDWARE_SERIAL_TX_PIN);
   Serial.begin(115200);
@@ -73,19 +76,13 @@ MotorControlInputs motor_control_inputs;
   // Can.enableMBInterrupts();
   Can.begin();
   Can.setBaudRate(1000000);
-  // Настройка прерываний CAN
+
-  CAN2->IER |= CAN_IER_FMPIE0;
   flash_rec = load_params();    //for update write_ptr
   if(flash_rec[firmw].value == FIRMWARE_FLAG) NVIC_SystemReset();  //if in flash go to the bootloader
   
   // Initialize FOC system
   setup_foc(&encoder, &motor, &driver, &current_sense,flash_rec);
 
-  CAN2->IER |= CAN_IER_FMPIE0 |  // Сообщение в FIFO0
-  CAN_IER_FFIE0 |   // FIFO0 full
-  CAN_IER_FOVIE0;    // FIFO0 overflow
-
-
   // Default motor configuration
   GPIOC->ODR |= GPIO_ODR_OD11;  //set LED
   motor.torque_controller = TorqueControlType::foc_current;
@@ -94,22 +91,29 @@ MotorControlInputs motor_control_inputs;
   
 }
 
-
+bool is_can_busy() {
-
+  return (CAN2->TSR & (CAN_TSR_TME0 | CAN_TSR_TME1 | CAN_TSR_TME2)) == 0;
-void loop() {
-  __enable_irq();
-  foc_step(&motor);
-  CAN_message_t msg;
-  
-  // Process incoming CAN messages
-  while (Can.read(msg)) {
-    listen_can(msg);
-    CAN_GET = true;
-  }
-  /*  If receive data from  CAN */
-  if(CAN_GET) {
-
-    CAN_GET = false;
-  }
 }
 
+void wait_for_can_tx_complete() {
+    uint32_t start_time = HAL_GetTick();
+
+    while (!(CAN2->TSR & (CAN_TSR_TME0 | CAN_TSR_TME1 | CAN_TSR_TME2))) {
+        if (HAL_GetTick() - start_time > 500) break;
+    }
+    CAN2->TSR |= CAN_TSR_ABRQ0 | CAN_TSR_ABRQ1 | CAN_TSR_ABRQ2;
+}
+
+void loop() {
+  send_velocity();
+  HAL_Delay(1);
+  send_angle();
+  HAL_Delay(1);
+  if(rx_can){
+    Can.read(msg);
+    listen_can(msg);
+    rx_can = false;
+    HAL_Delay(1);
+  }
+  foc_step(&motor);
+}

controller/fw/embed/src/process_can.cpp

@@ -3,8 +3,8 @@
 
 static CAN_message_t CAN_TX_msg;
 static CAN_message_t CAN_inMsg;
-
+static uint8_t data_type = DATA_TYPE_ANGLE;
-
+volatile bool send_blocked = false;
 template <typename T>
 void send_can_with_id_crc(uint8_t id, uint8_t message_type, T* data) {
   // Create CAN message
@@ -40,9 +40,9 @@ void send_velocity() {
       // Error handling: logging, alerts, etc.  
       return;
   }
-  float value = flash_rec[vel].value;
+  // float value = flash_rec[vel].value;
   uint8_t id = flash_rec[addr_id].value;
-  send_can_with_id_crc(id,'V',&value);
+  send_can_with_id_crc(id,'V',&current_velocity);
 }
           
 void send_angle() {
@@ -67,6 +67,15 @@ void send_motor_enabled() {
   send_can_with_id_crc(id,'M',&value);
 }
 
+
+// void send_torque() {
+//   float i_q = motor.current.q;  // Q-axis current (A)
+//   float torque = 100 * i_q;      // Torque calculation
+//   if (flash_rec == nullptr) return;
+//   uint8_t id = flash_rec[addr_id].value;
+//   send_can_with_id_crc(id, 'T', &torque);
+// }
+
 void send_id() {
   /* Firmware data reading */
   if (flash_rec == nullptr) {  // Null check
@@ -78,16 +87,10 @@ void send_id() {
   send_can_with_id_crc(id,'I',&id);
 }
 
-// void send_motor_torque() {
+void send_error(uint8_t error_code){
-//   float i_q = motor.current.q;  // Q-axis current (A)
+  uint8_t id = flash_rec[addr_id].value;
-//   float torque = kt * i_q;      // Torque calculation
+  send_can_with_id_crc(id,'P',&error_code);
-//   torque *= 100;
+}
-//   CAN_TX_msg.id = flash_rec->value;
-//   CAN_TX_msg.buf[0] = 'T';
-//   CAN_TX_msg.len = 5;
-//   memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
-//   Can.write(CAN_TX_msg);
-// }
 
 void send_pid_angle(uint8_t param_pid){
   if (flash_rec == nullptr) {  // Null check
@@ -129,31 +132,67 @@ void setup_angle(float target_angle) {
   // motor.move(target_angle);
 }
 
-// void setup_pid_angle(uint8_t param_pid, uint32_t data){
+/**
-//   conv_float_to_int.f = data;
+ * @brief Set the up velocity object
-//   switch (param_pid) {
+ * 
-//     case pid_p:
+ * @param target_velocity 
-//       motor.P_angle.P = conv_float_to_int.f;
+ */
-//       break;
+void setup_velocity(float target_velocity) {
-//     case pid_i:
+  motor.enable();
-//       motor.P_angle.I = conv_float_to_int.f;
+  motor_control_inputs.target_velocity = target_velocity;
-//       break;
+}
-//     case pid_d:
-//       motor.P_angle.D = conv_float_to_int.f;
-//       break;
-//     default:
-//       break;
-//   }
 
-//   write_param(param_pid,data);
+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);
+}
 
+// Вспомогательные функции
+void send_with_confirmation(void (*send_func)(void)) {
+  uint32_t t_start = HAL_GetTick();
+  send_func();
   
+  // Ожидание подтверждения отправки
+  while (!(CAN2->TSR & (CAN_TSR_RQCP0 | CAN_TSR_RQCP1 | CAN_TSR_RQCP2))) {
+    if (HAL_GetTick() - t_start > CAN_TIMEOUT) break;
+  }
+}
+
+void process_can_messages() {
+  static uint32_t last_received = HAL_GetTick();
+  CAN_message_t msg;
+  uint8_t count = 0;
+  
+  // block send while data cant
+  send_blocked = true;
+  
+  while(count < MAX_CAN_READS && Can.read(msg)) {
+    listen_can(msg);
+    last_received = HAL_GetTick();
+    count++;
+  }
+  
+  // Проверка таймаута молчания
+  if(HAL_GetTick() - last_received > CAN_SILENCE_TIMEOUT) {
+    // Разблокируем отправку при отсутствии сообщений
+    send_blocked = false;
+  }
+}
+
+/**
+ * @brief Function for process data from CAN
+ * @details Function check your ID deviceю. Compare receive and calculated CRC.
+ * If ID && CRC == TRUE, then process message or dont send data.
+ * If if and crc = true:
+ * Check CAN_REG 
+ * Gives your data type 
+ * Write with data or send data
+ * @param msg 
+ */
 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 
-
   /* 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
@@ -171,75 +210,124 @@ void listen_can(const CAN_message_t &msg) {
     return; // Ignore message on CRC mismatch
   }
   flash_rec = load_params();
+
   /* Message Structure: 0x691
      69 - Device address
      1 - Action code */
-  if(id_x == flash_rec[addr_id].value){
+  if(id_x != flash_rec[addr_id].value){
-    if(msg_ch == REG_WRITE){
+      return;
-      switch(msg.buf[0]) {
+  }
-        case REG_ID:
+  if(msg_ch == REG_WRITE){
-          setup_id(msg.buf[1]); 
+    switch(msg.buf[0]) {
-          break;
+      case REG_ID:
-        case REG_LED_BLINK:
+        setup_id(msg.buf[1]); 
-          for (int i = 0; i < 10; i++) {
+        break;
-            GPIOC->ODR ^= GPIO_ODR_OD10;
+      case REG_LED_BLINK:
-            delay(100);
+        for (int i = 0; i < 10; i++) {
-          }
+          GPIOC->ODR ^= GPIO_ODR_OD10;
+          delay(100);
+        }
+        break;
+        
+      case MOTOR_ANGLE:
+        memcpy(&motor_control_inputs.target_angle, &msg.buf[1],
+              sizeof(motor_control_inputs.target_angle));
+        setup_angle(motor_control_inputs.target_angle);
+        break;
+
+      case REG_MOTOR_POSPID_Kp:
+        memcpy(&motor.P_angle.P, &msg.buf[1], sizeof(float));
+        conv_float_to_int.f = motor.P_angle.P;
+        write_param(pid_p,conv_float_to_int.i);
+        break;
+
+      case REG_MOTOR_POSPID_Ki:
+        memcpy(&motor.P_angle.I, &msg.buf[1], sizeof(float));
+        conv_float_to_int.f = motor.P_angle.I;
+        write_param(pid_i,conv_float_to_int.i);
+        break;
+
+      case REG_MOTOR_POSPID_Kd:
+        memcpy(&motor.P_angle.D, &msg.buf[1], sizeof(float));
+        conv_float_to_int.f = motor.P_angle.D;
+        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 MOTOR_ANGLE:
+      case FIRMWARE_UPDATE:
-          memcpy(&motor_control_inputs.target_angle, &msg.buf[1],
+        firmware_update();
-                sizeof(motor_control_inputs.target_angle));
+        break;
+
+      case MOTOR_ENABLED:
+        if (msg.buf[1] == 1) {
+          motor.enable();
+          motor_control_inputs.motor_enabled = 1;
+        } else {
+          motor.disable();
+          motor_control_inputs.motor_enabled = 0;
+        }
+      default:
+        break;
+    }
+  }
+  else if (msg_ch == REG_READ) {
+    switch (msg.buf[0]) {
+      case REG_ID: send_id(); break;
+      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;
+      case REG_MOTOR_POSPID_Ki: send_pid_angle(pid_i); break;
+      case REG_MOTOR_POSPID_Kd: send_pid_angle(pid_d); break;
+      default: break;
+    }
+  }
+
+  /* If msg_ch != REG_WRITE or REG_READ, then SimpleFOC*/
+  else{
+    switch(data_type) {
+      /*  Read after write*/
+        case DATA_TYPE_ANGLE: 
+          send_angle();
+          delay(200);  
+          memcpy(&motor_control_inputs.target_angle, &msg.buf[1], sizeof(float));
           setup_angle(motor_control_inputs.target_angle);
           break;
         
-        case REG_MOTOR_POSPID_Kp:
+        case DATA_TYPE_VELOCITY:{
-          memcpy(&motor.P_angle.P, &msg.buf[1], sizeof(float));
+          send_velocity(); 
-          conv_float_to_int.f = motor.P_angle.P;
+          float vel = 0.0f;
-          write_param(pid_p,conv_float_to_int.i);
+          memcpy(&vel, &msg.buf[1], sizeof(float));
+          setup_velocity(vel);
+          break;
+        }
+    
+        case DATA_TYPE_TORQUE:
+          // send_torque();
           break;
 
-        case REG_MOTOR_POSPID_Ki:
-          memcpy(&motor.P_angle.I, &msg.buf[1], sizeof(float));
-          conv_float_to_int.f = motor.P_angle.I;
-          write_param(pid_i,conv_float_to_int.i);
-          break;
-
-        case REG_MOTOR_POSPID_Kd:
-          memcpy(&motor.P_angle.D, &msg.buf[1], sizeof(float));
-          conv_float_to_int.f = motor.P_angle.D;
-          write_param(pid_d,conv_float_to_int.i);
-          break;
-        
-        case FIRMWARE_UPDATE:
-          firmware_update();
-          break;
-
-        case MOTOR_ENABLED:
-          if (msg.buf[1] == 1) {
-            motor.enable();
-            motor_control_inputs.motor_enabled = 1;
-          } else {
-            motor.disable();
-            motor_control_inputs.motor_enabled = 0;
-          }
         default:
+          send_error(error_foc);
           break; 
-      }
+          
-    }
-    else if (msg_ch == REG_READ) {
-      switch (msg.buf[0]) {
-        case REG_ID: send_id(); break;
-        case MOTOR_VELOCITY: send_velocity(); break;
-        case MOTOR_ANGLE: send_angle(); break;
-        case MOTOR_ENABLED: send_motor_enabled(); 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;
-        case REG_MOTOR_POSPID_Ki: send_pid_angle(pid_i); break;
-        case REG_MOTOR_POSPID_Kd: send_pid_angle(pid_d); break;
-        default: break;
-      }
     }
   }
+  HAL_Delay(10);
 }
+

controller/fw/embed/test/read_id.py

@@ -1,12 +1,23 @@
 import can
 import time
 import sys
+
 # Конфигурация
 CAN_INTERFACE = 'can0'
-OLD_DEVICE_ID = int(sys.argv[1]) # Текущий ID устройства (по умолчанию)
+OLD_DEVICE_ID = int(sys.argv[1])  # Текущий ID устройства
 REG_READ = 0x7        # Код команды чтения
 REG_ID = 0x01          # Адрес регистра с ID устройства
 
+def flush_can_buffer(bus, duration=0.3):
+    """Очистка входного буфера CAN"""
+    start_time = time.time()
+    flushed_count = 0
+    while time.time() - start_time < duration:
+        msg = bus.recv(timeout=0)
+        if msg:
+            flushed_count += 1
+    print(f"Очищено сообщений из буфера: {flushed_count}")
+
 def send_can_message(bus, can_id, data):
     """Отправка CAN-сообщения"""
     try:
@@ -23,14 +34,14 @@ def send_can_message(bus, can_id, data):
         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("[Ошибка] Таймаут")
+    print("[Ошибка] Таймаут приема")
     return None
 
 def validate_crc16(data):
@@ -45,64 +56,91 @@ def validate_crc16(data):
                 crc >>= 1
     return crc
 
-# Инициализация CAN-интерфейса
+def main():
-bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
+    # Инициализация CAN-интерфейса
+    try:
+        bus = can.interface.Bus(
+            channel=CAN_INTERFACE,
+            bustype='socketcan',
+            bitrate=1000000  # Совпадает с устройством
+        )
+    except Exception as e:
+        print(f"Ошибка инициализации CAN: {e}")
+        sys.exit(1)
 
-# ======= 1. Запрос текущего ID устройства =======
+    # ======= 1. Подготовка запроса =======
+    can_id_read = (OLD_DEVICE_ID << 4) | REG_READ
+    data_read = [REG_ID, 0x00]
     
-# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ
+    # Формируем полные данные для расчета CRC:
-can_id_read = (OLD_DEVICE_ID << 4) | REG_READ
+    full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
     
-# Данные для запроса: [регистр, резервный байт]
+    # Рассчитываем CRC
-data_read = [REG_ID, 0x00]
+    crc = validate_crc16(full_data_for_crc)
+    crc_bytes = list(crc.to_bytes(2, 'little'))
     
-# Формируем полные данные для расчета CRC:
+    # Собираем итоговый пакет
-# - CAN ID разбивается на 2 байта (little-endian)
+    packet_read = data_read + crc_bytes
-# - Добавляем данные запроса
-full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
 
-# Рассчитываем CRC и разбиваем на байты (little-endian)
+    # ======= 2. Отправка запроса с повторами =======
-crc = validate_crc16(full_data_for_crc)
+    max_retries = 3
-crc_bytes = list(crc.to_bytes(2, 'little'))
+    response = None
     
-# Собираем итоговый пакет: данные + CRC
+    for attempt in range(max_retries):
-packet_read = data_read + crc_bytes
+        print(f"\nПопытка {attempt+1}/{max_retries}")
         
-print("Запрос на чтение ID:", packet_read)
+        # Очистка буфера перед отправкой
-send_can_message(bus, can_id_read, packet_read)
+        flush_can_buffer(bus, 0.3)
+        
+        # Отправка запроса
+        print(f"Отправка запроса на чтение ID: {packet_read}")
+        if not send_can_message(bus, can_id_read, packet_read):
+            print("Ошибка отправки, повтор...")
+            time.sleep(0.2)
+            continue
+            
+        # Ожидание ответа
+        response = receive_response(bus, timeout=0.5)
+        if response:
+            break
+            
+        print("Ответ не получен, повтор...")
+        time.sleep(0.2)
+    
+    # ======= 3. Обработка ответа =======
+    if not response:
+        print("Устройство не ответило после всех попыток")
+        bus.shutdown()
+        sys.exit(1)
         
-# ======= 2. Получение и проверка ответа =======
-response = receive_response(bus)     
-if response:
     data = response.data
-
     if len(data) < 4:
         print("Слишком короткий ответ")
+        bus.shutdown()
+        sys.exit(1)
 
     # Проверяем минимальную длину ответа (данные + CRC)
+    id_bytes = response.arbitration_id.to_bytes(1, byteorder='little')
+    full_data = list(id_bytes) + list(data[:-2])
+    print(f"Полные данные для CRC: {full_data}")
+    
+    received_crc = int.from_bytes(data[-2:], byteorder='little')
+    calc_crc = validate_crc16(full_data)
+    
+    print(f"Расчитанный CRC: 0x{calc_crc:04X}, Полученный CRC: 0x{received_crc:04X}")
+    
+    if received_crc == calc_crc:
+        print(f"Текущий ID устройства: 0x{data[1]:02X}")
     else:
-        id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
+        print("Ошибка: CRC не совпадает")
-        #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:
+    bus.shutdown()
-            # Если CRC совпадает, проверяем структуру ответа:
-            print(f"Текущий ID устройства: 0x{data[1]:02X}")
-        else:
-            print("Ошибка: CRC не совпадает")
-else:
-    print("Устройство не ответило")
-
-# Завершаем работу с шиной
-bus.shutdown()
 
 if __name__ == "__main__":
-    import sys
     if len(sys.argv) != 2:
-        print("Использование: python3 can_flasher.py address")
+        print("Использование: python3 can_flasher.py <адрес_устройства>")
+        print("Пример: python3 can_flasher.py 1")
         sys.exit(1)
+    
+    main()

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()