102 lines
3.8 KiB
Python
102 lines
3.8 KiB
Python
|
|
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()
|