Add py test and fix work with float in FLASH

controller/fw/embed/include/flash.h

@@ -9,14 +9,14 @@
 /*      no padding for this struct, beacuse storing 8 bytes*/
 typedef struct{
     uint8_t data_id;  // data_id = id register of can
-    uint8_t data_type;  //float or uint8_t  if 4 - float 1 - uint8_t
+    uint8_t data_type;  
     uint16_t crc;
     uint32_t value;
     // uint32_t write_ptr_now;
   }FLASH_RECORD;
 enum {
   addr_id   = 0,
-  pid_p, 
+  pid_p     = 1, 
   pid_i,
   pid_d,
   foc_id,
@@ -24,6 +24,12 @@ enum {
   vel
 };
 
+/*  for saved in FLASH float data*/
+union{
+    uint32_t i;
+    float f;
+}conv_float_to_int;
+
 #define FLASH_RECORD_SIZE sizeof(FLASH_RECORD)    //size flash struct
 #define PARAM_COUNT 4             // count data in flash
 

controller/fw/embed/src/flash.cpp

@@ -242,3 +242,5 @@ FLASH_RECORD* load_params(){
     __enable_irq();
     return latest;
 }
+
+

controller/fw/embed/src/main.cpp

@@ -42,7 +42,6 @@ volatile bool CAN_GET = false;
 volatile float kt = 0.1;  // Torque calculation constant
 
 static FLASH_RECORD* flash_rec;
-static FLASH_RECORD flash_buf[PARAM_COUNT];
 static CAN_message_t CAN_TX_msg;
 static CAN_message_t CAN_inMsg;
 
@@ -134,7 +133,8 @@ void send_can_with_id_crc(uint8_t id, uint8_t message_type, const T* data) {
   memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T));
   // Copy all data bytes
   memcpy(crc_data + 1, msg_l.buf, 6);
-  // Calculate CRC
+
+  // Calculate CRC  
   uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
  
   // Insert CRC into buffer
@@ -215,7 +215,8 @@ void send_pid(uint8_t param_pid){
     return;
   }
   uint8_t id = flash_rec[addr_id].value;
-  float data = flash_rec[param_pid].value;
+  conv_float_to_int.i = flash_rec[param_pid].value;
+  uint32_t data = conv_float_to_int.i;
   switch(param_pid){
     case pid_p:
       param_pid = REG_MOTOR_POSPID_Kp;
@@ -298,20 +299,31 @@ void listen_can(const CAN_message_t &msg) {
             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:
-          setup_pid_angle(pid_p,msg.buf[1]);
+          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:
-          setup_pid_angle(pid_i,msg.buf[1]);
+          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:
-          setup_pid_angle(pid_d,msg.buf[1]);
+          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 MOTOR_ENABLED:
           if (msg.buf[1] == 1) {
             motor.enable();
@@ -382,15 +394,13 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
   // Настройка прерываний CAN
   CAN2->IER |= CAN_IER_FMPIE0;
 
-  // flash_rec = load_params();
-  // /*  For test          */
-  // float value = 3.46;  //addr
-  // write_param(pid_p,value); //for update address in firmware
-  // // Load parameters from flash
   flash_rec = load_params();
-
+  /*  For test          */
-  for(int i = 0;i < PARAM_COUNT;i++)
+  // float value = 3.46;  //addr
-    flash_buf[i] = flash_rec[i];
+  // conv_float_to_int.f = value;
+  // write_param(pid_p,conv_float_to_int.i); //for update address in firmware
+  // // Load parameters from flash
+  // flash_rec = load_params();
   
   // Initialize FOC system
   setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);

controller/fw/embed/test/python_pid_p.py

@@ -0,0 +1,102 @@
+
+import can
+import time
+import struct
+
+# Конфигурация (замените значения на актуальные для вашей системы)
+CAN_INTERFACE = 'can0'
+DEVICE_ID = 0x00          # ID устройства по умолчанию
+REG_READ = 0x7            # Код команды чтения
+REG_MOTOR_POSPID_Kp = 0x30 # Адрес регистра Kp (пример, уточните в коде устройства)
+
+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. Запрос значения PID_P =======
+
+# Формируем CAN ID для чтения: (DEVICE_ID << 4) | REG_READ
+can_id = (DEVICE_ID << 4) | REG_READ
+
+# Данные для запроса: [регистр Kp, резервный байт]
+data_read = [REG_MOTOR_POSPID_Kp, 0x00]
+
+# Формируем данные для CRC: CAN ID (2 байта) + данные запроса
+full_data_for_crc = list(can_id.to_bytes(2, 'little')) + data_read
+
+# Расчет CRC и сборка пакета
+crc = validate_crc16(full_data_for_crc)
+packet = data_read + list(crc.to_bytes(2, 'little'))
+
+# Отправка запроса
+print("\nЗапрос параметра PID_P...")
+send_can_message(bus, can_id, packet)
+
+# ======= 2. Обработка ответа =======
+response = receive_response(bus)
+if response:
+    data = response.data
+    if len(data) < 7:
+        print(f"Ошибка: получено {len(data)} байт, ожидалось 7")
+    else:
+        # Извлекаем CRC из ответа
+        received_crc = int.from_bytes(data[-2:], byteorder='little')
+        
+        # Формируем данные для проверки CRC
+        arb_id_bytes = response.arbitration_id.to_bytes(2, 'little')
+        full_data = list(arb_id_bytes) + list(data[:-2])
+        
+        # Проверка CRC
+        calculated_crc = validate_crc16(full_data)
+        if calculated_crc != received_crc:
+            print(f"Ошибка CRC: рассчитанный 0x{calculated_crc:04X}, полученный 0x{received_crc:04X}")
+        else:
+            # Проверяем адрес регистра в ответе
+            if data[0] != REG_MOTOR_POSPID_Kp:
+                print(f"Ошибка: получен регистр 0x{data[0]:02X}, ожидался 0x{REG_MOTOR_POSPID_Kp:02X}")
+            else:
+                # Декодируем значение float (little-endian)
+                kp_value = struct.unpack('<f', bytes(data[1:5]))[0]
+                print(f"Успешно! Текущее значение PID_P: {kp_value}")
+else:
+    print("Устройство не ответило")
+
+# Завершение работы
+bus.shutdown()