servo/controller/fw/embed/test/python_test_id.py

115 lines
4 KiB
Python
Raw Normal View History

2025-04-16 22:51:39 +03:00
import can
import time
# Конфигурация
CAN_INTERFACE = 'can0'
2025-04-17 15:53:03 +03:00
OLD_DEVICE_ID = 0x00
NEW_DEVICE_ID = 0x69
2025-04-16 22:51:39 +03:00
REG_WRITE = 0x8
REG_READ = 0x7
REG_ID = 0x1
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=2.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
# Инициализация
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
# ======= 1. Отправляем команду изменить ID =======
# Весь буфер: id + команда + параметры
OLD_WITH_REG = (OLD_DEVICE_ID << 4) | REG_WRITE
id_bytes = list(OLD_WITH_REG.to_bytes(2, byteorder='little'))
# Важные части сообщения: address (id), команда, параметры
data_write = [REG_ID, NEW_DEVICE_ID] # команда изменить ID
# Полностью собираем массив для 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("Отправляем: команда изменить ID + CRC:", packet_write)
# Отправляем с `OLD_DEVICE_ID` в качестве адреса
send_can_message(bus, (OLD_DEVICE_ID << 4) | REG_WRITE, packet_write)
time.sleep(0.5)
2025-04-17 14:30:22 +03:00
# ======= 2. Запрашиваем текущий ID (используем новы й адрес) =======
2025-04-16 22:51:39 +03:00
# Теперь для запроса используем **уже новый id**
NEW_WITH_REG = (NEW_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, (NEW_DEVICE_ID << 4) | REG_READ, packet_read)
# ======= 3. Получение и проверка ответа =======
response = receive_response(bus)
if response:
data = response.data
if len(data) < 4:
print("Ответ слишком короткий")
else:
received_crc = int.from_bytes(data[-2:], byteorder='little')
# Расчет CRC по всему пакету без CRC
calc_crc = validate_crc16(data[:-2])
if received_crc == calc_crc:
if data[0] == ord('I') and data[1] == NEW_DEVICE_ID:
print(f"\nУСПЕХ! ID устройства изменен на 0x{NEW_DEVICE_ID:02X}")
else:
print(f"Некорректный ответ: {list(data)}")
else:
print("CRC не совпадает, данные повреждены.")
else:
print("Нет ответа от устройства.")
bus.shutdown()