From fca10d4140dc7baa559df9c3dd2a6c9377a3840e Mon Sep 17 00:00:00 2001 From: Valentin Dabstep Date: Tue, 13 May 2025 19:05:54 +0300 Subject: [PATCH] Fix CRC from read data --- controller/fw/embed/include/reg_cah.h | 7 ++ controller/fw/embed/src/main.cpp | 13 ++- controller/fw/embed/test/python_read_id.py | 102 +++++++++++++++++++++ controller/fw/embed/test/python_test_id.py | 15 +-- 4 files changed, 124 insertions(+), 13 deletions(-) create mode 100644 controller/fw/embed/test/python_read_id.py diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h index dbbfdc7..2760dcf 100644 --- a/controller/fw/embed/include/reg_cah.h +++ b/controller/fw/embed/include/reg_cah.h @@ -37,4 +37,11 @@ #define MOTOR_ANGLE 0x72 #define MOTOR_TORQUE 0x73 + +//For send +#define CAN_MSG_MAX_LEN 7 +#define CRC_SIZE 2 +#define ID_SIZE sizeof(uint8_t) + + #endif // REG_CAH_H_ diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 6379863..cf14e7b 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -123,19 +123,18 @@ void send_can_with_id_crc(uint8_t id, uint8_t message_type, const void* data, si msg_l.id = id; // msg_l.len = 8; // Protocol-defined message length memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t)); - memcpy(&msg_l.buf[1], data, 1); + memcpy(&msg_l.buf[1], data, ID_SIZE); // Prepare CRC calculation buffer (ID + data) - size_t crc_data_size = sizeof((uint8_t)msg_l.id) + 2 * sizeof(uint8_t); - uint8_t crc_data[7]; + uint8_t crc_data[CAN_MSG_MAX_LEN]; // Copy message ID - memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(uint8_t)); + memcpy(crc_data, (uint8_t*)&msg_l.id, ID_SIZE); // Copy all data bytes - for(int i = 1;i < 7;i++) - memcpy(crc_data + i,(uint8_t*)&msg_l.buf[i - 1],1); + for(int i = 1;i < CAN_MSG_MAX_LEN;i++) + memcpy(crc_data + i,(uint8_t*)&msg_l.buf[i - 1], sizeof(uint8_t)); //As byte variable // Calculate CRC - uint16_t crc_value = validate_crc16(crc_data, 7); + uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN); // Insert CRC into buffer msg_l.buf[6] = crc_value & 0xFF; diff --git a/controller/fw/embed/test/python_read_id.py b/controller/fw/embed/test/python_read_id.py new file mode 100644 index 0000000..7a90657 --- /dev/null +++ b/controller/fw/embed/test/python_read_id.py @@ -0,0 +1,102 @@ +import can +import time + +# Конфигурация +CAN_INTERFACE = 'can0' +OLD_DEVICE_ID = 0x03 # Текущий ID устройства (по умолчанию) +REG_READ = 0x7 # Код команды чтения +REG_ID = 0x1 # Адрес регистра с ID устройства + +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. Запрос текущего ID устройства ======= + +# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ +can_id_read = (OLD_DEVICE_ID << 4) | REG_READ + +# Данные для запроса: [регистр, резервный байт] +data_read = [REG_ID, 0x00] + +# Формируем полные данные для расчета CRC: +# - CAN ID разбивается на 2 байта (little-endian) +# - Добавляем данные запроса +full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read + +# Рассчитываем CRC и разбиваем на байты (little-endian) +crc = validate_crc16(full_data_for_crc) +crc_bytes = list(crc.to_bytes(2, 'little')) + +# Собираем итоговый пакет: данные + CRC +packet_read = data_read + crc_bytes + +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) < 4: + print("Слишком короткий ответ") + + # Проверяем минимальную длину ответа (данные + CRC) + 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) + + print(f"Расчитанный CRC PYTHON : 0x{calc_crc:02X}") + if received_crc == calc_crc: + # Если CRC совпадает, проверяем структуру ответа: + print(f"Текущий ID устройства: 0x{data[1]:02X}") + else: + print("Ошибка: CRC не совпадает") +else: + print("Устройство не ответило") + +# Завершаем работу с шиной +bus.shutdown() diff --git a/controller/fw/embed/test/python_test_id.py b/controller/fw/embed/test/python_test_id.py index b7a3084..1348477 100644 --- a/controller/fw/embed/test/python_test_id.py +++ b/controller/fw/embed/test/python_test_id.py @@ -3,7 +3,7 @@ import time # Конфигурация CAN_INTERFACE = 'can0' -OLD_DEVICE_ID = 0x00 +OLD_DEVICE_ID = 0x03 NEW_DEVICE_ID = 0x69 REG_WRITE = 0x8 REG_READ = 0x7 @@ -24,7 +24,7 @@ def send_can_message(bus, can_id, data): print(f"Ошибка CAN: {e}") return False -def receive_response(bus, timeout=2.0): +def receive_response(bus, timeout=1.0): """Ожидание ответа""" start_time = time.time() while time.time() - start_time < timeout: @@ -73,7 +73,7 @@ print("Отправляем: команда изменить ID + CRC:", packet_ # Отправляем с `OLD_DEVICE_ID` в качестве адреса send_can_message(bus, (OLD_DEVICE_ID << 4) | REG_WRITE, packet_write) -time.sleep(0.5) +time.sleep(1.0) # ======= 2. Запрашиваем текущий ID (используем новый адрес) ======= @@ -98,10 +98,13 @@ if response: 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') - print("Полученный CRC: ", received_crc) - # Расчет CRC по всему пакету без CRC - calc_crc = validate_crc16(data[:-2]) + #calc CRC + calc_crc = validate_crc16(full_data) 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}")