122 lines
4.3 KiB
Python
122 lines
4.3 KiB
Python
import can
|
||
import time
|
||
import struct
|
||
# Конфигурация
|
||
CAN_INTERFACE = 'can0'
|
||
DEVICE_ID = 0x00
|
||
SET_PID_P = 3.6
|
||
REG_WRITE = 0x8
|
||
REG_READ = 0x7
|
||
REG_ID = 0x30 #REG_MOTOR_POSPID_Kp
|
||
PID_P = 0x01
|
||
|
||
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):
|
||
print("Ожидание ответа")
|
||
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
|
||
|
||
# Инициализация
|
||
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
|
||
# Перевод float -> hex -> int
|
||
result = (struct.unpack('<I',struct.pack('<f', float(SET_PID_P)))[0])
|
||
result_bytes = result.to_bytes(4, byteorder='little')
|
||
# ======= 1. Отправляем команду изменить ID =======
|
||
|
||
# Весь буфер: id + команда + параметры
|
||
OLD_WITH_REG = (DEVICE_ID << 4) | REG_WRITE
|
||
id_bytes = list(OLD_WITH_REG.to_bytes(2, byteorder='little'))
|
||
|
||
# Важные части сообщения: address (id), команда, параметры
|
||
data_write = [REG_ID] + list(result_bytes) # команда изменить PID_P
|
||
|
||
# Полностью собираем массив для CRC (включая id и команду)
|
||
full_data_for_crc = id_bytes + data_write
|
||
|
||
# Расчет CRC по всему пакету
|
||
crc = validate_crc16(full_data_for_crc)
|
||
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
|
||
|
||
# Итоговый пакет: команда + параметры + CRC
|
||
packet_write = data_write + crc_bytes
|
||
|
||
print("Отправляем: команда изменить PID_p + CRC:", packet_write)
|
||
# Отправляем с `OLD_DEVICE_ID` в качестве адреса
|
||
send_can_message(bus, (DEVICE_ID << 4) | REG_WRITE, packet_write)
|
||
|
||
time.sleep(1.0)
|
||
|
||
# ======= 2. Запрашиваем текущий ID (используем новый адрес) =======
|
||
|
||
# Теперь для запроса используем **уже новый id**
|
||
NEW_WITH_REG = (DEVICE_ID << 4) | REG_READ
|
||
current_id_bytes = list(NEW_WITH_REG.to_bytes(2, byteorder='little'))
|
||
data_read = [REG_ID, 0x00]
|
||
|
||
full_data_for_crc = current_id_bytes + data_read
|
||
crc = validate_crc16(full_data_for_crc)
|
||
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
|
||
packet_read = data_read + crc_bytes
|
||
|
||
print("Запрос на чтение ID + CRC (после смены):", packet_read)
|
||
send_can_message(bus, (DEVICE_ID << 4) | REG_READ, packet_read)
|
||
|
||
# ======= 3. Получение и проверка ответа =======
|
||
|
||
response = receive_response(bus)
|
||
if response:
|
||
data = response.data
|
||
if len(data) < 4:
|
||
print("Ответ слишком короткий")
|
||
else:
|
||
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)
|
||
if received_crc == calc_crc:
|
||
if data[0] == int(REG_ID):
|
||
kp_val = struct.unpack('<f', bytes(data[1:5]))[0]
|
||
print(f"\nУСПЕХ! PID_P = {kp_val:.3f}")
|
||
else:
|
||
print(f"Некорректный ответ: {list(data)}")
|
||
else:
|
||
print("CRC не совпадает, данные повреждены.")
|
||
else:
|
||
print("Нет ответа от устройства.")
|
||
|
||
bus.shutdown()
|