diff --git a/controller/fw/embed/test/python_pid_p.py b/controller/fw/embed/test/python_pid_p.py index 9000052..ee3b444 100644 --- a/controller/fw/embed/test/python_pid_p.py +++ b/controller/fw/embed/test/python_pid_p.py @@ -1,13 +1,11 @@ - import can import time import struct - -# Конфигурация (замените значения на актуальные для вашей системы) +# Конфигурация CAN_INTERFACE = 'can0' -DEVICE_ID = 0x00 # ID устройства по умолчанию -REG_READ = 0x7 # Код команды чтения -REG_MOTOR_POSPID_Kp = 0x30 # Адрес регистра Kp (пример, уточните в коде устройства) +OLD_DEVICE_ID = 0x00 # Текущий ID устройства (по умолчанию) +REG_READ = 0x7 # Код команды чтения +REG_ID = 0x30 # Адрес регистра с REG_PMOTOR_POSPID_Kp устройства def send_can_message(bus, can_id, data): """Отправка CAN-сообщения""" @@ -36,7 +34,7 @@ def receive_response(bus, timeout=1.0): return None def validate_crc16(data): - """Расчет CRC16 (MODBUS)""" + """Расчет CRC16 (MODBUS) для проверки целостности данных""" crc = 0xFFFF for byte in data: crc ^= byte @@ -50,53 +48,56 @@ def validate_crc16(data): # Инициализация CAN-интерфейса bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan') -# ======= 1. Запрос значения PID_P ======= +# ======= 1. Запрос текущего ID устройства ======= -# Формируем CAN ID для чтения: (DEVICE_ID << 4) | REG_READ -can_id = (DEVICE_ID << 4) | REG_READ +# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ +can_id_read = (OLD_DEVICE_ID << 4) | REG_READ -# Данные для запроса: [регистр Kp, резервный байт] -data_read = [REG_MOTOR_POSPID_Kp, 0x00] +# Данные для запроса: [регистр, резервный байт] +data_read = [REG_ID, 0x00] -# Формируем данные для CRC: CAN ID (2 байта) + данные запроса -full_data_for_crc = list(can_id.to_bytes(2, 'little')) + data_read +# Формируем полные данные для расчета CRC: +# - CAN ID разбивается на 2 байта (little-endian) +# - Добавляем данные запроса +full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read -# Расчет CRC и сборка пакета +# Рассчитываем CRC и разбиваем на байты (little-endian) crc = validate_crc16(full_data_for_crc) -packet = data_read + list(crc.to_bytes(2, 'little')) +crc_bytes = list(crc.to_bytes(2, 'little')) -# Отправка запроса -print("\nЗапрос параметра PID_P...") -send_can_message(bus, can_id, packet) +# Собираем итоговый пакет: данные + CRC +packet_read = data_read + crc_bytes -# ======= 2. Обработка ответа ======= -response = receive_response(bus) +print("Запрос на чтение ID:", packet_read) +send_can_message(bus, can_id_read, packet_read) + +# ======= 2. Получение и проверка ответа ======= +response = receive_response(bus) if response: data = response.data - if len(data) < 7: - print(f"Ошибка: получено {len(data)} байт, ожидалось 7") + + if len(data) < 4: + print("Слишком короткий ответ") + + # Проверяем минимальную длину ответа (данные + CRC) else: - # Извлекаем CRC из ответа + id_bytes = response.arbitration_id.to_bytes(1,byteorder='little') + #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) - # Формируем данные для проверки 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}") + print(f"Расчитанный CRC PYTHON : 0x{calc_crc:02X}") + if received_crc == calc_crc: + # Если CRC совпадает, проверяем структуру ответа: + kp_value = struct.unpack('