diff --git a/docs/DroneCAN-Driver.md b/docs/DroneCAN-Driver.md index d4f2a5e9945..b857427f30f 100644 --- a/docs/DroneCAN-Driver.md +++ b/docs/DroneCAN-Driver.md @@ -1,8 +1,8 @@ # DroneCAN Driver Documentation -**Last Updated:** 2026-02-16 -**Status:** Complete -**Branch:** feature-dronecan-sitl +**Last Updated:** 2026-08-21 +**Status:** Complete for the driver core, param-getset, and node table; DNA server, node/battery ID filtering, and actuator control are separate in-flight branches not yet reflected here +**Branch:** feature/dronecan-param-getset --- @@ -16,6 +16,7 @@ The DroneCAN driver (`dronecan.c/dronecan.h`) provides CAN bus communication for - **Message Reception:** Receive GPS fixes, battery info, node status, and GNSS auxiliary data - **Message Transmission:** Broadcast node status at 1 Hz showing health, mode, and uptime - **Service Handling:** Respond to GetNodeInfo requests from other nodes +- **On-Demand Service Client:** Initiate GetNodeInfo, parameter Get/Set, ExecuteOpcode, and RestartNode requests to other nodes, polled by the configurator/GCS via MSP (see [On-Demand Async Service Client](#on-demand-async-service-client)) - **Bus Recovery:** Automatic recovery from CAN bus-off errors - **Integration:** Direct integration with GPS provider and battery sensor systems @@ -256,11 +257,15 @@ typedef struct dronecanNodeInfo_s { uint32_t uptime_sec; // Node uptime in seconds uint16_t vendor_status_code; // Vendor-specific status word uint32_t last_seen_ms; // FC millis() timestamp of last NodeStatus - uint8_t name_len; // Length of name (0 until GetNodeInfo implemented) - char name[32]; // Node name, zero-padded } dronecanNodeInfo_t; ``` +**Note:** the `name`/`name_len` fields that previously lived here have been removed. Node names are not +available from `NodeStatus` broadcasts and are no longer cached in the table — retrieve a node's name +(and other identity info) on demand via the async service client's `GetNodeInfo` request instead (see +[On-Demand Async Service Client](#on-demand-async-service-client)). The result is returned directly to +the caller, not written back into `nodeTable[]`. + The table holds up to `DRONECAN_MAX_NODES` (32) entries, indexed by arrival order. Entries are never removed at runtime; the table persists from boot until power-off. #### Accessor Functions @@ -282,15 +287,17 @@ The driver uses the following settings from `settings.yaml`: | Setting | Description | Type | Valid Range | Default | |---|---|---|---|---| -| `dronecan_mode` | Enable/disable DroneCAN | bool | 0/1 | 0 | -| `dronecan_node_id` | This FC's CAN node ID | uint8 | 1-125 | 0 | -| `dronecan_baudrate` | CAN bus bitrate | enum | 0-3 | 2 (500kbps) | +| `dronecan_node_id` | This FC's CAN node ID. 126/127 reserved for diagnostic tools. | uint8 | 1-127 | 1 | +| `dronecan_bitrate_kbps` | CAN bus bitrate | enum (`dronecan_bitrate_table`) | 125/250/500/1000 | 1000 | + +There is no separate enable/disable setting — DroneCAN support is a compile-time feature (`USE_DRONECAN`); +whether it's active at runtime is determined by whether the target was built with that flag. -**Bitrate Mapping:** +**Bitrate Mapping (`dronecanBitrate_e`):** - 0 = 125 kbps - 1 = 250 kbps -- 2 = 500 kbps (default) -- 3 = 1000 kbps +- 2 = 500 kbps +- 3 = 1000 kbps (default) #### Accessing Configuration @@ -337,7 +344,7 @@ The driver includes 7 built-in message handlers. Each handler decodes a message #### `handle_NodeStatus()` **Receives:** `uavcan_protocol_NodeStatus` (from other nodes) -**Processing:** Decodes the message and upserts into the node table (`nodeTable[]`). If the source node ID is already in the table, health, mode, uptime, vendor status, and `last_seen_ms` are updated. If it is a new node and the table is not full, a new entry is appended and `activeNodeCount` is incremented. Node names are not available from NodeStatus broadcasts; they remain empty until `GetNodeInfo` service requests are implemented. +**Processing:** Decodes the message and upserts into the node table (`nodeTable[]`). If the source node ID is already in the table, health, mode, uptime, vendor status, and `last_seen_ms` are updated. If it is a new node and the table is not full, a new entry is appended and `activeNodeCount` is incremented. Node names are not available from NodeStatus broadcasts and are not cached in the table at all — retrieve them on demand via the async service client's `GetNodeInfo` request (see [On-Demand Async Service Client](#on-demand-async-service-client)). **Status:** Node count accessible via `dronecanGetNodeCount()`; per-node detail via `dronecanGetNode()` ### Service Handlers @@ -357,6 +364,101 @@ The driver includes 7 built-in message handlers. Each handler decodes a message --- +## On-Demand Async Service Client + +Unlike the passive `handle_*()` handlers above (which respond to messages other nodes send us), this is +the driver acting as a **client** — initiating service requests to other nodes on demand, from the +configurator or a GCS. Implemented in `dronecan_async.c`/`dronecan_async.h`. + +### Why a Single Shared Slot + +Rather than a request/response queue per service type, there is exactly one in-flight request at a time, +tracked in a single shared `dronecanAsyncSlot_t` (declared in `dronecan.h`). This keeps the implementation +small and matches the actual usage pattern: the configurator drives one request, waits for the result, then +issues the next — there's no need for concurrent in-flight requests to different nodes. + +```c +typedef enum { + DRONECAN_ASYNC_IDLE = 0, + DRONECAN_ASYNC_PENDING, + DRONECAN_ASYNC_READY, + DRONECAN_ASYNC_ERROR, +} dronecanAsyncState_e; + +typedef struct dronecanAsyncSlot_s { + dronecanAsyncState_e state; + uint8_t seq; // Incremented on every new request; lets a poller confirm which request a result belongs to + uint8_t service_id; + uint8_t node_id; + uint8_t transfer_id; + uint32_t requested_at_ms; + union { + dronecanGetNodeInfoResult_t node_info; + dronecanParamResult_t param; + dronecanSimpleResult_t simple; + } result; +} dronecanAsyncSlot_t; +``` + +### Supported Services + +| `service_id` | Constant | UAVCAN Service | Request Payload | Result | +|---|---|---|---|---| +| 1 | `DRONECAN_SERVICE_GETNODEINFO` | `uavcan.protocol.GetNodeInfo` | None | Name, SW/HW version, unique ID → `result.node_info` | +| 5 | `DRONECAN_SERVICE_RESTART_NODE` | `uavcan.protocol.RestartNode` | None | `ok` → `result.simple` | +| 10 | `DRONECAN_SERVICE_EXECUTE_OPCODE` | `uavcan.protocol.param.ExecuteOpcode` | `dronecanParamRequest_t` (opcode only) | `ok` → `result.simple` | +| 11 | `DRONECAN_SERVICE_PARAM_GETSET` | `uavcan.protocol.param.GetSet` | `dronecanParamRequest_t` (index or name, write value if writing) | Name, type, value, min/max → `result.param` | + +Param values (`dronecanParamRequest_t`/`dronecanParamResult_t`) use a tagged encoding shared across +INT/FLOAT/BOOL/STRING: `DRONECAN_PARAM_TYPE_{EMPTY,INT,FLOAT,BOOL,STRING}`. `EMPTY` on `min_type`/`max_type` +means the node didn't provide a bound for that parameter. + +### Request Flow + +```c +bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload); +``` + +1. Refuses to start a new request while one is already `DRONECAN_ASYNC_PENDING` and not yet timed out + (returns `false` — caller should poll until the current request resolves). +2. Encodes the service-specific request struct and calls `canardRequestOrRespond()`. +3. On successful dispatch: sets `state = DRONECAN_ASYNC_PENDING`, increments `seq`, records `service_id`, + `node_id`, and `requested_at_ms`. + +```c +void dronecanAsyncCheckTimeout(void); // called once per dronecanUpdate() tick in STATE_DRONECAN_NORMAL +``` + +Expires a pending request that never got a response: if `state == DRONECAN_ASYNC_PENDING` and +`DRONECAN_ASYNC_TIMEOUT_MS` (2000ms) has elapsed since `requested_at_ms`, sets `state = DRONECAN_ASYNC_ERROR`. +Note this reflects "no response received in time" accurately — it does not distinguish between the request +never arriving, the node being too slow, or the node correctly performing the requested action (e.g. +`RestartNode`) but being unable to transmit its acknowledgement before resetting. A node that legitimately +restarts in response to `RestartNode` will often still show up as `ERROR` here for exactly that reason. + +```c +void dronecanAsyncHandleServiceResponse(CanardInstance *ins, CanardRxTransfer *transfer); +``` + +Called from `onTransferReceived()` for every `CanardTransferTypeResponse` frame. Matches the response +against the pending slot by `service_id`, `source_node_id`, and `transfer_id` (all three must match — this +guards against stale frames from a previous request, e.g. after bus-off recovery) before decoding and +setting `state = DRONECAN_ASYNC_READY` with the result populated. + +### MSP Access Pattern + +The configurator/GCS drives this over MSP, not by calling the C API directly: + +1. Send `MSP2_INAV_DRONECAN_ASYNC_REQUEST` (service_id, node_id, service-specific payload) → FC calls + `dronecanAsyncRequest()` and replies with `accepted` + the new `seq`. +2. Poll `MSP2_INAV_DRONECAN_ASYNC_RESULT` at roughly 100ms intervals until `state` is `READY` (2) or + `ERROR` (3). Reading a `READY`/`ERROR` result transitions the slot back to `IDLE`, so each result is + consumed exactly once. + +See [MSP Commands](#msp-commands) below for the exact wire format. + +--- + ## Handler Registration ### The `shouldAcceptTransfer()` Callback @@ -511,7 +613,9 @@ if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix) == 0) { ## MSP Commands -Two MSP2 commands expose the node table to external tools (configurator, GCS, test scripts): +Three MSP2 commands expose DroneCAN node/parameter data to external tools (configurator, GCS, test scripts). +Full wire-level field definitions are the source of truth in `docs/development/msp/msp_messages.json` — the +summaries below are for orientation; check that file (and its generated `README.md`) for exact byte offsets. ### `MSP2_INAV_DRONECAN_NODES` (0x2042) @@ -519,33 +623,43 @@ Two MSP2 commands expose the node table to external tools (configurator, GCS, te **Handler location:** `mspFcProcessOutCommand()` in `fc_msp.c` **Guard:** `#ifdef USE_DRONECAN` -**Reply layout:** - -| Offset | Size | Field | -|--------|------|-------| -| 0 | 1 | `nodeCount` | -| 1 + N×30 | 1 | `nodeID` | -| 2 + N×30 | 1 | `health` | -| 3 + N×30 | 1 | `mode` | -| 4 + N×30 | 4 | `uptime_sec` (little-endian) | -| 8 + N×30 | 2 | `vendor_status_code` | -| 10 + N×30 | 4 | `last_seen_ms` | -| 14 + N×30 | 1 | `name_len` | -| 15 + N×30 | 16 | `name` (zero-padded) | +**Reply:** `nodeCount` (1 byte) followed by `nodeCount` fixed 13-byte records: +`nodeID`(1) + `health`(1) + `mode`(1) + `last_seen_ms`(4, **milliseconds since last NodeStatus from this +node**, not an absolute timestamp) + `uptime_sec`(4) + `vendor_status_code`(2). -Total: 1 + nodeCount × 30 bytes. +Total: 1 + nodeCount × 13 bytes (max 417 bytes at `DRONECAN_MAX_NODES` = 32). No name field — per the node +table change above, names aren't cached; use `MSP2_INAV_DRONECAN_ASYNC_REQUEST` with +`service_id=DRONECAN_SERVICE_GETNODEINFO` for full node identity. --- -### `MSP2_INAV_DRONECAN_NODE_INFO` (0x2043) +### `MSP2_INAV_DRONECAN_ASYNC_REQUEST` (0x2043) -**Direction:** Request (1 byte node ID) → Reply -**Handler location:** `mspFCProcessInOutCommand()` in `fc_msp.c` +**Direction:** Request → Reply (dispatch acknowledgement only — the actual result comes from +`MSP2_INAV_DRONECAN_ASYNC_RESULT` below) **Guard:** `#ifdef USE_DRONECAN` -**Request:** 1 byte — target `nodeID` +**Request:** `service_id` (u16, low byte used: 1=GETNODEINFO, 5=RESTART_NODE, 10=EXECUTE_OPCODE, +11=PARAM_GETSET) + `nodeID` (u8) + service-specific fields (opcode for EXECUTE_OPCODE; index/name + +optional write value for PARAM_GETSET). + +**Reply:** `accepted` (u8: 0=accepted, 1=busy or unrecognised service, 0xFF=bus not ready) + `seq` (u8, to +correlate with the eventual result). + +This only *starts* the request — see [On-Demand Async Service Client](#on-demand-async-service-client) for +the underlying state machine and timeout behaviour. + +--- + +### `MSP2_INAV_DRONECAN_ASYNC_RESULT` (0x2044) + +**Direction:** Request (no payload) → Reply +**Guard:** `#ifdef USE_DRONECAN` -**Reply layout:** same fields as above but with a 32-byte `name` field (46 bytes total). Returns an empty response if the requested node ID is not in the table. +**Reply:** `state` (u8: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR) + `seq` (u8) + `service_id` (u16) + `node_id` +(u8), followed by service-specific result fields when `state=READY` (GETNODEINFO: name + SW/HW version + +unique ID; PARAM_GETSET: name + value + min/max; EXECUTE_OPCODE/RESTART_NODE: `ok` byte). Reading a +`READY`/`ERROR` result resets the slot to `IDLE`. Poll at ~100ms intervals after issuing a request. --- @@ -909,6 +1023,7 @@ void broadcastNodeStatus(void) { | Date | Version | Changes | |---|---|---| +| 2026-08-21 | 1.3 | Documented the on-demand async service client (`dronecan_async.c`): GetNodeInfo/ParamGetSet/ExecuteOpcode/RestartNode requests, the shared slot state machine, and the new `MSP2_INAV_DRONECAN_ASYNC_REQUEST`/`ASYNC_RESULT` messages that replace 0x2043's old NODE_INFO meaning. Corrected `dronecanNodeInfo_t` (name/name_len fields removed), the `MSP2_INAV_DRONECAN_NODES` reply layout (13 bytes/node, not 30; `last_seen_ms` is an elapsed delta, not an absolute timestamp), and the Settings table (real setting names are `dronecan_node_id`/`dronecan_bitrate_kbps`, not `dronecan_mode`/`dronecan_baudrate`). Scoped to the param-getset feature — this pass did not re-verify sections describing DNA server, node/battery ID filtering, or actuator control, which may have their own drift from other in-flight branches. | | 2026-04-30 | 1.2 | Added node table (`dronecanNodeInfo_t`), accessor functions, CLI status output, and MSP commands (0x2042/0x2043) | | 2026-02-18 | 1.1 | Added error recovery, graceful disable behavior, and safe initialization documentation | | 2026-02-16 | 1.0 | Initial version - handler-based architecture documentation | diff --git a/docs/DroneCAN.md b/docs/DroneCAN.md index 3a0312d2d56..77812547f5f 100644 --- a/docs/DroneCAN.md +++ b/docs/DroneCAN.md @@ -9,7 +9,7 @@ DroneCAN (formerly UAVCAN v0) is a lightweight protocol designed for reliable co | GPS | Supported | GNSS receivers via DroneCAN | | Battery Voltage | Supported | Voltage sensing from DroneCAN battery monitors | | Battery Current | Supported | Current sensing from DroneCAN battery monitors | -| Parameter Get/Set | Planned | Remote parameter configuration | +| Parameter Get/Set | Supported | Read/write DroneCAN peripheral parameters via the configurator's DroneCAN tab | | ESC Control | Planned | Motor control via DroneCAN ESCs | | Dynamic Node Assignment | Planned | Manage node IDs dynamically to minimize first time configuration | @@ -30,7 +30,7 @@ DroneCAN settings are configured via CLI: ``` set dronecan_node_id = 10 -set dronecan_bitrate = 1000KBPS +set dronecan_bitrate_kbps = 1000 save ``` @@ -38,8 +38,8 @@ save | Setting | Values | Default | Description | |---------|--------|---------|-------------| -| `dronecan_node_id` | 1-127 | 10 | CAN node ID for the flight controller | -| `dronecan_bitrate` | 125KBPS, 250KBPS, 500KBPS, 1000KBPS | 1000KBPS | CAN bus bitrate | +| `dronecan_node_id` | 1-127 (126/127 reserved for diagnostic tools) | 1 | CAN node ID for the flight controller | +| `dronecan_bitrate_kbps` | 125, 250, 500, 1000 | 1000 | CAN bus bitrate in kbps | All peripherals need to have the node ID and the bitrate set manually through the dronecan_gui for now. You can use your flight controller as a CAN interface by loading an Ardupilot image on it. Once the set up is complete, you can reflash it to Inav. @@ -73,6 +73,25 @@ save Both voltage and current come from the same DroneCAN BatteryInfo message, so they update together when using a DroneCAN battery monitor. +### Parameter Get/Set via DroneCAN + +DroneCAN peripheral parameters (e.g. GPS or battery monitor settings) are read and written from the +**DroneCAN tab in the INAV Configurator**, not via the flight controller's CLI — the CLI only configures the +flight controller's own DroneCAN settings (node ID, bitrate), not settings on other nodes on the bus. + +To configure a peripheral's parameters: + +1. Connect to the flight controller and open the **DroneCAN** tab. +2. Select the node from the detected node table. +3. Edit parameter values in the node's detail panel — values are checked against the min/max range the + node itself reports, where the node provides one. +4. Click **Write** to send a changed value to the node. +5. Click **Save to EEPROM** to persist changes on that node (writes are held in the node's RAM until saved). +6. Click **Restart Node** if the node requires a restart to apply the new value. Note: some peripherals + (depending on their firmware) may not send an acknowledgement before restarting, so the button can show + a timeout/failure even when the node restarted successfully — check the node reappears in the table + afterward to confirm. + ## Configuration Examples ### Example 1: GPS Only Setup diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index fafcf5d4762..6231331dc7a 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -418,7 +418,8 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: [8256 - MSP2_INAV_ESC_RPM](#msp2_inav_esc_rpm) [8257 - MSP2_INAV_ESC_TELEM](#msp2_inav_esc_telem) [8258 - MSP2_INAV_DRONECAN_NODES](#msp2_inav_dronecan_nodes) -[8259 - MSP2_INAV_DRONECAN_NODE_INFO](#msp2_inav_dronecan_node_info) +[8259 - MSP2_INAV_DRONECAN_ASYNC_REQUEST](#msp2_inav_dronecan_async_request) +[8260 - MSP2_INAV_DRONECAN_ASYNC_RESULT](#msp2_inav_dronecan_async_result) [8264 - MSP2_INAV_LED_STRIP_CONFIG_EX](#msp2_inav_led_strip_config_ex) [8265 - MSP2_INAV_SET_LED_STRIP_CONFIG_EX](#msp2_inav_set_led_strip_config_ex) [8266 - MSP2_INAV_FW_APPROACH](#msp2_inav_fw_approach) @@ -4165,34 +4166,49 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Request Payload:** **None** **Reply Payload:** -|Field|C Type|Size (Bytes)|Description| -|---|---|---|---| -| `nodeCount` | `uint8_t` | 1 | Number of detected DroneCAN nodes | -| `nodeData` | `dronecanNodeStatus_t[]` | array | Array of per-node status records, one per detected node. Each record: nodeID(1)+health(1)+mode(1)+last_seen_ms(4) = 7 bytes. Full detail available via MSP2_INAV_DRONECAN_NODE_INFO. | +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `nodeCount` | `uint8_t` | 1 | - | Number of detected DroneCAN nodes | +| `nodeID` | `uint8_t[]` | array | - | [per node] DroneCAN node ID (1-127) | +| `health` | `uint8_t` | 1 | - | [per node] Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL | +| `mode` | `uint8_t` | 1 | - | [per node] Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE | +| `last_seen_ms` | `uint32_t` | 4 | ms | [per node] Milliseconds since this node was last seen (FC-local timestamp delta) | +| `uptime_sec` | `uint32_t` | 4 | s | [per node] Node uptime in seconds (from NodeStatus broadcast) | +| `vendor_status_code` | `uint16_t` | 2 | - | [per node] Vendor-specific status code | -**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4). Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO. +**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 13 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4)+uptime_sec(4)+vendor_status_code(2). Maximum payload 1 + (DRONECAN_MAX_NODES * 13) = 417 bytes. For full node detail (name, SW/HW version, unique ID) use MSP2_INAV_DRONECAN_ASYNC_REQUEST with service_id=DRONECAN_SERVICE_GETNODEINFO(1). -## `MSP2_INAV_DRONECAN_NODE_INFO (8259 / 0x2043)` -**Description:** Returns full status detail for a single DroneCAN node by ID. +## `MSP2_INAV_DRONECAN_ASYNC_REQUEST (8259 / 0x2043)` +**Description:** Initiates an asynchronous DroneCAN service request (GetNodeInfo, ParamGetSet, ExecuteOpcode, RestartNode) to a specific node. Result retrieved via MSP2_INAV_DRONECAN_ASYNC_RESULT. **Request Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| -| `nodeID` | `uint8_t` | 1 | DroneCAN node ID to query (1-127) | +| `service_id` | `uint16_t` | 2 | Service to invoke: 1=GETNODEINFO, 5=RESTART_NODE, 10=EXECUTE_OPCODE, 11=PARAM_GETSET. Transmitted as u16 for MSP alignment; only low 8 bits used. | +| `nodeID` | `uint8_t` | 1 | Target DroneCAN node ID (1-127) | **Reply Payload:** -|Field|C Type|Size (Bytes)|Units|Description| -|---|---|---|---|---| -| `nodeID` | `uint8_t` | 1 | - | DroneCAN node ID | -| `health` | `uint8_t` | 1 | - | Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL | -| `mode` | `uint8_t` | 1 | - | Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE | -| `uptime_sec` | `uint32_t` | 4 | s | Node uptime in seconds | -| `vendor_status_code` | `uint16_t` | 2 | - | Vendor-specific status code | -| `last_seen_ms` | `uint32_t` | 4 | ms | FC millisecond timestamp when this node was last seen | -| `name_len` | `uint8_t` | 1 | - | Length of node name string (0 if unknown) | -| `name` | `char[32]` | 32 | - | Node name up to 32 bytes, zero-padded | +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `accepted` | `uint8_t` | 1 | 0=request accepted; 1=busy (slot in use) or unrecognised service_id; 0xFF=bus not in STATE_DRONECAN_NORMAL (not ready) | +| `seq` | `uint8_t` | 1 | Sequence number; correlate with MSP2_INAV_DRONECAN_ASYNC_RESULT to verify the result belongs to this request | + +**Notes:** Requires `USE_DRONECAN`. Initiates an async DroneCAN service request; poll MSP2_INAV_DRONECAN_ASYNC_RESULT at ~100ms intervals until state=READY(2) or ERROR(3). Only one request in-flight at a time. Service-specific request fields follow the common header in the request payload: EXECUTE_OPCODE appends opcode(u8); PARAM_GETSET appends index(u16)+is_write(u8) and optionally value_type(u8)+value(variable) for writes, then req_name_len(u8)+req_name(bytes) for named lookup. Param value encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data. Requests time out after DRONECAN_ASYNC_TIMEOUT_MS (2000ms). If bus is not in STATE_DRONECAN_NORMAL, returns accepted=0xFF without dispatching. + +## `MSP2_INAV_DRONECAN_ASYNC_RESULT (8260 / 0x2044)` +**Description:** Polls the result of the most recent MSP2_INAV_DRONECAN_ASYNC_REQUEST. Poll at ~100ms intervals until state is READY(2) or ERROR(3). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `state` | `uint8_t` | 1 | Async slot state: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR | +| `seq` | `uint8_t` | 1 | Sequence number matching the originating MSP2_INAV_DRONECAN_ASYNC_REQUEST reply | +| `service_id` | `uint16_t` | 2 | Service ID of the in-flight or just-completed request | +| `node_id` | `uint8_t` | 1 | Node ID of the target | -**Notes:** Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. +**Notes:** Requires `USE_DRONECAN`. When state=READY(2), service-specific result fields follow the 5-byte common header. GETNODEINFO: name_len(u8)+name(bytes)+sw_major(u8)+sw_minor(u8)+sw_optional_field_flags(u8)+sw_vcs_commit(u32)+hw_major(u8)+hw_minor(u8)+hw_unique_id(u8[16]). PARAM_GETSET: name_len(u8)+name(bytes)+type(u8)+value(variable)+min_type(u8)+min(variable)+max_type(u8)+max(variable); value/min/max encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data; EMPTY(0) min/max type means no bound is present. EXECUTE_OPCODE and RESTART_NODE: ok(u8) where 1=success. Reading result when state=READY transitions slot back to IDLE. ## `MSP2_INAV_LED_STRIP_CONFIG_EX (8264 / 0x2048)` **Description:** Retrieves the full configuration for each LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_LED_STRIP_CONFIG`. diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 3572d5462d4..196970c0a7e 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9830,86 +9830,123 @@ "units": "" }, { - "name": "nodeData", - "desc": "Array of per-node status records, one per detected node. Each record: nodeID(1)+health(1)+mode(1)+last_seen_ms(4) = 7 bytes. Full detail available via MSP2_INAV_DRONECAN_NODE_INFO.", - "ctype": "dronecanNodeStatus_t", + "name": "nodeID", + "ctype": "uint8_t", + "desc": "[per node] DroneCAN node ID (1-127)", + "units": "", "array": true, - "array_size": 0, + "array_size": 0 + }, + { + "name": "health", + "ctype": "uint8_t", + "desc": "[per node] Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL", + "units": "" + }, + { + "name": "mode", + "ctype": "uint8_t", + "desc": "[per node] Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE", + "units": "" + }, + { + "name": "last_seen_ms", + "ctype": "uint32_t", + "desc": "[per node] Milliseconds since this node was last seen (FC-local timestamp delta)", + "units": "ms" + }, + { + "name": "uptime_sec", + "ctype": "uint32_t", + "desc": "[per node] Node uptime in seconds (from NodeStatus broadcast)", + "units": "s" + }, + { + "name": "vendor_status_code", + "ctype": "uint16_t", + "desc": "[per node] Vendor-specific status code", "units": "" } ] }, "variable_len": true, - "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4). Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.", + "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 13 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4)+uptime_sec(4)+vendor_status_code(2). Maximum payload 1 + (DRONECAN_MAX_NODES * 13) = 417 bytes. For full node detail (name, SW/HW version, unique ID) use MSP2_INAV_DRONECAN_ASYNC_REQUEST with service_id=DRONECAN_SERVICE_GETNODEINFO(1).", "description": "Returns the list of all detected DroneCAN nodes with their current status." - }, - "MSP2_INAV_DRONECAN_NODE_INFO": { - "code": 8259, - "mspv": 2, + }, + "MSP2_INAV_DRONECAN_ASYNC_REQUEST": { + "code": 8259, + "mspv": 2, "request": { - "payload": [ - { + "payload": [ + { + "name": "service_id", + "ctype": "uint16_t", + "desc": "Service to invoke: 1=GETNODEINFO, 5=RESTART_NODE, 10=EXECUTE_OPCODE, 11=PARAM_GETSET. Transmitted as u16 for MSP alignment; only low 8 bits used.", + "units": "" + }, + { "name": "nodeID", "ctype": "uint8_t", - "desc": "DroneCAN node ID to query (1-127)", - "units": "" + "desc": "Target DroneCAN node ID (1-127)", + "units": "" } - ] - }, + ] + }, "reply": { "payload": [ { - "name": "nodeID", - "ctype": "uint8_t", - "desc": "DroneCAN node ID", - "units": "" - }, + "name": "accepted", + "ctype": "uint8_t", + "desc": "0=request accepted; 1=busy (slot in use) or unrecognised service_id; 0xFF=bus not in STATE_DRONECAN_NORMAL (not ready)", + "units": "" + }, { - "name": "health", + "name": "seq", + "ctype": "uint8_t", + "desc": "Sequence number; correlate with MSP2_INAV_DRONECAN_ASYNC_RESULT to verify the result belongs to this request", + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_DRONECAN`. Initiates an async DroneCAN service request; poll MSP2_INAV_DRONECAN_ASYNC_RESULT at ~100ms intervals until state=READY(2) or ERROR(3). Only one request in-flight at a time. Service-specific request fields follow the common header in the request payload: EXECUTE_OPCODE appends opcode(u8); PARAM_GETSET appends index(u16)+is_write(u8) and optionally value_type(u8)+value(variable) for writes, then req_name_len(u8)+req_name(bytes) for named lookup. Param value encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data. Requests time out after DRONECAN_ASYNC_TIMEOUT_MS (2000ms). If bus is not in STATE_DRONECAN_NORMAL, returns accepted=0xFF without dispatching.", + "description": "Initiates an asynchronous DroneCAN service request (GetNodeInfo, ParamGetSet, ExecuteOpcode, RestartNode) to a specific node. Result retrieved via MSP2_INAV_DRONECAN_ASYNC_RESULT." + }, + "MSP2_INAV_DRONECAN_ASYNC_RESULT": { + "code": 8260, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "state", "ctype": "uint8_t", - "desc": "Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL", + "desc": "Async slot state: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR", "units": "" }, { - "name": "mode", - "ctype": "uint8_t", - "desc": "Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE", - "units": "" - }, - { - "name": "uptime_sec", - "ctype": "uint32_t", - "desc": "Node uptime in seconds", - "units": "s" + "name": "seq", + "ctype": "uint8_t", + "desc": "Sequence number matching the originating MSP2_INAV_DRONECAN_ASYNC_REQUEST reply", + "units": "" }, { - "name": "vendor_status_code", + "name": "service_id", "ctype": "uint16_t", - "desc": "Vendor-specific status code", - "units": "" + "desc": "Service ID of the in-flight or just-completed request", + "units": "" }, { - "name": "last_seen_ms", - "ctype": "uint32_t", - "desc": "FC millisecond timestamp when this node was last seen", - "units": "ms" - }, - { - "name": "name_len", - "ctype": "uint8_t", - "desc": "Length of node name string (0 if unknown)", - "units": "" - }, - { - "name": "name", - "ctype": "char[32]", - "desc": "Node name up to 32 bytes, zero-padded", - "units": "" - } - ] - }, - "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table.", - "description": "Returns full status detail for a single DroneCAN node by ID." + "name": "node_id", + "ctype": "uint8_t", + "desc": "Node ID of the target", + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_DRONECAN`. When state=READY(2), service-specific result fields follow the 5-byte common header. GETNODEINFO: name_len(u8)+name(bytes)+sw_major(u8)+sw_minor(u8)+sw_optional_field_flags(u8)+sw_vcs_commit(u32)+hw_major(u8)+hw_minor(u8)+hw_unique_id(u8[16]). PARAM_GETSET: name_len(u8)+name(bytes)+type(u8)+value(variable)+min_type(u8)+min(variable)+max_type(u8)+max(variable); value/min/max encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data; EMPTY(0) min/max type means no bound is present. EXECUTE_OPCODE and RESTART_NODE: ok(u8) where 1=success. Reading result when state=READY transitions slot back to IDLE.", + "description": "Polls the result of the most recent MSP2_INAV_DRONECAN_ASYNC_REQUEST. Poll at ~100ms intervals until state is READY(2) or ERROR(3)." }, "MSP2_INAV_LED_STRIP_CONFIG_EX": { "code": 8264, diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index da19602209d..8f177ef227e 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -173,6 +173,8 @@ main_sources(COMMON_SRC drivers/dronecan/libcanard/canard.h drivers/dronecan/libcanard/canard_stm32_driver.h drivers/dronecan/dronecan.c + drivers/dronecan/dronecan_async.c + drivers/dronecan/dronecan_async.h drivers/dronecan/dronecan.h drivers/display.c @@ -309,6 +311,8 @@ main_sources(COMMON_SRC fc/fc_msp.h fc/fc_msp_box.c fc/fc_msp_box.h + fc/fc_msp_dronecan.c + fc/fc_msp_dronecan.h fc/firmware_update.c fc/firmware_update.h fc/firmware_update_common.c diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 0f671c2425c..682423802b9 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -26,10 +26,11 @@ #include #include #include +#include "dronecan_async.h" /* Private variables ---------------------------------------------------------*/ -static CanardInstance canard; +CanardInstance canard; /* non-static: dronecan_async.c needs extern access */ static uint8_t memory_pool[1024]; static struct uavcan_protocol_NodeStatus node_status; @@ -41,17 +42,31 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, ); static dronecanState_e dronecanState = STATE_DRONECAN_INIT; + +#ifdef UNIT_TEST +uint8_t activeNodeCount = 0; +dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; +static volatile uint32_t txErrCount = 0; +static uint32_t busOffCount = 0; +#else static uint8_t activeNodeCount = 0; static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; static volatile uint32_t txErrCount = 0; static uint32_t busOffCount = 0; +#endif /* Forward declarations ------------------------------------------------------*/ static void processCanardTxQueueSafe(void); static void process1HzTasks(timeUs_t timestamp_usec); +#ifdef UNIT_TEST +bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id); +void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer); +void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer); +#else static bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id); static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer); +#endif // ---- Public API ------------------------------------------------------------- @@ -149,6 +164,11 @@ void dronecanUpdate(timeUs_t currentTimeUs) // in the same task cycle so multi-frame transfers complete before timeout. processCanardTxQueueSafe(); + // Check for async request timeout only after this tick's RX frames have + // been processed, so a response already queued this tick can complete + // the request before it's considered expired. + dronecanAsyncCheckTimeout(); + if (currentTimeUs >= next_1hz_service_at) { next_1hz_service_at += 1000000ULL; @@ -183,11 +203,18 @@ void dronecanUpdate(timeUs_t currentTimeUs) case STATE_DRONECAN_BUS_OFF: if(currentTimeUs > (busoffTimeUs + 20000)) { // Wait 20ms: worst-case 128x11 recovery is 11.264ms at 125kbps + static uint8_t busoff_retries = 0; canardSTM32RecoverFromBusOff(); busoffTimeUs = currentTimeUs; canardSTM32GetProtocolStatus(&protocolStatus); if(protocolStatus.BusOff == 0) { + busoff_retries = 0; dronecanState = STATE_DRONECAN_NORMAL; + } else if (++busoff_retries >= 50) { + // ~1 second of 20ms recovery attempts with no success — permanent fault + busoff_retries = 0; + dronecanState = STATE_DRONECAN_FAILED; + LOG_DEBUG(CAN, "DroneCAN: bus-off recovery failed after 50 attempts, entering FAILED state"); } } break; @@ -319,6 +346,22 @@ static void processCanardTxQueueSafe(void) { // NOTE: All canard handlers and senders are based on this reference: https://dronecan.github.io/Specification/7._List_of_standard_data_types/ // Alternatively, you can look at the corresponding generated header file in the dsdlc_generated folder +static dronecanNodeInfo_t *findNodeByID(uint8_t nodeID) { + for (uint8_t i = 0; i < activeNodeCount; i++) { + if (nodeTable[i].nodeID == nodeID) { + return &nodeTable[i]; + } + } + return NULL; +} + +const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) { + return findNodeByID(nodeID); +} + +// Canard Handlers and Senders + + /* send the 1Hz NodeStatus message. This is what allows a node to show up in the DroneCAN GUI tool and in the flight controller logs @@ -375,6 +418,16 @@ static void process1HzTasks(timeUs_t timestamp_usec) canardCleanupStaleTransfers(&canard, timestamp_usec); } + // Remove nodes that have stopped broadcasting NodeStatus + for (uint8_t i = 0; i < activeNodeCount; ) { + if (millis() - nodeTable[i].last_seen_ms > DRONECAN_NODE_STALE_TIMEOUT_MS) { + nodeTable[i] = nodeTable[activeNodeCount - 1]; + activeNodeCount--; + } else { + i++; + } + } + /* Transmit the node status message */ @@ -390,66 +443,74 @@ static void process1HzTasks(timeUs_t timestamp_usec) This function must fill in the out_data_type_signature to be the signature of the message. */ +#ifdef UNIT_TEST +bool shouldAcceptTransfer(const CanardInstance *ins, +#else static bool shouldAcceptTransfer(const CanardInstance *ins, +#endif uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id) { - UNUSED(ins); + UNUSED(ins); UNUSED(source_node_id); if (transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; - return true; - } - } - } - if (transfer_type == CanardTransferTypeResponse) { - // check if we want to handle a specific service request - switch (data_type_id) { - } - } - if (transfer_type == CanardTransferTypeBroadcast) { - // see if we want to handle a specific broadcast packet - switch (data_type_id) { - - case UAVCAN_PROTOCOL_NODESTATUS_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE; - return true; - } - case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: { - *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE; + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; return true; } - case UAVCAN_EQUIPMENT_GNSS_FIX_ID: { - *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE; + } + if (transfer_type == CanardTransferTypeResponse) { + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE; + return true; + case UAVCAN_PROTOCOL_PARAM_GETSET_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE; + return true; + case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE; + return true; + case UAVCAN_PROTOCOL_RESTARTNODE_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; return true; } - case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: { + } + if (transfer_type == CanardTransferTypeBroadcast) { + switch (data_type_id) { + case UAVCAN_PROTOCOL_NODESTATUS_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE; + return true; + case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: + *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE; + return true; + case UAVCAN_EQUIPMENT_GNSS_FIX_ID: + *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE; + return true; + case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE; return true; - } - case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: { + case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE; return true; - } - case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: { + case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE; return true; } - } - } - // we don't want any other messages - return false; + } + return false; } // Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) +#ifdef UNIT_TEST +void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { +#else static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); +#endif + UNUSED(ins); struct uavcan_protocol_NodeStatus nodeStatus; if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { @@ -458,30 +519,29 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { } uint8_t nodeId = transfer->source_node_id; - for (uint8_t i = 0; i < activeNodeCount; i++) { - if (nodeTable[i].nodeID == nodeId) { - // update health, mode, uptime, vendor_status_code, last_seen_ms - nodeTable[i].health = nodeStatus.health; - nodeTable[i].mode = nodeStatus.mode; - nodeTable[i].uptime_sec = nodeStatus.uptime_sec; - nodeTable[i].vendor_status_code = nodeStatus.vendor_specific_status_code; - nodeTable[i].last_seen_ms = millis(); - return; - } + dronecanNodeInfo_t *node = findNodeByID(nodeId); + if (node) { + node->health = nodeStatus.health; + node->mode = nodeStatus.mode; + node->uptime_sec = nodeStatus.uptime_sec; + node->vendor_status_code = nodeStatus.vendor_specific_status_code; + node->last_seen_ms = millis(); + return; } // new node if (activeNodeCount < DRONECAN_MAX_NODES) { + memset(&nodeTable[activeNodeCount], 0, sizeof(dronecanNodeInfo_t)); nodeTable[activeNodeCount].nodeID = nodeId; nodeTable[activeNodeCount].health = nodeStatus.health; nodeTable[activeNodeCount].mode = nodeStatus.mode; nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec; nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code; - nodeTable[activeNodeCount].name_len = 0; - nodeTable[activeNodeCount].name[0] = 0; nodeTable[activeNodeCount].last_seen_ms = millis(); activeNodeCount++; - } + } else { + LOG_DEBUG(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeId); + } } static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -589,7 +649,11 @@ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) /* This callback is invoked by the library when a new message or request or response is received. */ +#ifdef UNIT_TEST +void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { +#else static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { +#endif // switch on data type ID to pass to the right handler function if (transfer->transfer_type == CanardTransferTypeRequest) { // check if we want to handle a specific service request @@ -600,10 +664,11 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } } } - if (transfer->transfer_type == CanardTransferTypeResponse) { - switch (transfer->data_type_id) { - } - } + + if (transfer->transfer_type == CanardTransferTypeResponse) { + dronecanAsyncHandleServiceResponse(&canard, transfer); + } + if (transfer->transfer_type == CanardTransferTypeBroadcast) { // check if we want to handle a specific broadcast message switch (transfer->data_type_id) { @@ -635,4 +700,5 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } } } + #endif diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index c69981b9692..8722bccb916 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -28,23 +28,100 @@ typedef struct dronecanConfig_s { } dronecanConfig_t; typedef struct dronecanNodeInfo_s { - uint8_t nodeID; - uint8_t health; - uint8_t mode; + uint8_t nodeID; + uint8_t health; + uint8_t mode; uint32_t uptime_sec; uint16_t vendor_status_code; uint32_t last_seen_ms; - uint8_t name_len; - char name[32]; } dronecanNodeInfo_t; -// Wire format for MSP2_INAV_DRONECAN_NODES records (7 bytes each, packed). -typedef struct dronecanNodeStatus_s { - uint8_t nodeID; - uint8_t health; - uint8_t mode; - uint32_t last_seen_ms; -} __attribute__((packed)) dronecanNodeStatus_t; +typedef enum { + DRONECAN_ASYNC_IDLE = 0, + DRONECAN_ASYNC_PENDING, + DRONECAN_ASYNC_READY, + DRONECAN_ASYNC_ERROR, +} dronecanAsyncState_e; + +#define DRONECAN_SERVICE_GETNODEINFO 1 +#define DRONECAN_SERVICE_RESTART_NODE 5 +#define DRONECAN_SERVICE_EXECUTE_OPCODE 10 +#define DRONECAN_SERVICE_PARAM_GETSET 11 + +#define DRONECAN_ASYNC_TIMEOUT_MS 2000 +#define DRONECAN_NODE_STALE_TIMEOUT_MS 10000 // Remove node from table if no NodeStatus received for this long +#define DRONECAN_STATE_NOT_READY 0xFF // MSP sentinel: bus not in STATE_NORMAL; outside dronecanAsyncState_e range + +#define DRONECAN_PARAM_TYPE_EMPTY 0 +#define DRONECAN_PARAM_TYPE_INT 1 +#define DRONECAN_PARAM_TYPE_FLOAT 2 +#define DRONECAN_PARAM_TYPE_BOOL 3 +#define DRONECAN_PARAM_TYPE_STRING 4 + +typedef struct dronecanParamRequest_s { + uint16_t index; + uint8_t is_write; + uint8_t value_type; + int64_t value_int; + float value_float; + uint8_t value_bool; + uint8_t value_str_len; + char value_str[128]; + uint8_t req_name_len; + char req_name[92]; +} dronecanParamRequest_t; + +typedef struct dronecanGetNodeInfoResult_s { + uint8_t sw_major; + uint8_t sw_minor; + uint8_t sw_optional_field_flags; + uint32_t sw_vcs_commit; + uint8_t hw_major; + uint8_t hw_minor; + uint8_t hw_unique_id[16]; + uint8_t name_len; + char name[81]; // 80 bytes max + null terminator +} dronecanGetNodeInfoResult_t; + +typedef struct dronecanParamResult_s { + uint8_t type; + int64_t value_int; + float value_float; + uint8_t value_bool; + uint8_t value_str_len; + char value_str[128]; + uint8_t name_len; + char name[93]; // 92 bytes max per UAVCAN param.GetSet DSDL + null terminator + // NumericValue range from the GetSet response; DRONECAN_PARAM_TYPE_EMPTY means not provided. + // Only INT and FLOAT variants are valid — BOOL and STRING have no numeric range. + uint8_t min_type; + int64_t min_int; + float min_float; + uint8_t max_type; + int64_t max_int; + float max_float; +} dronecanParamResult_t; + +typedef struct dronecanSimpleResult_s { + bool ok; +} dronecanSimpleResult_t; + +typedef struct dronecanAsyncSlot_s { + dronecanAsyncState_e state; + uint8_t seq; + uint8_t service_id; + uint8_t node_id; + uint8_t transfer_id; + uint32_t requested_at_ms; + union { + dronecanGetNodeInfoResult_t node_info; + dronecanParamResult_t param; + dronecanSimpleResult_t simple; + } result; +} dronecanAsyncSlot_t; + +extern dronecanAsyncSlot_t dronecanAsyncSlot; +bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload); void dronecanInit(void); void dronecanUpdate(timeUs_t currentTimeUs); @@ -54,5 +131,6 @@ uint32_t dronecanGetBitrateKbps(void); const dronecanNodeInfo_t *dronecanGetNode(uint8_t index); uint32_t dronecanGetBusOffCount(void); CanardPoolAllocatorStatistics dronecanGetPoolStats(void); +const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID); PG_DECLARE(dronecanConfig_t, dronecanConfig); diff --git a/src/main/drivers/dronecan/dronecan_async.c b/src/main/drivers/dronecan/dronecan_async.c new file mode 100644 index 00000000000..a6fd03a8467 --- /dev/null +++ b/src/main/drivers/dronecan/dronecan_async.c @@ -0,0 +1,294 @@ +#include "platform.h" +#if defined(USE_DRONECAN) + +#include +#include +#include + +#include "build/atomic.h" + +#include "common/log.h" +#include "common/time.h" +#include "common/utils.h" + +#include "drivers/time.h" +#include "drivers/nvic.h" + +#include "libcanard/canard.h" + +#include + +#include "dronecan.h" +#include "dronecan_async.h" + +extern CanardInstance canard; /* the FC's own canard instance, owned by dronecan.c */ + +dronecanAsyncSlot_t dronecanAsyncSlot = { .state = DRONECAN_ASYNC_IDLE }; + +/* + Send an asynchronous request for data from a dronecan node such as + a configuration parameter or the node info +*/ +bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload) +{ + if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING && + millis() - dronecanAsyncSlot.requested_at_ms < DRONECAN_ASYNC_TIMEOUT_MS) { + return false; + } + + // PARAM_GETSET_REQUEST is the largest payload; zero-init prevents garbage in UAVCAN reserved bits + uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_MAX_SIZE]; + memset(buffer, 0, sizeof(buffer)); + uint16_t len = 0; + uint64_t signature = 0; + const uint8_t *buf_ptr = NULL; + + switch (service_id) { + case DRONECAN_SERVICE_GETNODEINFO: + signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE; + len = 0; + break; + + case DRONECAN_SERVICE_PARAM_GETSET: { + if (!payload) return false; + const dronecanParamRequest_t *req = (const dronecanParamRequest_t *)payload; + struct uavcan_protocol_param_GetSetRequest getset; + memset(&getset, 0, sizeof(getset)); + getset.index = req->index; + if (req->is_write) { + getset.value.union_tag = (enum uavcan_protocol_param_Value_type_t)req->value_type; + switch (req->value_type) { + case DRONECAN_PARAM_TYPE_INT: + getset.value.integer_value = req->value_int; + break; + case DRONECAN_PARAM_TYPE_FLOAT: + getset.value.real_value = req->value_float; + break; + case DRONECAN_PARAM_TYPE_BOOL: + getset.value.boolean_value = req->value_bool; + break; + case DRONECAN_PARAM_TYPE_STRING: { + uint8_t slen = req->value_str_len < sizeof(getset.value.string_value.data) + ? req->value_str_len : sizeof(getset.value.string_value.data); + getset.value.string_value.len = slen; + memcpy(getset.value.string_value.data, req->value_str, slen); + break; + } + default: + getset.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY; + break; + } + } + uint8_t nlen = req->req_name_len < sizeof(getset.name.data) + ? req->req_name_len : sizeof(getset.name.data); + getset.name.len = nlen; + memcpy(getset.name.data, req->req_name, nlen); + len = uavcan_protocol_param_GetSetRequest_encode(&getset, buffer); + buf_ptr = buffer; + signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE; + break; + } + + case DRONECAN_SERVICE_EXECUTE_OPCODE: { + if (!payload) return false; + const uint8_t *opcode = (const uint8_t *)payload; + struct uavcan_protocol_param_ExecuteOpcodeRequest req; + memset(&req, 0, sizeof(req)); + req.opcode = *opcode; + req.argument = 0; + len = uavcan_protocol_param_ExecuteOpcodeRequest_encode(&req, buffer); + buf_ptr = buffer; + signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE; + break; + } + + case DRONECAN_SERVICE_RESTART_NODE: { + struct uavcan_protocol_RestartNodeRequest req; + memset(&req, 0, sizeof(req)); + req.magic_number = UAVCAN_PROTOCOL_RESTARTNODE_REQUEST_MAGIC_NUMBER; + len = uavcan_protocol_RestartNodeRequest_encode(&req, buffer); + buf_ptr = buffer; + signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; + break; + } + + default: + return false; + } + + // buf_ptr remains NULL only for GETNODEINFO (zero-length request); libcanard accepts NULL with len=0 + int16_t res; + ATOMIC_BLOCK(NVIC_PRIO_CAN) { + res = canardRequestOrRespond(&canard, node_id, signature, service_id, + &dronecanAsyncSlot.transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, CanardRequest, + buf_ptr, len); + } + + if (res < 0) { + LOG_WARNING(CAN, "dronecanAsyncRequest: service %u node %u failed: %d", service_id, node_id, res); + return false; + } + + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.seq++; + dronecanAsyncSlot.service_id = service_id; + dronecanAsyncSlot.node_id = node_id; + dronecanAsyncSlot.requested_at_ms = millis(); + return true; +} + +void dronecanAsyncCheckTimeout(void) +{ + // Check for and expire any pending async requests that have timed out. + if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING && + millis() - dronecanAsyncSlot.requested_at_ms >= DRONECAN_ASYNC_TIMEOUT_MS) { + dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR; + } +} + +/* + Handle responses for any pending async service request + (GETNODEINFO, PARAM_GETSET, EXECUTE_OPCODE, RESTART_NODE). + A single handler serialises all on-demand service requests through + one shared slot, avoiding the need for per-service response queues. +*/ +void dronecanAsyncHandleServiceResponse(CanardInstance *ins, CanardRxTransfer *transfer) +{ + UNUSED(ins); + + if (dronecanAsyncSlot.state != DRONECAN_ASYNC_PENDING) // timed out or already received + return; + if (transfer->data_type_id != dronecanAsyncSlot.service_id) // response service_id does not match the pending request + return; + if (transfer->source_node_id != dronecanAsyncSlot.node_id) // response received for different node_id + return; + // UAVCAN requires matching transfer_id to guard against stale frames (e.g. after bus-off recovery). + // canardRequestOrRespond increments the slot's transfer_id after sending, so the in-flight id is (transfer_id-1) mod 32. + if (transfer->transfer_id != ((dronecanAsyncSlot.transfer_id - 1) & 0x1F)) + return; + + switch (dronecanAsyncSlot.service_id) { + case DRONECAN_SERVICE_GETNODEINFO: { + struct uavcan_protocol_GetNodeInfoResponse resp; + if (uavcan_protocol_GetNodeInfoResponse_decode(transfer, &resp)) { + LOG_WARNING(CAN, "GetNodeInfoResponse decode failed"); + dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR; + return; + } + dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info; + uint8_t len = resp.name.len < (sizeof(r->name) - 1) ? resp.name.len : (sizeof(r->name) - 1); + r->name_len = len; + memcpy(r->name, resp.name.data, len); + r->name[len] = '\0'; + r->sw_major = resp.software_version.major; + r->sw_minor = resp.software_version.minor; + r->sw_optional_field_flags = resp.software_version.optional_field_flags; + r->sw_vcs_commit = (resp.software_version.optional_field_flags & + UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT) + ? resp.software_version.vcs_commit : 0; + r->hw_major = resp.hardware_version.major; + r->hw_minor = resp.hardware_version.minor; + memcpy(r->hw_unique_id, resp.hardware_version.unique_id, 16); + dronecanAsyncSlot.state = DRONECAN_ASYNC_READY; + break; + } + + case DRONECAN_SERVICE_PARAM_GETSET: { + struct uavcan_protocol_param_GetSetResponse resp; + if (uavcan_protocol_param_GetSetResponse_decode(transfer, &resp)) { + LOG_WARNING(CAN, "ParamGetSetResponse decode failed"); + dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR; + return; + } + dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + uint8_t name_len = resp.name.len < (sizeof(r->name) - 1) ? resp.name.len : (sizeof(r->name) - 1); + r->name_len = name_len; + memcpy(r->name, resp.name.data, name_len); + r->name[name_len] = '\0'; + r->type = (uint8_t)resp.value.union_tag; + switch (resp.value.union_tag) { + case UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE: + r->value_int = resp.value.integer_value; + break; + case UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE: + r->value_float = resp.value.real_value; + break; + case UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE: + r->value_bool = resp.value.boolean_value; + break; + case UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE: { + uint8_t slen = resp.value.string_value.len < (sizeof(r->value_str) - 1) + ? resp.value.string_value.len : (sizeof(r->value_str) - 1); + r->value_str_len = slen; + memcpy(r->value_str, resp.value.string_value.data, slen); + r->value_str[slen] = '\0'; + break; + } + default: + r->type = DRONECAN_PARAM_TYPE_EMPTY; + break; + } + r->min_type = DRONECAN_PARAM_TYPE_EMPTY; + r->min_int = 0; + r->min_float = 0.0f; + switch (resp.min_value.union_tag) { + case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE: + r->min_type = DRONECAN_PARAM_TYPE_INT; + r->min_int = resp.min_value.integer_value; + break; + case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE: + r->min_type = DRONECAN_PARAM_TYPE_FLOAT; + r->min_float = resp.min_value.real_value; + break; + default: + break; + } + r->max_type = DRONECAN_PARAM_TYPE_EMPTY; + r->max_int = 0; + r->max_float = 0.0f; + switch (resp.max_value.union_tag) { + case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE: + r->max_type = DRONECAN_PARAM_TYPE_INT; + r->max_int = resp.max_value.integer_value; + break; + case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE: + r->max_type = DRONECAN_PARAM_TYPE_FLOAT; + r->max_float = resp.max_value.real_value; + break; + default: + break; + } + dronecanAsyncSlot.state = DRONECAN_ASYNC_READY; + break; + } + + case DRONECAN_SERVICE_EXECUTE_OPCODE: { + struct uavcan_protocol_param_ExecuteOpcodeResponse resp; + if (uavcan_protocol_param_ExecuteOpcodeResponse_decode(transfer, &resp)) { + LOG_WARNING(CAN, "ExecuteOpcodeResponse decode failed"); + dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR; + return; + } + dronecanAsyncSlot.result.simple.ok = resp.ok; + dronecanAsyncSlot.state = DRONECAN_ASYNC_READY; + break; + } + + case DRONECAN_SERVICE_RESTART_NODE: { + struct uavcan_protocol_RestartNodeResponse resp; + if (uavcan_protocol_RestartNodeResponse_decode(transfer, &resp)) { + LOG_WARNING(CAN, "RestartNodeResponse decode failed"); + dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR; + return; + } + dronecanAsyncSlot.result.simple.ok = resp.ok; + dronecanAsyncSlot.state = DRONECAN_ASYNC_READY; + break; + } + + default: + break; + } +} + +#endif // USE_DRONECAN diff --git a/src/main/drivers/dronecan/dronecan_async.h b/src/main/drivers/dronecan/dronecan_async.h new file mode 100644 index 00000000000..39631e496b9 --- /dev/null +++ b/src/main/drivers/dronecan/dronecan_async.h @@ -0,0 +1,16 @@ +#pragma once + +#include "libcanard/canard.h" + +#ifdef USE_DRONECAN + +/* Called from onTransferReceived() for every CanardTransferTypeResponse + frame - matches it against the single in-flight async request slot + (dronecanAsyncSlot, declared in dronecan.h) and decodes the response. */ +void dronecanAsyncHandleServiceResponse(CanardInstance *ins, CanardRxTransfer *transfer); + +/* Called once per dronecanUpdate() tick while in STATE_DRONECAN_NORMAL - + expires a pending request that never got a response. */ +void dronecanAsyncCheckTimeout(void); + +#endif // USE_DRONECAN diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 919665397a3..998c130491f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -70,6 +70,7 @@ #include "fc/control_profile.h" #include "fc/fc_msp.h" #include "fc/fc_msp_box.h" +#include "fc/fc_msp_dronecan.h" #include "fc/firmware_update.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" @@ -147,10 +148,6 @@ #include "hardware_revision.h" #endif -#ifdef USE_DRONECAN -#include "drivers/dronecan/dronecan.h" -#endif - extern timeDelta_t cycleTime; // FIXME dependency on mw.c static const char * const flightControllerIdentifier = INAV_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. @@ -1924,19 +1921,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #ifdef USE_DRONECAN case MSP2_INAV_DRONECAN_NODES: - { - uint8_t count = dronecanGetNodeCount(); - sbufWriteU8(dst, count); - for (uint8_t i = 0; i < count; i++) { - const dronecanNodeInfo_t *node = dronecanGetNode(i); - sbufWriteDataSafe(dst, &(dronecanNodeStatus_t){ - .nodeID = node->nodeID, - .health = node->health, - .mode = node->mode, - .last_seen_ms = millis() - node->last_seen_ms, - }, sizeof(dronecanNodeStatus_t)); - } - } + mspSerializeDronecanNodes(dst); break; #endif @@ -4620,40 +4605,13 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #ifdef USE_DRONECAN - case MSP2_INAV_DRONECAN_NODE_INFO: - { - if (sbufBytesRemaining(src) < 1) { - *ret = MSP_RESULT_ERROR; - break; - } - uint8_t nodeId = sbufReadU8(src); - uint8_t count = dronecanGetNodeCount(); - bool found = false; - for (uint8_t i = 0; i < count; i++) { - const dronecanNodeInfo_t *node = dronecanGetNode(i); - if (node->nodeID == nodeId) { - found = true; - if (sbufBytesRemaining(dst) < 46) { - *ret = MSP_RESULT_ERROR; - break; - } - sbufWriteU8(dst, node->nodeID); - sbufWriteU8(dst, node->health); - sbufWriteU8(dst, node->mode); - sbufWriteU32(dst, node->uptime_sec); - sbufWriteU16(dst, node->vendor_status_code); - sbufWriteU32(dst, millis() - node->last_seen_ms); - sbufWriteU8(dst, node->name_len); - sbufWriteDataSafe(dst, node->name, 32); - found = true; - *ret = MSP_RESULT_ACK; - break; - } - } - if (!found) { - *ret = MSP_RESULT_ERROR; - } - } + case MSP2_INAV_DRONECAN_ASYNC_REQUEST: + mspHandleDronecanAsyncRequest(src, dst, ret); + break; + + case MSP2_INAV_DRONECAN_ASYNC_RESULT: + mspSerializeDronecanAsyncResult(dst); + *ret = MSP_RESULT_ACK; break; #endif diff --git a/src/main/fc/fc_msp_dronecan.c b/src/main/fc/fc_msp_dronecan.c new file mode 100644 index 00000000000..ff53e81b1fb --- /dev/null +++ b/src/main/fc/fc_msp_dronecan.c @@ -0,0 +1,244 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#ifdef USE_DRONECAN + +#include "common/streambuf.h" + +#include "drivers/time.h" + +#include "fc/fc_msp_dronecan.h" + +bool mspParseDronecanParamGetSetRequest(sbuf_t *src, dronecanParamRequest_t *req) +{ + if (sbufBytesRemaining(src) < 3) { // index(2) + is_write(1) minimum + return false; + } + + memset(req, 0, sizeof(*req)); + req->index = sbufReadU16(src); + req->is_write = sbufReadU8(src); + + if (req->is_write) { + if (sbufBytesRemaining(src) < 1) { // value_type + return false; + } + req->value_type = sbufReadU8(src); + switch (req->value_type) { + case DRONECAN_PARAM_TYPE_INT: { + if (sbufBytesRemaining(src) < 8) { + return false; + } + uint64_t tmp; + sbufReadData(src, &tmp, sizeof(tmp)); + sbufAdvance(src, sizeof(tmp)); + req->value_int = (int64_t)tmp; + break; + } + case DRONECAN_PARAM_TYPE_FLOAT: { + if (sbufBytesRemaining(src) < 4) { + return false; + } + uint32_t raw = sbufReadU32(src); + memcpy(&req->value_float, &raw, 4); + break; + } + case DRONECAN_PARAM_TYPE_BOOL: + if (sbufBytesRemaining(src) < 1) { + return false; + } + req->value_bool = sbufReadU8(src); + break; + case DRONECAN_PARAM_TYPE_STRING: + if (sbufBytesRemaining(src) < 1) { + return false; + } + req->value_str_len = sbufReadU8(src); + if (req->value_str_len > sizeof(req->value_str)) { + req->value_str_len = sizeof(req->value_str); + } + if (sbufBytesRemaining(src) < req->value_str_len) { + return false; + } + sbufReadData(src, req->value_str, req->value_str_len); + sbufAdvance(src, req->value_str_len); + break; + default: // includes DRONECAN_PARAM_TYPE_EMPTY, which is nonsensical on a write + return false; + } + } + + if (sbufBytesRemaining(src) >= 1) { + req->req_name_len = sbufReadU8(src); + if (req->req_name_len > sizeof(req->req_name)) { + req->req_name_len = sizeof(req->req_name); + } + if (sbufBytesRemaining(src) < req->req_name_len) { + return false; + } + sbufReadData(src, req->req_name, req->req_name_len); + sbufAdvance(src, req->req_name_len); + } + + return true; +} + +void mspSerializeDronecanNodes(sbuf_t *dst) +{ + uint8_t count = dronecanGetNodeCount(); + sbufWriteU8(dst, count); + for (uint8_t i = 0; i < count; i++) { + const dronecanNodeInfo_t *node = dronecanGetNode(i); + sbufWriteU8(dst, node->nodeID); + sbufWriteU8(dst, node->health); + sbufWriteU8(dst, node->mode); + sbufWriteU32(dst, millis() - node->last_seen_ms); + sbufWriteU32(dst, node->uptime_sec); + sbufWriteU16(dst, node->vendor_status_code); + } +} + +void mspHandleDronecanAsyncRequest(sbuf_t *src, sbuf_t *dst, mspResult_e *ret) +{ + if (sbufBytesRemaining(src) < 3) { + *ret = MSP_RESULT_ERROR; + return; + } + uint8_t service_id = (uint8_t)sbufReadU16(src); // MSP uses u16 for protocol compat; UAVCAN service IDs are 8-bit + uint8_t nodeID = sbufReadU8(src); + + if (dronecanGetState() != STATE_DRONECAN_NORMAL) { + sbufWriteU8(dst, DRONECAN_STATE_NOT_READY); + sbufWriteU8(dst, 0); + *ret = MSP_RESULT_ACK; + return; + } + + bool accepted = false; + if (service_id == DRONECAN_SERVICE_GETNODEINFO) { + accepted = dronecanAsyncRequest(service_id, nodeID, NULL); + } else if (service_id == DRONECAN_SERVICE_PARAM_GETSET) { + dronecanParamRequest_t req; + if (!mspParseDronecanParamGetSetRequest(src, &req)) { + *ret = MSP_RESULT_ERROR; + return; + } + accepted = dronecanAsyncRequest(service_id, nodeID, &req); + } else if (service_id == DRONECAN_SERVICE_EXECUTE_OPCODE) { + if (sbufBytesRemaining(src) < 1) { + *ret = MSP_RESULT_ERROR; + return; + } + uint8_t opcode = sbufReadU8(src); + accepted = dronecanAsyncRequest(service_id, nodeID, &opcode); + } else if (service_id == DRONECAN_SERVICE_RESTART_NODE) { + accepted = dronecanAsyncRequest(service_id, nodeID, NULL); + } + + sbufWriteU8(dst, accepted ? 0 : 1); // 0=accepted, 1=busy or unrecognised service_id + sbufWriteU8(dst, dronecanAsyncSlot.seq); + *ret = MSP_RESULT_ACK; +} + +void mspSerializeDronecanAsyncResult(sbuf_t *dst) +{ + sbufWriteU8(dst, (uint8_t)dronecanAsyncSlot.state); + sbufWriteU8(dst, dronecanAsyncSlot.seq); + sbufWriteU16(dst, dronecanAsyncSlot.service_id); + sbufWriteU8(dst, dronecanAsyncSlot.node_id); + + if (dronecanAsyncSlot.state == DRONECAN_ASYNC_READY) { + switch (dronecanAsyncSlot.service_id) { + case DRONECAN_SERVICE_GETNODEINFO: { + const dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info; + sbufWriteU8(dst, r->name_len); + sbufWriteDataSafe(dst, r->name, r->name_len); + sbufWriteU8(dst, r->sw_major); + sbufWriteU8(dst, r->sw_minor); + sbufWriteU8(dst, r->sw_optional_field_flags); + sbufWriteU32(dst, r->sw_vcs_commit); + sbufWriteU8(dst, r->hw_major); + sbufWriteU8(dst, r->hw_minor); + sbufWriteDataSafe(dst, r->hw_unique_id, 16); + break; + } + case DRONECAN_SERVICE_PARAM_GETSET: { + const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + sbufWriteU8(dst, r->name_len); + sbufWriteDataSafe(dst, r->name, r->name_len); + sbufWriteU8(dst, r->type); + switch (r->type) { + case DRONECAN_PARAM_TYPE_INT: { + uint64_t tmp; + memcpy(&tmp, &r->value_int, sizeof(tmp)); + sbufWriteData(dst, &tmp, sizeof(tmp)); + break; + } + case DRONECAN_PARAM_TYPE_FLOAT: { + uint32_t raw; + memcpy(&raw, &r->value_float, 4); + sbufWriteU32(dst, raw); + break; + } + case DRONECAN_PARAM_TYPE_BOOL: + sbufWriteU8(dst, r->value_bool); + break; + case DRONECAN_PARAM_TYPE_STRING: + sbufWriteU8(dst, r->value_str_len); + sbufWriteDataSafe(dst, r->value_str, r->value_str_len); + break; + default: + break; + } + sbufWriteU8(dst, r->min_type); + if (r->min_type == DRONECAN_PARAM_TYPE_INT) { + uint64_t utmp; + memcpy(&utmp, &r->min_int, sizeof(utmp)); + sbufWriteData(dst, &utmp, sizeof(utmp)); + } else if (r->min_type == DRONECAN_PARAM_TYPE_FLOAT) { + uint32_t raw; + memcpy(&raw, &r->min_float, 4); + sbufWriteU32(dst, raw); + } + sbufWriteU8(dst, r->max_type); + if (r->max_type == DRONECAN_PARAM_TYPE_INT) { + uint64_t utmp; + memcpy(&utmp, &r->max_int, sizeof(utmp)); + sbufWriteData(dst, &utmp, sizeof(utmp)); + } else if (r->max_type == DRONECAN_PARAM_TYPE_FLOAT) { + uint32_t raw; + memcpy(&raw, &r->max_float, 4); + sbufWriteU32(dst, raw); + } + break; + } + case DRONECAN_SERVICE_EXECUTE_OPCODE: + case DRONECAN_SERVICE_RESTART_NODE: + sbufWriteU8(dst, dronecanAsyncSlot.result.simple.ok ? 1 : 0); + break; + } + dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE; + } else if (dronecanAsyncSlot.state == DRONECAN_ASYNC_ERROR) { + dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE; + } +} + +#endif diff --git a/src/main/fc/fc_msp_dronecan.h b/src/main/fc/fc_msp_dronecan.h new file mode 100644 index 00000000000..16acb8301ad --- /dev/null +++ b/src/main/fc/fc_msp_dronecan.h @@ -0,0 +1,49 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include + +#include "platform.h" + +#ifdef USE_DRONECAN + +#include "drivers/dronecan/dronecan.h" +#include "msp/msp.h" + +/* Parses the DRONECAN_SERVICE_PARAM_GETSET body of an + * MSP2_INAV_DRONECAN_ASYNC_REQUEST payload (everything after service_id and + * nodeID) into *req. Returns false, leaving *ret-handling to the caller, if + * the payload is truncated for the declared is_write/value_type/name + * lengths -- never dispatches a request built from a short read. */ +bool mspParseDronecanParamGetSetRequest(sbuf_t *src, dronecanParamRequest_t *req); + +/* MSP2_INAV_DRONECAN_NODES: serializes the DroneCAN node table to dst. */ +void mspSerializeDronecanNodes(sbuf_t *dst); + +/* MSP2_INAV_DRONECAN_ASYNC_REQUEST: parses service_id/nodeID and the + * per-service payload from src, kicks off the async request, and writes the + * accepted/seq reply to dst. Sets *ret on both the error and success paths, + * matching the calling convention of fc_msp.c's command switch. */ +void mspHandleDronecanAsyncRequest(sbuf_t *src, sbuf_t *dst, mspResult_e *ret); + +/* MSP2_INAV_DRONECAN_ASYNC_RESULT: serializes the current dronecanAsyncSlot + * state/result to dst, then clears a READY or ERROR slot back to IDLE. */ +void mspSerializeDronecanAsyncResult(sbuf_t *dst); + +#endif diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index ea2604e28b4..5a9a4db5a5d 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -97,7 +97,8 @@ #define MSP2_INAV_ESC_TELEM 0x2041 #define MSP2_INAV_DRONECAN_NODES 0x2042 -#define MSP2_INAV_DRONECAN_NODE_INFO 0x2043 +#define MSP2_INAV_DRONECAN_ASYNC_REQUEST 0x2043 +#define MSP2_INAV_DRONECAN_ASYNC_RESULT 0x2044 #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 4eb2d0da1a7..1009df17ee5 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -53,6 +53,60 @@ set_property(SOURCE dronecan_messages_unittest.cc PROPERTY extra_includes "../../lib/main/Dronecan/dsdlc_generated/include") set_property(SOURCE dronecan_messages_unittest.cc PROPERTY definitions USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0) +# GetNodeInfo, SoftwareVersion, HardwareVersion, RTCMStream tests +set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY depends + "drivers/dronecan/libcanard/canard.c") +set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY extra_sources + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.SoftwareVersion.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.HardwareVersion.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.NodeStatus.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.RTCMStream.c") +set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY extra_includes + "../../lib/main/Dronecan/dsdlc_generated/include") +set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY definitions USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0) + +# DroneCAN application-layer tests - compiles dronecan.c with INAV stubs. +# UNIT_TEST exposes activeNodeCount and nodeTable as non-static for SetUp reset. +set_property(SOURCE dronecan_application_unittest.cc PROPERTY depends + "drivers/dronecan/dronecan.c" + "drivers/dronecan/dronecan_async.c" + "drivers/dronecan/libcanard/canard.c") +set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.NodeStatus.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.SoftwareVersion.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.HardwareVersion.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix2.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Auxiliary.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.power.BatteryInfo.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.RTCMStream.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.Timestamp.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.ECEFPositionVelocity.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.ExecuteOpcode_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.ExecuteOpcode_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Value.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.NumericValue.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Empty.c") +set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_includes + "../../lib/main/Dronecan/dsdlc_generated/include") +set_property(SOURCE dronecan_application_unittest.cc PROPERTY definitions + USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0 + FC_VERSION_MAJOR=10 FC_VERSION_MINOR=0 FC_VERSION_PATCH_LEVEL=0) + +# PARAM_GETSET MSP payload parser - extracted from fc_msp.c's giant command +# switch (PR #11683 Qodo review Finding 2) specifically so it's unit testable. +set_property(SOURCE fc_msp_dronecan_unittest.cc PROPERTY depends + "fc/fc_msp_dronecan.c" "common/streambuf.c") +set_property(SOURCE fc_msp_dronecan_unittest.cc PROPERTY definitions USE_DRONECAN) + # CAN bit-timing solver tests - links the real, HAL-free timing core shared # by the F7 (bxCAN) and H7 (FDCAN) drivers, so there is nothing to keep in sync set_property(SOURCE bxcan_timing_unittest.cc PROPERTY depends diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc new file mode 100644 index 00000000000..34788606a8d --- /dev/null +++ b/src/test/unit/dronecan_application_unittest.cc @@ -0,0 +1,988 @@ +/** + * DroneCAN Application-Layer Unit Tests + * + * Tests node table management and transfer acceptance filter using the real + * dronecan.c compiled against INAV stubs. The UNIT_TEST build makes + * activeNodeCount and nodeTable non-static so tests can reset state in SetUp. + * + * Coverage: + * GAP-N1 New node ID → added to table; no slot if table full + * GAP-N2 Subsequent NodeStatus from same node → fields updated in place + * GAP-N3 last_seen_ms follows controllable millis() value + * GAP-N4 33rd unique node → table overflow rejected, count stays at 32 + * GAP-S1 shouldAcceptTransfer: NodeStatus ✓, GetNodeInfo request ✓, + * GetNodeInfo response ✓, unknown ID ✗ + */ + +#include "gtest/gtest.h" + +extern "C" { +#include +#include +#include + +#include "platform.h" + +/* DSDL types used by dronecan.c handlers */ +#include "uavcan.protocol.NodeStatus.h" +#include "uavcan.protocol.GetNodeInfo.h" +#include "uavcan.protocol.param.GetSet_res.h" +#include "uavcan.protocol.param.GetSet.h" +#include "uavcan.protocol.param.ExecuteOpcode_res.h" +#include "uavcan.protocol.RestartNode_res.h" + +/* Canard core and STM32 driver declarations */ +#include "drivers/dronecan/libcanard/canard.h" +#include "drivers/dronecan/libcanard/canard_stm32_driver.h" + +/* INAV headers pulled in by dronecan.c — included here so the types are + available when we define stub globals below. */ +#include "io/gps.h" +#include "sensors/battery_sensor_dronecan.h" +#include "fc/runtime_config.h" +#include "sensors/diagnostics.h" +#include "build/version.h" +#include "common/log.h" + +/* Public API we test against */ +#include "drivers/dronecan/dronecan.h" + +/* Private state made non-static in UNIT_TEST builds */ +extern uint8_t activeNodeCount; +extern dronecanNodeInfo_t nodeTable[]; + +/* dronecan.c's module-global CanardInstance used by dronecanUpdate() and + dronecan_async.c (declared non-static there for that reason). Tests that + call dronecanUpdate() directly must initialize this instance themselves; + it is distinct from the local `ins` CanardInstance used by fixtures below + that call onTransferReceived()/handle_NodeStatus() directly. */ +extern CanardInstance canard; + +/* Private functions not exposed in dronecan.h */ +void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer); +bool shouldAcceptTransfer(const CanardInstance *ins, + uint64_t *out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type, + uint8_t source_node_id); +void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer); + +/* ========================================================================= + * Stubs — provide every symbol dronecan.c references that isn't supplied by + * the compiled dependencies (dronecan.c, canard.c, DSDL .c files). + * ========================================================================= */ + +/* Controllable time source */ +static uint32_t mock_time_ms = 0; +uint32_t millis(void) { return mock_time_ms; } + +/* Arming state — dronecan.c reads this for send_NodeStatus vendor code */ +uint32_t armingFlags = 0; + +/* GPS config — provider != GPS_DRONECAN so all GPS handlers return early */ +gpsConfig_t gpsConfig_System; +gpsConfig_t gpsConfig_Copy; + +/* Hardware health — dronecan.c reads this in send_NodeStatus */ +bool isHardwareHealthy(void) { return true; } + +/* Logging — USE_LOG is unconditionally defined by target/common.h (pulled in + via platform.h), so LOG_ERROR/LOG_DEBUG in dronecan.c expand to real _logf() + calls. Stubbed as a no-op rather than linking common/log.c, which would pull + in drivers/serial.h, msp/msp.h, msp/msp_serial.h, fc/config.h and + config/feature.h — unrelated production dependencies this test has no need + for. Tests don't assert on logging output. */ +void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) { (void)topic; (void)level; (void)fmt; } + +/* GPS and battery DroneCAN receive stubs */ +void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix *p) { (void)p; } +void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 *p) { (void)p; } +void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary *p) { (void)p; } +void dronecanBatterySensorReceiveInfo(struct uavcan_equipment_power_BatteryInfo *p) { (void)p; } + +/* STM32 CAN driver stubs */ +int16_t canardSTM32CAN1_Init(uint32_t b) { (void)b; return CANARD_OK; } +uint32_t canardSTM32GetAndClearRxDropCount(void) { return 0; } +int16_t canardSTM32Transmit(const CanardCANFrame *f) { (void)f; return 1; } +void canardSTM32GetProtocolStatus(canardProtocolStatus_t *s) { memset(s, 0, sizeof(*s)); } +void canardSTM32RecoverFromBusOff(void) {} +void canardSTM32GetUniqueID(uint8_t id[16]) { memset(id, 0, 16); } + +/* Controllable mock RX FIFO, used only by tests that call dronecanUpdate() + * directly (currently just DronecanUpdate_TimeoutCheckedAfterRxDrain_ResponseNotDropped). + * Defaults to empty (count == pos == 0), which is observably identical to the + * previous hardcoded "always empty" stubs — canardSTM32GetRxFifoFillLevel() + * returned 0 and canardSTM32Receive() returned 0 either way. No other test in + * this file calls dronecanUpdate(), so none of them touch this queue. */ +static CanardCANFrame mock_rx_queue[4]; +static int mock_rx_queue_count = 0; +static int mock_rx_queue_pos = 0; +int32_t canardSTM32GetRxFifoFillLevel(void) { return mock_rx_queue_count - mock_rx_queue_pos; } +int16_t canardSTM32Receive(CanardCANFrame *f) { + if (mock_rx_queue_pos >= mock_rx_queue_count) { + return 0; + } + *f = mock_rx_queue[mock_rx_queue_pos++]; + return 1; +} + +/* Version strings declared in build/version.h */ +const char* const shortGitRevision = "00000000"; +const char* const compilerVersion = "test"; +const char* const targetName = "TEST"; +const char* const buildDate = "Jan 01 2026"; +const char* const buildTime = "00:00:00"; + +} /* extern "C" */ + +/* ========================================================================= + * Helper: encode a NodeStatus and build a single-frame CanardRxTransfer. + * buf must be at least UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE bytes. + * ========================================================================= */ +static CanardRxTransfer makeNodeStatusTransfer( + uint8_t nodeId, + uint32_t uptime_sec, + uint8_t health, + uint8_t mode, + uint16_t vendor_code, + uint8_t *buf) +{ + struct uavcan_protocol_NodeStatus ns; + memset(&ns, 0, sizeof(ns)); + ns.uptime_sec = uptime_sec; + ns.health = health; + ns.mode = mode; + ns.vendor_specific_status_code = vendor_code; + + uint32_t len = uavcan_protocol_NodeStatus_encode(&ns, buf); + + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeBroadcast; + xfer.data_type_id = UAVCAN_PROTOCOL_NODESTATUS_ID; + xfer.source_node_id = nodeId; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + +/* ========================================================================= + * Helpers: encode response structs and build CanardRxTransfer objects. + * ========================================================================= */ + +static CanardRxTransfer makeParamGetSetTransfer( + uint8_t source_node_id, uint8_t transfer_id, + struct uavcan_protocol_param_GetSetResponse *resp, + uint8_t *buf) +{ + uint32_t len = uavcan_protocol_param_GetSetResponse_encode(resp, buf); + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_ID; + xfer.source_node_id = source_node_id; + xfer.transfer_id = transfer_id; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + +static CanardRxTransfer makeExecuteOpcodeTransfer( + uint8_t source_node_id, uint8_t transfer_id, + bool ok, uint8_t *buf) +{ + struct uavcan_protocol_param_ExecuteOpcodeResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.ok = ok; + uint32_t len = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&resp, buf); + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_ID; + xfer.source_node_id = source_node_id; + xfer.transfer_id = transfer_id; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + +static CanardRxTransfer makeRestartNodeTransfer( + uint8_t source_node_id, uint8_t transfer_id, + bool ok, uint8_t *buf) +{ + struct uavcan_protocol_RestartNodeResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.ok = ok; + uint32_t len = uavcan_protocol_RestartNodeResponse_encode(&resp, buf); + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_ID; + xfer.source_node_id = source_node_id; + xfer.transfer_id = transfer_id; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + +/* ========================================================================= + * Node table tests (GAP-N1 … GAP-N4) + * ========================================================================= */ + +class DroneCANNodeTableTest : public ::testing::Test { +protected: + CanardInstance ins; + uint8_t memory_pool[4096]; /* generous pool: 32 nodes × 1 frame each */ + uint8_t buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4]; + + void SetUp() override { + activeNodeCount = 0; + memset(nodeTable, 0, sizeof(dronecanNodeInfo_t) * DRONECAN_MAX_NODES); + mock_time_ms = 0; + canardInit(&ins, memory_pool, sizeof(memory_pool), + onTransferReceived, shouldAcceptTransfer, NULL); + canardSetLocalNodeID(&ins, 1); /* FC node ID required for canardRequestOrRespond */ + } +}; + +/* GAP-N1: First NodeStatus from an unseen node ID → entry added to table */ +TEST_F(DroneCANNodeTableTest, NewNodeAddedOnFirstStatus) +{ + ASSERT_EQ(dronecanGetNodeCount(), 0u); + + CanardRxTransfer xfer = makeNodeStatusTransfer( + 10, 100, + UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK, + UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL, + 0xABCD, buf); + handle_NodeStatus(&ins, &xfer); + + EXPECT_EQ(dronecanGetNodeCount(), 1u); + + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->nodeID, 10u); + EXPECT_EQ(node->health, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK); + EXPECT_EQ(node->mode, UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL); + EXPECT_EQ(node->uptime_sec, 100u); + EXPECT_EQ(node->vendor_status_code, 0xABCDu); +} + +/* GAP-N1 (second node): Two distinct IDs → two separate entries */ +TEST_F(DroneCANNodeTableTest, TwoDistinctNodesStoredSeparately) +{ + CanardRxTransfer x1 = makeNodeStatusTransfer(10, 100, 0, 0, 0, buf); + handle_NodeStatus(&ins, &x1); + CanardRxTransfer x2 = makeNodeStatusTransfer(20, 200, 0, 0, 0, buf); + handle_NodeStatus(&ins, &x2); + + EXPECT_EQ(dronecanGetNodeCount(), 2u); + EXPECT_EQ(dronecanGetNode(0)->nodeID, 10u); + EXPECT_EQ(dronecanGetNode(1)->nodeID, 20u); +} + +/* GAP-N2: Second NodeStatus from the same node → fields updated, no new entry */ +TEST_F(DroneCANNodeTableTest, ExistingNodeUpdatedInPlace) +{ + CanardRxTransfer x1 = makeNodeStatusTransfer( + 10, 100, + UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK, + UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL, + 0x0000, buf); + handle_NodeStatus(&ins, &x1); + ASSERT_EQ(dronecanGetNodeCount(), 1u); + + CanardRxTransfer x2 = makeNodeStatusTransfer( + 10, 500, + UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING, + UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE, + 0xBEEF, buf); + handle_NodeStatus(&ins, &x2); + + EXPECT_EQ(dronecanGetNodeCount(), 1u); /* still one node */ + + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->health, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING); + EXPECT_EQ(node->mode, UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE); + EXPECT_EQ(node->uptime_sec, 500u); + EXPECT_EQ(node->vendor_status_code, 0xBEEFu); +} + +/* GAP-N3: last_seen_ms is set from millis() at the time of each call */ +TEST_F(DroneCANNodeTableTest, LastSeenMsFollowsMillis) +{ + mock_time_ms = 1000; + CanardRxTransfer x1 = makeNodeStatusTransfer(20, 10, 0, 0, 0, buf); + handle_NodeStatus(&ins, &x1); + + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->last_seen_ms, 1000u); + + mock_time_ms = 2500; + CanardRxTransfer x2 = makeNodeStatusTransfer(20, 20, 0, 0, 0, buf); + handle_NodeStatus(&ins, &x2); + + EXPECT_EQ(node->last_seen_ms, 2500u); +} + +/* GAP-N3: last_seen_ms for a new node also uses current millis() */ +TEST_F(DroneCANNodeTableTest, LastSeenMsSetOnInsert) +{ + mock_time_ms = 9999; + CanardRxTransfer xfer = makeNodeStatusTransfer(5, 0, 0, 0, 0, buf); + handle_NodeStatus(&ins, &xfer); + + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->last_seen_ms, 9999u); +} + +/* GAP-N4: Fill the table to DRONECAN_MAX_NODES, then a 33rd node is silently + dropped — count stays at 32 and the overflow ID is not present. */ +TEST_F(DroneCANNodeTableTest, TableFullNodeRejected) +{ + for (uint8_t i = 1; i <= DRONECAN_MAX_NODES; i++) { + CanardRxTransfer xfer = makeNodeStatusTransfer(i, 0, 0, 0, 0, buf); + handle_NodeStatus(&ins, &xfer); + } + ASSERT_EQ(dronecanGetNodeCount(), (uint8_t)DRONECAN_MAX_NODES); + + /* Try to add a 33rd node (ID 100, not in 1..32) */ + CanardRxTransfer overflow = makeNodeStatusTransfer(100, 0, 0, 0, 0, buf); + handle_NodeStatus(&ins, &overflow); + + EXPECT_EQ(dronecanGetNodeCount(), (uint8_t)DRONECAN_MAX_NODES); + + for (uint8_t i = 0; i < DRONECAN_MAX_NODES; i++) { + const dronecanNodeInfo_t *n = dronecanGetNode(i); + ASSERT_NE(n, nullptr); + EXPECT_NE(n->nodeID, 100u) << "overflow node ID 100 should not be in slot " << (int)i; + } +} + +/* GAP-N4 boundary: dronecanGetNode at index == DRONECAN_MAX_NODES returns NULL */ +TEST_F(DroneCANNodeTableTest, GetNodeOutOfBoundsReturnsNull) +{ + EXPECT_EQ(dronecanGetNode(DRONECAN_MAX_NODES), nullptr); + EXPECT_EQ(dronecanGetNode(255), nullptr); +} + +/* ========================================================================= + * shouldAcceptTransfer tests (GAP-S1) + * ========================================================================= */ + +/* shouldAcceptTransfer does not use the CanardInstance — pass NULL. */ + +TEST(DroneCANShouldAcceptTransfer, AcceptsNodeStatusBroadcast) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_NODESTATUS_ID, + CanardTransferTypeBroadcast, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsGetNodeInfoRequest) +{ + /* The FC handles incoming GetNodeInfo requests and sends a response */ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + CanardTransferTypeRequest, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsGetNodeInfoResponse) +{ + /* Phase 3: FC now accepts GetNodeInfo responses so handle_GetNodeInfoResponse + can populate the node table with name and version data. */ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, RejectsUnknownBroadcastId) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + 0xFFFF, /* not a real UAVCAN data type ID */ + CanardTransferTypeBroadcast, + 42); + + EXPECT_FALSE(accept); +} + +TEST(DroneCANShouldAcceptTransfer, RejectsUnknownResponseId) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + 0xFFFF, + CanardTransferTypeResponse, + 42); + + EXPECT_FALSE(accept); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsParamGetSetResponse) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsExecuteOpcodeResponse) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsRestartNodeResponse) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_SIGNATURE); +} + +/* ========================================================================= + * onTransferReceived dispatch test (GAP-S2) + * + * Verifies that a GetNodeInfo response transfer is dispatched to + * handle_GetNodeInfoResponse and populates the node table entry. + * Written before Phase 4 — fails until the handler is implemented. + * ========================================================================= */ + +class DroneCANDispatchTest : public ::testing::Test { +protected: + CanardInstance ins; + uint8_t memory_pool[4096]; + uint8_t buf[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE + 16]; + + void SetUp() override { + activeNodeCount = 0; + memset(nodeTable, 0, sizeof(dronecanNodeInfo_t) * DRONECAN_MAX_NODES); + memset(&dronecanAsyncSlot, 0, sizeof(dronecanAsyncSlot)); + dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE; + mock_time_ms = 0; + canardInit(&ins, memory_pool, sizeof(memory_pool), + onTransferReceived, shouldAcceptTransfer, NULL); + canardSetLocalNodeID(&ins, 1); + } +}; + +/* GAP-S2: GetNodeInfo response → handler populates async slot result. + * The node table (dronecanNodeInfo_t) holds only NodeStatus-level fields since + * commit 96f8a4bd9 stripped the GetNodeInfo fields to save ~3.5 KB RAM and + * replaced auto-fetch with the on-demand async slot pattern. */ +TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesAsyncSlot) +{ + /* Pre-insert node 42 via a NodeStatus (node table is independent of async slot) */ + uint8_t ns_buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4]; + CanardRxTransfer ns_xfer = makeNodeStatusTransfer(42, 10, 0, 0, 0, ns_buf); + handle_NodeStatus(&ins, &ns_xfer); + ASSERT_EQ(dronecanGetNodeCount(), 1u); + + /* Prime the async slot — handle_AsyncServiceResponse guards on state, service_id, + * node_id, and transfer_id. The guard checks transfer_id == (slot.transfer_id-1)&0x1F, + * so set transfer_id=1 so the expected in-flight id is 0 (matching xfer.transfer_id). */ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_GETNODEINFO; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + /* Build a GetNodeInfo response from node 42 */ + struct uavcan_protocol_GetNodeInfoResponse resp; + memset(&resp, 0, sizeof(resp)); + + resp.status.uptime_sec = 10; + resp.status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + resp.status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + + resp.software_version.major = 1; + resp.software_version.minor = 7; + resp.software_version.optional_field_flags = 1; /* vcs_commit valid */ + resp.software_version.vcs_commit = 0xDEADBEEF; + + resp.hardware_version.major = 2; + resp.hardware_version.minor = 0; + for (int i = 0; i < 16; i++) + resp.hardware_version.unique_id[i] = (uint8_t)(0xA0 + i); + + const char *name = "com.example.gps"; + resp.name.len = (uint8_t)strlen(name); + memcpy(resp.name.data, name, resp.name.len); + + uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&resp, buf); + + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_GETNODEINFO_ID; + xfer.source_node_id = 42; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)encoded_len; + + onTransferReceived(&ins, &xfer); + + /* Slot must now be READY */ + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + + /* Result fields populated from the GetNodeInfo response */ + const dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info; + EXPECT_EQ(r->name_len, (uint8_t)strlen(name)); + EXPECT_EQ(0, memcmp(r->name, name, r->name_len)); + + EXPECT_EQ(r->sw_major, 1u); + EXPECT_EQ(r->sw_minor, 7u); + EXPECT_EQ(r->sw_optional_field_flags, 1u); + EXPECT_EQ(r->sw_vcs_commit, 0xDEADBEEFu); + + EXPECT_EQ(r->hw_major, 2u); + EXPECT_EQ(r->hw_minor, 0u); + for (int i = 0; i < 16; i++) + EXPECT_EQ(r->hw_unique_id[i], (uint8_t)(0xA0 + i)) + << "unique_id mismatch at byte " << i; + + /* Node table entry still exists (populated by the preceding NodeStatus) */ + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->nodeID, 42u); +} + +/* ========================================================================= + * Async service response guard rejection tests (GAP-S3) + * + * handle_AsyncServiceResponse has four guards before decoding the payload. + * Each test confirms a mismatched guard leaves the slot state unchanged. + * ========================================================================= */ + +/* GAP-S3a: Slot in IDLE state → response silently ignored */ +TEST_F(DroneCANDispatchTest, AsyncSlot_IdleState_IgnoresParamGetSetResponse) +{ + /* slot stays IDLE (SetUp default); send a valid PARAM_GETSET response */ + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + resp.value.integer_value = 7; + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_IDLE); +} + +/* GAP-S3b: Slot PENDING but response comes from the wrong node ID */ +TEST_F(DroneCANDispatchTest, AsyncSlot_WrongNodeId_IgnoresResponse) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; /* guard expects in-flight id (1-1)&0x1F = 0 */ + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + + /* source_node_id = 99, not 42 */ + CanardRxTransfer xfer = makeParamGetSetTransfer(99, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING); +} + +/* GAP-S3c: Slot PENDING but transfer_id does not match the in-flight id */ +TEST_F(DroneCANDispatchTest, AsyncSlot_WrongTransferId_IgnoresResponse) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; /* guard expects xfer.transfer_id == 0 */ + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + + /* xfer.transfer_id = 5, which != (1-1)&0x1F = 0 */ + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 5, &resp, buf); + onTransferReceived(&ins, &xfer); + + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING); +} + +/* GAP-S3d: Slot PENDING for GETNODEINFO; a PARAM_GETSET response arrives → + * data_type_id mismatch rejects it before any decode. */ +TEST_F(DroneCANDispatchTest, AsyncSlot_WrongServiceId_IgnoresResponse) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_GETNODEINFO; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + + /* xfer.data_type_id == PARAM_GETSET(11) != slot.service_id(GETNODEINFO=1) */ + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING); +} + +/* ========================================================================= + * PARAM_GETSET response decode tests (GAP-S4) + * ========================================================================= */ + +/* GAP-S4a: Integer value with integer min/max range */ +TEST_F(DroneCANDispatchTest, ParamGetSetIntResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + resp.value.integer_value = 42; + resp.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; + resp.min_value.integer_value = 0; + resp.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; + resp.max_value.integer_value = 100; + const char *name = "MOT_SPIN_MIN"; + resp.name.len = (uint8_t)strlen(name); + memcpy(resp.name.data, name, resp.name.len); + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + EXPECT_EQ(r->type, (uint8_t)DRONECAN_PARAM_TYPE_INT); + EXPECT_EQ(r->value_int, 42); + EXPECT_EQ(r->name_len, (uint8_t)strlen(name)); + EXPECT_EQ(0, memcmp(r->name, name, r->name_len)); + EXPECT_EQ(r->min_type, (uint8_t)DRONECAN_PARAM_TYPE_INT); + EXPECT_EQ(r->min_int, 0); + EXPECT_EQ(r->max_type, (uint8_t)DRONECAN_PARAM_TYPE_INT); + EXPECT_EQ(r->max_int, 100); +} + +/* GAP-S4b: Float value with float min/max range */ +TEST_F(DroneCANDispatchTest, ParamGetSetFloatResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE; + resp.value.real_value = 3.14f; + resp.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE; + resp.min_value.real_value = 0.0f; + resp.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE; + resp.max_value.real_value = 10.0f; + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + EXPECT_EQ(r->type, (uint8_t)DRONECAN_PARAM_TYPE_FLOAT); + EXPECT_FLOAT_EQ(r->value_float, 3.14f); + EXPECT_EQ(r->min_type, (uint8_t)DRONECAN_PARAM_TYPE_FLOAT); + EXPECT_FLOAT_EQ(r->min_float, 0.0f); + EXPECT_EQ(r->max_type, (uint8_t)DRONECAN_PARAM_TYPE_FLOAT); + EXPECT_FLOAT_EQ(r->max_float, 10.0f); +} + +/* GAP-S4c: Boolean value (no numeric range) */ +TEST_F(DroneCANDispatchTest, ParamGetSetBoolResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE; + resp.value.boolean_value = 1; + /* min/max remain EMPTY (memset to 0 = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_EMPTY) */ + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + EXPECT_EQ(r->type, (uint8_t)DRONECAN_PARAM_TYPE_BOOL); + EXPECT_EQ(r->value_bool, 1u); + EXPECT_EQ(r->min_type, (uint8_t)DRONECAN_PARAM_TYPE_EMPTY); + EXPECT_EQ(r->max_type, (uint8_t)DRONECAN_PARAM_TYPE_EMPTY); +} + +/* GAP-S4d: String value */ +TEST_F(DroneCANDispatchTest, ParamGetSetStringResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE; + const char *str = "hello"; + resp.value.string_value.len = (uint8_t)strlen(str); + memcpy(resp.value.string_value.data, str, resp.value.string_value.len); + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + EXPECT_EQ(r->type, (uint8_t)DRONECAN_PARAM_TYPE_STRING); + EXPECT_EQ(r->value_str_len, (uint8_t)strlen(str)); + EXPECT_EQ(0, memcmp(r->value_str, str, r->value_str_len)); +} + +/* GAP-S4e: Empty value (unknown union_tag) → type forced to DRONECAN_PARAM_TYPE_EMPTY */ +TEST_F(DroneCANDispatchTest, ParamGetSetEmptyResponse_SetsEmptyType) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + /* union_tag == 0 == UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY */ + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY; + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + EXPECT_EQ(dronecanAsyncSlot.result.param.type, (uint8_t)DRONECAN_PARAM_TYPE_EMPTY); +} + +/* ========================================================================= + * EXECUTE_OPCODE response decode tests (GAP-S5) + * ========================================================================= */ + +/* GAP-S5a: ok=true */ +TEST_F(DroneCANDispatchTest, ExecuteOpcodeOkResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_EXECUTE_OPCODE; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + CanardRxTransfer xfer = makeExecuteOpcodeTransfer(42, 0, true, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + EXPECT_TRUE(dronecanAsyncSlot.result.simple.ok); +} + +/* GAP-S5b: ok=false */ +TEST_F(DroneCANDispatchTest, ExecuteOpcodeFailResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_EXECUTE_OPCODE; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + CanardRxTransfer xfer = makeExecuteOpcodeTransfer(42, 0, false, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + EXPECT_FALSE(dronecanAsyncSlot.result.simple.ok); +} + +/* ========================================================================= + * RESTART_NODE response decode test (GAP-S6) + * ========================================================================= */ + +/* GAP-S6: ok=true */ +TEST_F(DroneCANDispatchTest, RestartNodeOkResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_RESTART_NODE; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + CanardRxTransfer xfer = makeRestartNodeTransfer(42, 0, true, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + EXPECT_TRUE(dronecanAsyncSlot.result.simple.ok); +} + +/* ========================================================================= + * dronecanAsyncRequest re-entry guard test (GAP-S7) + * ========================================================================= */ + +/* GAP-S7: A second async request is rejected while one is already in flight. + * Uses RESTART_NODE (no null-payload check) so the re-entry guard is the only + * reason dronecanAsyncRequest returns false. Slot PENDING with + * requested_at_ms=0 and mock_time_ms=0 keeps the timeout condition satisfied + * (0 < DRONECAN_ASYNC_TIMEOUT_MS), so the guard fires before touching the bus. */ +TEST_F(DroneCANDispatchTest, AsyncRequest_RejectedWhilePending) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.requested_at_ms = 0; + mock_time_ms = 0; + + EXPECT_FALSE(dronecanAsyncRequest(DRONECAN_SERVICE_RESTART_NODE, 42, nullptr)); + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING); +} + +/* ========================================================================= + * Qodo PR #11683 Finding 1 — timeout-before-drain race + * + * Regression test for dronecanUpdate() itself (not just the handlers it + * calls). The original bug was in the *call order* inside dronecanUpdate()'s + * STATE_DRONECAN_NORMAL case: dronecanAsyncCheckTimeout() ran BEFORE the CAN + * RX FIFO was drained. If a response for a pending async request was already + * sitting in the RX FIFO when the timeout deadline was reached, the timeout + * check flipped the slot to ERROR before the RX-drain loop had a chance to + * process the response, so a legitimately on-time response was silently + * dropped (dronecanAsyncHandleServiceResponse() only accepts slots in the + * PENDING state). + * + * A prior version of this test called dronecanAsyncCheckTimeout() and + * onTransferReceived() directly, in a hardcoded order chosen by the test + * itself — it never called dronecanUpdate() at all, so it could not actually + * validate the production call order in dronecan.c. This version drives the + * real dronecanUpdate() state machine so it fails/passes based on the actual + * order of operations in dronecan.c. + * ========================================================================= */ + +TEST_F(DroneCANDispatchTest, DronecanUpdate_TimeoutCheckedAfterRxDrain_ResponseNotDropped) +{ + /* dronecanUpdate() operates on the module's own global `canard` instance + * (declared non-static in dronecan.c), not the fixture's local `ins` used + * by the direct onTransferReceived() tests elsewhere in this file. + * Initialize it the way dronecanInit() would. */ + static uint8_t canard_memory_pool[4096]; + canardInit(&canard, canard_memory_pool, sizeof(canard_memory_pool), + onTransferReceived, shouldAcceptTransfer, NULL); + canardSetLocalNodeID(&canard, 1); + + /* Ensure this test starts with an empty mock RX queue regardless of + * execution order relative to other tests. */ + mock_rx_queue_count = 0; + mock_rx_queue_pos = 0; + + /* Prime the async slot as if a PARAM_GETSET request is in flight to node 42. */ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; /* in-flight id expected by the guard: (1-1)&0x1F = 0 */ + dronecanAsyncSlot.requested_at_ms = 0; + + /* Build a real, correctly-encoded response frame using canard's own + * encoder from a throwaway "peer" CanardInstance representing node 42 + * responding to node 1 — this avoids hand-rolling the UAVCAN extended CAN + * ID bit layout (priority/data_type_id/transfer_type/dest/src bits). */ + CanardInstance peer_ins; + uint8_t peer_memory_pool[1024]; + canardInit(&peer_ins, peer_memory_pool, sizeof(peer_memory_pool), NULL, NULL, NULL); + canardSetLocalNodeID(&peer_ins, 42); + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + resp.value.integer_value = 7; + uint8_t payload[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE]; + uint32_t payload_len = uavcan_protocol_param_GetSetResponse_encode(&resp, payload); + + /* Response transfer IDs must NOT be altered by canardRequestOrRespond + * (see canard.c canardRequestOrRespondObj: only CanardTransferTypeRequest + * increments inout_transfer_id). Use 0 to match the slot's expected + * in-flight id above. */ + uint8_t peer_transfer_id = 0; + int16_t enq_res = canardRequestOrRespond(&peer_ins, /*destination_node_id=*/1, + UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_ID, + &peer_transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, CanardResponse, + payload, (uint16_t)payload_len); + ASSERT_GT(enq_res, 0) << "failed to encode/enqueue the fake response frame"; + + /* The GetSetResponse payload (12 bytes here) exceeds a single CAN frame's + * 7-byte capacity (8 bytes minus the tail byte), so canard splits it into + * a multi-frame transfer. Drain every frame libcanard queued for the + * peer into dronecanUpdate()'s mocked RX FIFO, in order, so its real + * RX-drain loop (canardSTM32GetRxFifoFillLevel/canardSTM32Receive) + * reconstructs the complete transfer before dispatching it. */ + const CanardCANFrame *queued; + while ((queued = canardPeekTxQueue(&peer_ins)) != nullptr) { + ASSERT_LT(mock_rx_queue_count, (int)(sizeof(mock_rx_queue) / sizeof(mock_rx_queue[0]))) + << "mock_rx_queue too small for this transfer's frame count"; + mock_rx_queue[mock_rx_queue_count++] = *queued; + canardPopTxQueue(&peer_ins); + } + ASSERT_GT(mock_rx_queue_count, 0) << "no frames were queued for the fake response"; + mock_rx_queue_pos = 0; + + /* First call: STATE_DRONECAN_INIT -> STATE_DRONECAN_NORMAL transition. + * This branch does not touch the RX queue or the async slot at all, so + * it's safe to call before setting up the "real" scenario timing. */ + mock_time_ms = 0; + dronecanUpdate(0); + + /* Second call is the one under test: the response frame is already + * queued, and mock_time_ms is set exactly at the timeout deadline for the + * pending request — the exact race window Qodo Finding 1 describes. + * currentTimeUs=1000 is far below next_1hz_service_at (set to 1,000,000 + * by the first call), so this stays in the STATE_DRONECAN_NORMAL branch + * without also triggering process1HzTasks(). */ + mock_time_ms = DRONECAN_ASYNC_TIMEOUT_MS; + dronecanUpdate(1000); + + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY) + << "response arrived within DRONECAN_ASYNC_TIMEOUT_MS but was dropped " + "because dronecanAsyncCheckTimeout() ran before the RX frame was " + "processed (PR #11683 Qodo Finding 1)"; + const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + EXPECT_EQ(r->type, (uint8_t)DRONECAN_PARAM_TYPE_INT); + EXPECT_EQ(r->value_int, 7); +} diff --git a/src/test/unit/dronecan_getnodeinfo_unittest.cc b/src/test/unit/dronecan_getnodeinfo_unittest.cc new file mode 100644 index 00000000000..f9a8e5fa7ea --- /dev/null +++ b/src/test/unit/dronecan_getnodeinfo_unittest.cc @@ -0,0 +1,362 @@ +/** + * DroneCAN GetNodeInfo and Service Message Unit Tests + * + * Covers coverage gaps identified in audit 2026-06-01: + * GAP-D1 GetNodeInfoResponse encode/decode round-trip + * GAP-D2 RTCMStream encode/decode + * GAP-D3 SoftwareVersion optional_field_flags wire behaviour + * (vcs_commit/image_crc are ALWAYS encoded; flags are app-level hint) + * + * Node table logic (GAP-N1..N4), MSP byte-layout (GAP-M1..M2), and + * shouldAcceptTransfer dispatch (GAP-S1..S3) require dronecan.c to be + * compiled with mocked INAV dependencies. That infrastructure belongs in a + * separate dronecan_application_unittest.cc — tracked in the project todo. + */ + +#include +#include + +extern "C" { +#include "drivers/dronecan/libcanard/canard.h" +#include "uavcan.protocol.GetNodeInfo.h" +#include "uavcan.protocol.GetNodeInfo_res.h" +#include "uavcan.protocol.GetNodeInfo_req.h" +#include "uavcan.protocol.SoftwareVersion.h" +#include "uavcan.protocol.HardwareVersion.h" +#include "uavcan.protocol.NodeStatus.h" +#include "uavcan.equipment.gnss.RTCMStream.h" +} + +#include "gtest/gtest.h" + +class DroneCANGetNodeInfoTest : public ::testing::Test { +protected: + void SetUp() override { + memset(buffer, 0, sizeof(buffer)); + } + + CanardRxTransfer makeTransfer(uint32_t len) { + CanardRxTransfer transfer; + memset(&transfer, 0, sizeof(transfer)); + transfer.payload_len = len; + transfer.payload_head = buffer; + transfer.payload_middle = NULL; + transfer.payload_tail = NULL; + return transfer; + } + + // Buffer large enough for the largest GetNodeInfo response (377 bytes). + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE + 16]; +}; + +// =========================================================================== +// GetNodeInfoResponse encode/decode (GAP-D1) +// =========================================================================== + +TEST_F(DroneCANGetNodeInfoTest, GetNodeInfoResponse_RoundTrip) +{ + struct uavcan_protocol_GetNodeInfoResponse tx; + memset(&tx, 0, sizeof(tx)); + + // NodeStatus + tx.status.uptime_sec = 12345; + tx.status.health = 1; // WARNING + tx.status.mode = 0; // OPERATIONAL + tx.status.vendor_specific_status_code = 0xABCD; + + // SoftwareVersion + tx.software_version.major = 1; + tx.software_version.minor = 7; + tx.software_version.optional_field_flags = 1; // vcs_commit valid + tx.software_version.vcs_commit = 0xDEADBEEF; + tx.software_version.image_crc = 0; // not flagged + + // HardwareVersion + tx.hardware_version.major = 2; + tx.hardware_version.minor = 0; + for (int i = 0; i < 16; i++) { + tx.hardware_version.unique_id[i] = (uint8_t)(0x10 + i); + } + tx.hardware_version.certificate_of_authenticity.len = 0; + + // Name + const char *name = "com.example.sensor"; + tx.name.len = (uint8_t)strlen(name); + memcpy(tx.name.data, name, tx.name.len); + + uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&tx, buffer); + + EXPECT_GT(encoded_len, 0u); + EXPECT_LE(encoded_len, (uint32_t)UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_GetNodeInfoResponse rx; + memset(&rx, 0, sizeof(rx)); + bool decode_failed = uavcan_protocol_GetNodeInfoResponse_decode(&transfer, &rx); + + EXPECT_FALSE(decode_failed); + + EXPECT_EQ(rx.status.uptime_sec, tx.status.uptime_sec); + EXPECT_EQ(rx.status.health, tx.status.health); + EXPECT_EQ(rx.status.mode, tx.status.mode); + EXPECT_EQ(rx.status.vendor_specific_status_code, tx.status.vendor_specific_status_code); + + EXPECT_EQ(rx.software_version.major, tx.software_version.major); + EXPECT_EQ(rx.software_version.minor, tx.software_version.minor); + EXPECT_EQ(rx.software_version.optional_field_flags, tx.software_version.optional_field_flags); + EXPECT_EQ(rx.software_version.vcs_commit, tx.software_version.vcs_commit); + + EXPECT_EQ(rx.hardware_version.major, tx.hardware_version.major); + EXPECT_EQ(rx.hardware_version.minor, tx.hardware_version.minor); + for (int i = 0; i < 16; i++) { + EXPECT_EQ(rx.hardware_version.unique_id[i], tx.hardware_version.unique_id[i]) + << "unique_id mismatch at byte " << i; + } + + EXPECT_EQ(rx.name.len, tx.name.len); + EXPECT_EQ(0, memcmp(rx.name.data, tx.name.data, tx.name.len)); +} + +TEST_F(DroneCANGetNodeInfoTest, GetNodeInfoResponse_EmptyName) +{ + // TAO-encoded name length is inferred from remaining payload when len=0. + // A zero-length name must decode without error and name.len must be 0. + struct uavcan_protocol_GetNodeInfoResponse tx; + memset(&tx, 0, sizeof(tx)); + tx.name.len = 0; + + uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&tx, buffer); + EXPECT_GT(encoded_len, 0u); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_GetNodeInfoResponse rx; + memset(&rx, 0xFF, sizeof(rx)); + bool decode_failed = uavcan_protocol_GetNodeInfoResponse_decode(&transfer, &rx); + + EXPECT_FALSE(decode_failed); + EXPECT_EQ(rx.name.len, 0u); +} + +TEST_F(DroneCANGetNodeInfoTest, GetNodeInfoResponse_MaxLengthName) +{ + struct uavcan_protocol_GetNodeInfoResponse tx; + memset(&tx, 0, sizeof(tx)); + tx.name.len = 80; + for (int i = 0; i < 80; i++) { + tx.name.data[i] = (uint8_t)('a' + (i % 26)); + } + + uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&tx, buffer); + EXPECT_LE(encoded_len, (uint32_t)UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_GetNodeInfoResponse rx; + memset(&rx, 0, sizeof(rx)); + bool decode_failed = uavcan_protocol_GetNodeInfoResponse_decode(&transfer, &rx); + + EXPECT_FALSE(decode_failed); + EXPECT_EQ(rx.name.len, 80u); + EXPECT_EQ(0, memcmp(rx.name.data, tx.name.data, 80)); +} + +// =========================================================================== +// SoftwareVersion optional_field_flags (GAP-D3) +// +// The DSDL-generated encoder writes vcs_commit and image_crc unconditionally +// (always 15 bytes on the wire). optional_field_flags is an app-level hint +// that tells the receiver which fields are meaningful — it does NOT gate the +// wire encoding. Tests here document this behaviour and ensure that +// handle_GetNodeInfoResponse checks the flag before storing vcs_commit. +// =========================================================================== + +TEST_F(DroneCANGetNodeInfoTest, SoftwareVersion_AlwaysEncodesAllFields) +{ + // Even with flags=0, vcs_commit and image_crc bytes are present on wire. + // Verify that a non-zero vcs_commit set with flags=0 still survives the + // round-trip — the application must use the flag to decide whether to use + // the value, not rely on the decoder zeroing it out. + struct uavcan_protocol_SoftwareVersion tx; + memset(&tx, 0, sizeof(tx)); + tx.major = 3; + tx.minor = 1; + tx.optional_field_flags = 0; // neither field is flagged as valid + tx.vcs_commit = 0xCAFEBABE; // present on wire, but not flagged + tx.image_crc = 0; + + uint32_t encoded_len = uavcan_protocol_SoftwareVersion_encode(&tx, buffer); + EXPECT_GT(encoded_len, 0u); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_SoftwareVersion rx; + memset(&rx, 0, sizeof(rx)); + bool decode_failed = uavcan_protocol_SoftwareVersion_decode(&transfer, &rx); + + EXPECT_FALSE(decode_failed); + EXPECT_EQ(rx.major, tx.major); + EXPECT_EQ(rx.minor, tx.minor); + EXPECT_EQ(rx.optional_field_flags, 0u); + // vcs_commit IS decoded (wire is always 15 bytes) but flags=0 means + // the application must NOT trust it — assert the flag is checked: + EXPECT_EQ(rx.optional_field_flags & 1u, 0u) << "vcs_commit flag must not be set"; +} + +TEST_F(DroneCANGetNodeInfoTest, SoftwareVersion_VCSCommitFlaggedAndValid) +{ + struct uavcan_protocol_SoftwareVersion tx; + memset(&tx, 0, sizeof(tx)); + tx.major = 1; + tx.minor = 5; + tx.optional_field_flags = 1; // VCS_COMMIT valid + tx.vcs_commit = 0xDEADBEEF; + tx.image_crc = 0; + + uint32_t encoded_len = uavcan_protocol_SoftwareVersion_encode(&tx, buffer); + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_SoftwareVersion rx; + memset(&rx, 0, sizeof(rx)); + + EXPECT_FALSE(uavcan_protocol_SoftwareVersion_decode(&transfer, &rx)); + EXPECT_EQ(rx.optional_field_flags & 1u, 1u); + EXPECT_EQ(rx.vcs_commit, 0xDEADBEEFu); +} + +TEST_F(DroneCANGetNodeInfoTest, SoftwareVersion_BothOptionalFieldsFlagged) +{ + struct uavcan_protocol_SoftwareVersion tx; + memset(&tx, 0, sizeof(tx)); + tx.major = 2; + tx.minor = 0; + tx.optional_field_flags = 3; // VCS_COMMIT and IMAGE_CRC both valid + tx.vcs_commit = 0x12345678; + tx.image_crc = 0xABCDEF0123456789ULL; + + uint32_t encoded_len = uavcan_protocol_SoftwareVersion_encode(&tx, buffer); + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_SoftwareVersion rx; + memset(&rx, 0, sizeof(rx)); + + EXPECT_FALSE(uavcan_protocol_SoftwareVersion_decode(&transfer, &rx)); + EXPECT_EQ(rx.optional_field_flags, 3u); + EXPECT_EQ(rx.vcs_commit, 0x12345678u); + EXPECT_EQ(rx.image_crc, 0xABCDEF0123456789ULL); +} + +// =========================================================================== +// HardwareVersion unique_id (part of GAP-D1) +// =========================================================================== + +TEST_F(DroneCANGetNodeInfoTest, HardwareVersion_UniqueIdRoundTrip) +{ + struct uavcan_protocol_HardwareVersion tx; + memset(&tx, 0, sizeof(tx)); + tx.major = 1; + tx.minor = 0; + for (int i = 0; i < 16; i++) { + tx.unique_id[i] = (uint8_t)(0xA0 + i); + } + tx.certificate_of_authenticity.len = 0; + + uint32_t encoded_len = uavcan_protocol_HardwareVersion_encode(&tx, buffer); + EXPECT_GT(encoded_len, 0u); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_HardwareVersion rx; + memset(&rx, 0, sizeof(rx)); + + EXPECT_FALSE(uavcan_protocol_HardwareVersion_decode(&transfer, &rx)); + EXPECT_EQ(rx.major, tx.major); + EXPECT_EQ(rx.minor, tx.minor); + for (int i = 0; i < 16; i++) { + EXPECT_EQ(rx.unique_id[i], tx.unique_id[i]) << "unique_id mismatch at byte " << i; + } + EXPECT_EQ(rx.certificate_of_authenticity.len, 0u); +} + +TEST_F(DroneCANGetNodeInfoTest, HardwareVersion_ZeroUniqueId) +{ + struct uavcan_protocol_HardwareVersion tx; + memset(&tx, 0, sizeof(tx)); + // All unique_id bytes zero — valid for nodes that don't implement unique ID. + + uint32_t encoded_len = uavcan_protocol_HardwareVersion_encode(&tx, buffer); + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_HardwareVersion rx; + memset(&rx, 0xFF, sizeof(rx)); + + EXPECT_FALSE(uavcan_protocol_HardwareVersion_decode(&transfer, &rx)); + for (int i = 0; i < 16; i++) { + EXPECT_EQ(rx.unique_id[i], 0u) << "unique_id byte " << i << " should be zero"; + } +} + +// =========================================================================== +// RTCMStream encode/decode (GAP-D2) +// =========================================================================== + +TEST_F(DroneCANGetNodeInfoTest, RTCMStream_BasicEncodeDecode) +{ + struct uavcan_equipment_gnss_RTCMStream tx; + memset(&tx, 0, sizeof(tx)); + tx.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3; + + const uint8_t payload[] = {0xD3, 0x00, 0x13, 0x3E, 0xD0, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x70}; + tx.data.len = sizeof(payload); + memcpy(tx.data.data, payload, sizeof(payload)); + + uint32_t encoded_len = uavcan_equipment_gnss_RTCMStream_encode(&tx, buffer); + + EXPECT_GT(encoded_len, 0u); + EXPECT_LE(encoded_len, (uint32_t)UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_MAX_SIZE); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_equipment_gnss_RTCMStream rx; + memset(&rx, 0, sizeof(rx)); + + EXPECT_FALSE(uavcan_equipment_gnss_RTCMStream_decode(&transfer, &rx)); + EXPECT_EQ(rx.protocol_id, UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3); + EXPECT_EQ(rx.data.len, tx.data.len); + EXPECT_EQ(0, memcmp(rx.data.data, tx.data.data, tx.data.len)); +} + +TEST_F(DroneCANGetNodeInfoTest, RTCMStream_EmptyPayload) +{ + struct uavcan_equipment_gnss_RTCMStream tx; + memset(&tx, 0, sizeof(tx)); + tx.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM2; + tx.data.len = 0; + + uint32_t encoded_len = uavcan_equipment_gnss_RTCMStream_encode(&tx, buffer); + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_equipment_gnss_RTCMStream rx; + memset(&rx, 0xFF, sizeof(rx)); + + EXPECT_FALSE(uavcan_equipment_gnss_RTCMStream_decode(&transfer, &rx)); + EXPECT_EQ(rx.protocol_id, UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM2); + EXPECT_EQ(rx.data.len, 0u); +} + +// =========================================================================== +// Constants (extend GAP-S2: signatures must match DSDL spec) +// =========================================================================== + +TEST(DroneCANGetNodeInfoConstants, Signatures) +{ + EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, 0xEE468A8121C46A9EULL); + EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE, 0xEE468A8121C46A9EULL); + EXPECT_EQ(UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE, 0x1F56030ECB171501ULL); +} + +TEST(DroneCANGetNodeInfoConstants, IDs) +{ + EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_ID, 1u); + EXPECT_EQ(UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID, 1062u); +} + +TEST(DroneCANGetNodeInfoConstants, MessageSizes) +{ + // Response max size accounts for 80-char name + all nested structs. + EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE, 377); + EXPECT_EQ(UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_MAX_SIZE, 130); +} diff --git a/src/test/unit/fc_msp_dronecan_unittest.cc b/src/test/unit/fc_msp_dronecan_unittest.cc new file mode 100644 index 00000000000..d37f9339691 --- /dev/null +++ b/src/test/unit/fc_msp_dronecan_unittest.cc @@ -0,0 +1,239 @@ +/** + * fc_msp_dronecan unit tests + * + * Covers PR #11683 Qodo review Finding 2: MSP2_INAV_DRONECAN_ASYNC_REQUEST's + * PARAM_GETSET write payload must be rejected, not silently zero-filled and + * dispatched, when truncated for the declared is_write/value_type/name + * lengths. This logic used to live inline in fc_msp.c's giant command + * switch (untestable in isolation); it was extracted into + * mspParseDronecanParamGetSetRequest() specifically so it could be unit + * tested here. + */ + +extern "C" { +#include +#include + +#include "common/streambuf.h" +#include "drivers/dronecan/dronecan.h" +#include "fc/fc_msp_dronecan.h" +} + +#include "gtest/gtest.h" + +/* ========================================================================= + * Stubs — mspSerializeDronecanNodes()/mspHandleDronecanAsyncRequest()/ + * mspSerializeDronecanAsyncResult() (also compiled into this TU, since + * they live in the same file as mspParseDronecanParamGetSetRequest()) + * reference real dronecan.c/drivers/time.c symbols. None of the tests + * below exercise those three functions, so these are link-satisfying + * stubs only, not behavioral fakes -- see dronecan_application_unittest.cc + * for real DroneCAN application-layer test coverage. + * ========================================================================= */ +extern "C" { +uint32_t millis(void) { return 0; } +dronecanAsyncSlot_t dronecanAsyncSlot; +dronecanState_e dronecanGetState(void) { return STATE_DRONECAN_NORMAL; } +uint8_t dronecanGetNodeCount(void) { return 0; } +const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) { (void)index; return nullptr; } +bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload) +{ + (void)service_id; (void)node_id; (void)payload; + return false; +} +} + +class MspDronecanParamGetSetTest : public ::testing::Test { +protected: + uint8_t buf[64]; + sbuf_t sbuf; + dronecanParamRequest_t req; + + void SetUp() override { + memset(buf, 0, sizeof(buf)); + memset(&req, 0xAA, sizeof(req)); // non-zero canary; parser must fully own req on success + sbufInit(&sbuf, buf, buf + sizeof(buf)); + } + + // Switches the write-mode sbuf used to build the payload into read mode. + sbuf_t *reader() { + sbufSwitchToReader(&sbuf, buf); + return &sbuf; + } +}; + +/* ------------------------------------------------------------------------- + * Underflow at the very start (index/is_write themselves truncated) + * ---------------------------------------------------------------------- */ + +TEST_F(MspDronecanParamGetSetTest, TooShortForIndexAndIsWrite_Rejected) +{ + sbufWriteU8(&sbuf, 0); // only 1 byte total, need at least 3 (index u16 + is_write u8) + EXPECT_FALSE(mspParseDronecanParamGetSetRequest(reader(), &req)); +} + +/* ------------------------------------------------------------------------- + * Read requests (is_write == 0) — value bytes are not expected at all + * ---------------------------------------------------------------------- */ + +TEST_F(MspDronecanParamGetSetTest, ReadRequest_NoValueBytesNeeded_Accepted) +{ + sbufWriteU16(&sbuf, 3); // index + sbufWriteU8(&sbuf, 0); // is_write = false + ASSERT_TRUE(mspParseDronecanParamGetSetRequest(reader(), &req)); + EXPECT_EQ(req.index, 3); + EXPECT_EQ(req.is_write, 0); + EXPECT_EQ(req.req_name_len, 0); +} + +/* ------------------------------------------------------------------------- + * INT writes + * ---------------------------------------------------------------------- */ + +TEST_F(MspDronecanParamGetSetTest, IntWrite_Complete_Accepted) +{ + sbufWriteU16(&sbuf, 1); + sbufWriteU8(&sbuf, 1); // is_write = true + sbufWriteU8(&sbuf, DRONECAN_PARAM_TYPE_INT); + uint64_t value = 123456789; + sbufWriteData(&sbuf, &value, sizeof(value)); + + ASSERT_TRUE(mspParseDronecanParamGetSetRequest(reader(), &req)); + EXPECT_EQ(req.value_type, DRONECAN_PARAM_TYPE_INT); + EXPECT_EQ(req.value_int, 123456789); + EXPECT_EQ(req.req_name_len, 0); +} + +TEST_F(MspDronecanParamGetSetTest, IntWrite_Truncated_Rejected) +{ + sbufWriteU16(&sbuf, 1); + sbufWriteU8(&sbuf, 1); + sbufWriteU8(&sbuf, DRONECAN_PARAM_TYPE_INT); + uint32_t half = 0xDEADBEEF; // only 4 of the required 8 value bytes + sbufWriteData(&sbuf, &half, sizeof(half)); + + EXPECT_FALSE(mspParseDronecanParamGetSetRequest(reader(), &req)); +} + +/* ------------------------------------------------------------------------- + * FLOAT writes + * ---------------------------------------------------------------------- */ + +TEST_F(MspDronecanParamGetSetTest, FloatWrite_Complete_Accepted) +{ + sbufWriteU16(&sbuf, 2); + sbufWriteU8(&sbuf, 1); + sbufWriteU8(&sbuf, DRONECAN_PARAM_TYPE_FLOAT); + sbufWriteU32(&sbuf, 0x3F800000); // 1.0f + + ASSERT_TRUE(mspParseDronecanParamGetSetRequest(reader(), &req)); + EXPECT_EQ(req.value_type, DRONECAN_PARAM_TYPE_FLOAT); + EXPECT_FLOAT_EQ(req.value_float, 1.0f); +} + +TEST_F(MspDronecanParamGetSetTest, FloatWrite_Truncated_Rejected) +{ + sbufWriteU16(&sbuf, 2); + sbufWriteU8(&sbuf, 1); + sbufWriteU8(&sbuf, DRONECAN_PARAM_TYPE_FLOAT); + sbufWriteU8(&sbuf, 0); // only 1 of the required 4 value bytes + + EXPECT_FALSE(mspParseDronecanParamGetSetRequest(reader(), &req)); +} + +/* ------------------------------------------------------------------------- + * BOOL writes + * ---------------------------------------------------------------------- */ + +TEST_F(MspDronecanParamGetSetTest, BoolWrite_Complete_Accepted) +{ + sbufWriteU16(&sbuf, 4); + sbufWriteU8(&sbuf, 1); + sbufWriteU8(&sbuf, DRONECAN_PARAM_TYPE_BOOL); + sbufWriteU8(&sbuf, 1); + + ASSERT_TRUE(mspParseDronecanParamGetSetRequest(reader(), &req)); + EXPECT_EQ(req.value_type, DRONECAN_PARAM_TYPE_BOOL); + EXPECT_EQ(req.value_bool, 1); +} + +TEST_F(MspDronecanParamGetSetTest, BoolWrite_Truncated_Rejected) +{ + sbufWriteU16(&sbuf, 4); + sbufWriteU8(&sbuf, 1); + sbufWriteU8(&sbuf, DRONECAN_PARAM_TYPE_BOOL); // no value byte follows at all + + EXPECT_FALSE(mspParseDronecanParamGetSetRequest(reader(), &req)); +} + +/* ------------------------------------------------------------------------- + * STRING writes + * ---------------------------------------------------------------------- */ + +TEST_F(MspDronecanParamGetSetTest, StringWrite_Complete_Accepted) +{ + sbufWriteU16(&sbuf, 5); + sbufWriteU8(&sbuf, 1); + sbufWriteU8(&sbuf, DRONECAN_PARAM_TYPE_STRING); + const char *value = "hello"; + sbufWriteU8(&sbuf, (uint8_t)strlen(value)); + sbufWriteData(&sbuf, value, strlen(value)); + + ASSERT_TRUE(mspParseDronecanParamGetSetRequest(reader(), &req)); + EXPECT_EQ(req.value_type, DRONECAN_PARAM_TYPE_STRING); + EXPECT_EQ(req.value_str_len, strlen(value)); + EXPECT_EQ(0, memcmp(req.value_str, value, strlen(value))); +} + +TEST_F(MspDronecanParamGetSetTest, StringWrite_DeclaredLengthExceedsPayload_Rejected) +{ + sbufWriteU16(&sbuf, 5); + sbufWriteU8(&sbuf, 1); + sbufWriteU8(&sbuf, DRONECAN_PARAM_TYPE_STRING); + sbufWriteU8(&sbuf, 10); // declares 10 bytes of string data... + sbufWriteData(&sbuf, "abc", 3); // ...but only 3 are actually present + + EXPECT_FALSE(mspParseDronecanParamGetSetRequest(reader(), &req)); +} + +/* ------------------------------------------------------------------------- + * value_type == EMPTY on a write is nonsensical + * ---------------------------------------------------------------------- */ + +TEST_F(MspDronecanParamGetSetTest, EmptyTypeOnWrite_Rejected) +{ + sbufWriteU16(&sbuf, 6); + sbufWriteU8(&sbuf, 1); + sbufWriteU8(&sbuf, DRONECAN_PARAM_TYPE_EMPTY); + + EXPECT_FALSE(mspParseDronecanParamGetSetRequest(reader(), &req)); +} + +/* ------------------------------------------------------------------------- + * Trailing param-name field + * ---------------------------------------------------------------------- */ + +TEST_F(MspDronecanParamGetSetTest, NameTruncated_Rejected) +{ + sbufWriteU16(&sbuf, 7); + sbufWriteU8(&sbuf, 1); + sbufWriteU8(&sbuf, DRONECAN_PARAM_TYPE_BOOL); + sbufWriteU8(&sbuf, 1); // valid bool value + sbufWriteU8(&sbuf, 20); // declares a 20-byte name... + sbufWriteData(&sbuf, "short", 5); // ...but only 5 bytes follow + + EXPECT_FALSE(mspParseDronecanParamGetSetRequest(reader(), &req)); +} + +TEST_F(MspDronecanParamGetSetTest, NameComplete_Accepted) +{ + sbufWriteU16(&sbuf, 8); + sbufWriteU8(&sbuf, 0); // read request, no value bytes + const char *name = "MOT_SPIN_MIN"; + sbufWriteU8(&sbuf, (uint8_t)strlen(name)); + sbufWriteData(&sbuf, name, strlen(name)); + + ASSERT_TRUE(mspParseDronecanParamGetSetRequest(reader(), &req)); + EXPECT_EQ(req.req_name_len, strlen(name)); + EXPECT_EQ(0, memcmp(req.req_name, name, strlen(name))); +}