454 lines
16 KiB
Markdown
454 lines
16 KiB
Markdown
# Протокол телеметрии
|
||
|
||
Документ описывает текущий бинарный пакет телеметрии из прошивки
|
||
`AD_Keil_Project`.
|
||
|
||
Основные файлы реализации:
|
||
|
||
| Файл | Назначение |
|
||
|---|---|
|
||
| `Core/Inc/simulink_interface.h` | Формат пакета, версия, размеры, API |
|
||
| `Core/Src/simulink_interface.c` | Заполнение пакета и CRC16 |
|
||
| `Core/Inc/ad_binary_transport.h` | Выбор протокола и транспорта |
|
||
| `Core/Src/ad_binary_transport.c` | Отправка готового пакета |
|
||
| `Core/Src/ad_usb_cdc.c` | USB FS CDC ACM транспорт |
|
||
| `Core/Src/ad_can_telemetry.c` | FDCAN1 classic CAN транспорт |
|
||
| `Core/Src/ad_modbus.c` | Modbus-регистры включения бинарной телеметрии |
|
||
|
||
## Кратко
|
||
|
||
Прошивка формирует один бинарный пакет `SimulinkTelemetryPacket_t`.
|
||
Пакет можно отдавать через USB CDC, через FDCAN1 classic CAN или одновременно
|
||
через оба транспорта.
|
||
|
||
По умолчанию бинарный поток выключен:
|
||
|
||
```c
|
||
AD_HOST_PROTOCOL_DEFAULT = 0
|
||
AD_BINARY_TELEMETRY_TRANSPORT_DEFAULT = 0
|
||
```
|
||
|
||
То есть после старта активен режим `0` - Modbus-контроль без бинарной
|
||
телеметрии. Чтобы включить поток, нужно через Modbus записать:
|
||
|
||
```text
|
||
0x001D / 29 = транспорт
|
||
0x001C / 28 = 1
|
||
```
|
||
|
||
Где транспорт:
|
||
|
||
| Значение | Транспорт |
|
||
|---:|---|
|
||
| `0` | USB CDC |
|
||
| `1` | CAN/FDCAN1 |
|
||
| `2` | USB CDC и CAN/FDCAN1 одновременно |
|
||
|
||
Чтобы выключить бинарный поток:
|
||
|
||
```text
|
||
0x001C / 28 = 0
|
||
```
|
||
|
||
## Частота
|
||
|
||
Пакет обновляется в `SimulinkInterface_StepSlow()`, который вызывается из
|
||
`AD_Board_Loop()` раз в `AD_BOARD_SLOW_PERIOD_MS = 10` мс.
|
||
|
||
Отправка из `AD_BinaryTransport_Loop()` также ограничена периодом:
|
||
|
||
```c
|
||
AD_BINARY_TELEMETRY_PERIOD_MS = 10
|
||
```
|
||
|
||
Итого штатная частота телеметрии - около `100 Гц`, если главный цикл успевает
|
||
обслуживать транспорт.
|
||
|
||
## Кодирование данных
|
||
|
||
| Параметр | Значение |
|
||
|---|---|
|
||
| Порядок байт | little-endian, как у STM32 Cortex-M |
|
||
| `float` | IEEE-754 binary32, 4 байта |
|
||
| Выравнивание | обычное C-выравнивание ARMCLANG, без `packed` |
|
||
| Текущий размер заголовка | `16` байт |
|
||
| Текущий размер payload | `136` байт |
|
||
| Текущий размер полного пакета | `152` байта |
|
||
| Максимум, разрешенный кодом | `SIMULINK_TELEMETRY_MAX_BYTES = 256` |
|
||
|
||
Если структура в C будет изменена, PC-парсер должен проверять
|
||
`header.version` и `header.payload_size`.
|
||
|
||
## Заголовок пакета
|
||
|
||
Тип: `SimulinkTelemetryHeader_t`.
|
||
|
||
Смещения указаны от начала пакета.
|
||
|
||
| Offset | Размер | Тип | Поле | Значение / описание |
|
||
|---:|---:|---|---|---|
|
||
| `0` | `4` | `uint32_t` | `magic` | `0x41444944` |
|
||
| `4` | `2` | `uint16_t` | `version` | `3` |
|
||
| `6` | `2` | `uint16_t` | `payload_size` | `136` для текущей версии |
|
||
| `8` | `4` | `uint32_t` | `sequence` | Увеличивается при каждой упаковке |
|
||
| `12` | `2` | `uint16_t` | `crc16` | CRC16-CCITT по payload |
|
||
| `14` | `2` | `uint16_t` | `reserved` | `0` |
|
||
|
||
`magic` сравнивать как `uint32_t` little-endian. На проводе байты идут так:
|
||
|
||
```text
|
||
44 49 44 41
|
||
```
|
||
|
||
## CRC16
|
||
|
||
CRC считается только по payload, без заголовка.
|
||
|
||
Параметры:
|
||
|
||
| Параметр | Значение |
|
||
|---|---|
|
||
| Алгоритм | CRC16-CCITT |
|
||
| Начальное значение | `0xFFFF` |
|
||
| Полином | `0x1021` |
|
||
| Reflection | нет |
|
||
| Final XOR | нет |
|
||
|
||
Проверка на стороне ПК:
|
||
|
||
```text
|
||
crc16_ccitt(packet[16 : 16 + payload_size]) == header.crc16
|
||
```
|
||
|
||
## Payload
|
||
|
||
Тип: `SimulinkTelemetryPayload_t`.
|
||
|
||
Смещения в таблице указаны от начала payload. Чтобы получить смещение от начала
|
||
пакета, прибавь `16`.
|
||
|
||
### Измерения
|
||
|
||
Блок `measurements`, тип `AD_Measurements_t`, размер `64` байта.
|
||
|
||
| Payload offset | Тип | Поле | Единицы |
|
||
|---:|---|---|---|
|
||
| `0` | `float` | `measurements.ia_A` | A |
|
||
| `4` | `float` | `measurements.ib_A` | A |
|
||
| `8` | `float` | `measurements.ic_A` | A |
|
||
| `12` | `float` | `measurements.ia_rms_A` | A RMS |
|
||
| `16` | `float` | `measurements.ib_rms_A` | A RMS |
|
||
| `20` | `float` | `measurements.ic_rms_A` | A RMS |
|
||
| `24` | `float` | `measurements.vdc_V` | V |
|
||
| `28` | `float` | `measurements.va_V` | V |
|
||
| `32` | `float` | `measurements.vb_V` | V |
|
||
| `36` | `float` | `measurements.vc_V` | V |
|
||
| `40` | `float` | `measurements.speed_rpm` | rpm |
|
||
| `44` | `float` | `measurements.torque_Nm` | N*m |
|
||
| `48` | `float` | `measurements.temperature_C` | град C |
|
||
| `52` | `uint32_t` | `measurements.timestamp_us` | us |
|
||
| `56` | `uint32_t` | `measurements.status_flags` | `AD_MEAS_STATUS_*` |
|
||
| `60` | `float` | `measurements.slip_percent` | % |
|
||
|
||
`measurements.status_flags`:
|
||
|
||
| Бит | Маска | Флаг |
|
||
|---:|---:|---|
|
||
| `0` | `0x00000001` | `AD_MEAS_STATUS_OVERCURRENT` |
|
||
| `1` | `0x00000002` | `AD_MEAS_STATUS_OVERVOLTAGE` |
|
||
| `2` | `0x00000004` | `AD_MEAS_STATUS_UNDERVOLTAGE` |
|
||
| `3` | `0x00000008` | `AD_MEAS_STATUS_OVERTEMPERATURE` |
|
||
| `4` | `0x00000010` | `AD_MEAS_STATUS_DRIVER_FAULT` |
|
||
| `5` | `0x00000020` | `AD_MEAS_STATUS_EMERGENCY_STOP` |
|
||
|
||
### Параметры двигателя
|
||
|
||
Блок `motor_parameters`, тип `AD_MotorParameters_t`, размер `60` байт.
|
||
|
||
| Payload offset | Тип | Поле | Единицы |
|
||
|---:|---|---|---|
|
||
| `64` | `float` | `motor_parameters.Rs_ohm` | Ohm |
|
||
| `68` | `float` | `motor_parameters.Rr_ohm` | Ohm |
|
||
| `72` | `float` | `motor_parameters.Ls_H` | H |
|
||
| `76` | `float` | `motor_parameters.Lr_H` | H |
|
||
| `80` | `float` | `motor_parameters.Lm_H` | H |
|
||
| `84` | `float` | `motor_parameters.Ll_H` | H |
|
||
| `88` | `float` | `motor_parameters.J_kg_m2` | kg*m2 |
|
||
| `92` | `float` | `motor_parameters.B_Nm_s` | N*m*s |
|
||
| `96` | `float` | `motor_parameters.pole_pairs` | пары полюсов |
|
||
| `100` | `float` | `motor_parameters.nominal_voltage_V` | V |
|
||
| `104` | `float` | `motor_parameters.nominal_current_A` | A |
|
||
| `108` | `float` | `motor_parameters.nominal_frequency_Hz` | Hz |
|
||
| `112` | `float` | `motor_parameters.nominal_power_W` | W |
|
||
| `116` | `float` | `motor_parameters.nominal_speed_rpm` | rpm |
|
||
| `120` | `uint32_t` | `motor_parameters.valid_mask` | `AD_MOTOR_PARAM_VALID_*` |
|
||
|
||
`valid_mask`:
|
||
|
||
| Бит | Маска | Флаг | Поле |
|
||
|---:|---:|---|---|
|
||
| `0` | `0x00000001` | `AD_MOTOR_PARAM_VALID_RS` | `Rs_ohm` |
|
||
| `1` | `0x00000002` | `AD_MOTOR_PARAM_VALID_RR` | `Rr_ohm` |
|
||
| `2` | `0x00000004` | `AD_MOTOR_PARAM_VALID_LS` | `Ls_H` |
|
||
| `3` | `0x00000008` | `AD_MOTOR_PARAM_VALID_LR` | `Lr_H` |
|
||
| `4` | `0x00000010` | `AD_MOTOR_PARAM_VALID_LM` | `Lm_H` |
|
||
| `5` | `0x00000020` | `AD_MOTOR_PARAM_VALID_LL` | `Ll_H` |
|
||
| `6` | `0x00000040` | `AD_MOTOR_PARAM_VALID_J` | `J_kg_m2` |
|
||
| `7` | `0x00000080` | `AD_MOTOR_PARAM_VALID_B` | `B_Nm_s` |
|
||
| `8` | `0x00000100` | `AD_MOTOR_PARAM_VALID_NOMINALS` | nominal fields |
|
||
| `9` | `0x00000200` | `AD_MOTOR_PARAM_VALID_POLE_PAIRS` | `pole_pairs` |
|
||
|
||
Перед использованием результата идентификации обязательно проверять
|
||
соответствующий бит `valid_mask`.
|
||
|
||
### Статус идентификации и команда
|
||
|
||
| Payload offset | Тип | Поле | Описание |
|
||
|---:|---|---|---|
|
||
| `124` | `uint32_t` | `param_id_status` | `AD_PARAM_ID_STATUS_*` |
|
||
| `128` | `uint32_t` | `param_id_faults` | `AD_PARAM_ID_FAULT_*` |
|
||
| `132` | `uint8_t` | `param_id_mode` | Текущий режим `AD_PARAM_ID_MODE_*` |
|
||
| `133` | `uint8_t` | `command_enable` | Последняя команда enable |
|
||
| `134` | `uint8_t` | `command_test_mode` | Последняя команда test mode |
|
||
| `135` | `uint8_t` | `reserved` | `0` |
|
||
|
||
`param_id_status`:
|
||
|
||
| Бит | Маска | Флаг |
|
||
|---:|---:|---|
|
||
| `0` | `0x00000001` | `AD_PARAM_ID_STATUS_ACTIVE` |
|
||
| `1` | `0x00000002` | `AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED` |
|
||
| `2` | `0x00000004` | `AD_PARAM_ID_STATUS_POWER_STAGE_ARMED` |
|
||
| `3` | `0x00000008` | `AD_PARAM_ID_STATUS_FAULT_LATCHED` |
|
||
| `4` | `0x00000010` | `AD_PARAM_ID_STATUS_TIMEOUT` |
|
||
| `5` | `0x00000020` | `AD_PARAM_ID_STATUS_LOCKED_ROTOR_BLOCKED` |
|
||
| `6` | `0x00000040` | `AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN` |
|
||
| `7` | `0x00000080` | `AD_PARAM_ID_STATUS_DATA_VALID` |
|
||
| `8` | `0x00000100` | `AD_PARAM_ID_STATUS_COMPLETE` |
|
||
|
||
`param_id_faults`:
|
||
|
||
| Бит | Маска | Флаг |
|
||
|---:|---:|---|
|
||
| `0` | `0x00000001` | `AD_PARAM_ID_FAULT_OVERCURRENT` |
|
||
| `1` | `0x00000002` | `AD_PARAM_ID_FAULT_OVERVOLTAGE` |
|
||
| `2` | `0x00000004` | `AD_PARAM_ID_FAULT_UNDERVOLTAGE` |
|
||
| `3` | `0x00000008` | `AD_PARAM_ID_FAULT_OVERTEMPERATURE` |
|
||
| `4` | `0x00000010` | `AD_PARAM_ID_FAULT_DRIVER` |
|
||
| `5` | `0x00000020` | `AD_PARAM_ID_FAULT_EMERGENCY_STOP` |
|
||
| `6` | `0x00000040` | `AD_PARAM_ID_FAULT_TIMEOUT` |
|
||
| `7` | `0x00000080` | `AD_PARAM_ID_FAULT_NULL_INPUT` |
|
||
|
||
Режимы `param_id_mode` и `command_test_mode`:
|
||
|
||
| Значение | Режим |
|
||
|---:|---|
|
||
| `0` | `AD_PARAM_ID_MODE_IDLE` |
|
||
| `1` | `AD_PARAM_ID_MODE_STATOR_RESISTANCE` |
|
||
| `2` | `AD_PARAM_ID_MODE_NO_LOAD_MAGNETIZING` |
|
||
| `3` | `AD_PARAM_ID_MODE_LOCKED_ROTOR_LEAKAGE` |
|
||
| `4` | `AD_PARAM_ID_MODE_INERTIA_FRICTION` |
|
||
| `5` | `AD_PARAM_ID_MODE_DATA_LOGGING` |
|
||
| `6` | `AD_PARAM_ID_MODE_PWM_TEST_UH` |
|
||
| `7` | `AD_PARAM_ID_MODE_PWM_TEST_UL` |
|
||
| `8` | `AD_PARAM_ID_MODE_PWM_TEST_VH` |
|
||
| `9` | `AD_PARAM_ID_MODE_PWM_TEST_VL` |
|
||
| `10` | `AD_PARAM_ID_MODE_PWM_TEST_WH` |
|
||
| `11` | `AD_PARAM_ID_MODE_PWM_TEST_WL` |
|
||
| `12` | `AD_PARAM_ID_MODE_PWM_TEST_ALL` |
|
||
| `13` | `AD_PARAM_ID_MODE_AUTO_IDENTIFICATION` |
|
||
| `14` | `AD_PARAM_ID_MODE_ROTATION_3HZ` |
|
||
|
||
## USB CDC транспорт
|
||
|
||
USB реализован как CDC ACM устройство на USB FS:
|
||
|
||
| Параметр | Значение |
|
||
|---|---|
|
||
| Линии | `PA11 USB_DM`, `PA12 USB_DP` |
|
||
| Тактирование | `HSI48` |
|
||
| VID/PID | `0x0483 / 0x5740` |
|
||
| Product string | `AD Telemetry CDC` |
|
||
| Data IN endpoint | `0x81`, bulk, 64 байта |
|
||
| Data OUT endpoint | `0x01`, bulk, 64 байта |
|
||
| CMD endpoint | `0x82`, interrupt, 8 байт |
|
||
|
||
Код вызывает `HAL_PCD_EP_Transmit()` на весь пакет сразу, но CDC на стороне ПК
|
||
нужно считать потоком байт. Один `read()` на ПК не обязан совпадать с одним
|
||
пакетом прошивки.
|
||
|
||
Рекомендуемый парсер:
|
||
|
||
1. Искать в потоке magic bytes `44 49 44 41`.
|
||
2. Дочитать 16-байтный заголовок.
|
||
3. Проверить `version == 3`.
|
||
4. Проверить `payload_size == 136` или обработать совместимую новую версию.
|
||
5. Дочитать `payload_size` байт.
|
||
6. Проверить CRC16.
|
||
7. Передать payload в декодер.
|
||
|
||
## CAN/FDCAN транспорт
|
||
|
||
CAN реализован через FDCAN1 в режиме classic CAN, кадры по 8 байт.
|
||
|
||
| Параметр | Значение |
|
||
|---|---|
|
||
| Периферия | `FDCAN1` |
|
||
| RX/TX | `PB8 / PB9` |
|
||
| Frame format | Classic CAN |
|
||
| Bit rate switch | Off |
|
||
| Default base standard ID | `0x5A0` |
|
||
| Payload одного CAN-кадра | до 8 байт |
|
||
|
||
Базовый ID задается:
|
||
|
||
```c
|
||
AD_CAN_TELEMETRY_STD_ID_BASE = 0x5A0
|
||
```
|
||
|
||
Полный бинарный пакет режется последовательно:
|
||
|
||
| CAN ID | Данные |
|
||
|---:|---|
|
||
| `0x5A0` | bytes `0..7` |
|
||
| `0x5A1` | bytes `8..15` |
|
||
| `0x5A2` | bytes `16..23` |
|
||
| `...` | `...` |
|
||
|
||
Для текущего пакета `152` байта нужно `19` CAN-кадров:
|
||
|
||
```text
|
||
ceil(152 / 8) = 19
|
||
IDs: 0x5A0..0x5B2
|
||
```
|
||
|
||
Последний кадр несет полный 8-байтный блок.
|
||
|
||
Рекомендация для приемника CAN:
|
||
|
||
1. Считать кадр с `base_id`.
|
||
2. Начать буфер нового пакета.
|
||
3. Добавлять кадры `base_id + index`.
|
||
4. После первых 16 байт прочитать `payload_size`.
|
||
5. Ждать `16 + payload_size` байт.
|
||
6. Проверить `magic`, `version`, `sequence`, `crc16`.
|
||
7. При пропуске ID или смене `sequence` сбросить сборку пакета.
|
||
|
||
## Включение через Modbus
|
||
|
||
Modbus RTU остается каналом управления.
|
||
|
||
| Параметр | Значение |
|
||
|---|---|
|
||
| UART | `USART2` |
|
||
| TX/RX | `PA2 / PA3` |
|
||
| Скорость | `115200` |
|
||
| Формат | `8N1` |
|
||
| Slave address | `1` |
|
||
|
||
Регистры:
|
||
|
||
| Адрес hex / dec | Имя | Доступ | Значение |
|
||
|---:|---|---|---|
|
||
| `0x001C` / `28` | `control_host_protocol` | R/W | `0` = Modbus, `1` = binary telemetry |
|
||
| `0x001D` / `29` | `control_binary_transport` | R/W | `0` = USB, `1` = CAN, `2` = USB + CAN |
|
||
|
||
Пример включения USB CDC:
|
||
|
||
```text
|
||
write 0x001D / 29 = 0
|
||
write 0x001C / 28 = 1
|
||
```
|
||
|
||
Пример включения CAN:
|
||
|
||
```text
|
||
write 0x001D / 29 = 1
|
||
write 0x001C / 28 = 1
|
||
```
|
||
|
||
Пример одновременной отправки USB и CAN:
|
||
|
||
```text
|
||
write 0x001D / 29 = 2
|
||
write 0x001C / 28 = 1
|
||
```
|
||
|
||
Остановка бинарной телеметрии:
|
||
|
||
```text
|
||
write 0x001C / 28 = 0
|
||
```
|
||
|
||
## Состояние транспорта в отладчике
|
||
|
||
Основной debug-view:
|
||
|
||
```c
|
||
g_ad_debug.binary_transport
|
||
g_ad_debug.modbus
|
||
```
|
||
|
||
`g_ad_debug.binary_transport.status_flags`:
|
||
|
||
| Бит | Маска | Флаг |
|
||
|---:|---:|---|
|
||
| `0` | `0x00000001` | `AD_BINARY_TRANSPORT_STATUS_INITIALIZED` |
|
||
| `1` | `0x00000002` | `AD_BINARY_TRANSPORT_STATUS_USB_READY` |
|
||
| `2` | `0x00000004` | `AD_BINARY_TRANSPORT_STATUS_CAN_READY` |
|
||
| `3` | `0x00000008` | `AD_BINARY_TRANSPORT_STATUS_USB_CONFIGURED` |
|
||
| `4` | `0x00000010` | `AD_BINARY_TRANSPORT_STATUS_USB_TX_BUSY` |
|
||
| `5` | `0x00000020` | `AD_BINARY_TRANSPORT_STATUS_CAN_TX_BUSY` |
|
||
| `6` | `0x00000040` | `AD_BINARY_TRANSPORT_STATUS_LAST_DROPPED` |
|
||
|
||
Полезные счетчики:
|
||
|
||
| Поле | Значение |
|
||
|---|---|
|
||
| `tx_packets` | Сколько пакетов успешно поставлено хотя бы в один транспорт |
|
||
| `usb_packets` | Сколько пакетов принято USB TX |
|
||
| `can_packets` | Сколько пакетов принято CAN TX |
|
||
| `dropped_packets` | Сколько пакетов сброшено из-за busy/error/size |
|
||
| `last_size_bytes` | Последний размер пакета, сейчас должен быть `152` |
|
||
| `last_tx_tick_ms` | `HAL_GetTick()` последней успешной постановки |
|
||
|
||
## API прошивки
|
||
|
||
```c
|
||
void SimulinkInterface_PackTelemetry(void);
|
||
const SimulinkTelemetryPacket_t* SimulinkInterface_GetTelemetryPacket(void);
|
||
const uint8_t* SimulinkInterface_GetTelemetryBytes(void);
|
||
size_t SimulinkInterface_GetTelemetrySize(void);
|
||
|
||
void AD_BinaryTransport_Init(void);
|
||
void AD_BinaryTransport_Loop(void);
|
||
uint8_t AD_BinaryTransport_SetProtocol(uint16_t protocol);
|
||
uint8_t AD_BinaryTransport_SetTransport(uint16_t transport);
|
||
const AD_BinaryTransportState_t* AD_BinaryTransport_GetState(void);
|
||
```
|
||
|
||
## Минимальный Python-декодер CRC
|
||
|
||
```python
|
||
def crc16_ccitt(data: bytes) -> int:
|
||
crc = 0xFFFF
|
||
for value in data:
|
||
crc ^= value << 8
|
||
for _ in range(8):
|
||
if crc & 0x8000:
|
||
crc = ((crc << 1) ^ 0x1021) & 0xFFFF
|
||
else:
|
||
crc = (crc << 1) & 0xFFFF
|
||
return crc
|
||
```
|
||
|
||
Поля unpack для текущей версии:
|
||
|
||
```python
|
||
HEADER_FMT = "<IHHIHH"
|
||
PAYLOAD_SIZE_V3 = 136
|
||
PACKET_SIZE_V3 = 16 + PAYLOAD_SIZE_V3
|
||
MAGIC = 0x41444944
|
||
VERSION = 3
|
||
```
|