diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h index 0adad30..c550a9a 100644 --- a/controller/fw/embed/include/flash.h +++ b/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 diff --git a/controller/fw/embed/src/flash.cpp b/controller/fw/embed/src/flash.cpp index 686163f..b9266b6 100644 --- a/controller/fw/embed/src/flash.cpp +++ b/controller/fw/embed/src/flash.cpp @@ -242,3 +242,5 @@ FLASH_RECORD* load_params(){ __enable_irq(); return latest; } + + diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 56f17ec..4674d2d 100644 --- a/controller/fw/embed/src/main.cpp +++ b/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(int i = 0;i < PARAM_COUNT;i++) - flash_buf[i] = flash_rec[i]; + /* For test */ + // float value = 3.46; //addr + // 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, ¤t_sense, &command, doMotor); diff --git a/controller/fw/embed/test/python_pid_p.py b/controller/fw/embed/test/python_pid_p.py new file mode 100644 index 0000000..9000052 --- /dev/null +++ b/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('