103 lines
3.5 KiB
Python
103 lines
3.5 KiB
Python
import can
|
|
import struct
|
|
import time
|
|
|
|
|
|
REG_READ = 0x7
|
|
DEVICE_ID = 0x00 # Старый ID устройства
|
|
REG_POS = 0x72 # Предположим, что команда записи позиции — 0x2
|
|
CAN_INTERFACE = 'can0'
|
|
|
|
|
|
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
|
|
|
|
def process_can_message(bus):
|
|
# ID и команда
|
|
arbitration_id = (DEVICE_ID << 4) | REG_READ
|
|
id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
|
|
|
|
# Команда и параметры
|
|
data_read = [REG_POS] + list(struct.pack('<f', 0))
|
|
|
|
# Для CRC — весь пакет: id + команда + параметры
|
|
full_data_for_crc = id_bytes + data_read
|
|
crc = validate_crc16(full_data_for_crc)
|
|
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
|
|
|
|
# Итоговый пакет
|
|
packet = data_read + crc_bytes
|
|
|
|
# Создание и отправка сообщения
|
|
msg = can.Message(
|
|
arbitration_id=arbitration_id,
|
|
is_extended_id=False,
|
|
data=packet
|
|
)
|
|
|
|
try:
|
|
bus.send(msg)
|
|
except can.CanError:
|
|
print("Ошибка отправки сообщения")
|
|
|
|
|
|
def receive_response(bus, timeout=2.0):
|
|
"""Ожидание ответа"""
|
|
start_time = time.time()
|
|
while time.time() - start_time < timeout:
|
|
msg = bus.recv(timeout=timeout)
|
|
data_bytes = msg.data[1:5] # берём первые 4 байта
|
|
print(data_bytes)
|
|
angle = struct.unpack('<f', bytes(data_bytes))[0] # '<f' = little-endian float
|
|
# angle = int.from_bytes(data_bytes, byteorder='little', signed=True) # или 'big'
|
|
if msg:
|
|
print(f"[Прием] Angle: 0x{msg.arbitration_id:03X}, Данные: {angle}, RAW: {bytes(msg.data)}")
|
|
return msg
|
|
print("[Ошибка] Таймаут")
|
|
return None
|
|
|
|
def receive_can_messages():
|
|
try:
|
|
# Connect to the CAN bus
|
|
bus = can.interface.Bus(channel='can0', bustype='socketcan')
|
|
|
|
print("Waiting for messages on the CAN bus...")
|
|
|
|
while True:
|
|
process_can_message(bus)
|
|
response = receive_response(bus)
|
|
if response:
|
|
data = response.data
|
|
if len(data) < 4:
|
|
print("Ответ слишком короткий")
|
|
else:
|
|
received_crc = int.from_bytes(data[-2:], byteorder='little')
|
|
print("Полученный CRC: ", received_crc)
|
|
# Расчет CRC по всему пакету без CRC
|
|
calc_crc = validate_crc16(data[:-2])
|
|
if received_crc == calc_crc:
|
|
if data[0] == REG_POS:
|
|
print("\nУСПЕХ!")
|
|
else:
|
|
print(f"Некорректный ответ: {list(data)}")
|
|
else:
|
|
print("CRC не совпадает, данные повреждены.")
|
|
else:
|
|
print("Нет ответа от устройства.")
|
|
|
|
except KeyboardInterrupt:
|
|
print("\nExiting program...")
|
|
except Exception as e:
|
|
print(f"Error: {e}")
|
|
|
|
if __name__ == '__main__':
|
|
receive_can_messages()
|