From 2f9af7c161d01f1785ae77cf33631cca4c53bad3 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sun, 16 Aug 2026 18:47:16 -0700 Subject: [PATCH 1/6] feat(dronecan): request and accept GetNodeInfo from a target node Add on-demand GetNodeInfo support: request a target node's software/ hardware version and name over DroneCAN, decode the response into the node table, and expose it to the configurator/CLI via MSP (MSP2_INAV_DRONECAN_NODE_INFO). This is the first on-demand (as opposed to broadcast-only) DroneCAN service request INAV makes - GetNodeInfo uses canardRequestOrRespond() under ATOMIC_BLOCK(NVIC_PRIO_CAN), matching the ISR masking the H7/F7 driver rework established for TX. MSP2_INAV_DRONECAN_NODE_INFO response grew to 71 bytes to carry the decoded version/name fields (docs/msp regenerated to match; MSP2_DRONECAN_NODE_INFO_SIZE replaces an inline field-count literal wherever the response size was checked, including the msp_protocol_v2_inav.h constant's own location, which moved out of fc_msp.c). Also: per-node transfer_id (rather than one shared counter across all nodes) so concurrent/overlapping GetNodeInfo requests to different nodes don't cross-contaminate; node name storage extended to 80 bytes with overflow logging; dronecanGetNodeByID() added to eliminate node-table lookups duplicated across handlers; bus-off recovery now gives up after 50 attempts and enters STATE_DRONECAN_FAILED instead of retrying forever. Full unit test suite passes: GetNodeInfo/SoftwareVersion/HardwareVersion/ RTCMStream response decode, shouldAcceptTransfer dispatch (GAP-S1/S2), and node-table tests. Squashed from the original feature/dronecan-getnodeinfo commit sequence (24 commits - the initial multi-phase implementation, several rounds of code-review fixups, and one rebase-artifact cleanup, "fixup: remove orphaned TX loop and duplicate process1HzTasks from rebase artifact" - into this single commit for a clean PR diff. No functional changes from the squash itself. --- docs/development/msp/README.md | 19 +- docs/development/msp/msp_messages.json | 64 ++- src/main/drivers/dronecan/dronecan.c | 128 ++++- src/main/drivers/dronecan/dronecan.h | 15 +- src/main/fc/fc_msp.c | 47 +- src/main/msp/msp_protocol_v2_inav.h | 7 +- src/test/unit/CMakeLists.txt | 38 ++ .../unit/dronecan_application_unittest.cc | 447 ++++++++++++++++++ .../unit/dronecan_getnodeinfo_unittest.cc | 362 ++++++++++++++ 9 files changed, 1069 insertions(+), 58 deletions(-) create mode 100644 src/test/unit/dronecan_application_unittest.cc create mode 100644 src/test/unit/dronecan_getnodeinfo_unittest.cc diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index fafcf5d4762..56d177ffbad 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -4170,10 +4170,10 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `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. | -**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 7 bytes each: nodeID(1)+health(1)+mode(1)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. 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. ## `MSP2_INAV_DRONECAN_NODE_INFO (8259 / 0x2043)` -**Description:** Returns full status detail for a single DroneCAN node by ID. +**Description:** Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service. **Request Payload:** |Field|C Type|Size (Bytes)|Description| @@ -4188,11 +4188,18 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `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 | +| `elapsed_ms` | `uint32_t` | 4 | ms | Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request) | | `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 | - -**Notes:** Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. +| `name` | `char[80]` | 80 | - | Node name up to 80 bytes, zero-padded | +| `sw_major` | `uint8_t` | 1 | - | Software version major (from GetNodeInfo response) | +| `sw_minor` | `uint8_t` | 1 | - | Software version minor (from GetNodeInfo response) | +| `sw_optional_field_flags` | `uint8_t` | 1 | - | UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid | +| `sw_vcs_commit` | `uint32_t` | 4 | - | Git commit hash (valid when sw_optional_field_flags bit 0 is set) | +| `hw_major` | `uint8_t` | 1 | - | Hardware version major (from GetNodeInfo response) | +| `hw_minor` | `uint8_t` | 1 | - | Hardware version minor (from GetNodeInfo response) | +| `hw_unique_id` | `uint8_t[16]` | 16 | - | 128-bit hardware unique ID (from GetNodeInfo response) | + +**Notes:** Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes. ## `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..d303bc2928b 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9840,7 +9840,7 @@ ] }, "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 7 bytes each: nodeID(1)+health(1)+mode(1)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. 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.", "description": "Returns the list of all detected DroneCAN nodes with their current status." }, "MSP2_INAV_DRONECAN_NODE_INFO": { @@ -9889,10 +9889,10 @@ "units": "" }, { - "name": "last_seen_ms", - "ctype": "uint32_t", - "desc": "FC millisecond timestamp when this node was last seen", - "units": "ms" + "name": "elapsed_ms", + "ctype": "uint32_t", + "desc": "Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request)", + "units": "ms" }, { "name": "name_len", @@ -9902,14 +9902,56 @@ }, { "name": "name", - "ctype": "char[32]", - "desc": "Node name up to 32 bytes, zero-padded", + "ctype": "char[80]", + "desc": "Node name up to 80 bytes, zero-padded", + "units": "" + }, + { + "name": "sw_major", + "ctype": "uint8_t", + "desc": "Software version major (from GetNodeInfo response)", + "units": "" + }, + { + "name": "sw_minor", + "ctype": "uint8_t", + "desc": "Software version minor (from GetNodeInfo response)", + "units": "" + }, + { + "name": "sw_optional_field_flags", + "ctype": "uint8_t", + "desc": "UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid", "units": "" - } - ] + }, + { + "name": "sw_vcs_commit", + "ctype": "uint32_t", + "desc": "Git commit hash (valid when sw_optional_field_flags bit 0 is set)", + "units": "" + }, + { + "name": "hw_major", + "ctype": "uint8_t", + "desc": "Hardware version major (from GetNodeInfo response)", + "units": "" + }, + { + "name": "hw_minor", + "ctype": "uint8_t", + "desc": "Hardware version minor (from GetNodeInfo response)", + "units": "" + }, + { + "name": "hw_unique_id", + "ctype": "uint8_t[16]", + "desc": "128-bit hardware unique ID (from GetNodeInfo response)", + "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." + "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes.", + "description": "Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service." }, "MSP2_INAV_LED_STRIP_CONFIG_EX": { "code": 8264, diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 0f671c2425c..8fa6bd60944 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -41,17 +41,30 @@ 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 ------------------------------------------------------------- @@ -183,11 +196,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 +339,57 @@ 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); +} + +static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_protocol_GetNodeInfoResponse resp; + + if (uavcan_protocol_GetNodeInfoResponse_decode(transfer, &resp)) { + LOG_DEBUG(CAN, "GetNodeInfoResponse decode failed"); + return; + } + + uint8_t nodeID = transfer->source_node_id; + dronecanNodeInfo_t *node = findNodeByID(nodeID); + if (!node) { + LOG_DEBUG(CAN, "GetNodeInfoResponse from unknown node %u", nodeID); + return; + } + + if (transfer->transfer_id != ((node->getNodeInfo_transfer_id - 1) & 0x1F)) { + LOG_DEBUG(CAN, "GetNodeInfoResponse from node %u: stale tid %u", nodeID, transfer->transfer_id); + return; + } + + uint8_t len = resp.name.len < sizeof(node->name) ? resp.name.len : sizeof(node->name); + node->name_len = len; + memcpy(node->name, resp.name.data, len); + + node->sw_major = resp.software_version.major; + node->sw_minor = resp.software_version.minor; + node->sw_optional_field_flags = resp.software_version.optional_field_flags; + node->sw_vcs_commit = (resp.software_version.optional_field_flags & UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT) + ? resp.software_version.vcs_commit : 0; + + node->hw_major = resp.hardware_version.major; + node->hw_minor = resp.hardware_version.minor; + memcpy(node->hw_unique_id, resp.hardware_version.unique_id, sizeof(node->hw_unique_id)); +} +// 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 @@ -390,7 +461,11 @@ 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, @@ -408,8 +483,11 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, } } if (transfer_type == CanardTransferTypeResponse) { - // 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_RESPONSE_SIGNATURE; + return true; + } } } if (transfer_type == CanardTransferTypeBroadcast) { @@ -448,8 +526,11 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, // 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 struct uavcan_protocol_NodeStatus nodeStatus; if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { @@ -458,30 +539,39 @@ 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++; - } + int16_t res; + ATOMIC_BLOCK(NVIC_PRIO_CAN) { + res = canardRequestOrRespond(ins, nodeId, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID, + &nodeTable[activeNodeCount - 1].getNodeInfo_transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, CanardRequest, NULL, 0); + } + if (res < 0) { + LOG_DEBUG(CAN, "GetNodeInfo request failed for node %u: %d", nodeId, res); + } + } 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 +679,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 @@ -602,6 +696,9 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } if (transfer->transfer_type == CanardTransferTypeResponse) { switch (transfer->data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + handle_GetNodeInfoResponse(ins, transfer); + break; } } if (transfer->transfer_type == CanardTransferTypeBroadcast) { @@ -635,4 +732,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..c4029cb0244 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -35,7 +35,19 @@ typedef struct dronecanNodeInfo_s { uint16_t vendor_status_code; uint32_t last_seen_ms; uint8_t name_len; - char name[32]; + char name[80]; + /* Software version (from GetNodeInfo response)*/ + uint8_t sw_major; + uint8_t sw_minor; + uint8_t sw_optional_field_flags; + uint32_t sw_vcs_commit; + /* Hardware version (from GetNodeInfo response)*/ + uint8_t hw_major; + uint8_t hw_minor; + uint8_t hw_unique_id[16]; + /* Canard transfer ID for outgoing GetNodeInfo requests to this node. + * Must be per-node: Canard forbids sharing a counter across different dst_node_id. */ + uint8_t getNodeInfo_transfer_id; } dronecanNodeInfo_t; // Wire format for MSP2_INAV_DRONECAN_NODES records (7 bytes each, packed). @@ -54,5 +66,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/fc/fc_msp.c b/src/main/fc/fc_msp.c index 919665397a3..1f6f627bcdc 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4626,33 +4626,32 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *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; - } + uint8_t nodeID = sbufReadU8(src); + const dronecanNodeInfo_t *node = dronecanGetNodeByID(nodeID); + if (!node) { + *ret = MSP_RESULT_ERROR; + break; } - if (!found) { + if (sbufBytesRemaining(dst) < MSP2_DRONECAN_NODE_INFO_SIZE) { *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, 80); + sbufWriteU8(dst, node->sw_major); + sbufWriteU8(dst, node->sw_minor); + sbufWriteU8(dst, node->sw_optional_field_flags); + sbufWriteU32(dst, node->sw_vcs_commit); + sbufWriteU8(dst, node->hw_major); + sbufWriteU8(dst, node->hw_minor); + sbufWriteDataSafe(dst, node->hw_unique_id, 16); + *ret = MSP_RESULT_ACK; } break; #endif diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index ea2604e28b4..a85c02d1859 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -97,7 +97,12 @@ #define MSP2_INAV_ESC_TELEM 0x2041 #define MSP2_INAV_DRONECAN_NODES 0x2042 -#define MSP2_INAV_DRONECAN_NODE_INFO 0x2043 +#define MSP2_INAV_DRONECAN_NODE_INFO 0x2043 +// MSP2_INAV_DRONECAN_NODE_INFO reply size: +// nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) +// +name_len(1)+name(80)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) +// +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 119 +#define MSP2_DRONECAN_NODE_INFO_SIZE 119 #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..703295793aa 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -53,6 +53,44 @@ 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/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") +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) + # 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..89492d795f2 --- /dev/null +++ b/src/test/unit/dronecan_application_unittest.cc @@ -0,0 +1,447 @@ +/** + * 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" + +/* 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[]; + +/* 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; } +int16_t canardSTM32Receive(CanardCANFrame *f) { (void)f; return 0; } +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)); } +int32_t canardSTM32GetRxFifoFillLevel(void) { return 0; } +void canardSTM32RecoverFromBusOff(void) {} +void canardSTM32GetUniqueID(uint8_t id[16]) { memset(id, 0, 16); } + +/* 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; +} + +/* ========================================================================= + * 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); + EXPECT_EQ(node->name_len, 0u); + EXPECT_EQ(node->name[0], '\0'); +} + +/* 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); +} + +/* ========================================================================= + * 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); + mock_time_ms = 0; + canardInit(&ins, memory_pool, sizeof(memory_pool), + onTransferReceived, shouldAcceptTransfer, NULL); + canardSetLocalNodeID(&ins, 1); + } +}; + +/* GAP-S2: GetNodeInfo response → handler populates name and version fields */ +TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry) +{ + /* Pre-insert node 42 via a NodeStatus so the table has a slot for it */ + 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); + + /* 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); + + /* Verify the node table entry was populated */ + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->nodeID, 42u); + + EXPECT_EQ(node->name_len, (uint8_t)strlen(name)); + EXPECT_EQ(0, memcmp(node->name, name, node->name_len)); + + EXPECT_EQ(node->sw_major, 1u); + EXPECT_EQ(node->sw_minor, 7u); + EXPECT_EQ(node->sw_optional_field_flags, 1u); + EXPECT_EQ(node->sw_vcs_commit, 0xDEADBEEFu); + + EXPECT_EQ(node->hw_major, 2u); + EXPECT_EQ(node->hw_minor, 0u); + for (int i = 0; i < 16; i++) { + EXPECT_EQ(node->hw_unique_id[i], (uint8_t)(0xA0 + i)) + << "unique_id mismatch at byte " << i; + } +} 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); +} From 368c2f998fcb8158d2953d40d351aa5ad68646b9 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sun, 16 Aug 2026 18:49:00 -0700 Subject: [PATCH 2/6] feat(dronecan): on-demand GetSet, ExecuteOpcode, RestartNode via shared async slot Extend on-demand DroneCAN service requests beyond GetNodeInfo to param GetSet, ExecuteOpcode, and RestartNode. All four services now share a single in-flight async request slot (dronecanAsyncSlot) rather than per-service state, since only one on-demand request is ever outstanding at a time in practice: dronecanAsyncRequest() encodes and sends whichever service's request (masked under ATOMIC_BLOCK against the CAN TX ISR), and one response handler decodes whichever service's response arrives, guarded by service_id/node_id/transfer_id matching so a stale or mismatched response can't be misattributed to the wrong in-flight request. A timeout (DRONECAN_ASYNC_TIMEOUT_MS) expires a request that never gets a response, so the slot can't wedge waiting forever. GetSet: full int/float/bool/string value union plus min/max NumericValue range, exposed through MSP so the configurator can read/write a remote node's parameters and see their valid range. ExecuteOpcode/RestartNode: simple ok/fail response, for triggering a remote node's save/erase opcodes or a restart. Full unit test suite passes: response-decode coverage for GetSet (int/float/bool/string/empty), ExecuteOpcode, and RestartNode, plus the async-slot dispatch tests (GAP-S2) updated for the new shared-slot architecture. Squashed from the original feature/dronecan-param-getset commit sequence (20 commits - the initial async-slot/GetSet/ExecuteOpcode/RestartNode implementation plus several rounds of code-review fixups) into this single commit for a clean PR diff. No functional changes from the squash itself. --- docs/development/msp/README.md | 63 +-- docs/development/msp/msp_messages.json | 153 +++--- src/main/drivers/dronecan/dronecan.c | 383 +++++++++++---- src/main/drivers/dronecan/dronecan.h | 113 ++++- src/main/fc/fc_msp.c | 197 ++++++-- src/main/msp/msp_protocol_v2_inav.h | 8 +- src/test/unit/CMakeLists.txt | 11 +- .../unit/dronecan_application_unittest.cc | 446 +++++++++++++++++- 8 files changed, 1108 insertions(+), 266 deletions(-) diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index 56d177ffbad..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,41 +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)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. 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, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service. +## `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 | -| `elapsed_ms` | `uint32_t` | 4 | ms | Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request) | -| `name_len` | `uint8_t` | 1 | - | Length of node name string (0 if unknown) | -| `name` | `char[80]` | 80 | - | Node name up to 80 bytes, zero-padded | -| `sw_major` | `uint8_t` | 1 | - | Software version major (from GetNodeInfo response) | -| `sw_minor` | `uint8_t` | 1 | - | Software version minor (from GetNodeInfo response) | -| `sw_optional_field_flags` | `uint8_t` | 1 | - | UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid | -| `sw_vcs_commit` | `uint32_t` | 4 | - | Git commit hash (valid when sw_optional_field_flags bit 0 is set) | -| `hw_major` | `uint8_t` | 1 | - | Hardware version major (from GetNodeInfo response) | -| `hw_minor` | `uint8_t` | 1 | - | Hardware version minor (from GetNodeInfo response) | -| `hw_unique_id` | `uint8_t[16]` | 16 | - | 128-bit hardware unique ID (from GetNodeInfo response) | +|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. Response is always 119 bytes. +**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 d303bc2928b..196970c0a7e 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9830,128 +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", - "array": true, - "array_size": 0, - "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)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. 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.", - "description": "Returns the list of all detected DroneCAN nodes with their current status." - }, - "MSP2_INAV_DRONECAN_NODE_INFO": { - "code": 8259, - "mspv": 2, - "request": { - "payload": [ - { "name": "nodeID", "ctype": "uint8_t", - "desc": "DroneCAN node ID to query (1-127)", - "units": "" - } - ] - }, - "reply": { - "payload": [ - { - "name": "nodeID", - "ctype": "uint8_t", - "desc": "DroneCAN node ID", - "units": "" - }, + "desc": "[per node] DroneCAN node ID (1-127)", + "units": "", + "array": true, + "array_size": 0 + }, { "name": "health", "ctype": "uint8_t", - "desc": "Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL", + "desc": "[per node] Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL", "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" + "ctype": "uint8_t", + "desc": "[per node] Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE", + "units": "" }, { - "name": "vendor_status_code", - "ctype": "uint16_t", - "desc": "Vendor-specific status code", - "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": "elapsed_ms", + "name": "uptime_sec", "ctype": "uint32_t", - "desc": "Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request)", - "units": "ms" - }, + "desc": "[per node] Node uptime in seconds (from NodeStatus broadcast)", + "units": "s" + }, { - "name": "name_len", - "ctype": "uint8_t", - "desc": "Length of node name string (0 if unknown)", - "units": "" - }, + "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 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_ASYNC_REQUEST": { + "code": 8259, + "mspv": 2, + "request": { + "payload": [ { - "name": "name", - "ctype": "char[80]", - "desc": "Node name up to 80 bytes, zero-padded", + "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": "sw_major", + "name": "nodeID", "ctype": "uint8_t", - "desc": "Software version major (from GetNodeInfo response)", + "desc": "Target DroneCAN node ID (1-127)", "units": "" - }, + } + ] + }, + "reply": { + "payload": [ { - "name": "sw_minor", + "name": "accepted", "ctype": "uint8_t", - "desc": "Software version minor (from GetNodeInfo response)", + "desc": "0=request accepted; 1=busy (slot in use) or unrecognised service_id; 0xFF=bus not in STATE_DRONECAN_NORMAL (not ready)", "units": "" }, { - "name": "sw_optional_field_flags", + "name": "seq", "ctype": "uint8_t", - "desc": "UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid", + "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": "sw_vcs_commit", - "ctype": "uint32_t", - "desc": "Git commit hash (valid when sw_optional_field_flags bit 0 is set)", + "name": "state", + "ctype": "uint8_t", + "desc": "Async slot state: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR", "units": "" }, { - "name": "hw_major", + "name": "seq", "ctype": "uint8_t", - "desc": "Hardware version major (from GetNodeInfo response)", + "desc": "Sequence number matching the originating MSP2_INAV_DRONECAN_ASYNC_REQUEST reply", "units": "" }, { - "name": "hw_minor", - "ctype": "uint8_t", - "desc": "Hardware version minor (from GetNodeInfo response)", + "name": "service_id", + "ctype": "uint16_t", + "desc": "Service ID of the in-flight or just-completed request", "units": "" }, { - "name": "hw_unique_id", - "ctype": "uint8_t[16]", - "desc": "128-bit hardware unique ID (from GetNodeInfo response)", + "name": "node_id", + "ctype": "uint8_t", + "desc": "Node ID of the target", "units": "" } ] }, - "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes.", - "description": "Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service." + "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/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 8fa6bd60944..77ce978fde1 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -41,6 +41,8 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, ); static dronecanState_e dronecanState = STATE_DRONECAN_INIT; +dronecanAsyncSlot_t dronecanAsyncSlot = { .state = DRONECAN_ASYNC_IDLE }; + #ifdef UNIT_TEST uint8_t activeNodeCount = 0; dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; @@ -143,6 +145,12 @@ void dronecanUpdate(timeUs_t currentTimeUs) case STATE_DRONECAN_NORMAL: processCanardTxQueueSafe(); + // 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; + } + for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) { timestamp = millis() * 1000ULL; @@ -352,41 +360,260 @@ const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) { return findNodeByID(nodeID); } -static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_protocol_GetNodeInfoResponse resp; - if (uavcan_protocol_GetNodeInfoResponse_decode(transfer, &resp)) { - LOG_DEBUG(CAN, "GetNodeInfoResponse decode failed"); - return; +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; } - uint8_t nodeID = transfer->source_node_id; - dronecanNodeInfo_t *node = findNodeByID(nodeID); - if (!node) { - LOG_DEBUG(CAN, "GetNodeInfoResponse from unknown node %u", nodeID); - return; + // 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; } - if (transfer->transfer_id != ((node->getNodeInfo_transfer_id - 1) & 0x1F)) { - LOG_DEBUG(CAN, "GetNodeInfoResponse from node %u: stale tid %u", nodeID, transfer->transfer_id); - return; + // 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); } - uint8_t len = resp.name.len < sizeof(node->name) ? resp.name.len : sizeof(node->name); - node->name_len = len; - memcpy(node->name, resp.name.data, len); + if (res < 0) { + LOG_WARNING(CAN, "dronecanAsyncRequest: service %u node %u failed: %d", service_id, node_id, res); + return false; + } - node->sw_major = resp.software_version.major; - node->sw_minor = resp.software_version.minor; - node->sw_optional_field_flags = resp.software_version.optional_field_flags; - node->sw_vcs_commit = (resp.software_version.optional_field_flags & UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT) - ? resp.software_version.vcs_commit : 0; + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.seq++; + dronecanAsyncSlot.service_id = service_id; + dronecanAsyncSlot.node_id = node_id; + dronecanAsyncSlot.requested_at_ms = millis(); + return true; +} - node->hw_major = resp.hardware_version.major; - node->hw_minor = resp.hardware_version.minor; - memcpy(node->hw_unique_id, resp.hardware_version.unique_id, sizeof(node->hw_unique_id)); +/* + 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. +*/ +static void handle_AsyncServiceResponse(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; + } } + // Canard Handlers and Senders @@ -446,6 +673,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 */ @@ -471,57 +708,54 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, 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) { - switch (data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE; - return true; - } - } - } - 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 ) @@ -531,6 +765,7 @@ void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { #else static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { #endif + UNUSED(ins); struct uavcan_protocol_NodeStatus nodeStatus; if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { @@ -559,16 +794,6 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { nodeTable[activeNodeCount].last_seen_ms = millis(); activeNodeCount++; - int16_t res; - ATOMIC_BLOCK(NVIC_PRIO_CAN) { - res = canardRequestOrRespond(ins, nodeId, - UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID, - &nodeTable[activeNodeCount - 1].getNodeInfo_transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, CanardRequest, NULL, 0); - } - if (res < 0) { - LOG_DEBUG(CAN, "GetNodeInfo request failed for node %u: %d", nodeId, res); - } } else { LOG_DEBUG(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeId); } @@ -694,13 +919,11 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } } } - if (transfer->transfer_type == CanardTransferTypeResponse) { - switch (transfer->data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: - handle_GetNodeInfoResponse(ins, transfer); - break; - } - } + + if (transfer->transfer_type == CanardTransferTypeResponse) { + handle_AsyncServiceResponse(&canard, transfer); + } + if (transfer->transfer_type == CanardTransferTypeBroadcast) { // check if we want to handle a specific broadcast message switch (transfer->data_type_id) { diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index c4029cb0244..8722bccb916 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -28,35 +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[80]; - /* Software version (from GetNodeInfo response)*/ - uint8_t sw_major; - uint8_t sw_minor; - uint8_t sw_optional_field_flags; - uint32_t sw_vcs_commit; - /* Hardware version (from GetNodeInfo response)*/ - uint8_t hw_major; - uint8_t hw_minor; - uint8_t hw_unique_id[16]; - /* Canard transfer ID for outgoing GetNodeInfo requests to this node. - * Must be per-node: Canard forbids sharing a counter across different dst_node_id. */ - uint8_t getNodeInfo_transfer_id; } 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); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 1f6f627bcdc..db86f748f34 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1929,12 +1929,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF 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)); + 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); } } break; @@ -4620,37 +4620,176 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #ifdef USE_DRONECAN - case MSP2_INAV_DRONECAN_NODE_INFO: + case MSP2_INAV_DRONECAN_ASYNC_REQUEST: { - if (sbufBytesRemaining(src) < 1) { + if (sbufBytesRemaining(src) < 3) { *ret = MSP_RESULT_ERROR; break; } + 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); - const dronecanNodeInfo_t *node = dronecanGetNodeByID(nodeID); - if (!node) { - *ret = MSP_RESULT_ERROR; + + if (dronecanGetState() != STATE_DRONECAN_NORMAL) { + sbufWriteU8(dst, DRONECAN_STATE_NOT_READY); + sbufWriteU8(dst, 0); + *ret = MSP_RESULT_ACK; break; } - if (sbufBytesRemaining(dst) < MSP2_DRONECAN_NODE_INFO_SIZE) { - *ret = MSP_RESULT_ERROR; - break; + + bool accepted = false; + if (service_id == DRONECAN_SERVICE_GETNODEINFO) { + accepted = dronecanAsyncRequest(service_id, nodeID, NULL); + } else if (service_id == DRONECAN_SERVICE_PARAM_GETSET) { + if (sbufBytesRemaining(src) < 3) { // index(2) + is_write(1) minimum + *ret = MSP_RESULT_ERROR; + break; + } + dronecanParamRequest_t req; + memset(&req, 0, sizeof(req)); + req.index = sbufReadU16(src); + req.is_write = sbufReadU8(src); + if (req.is_write && sbufBytesRemaining(src) >= 1) { + req.value_type = sbufReadU8(src); + switch (req.value_type) { + case DRONECAN_PARAM_TYPE_INT: + if (sbufBytesRemaining(src) >= 8) { + 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) { + uint32_t raw = sbufReadU32(src); + memcpy(&req.value_float, &raw, 4); + } + break; + case DRONECAN_PARAM_TYPE_BOOL: + if (sbufBytesRemaining(src) >= 1) + req.value_bool = sbufReadU8(src); + break; + case DRONECAN_PARAM_TYPE_STRING: + if (sbufBytesRemaining(src) >= 1) { + 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) { + sbufReadData(src, req.value_str, req.value_str_len); + sbufAdvance(src, req.value_str_len); + } + } + break; + } + } + 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) { + sbufReadData(src, req.req_name, req.req_name_len); + sbufAdvance(src, req.req_name_len); + } + } + accepted = dronecanAsyncRequest(service_id, nodeID, &req); + } else if (service_id == DRONECAN_SERVICE_EXECUTE_OPCODE) { + if (sbufBytesRemaining(src) < 1) { + *ret = MSP_RESULT_ERROR; + break; + } + 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; + } + break; + + case MSP2_INAV_DRONECAN_ASYNC_RESULT: + { + 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; } - 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, 80); - sbufWriteU8(dst, node->sw_major); - sbufWriteU8(dst, node->sw_minor); - sbufWriteU8(dst, node->sw_optional_field_flags); - sbufWriteU32(dst, node->sw_vcs_commit); - sbufWriteU8(dst, node->hw_major); - sbufWriteU8(dst, node->hw_minor); - sbufWriteDataSafe(dst, node->hw_unique_id, 16); *ret = MSP_RESULT_ACK; } break; diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index a85c02d1859..5a9a4db5a5d 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -97,12 +97,8 @@ #define MSP2_INAV_ESC_TELEM 0x2041 #define MSP2_INAV_DRONECAN_NODES 0x2042 -#define MSP2_INAV_DRONECAN_NODE_INFO 0x2043 -// MSP2_INAV_DRONECAN_NODE_INFO reply size: -// nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) -// +name_len(1)+name(80)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) -// +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 119 -#define MSP2_DRONECAN_NODE_INFO_SIZE 119 +#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 703295793aa..3a4f3798d9e 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -84,7 +84,16 @@ set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources "../../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.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 diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index 89492d795f2..41ab67b1ffd 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -26,6 +26,9 @@ extern "C" { /* 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.ExecuteOpcode_res.h" +#include "uavcan.protocol.RestartNode_res.h" /* Canard core and STM32 driver declarations */ #include "drivers/dronecan/libcanard/canard.h" @@ -139,6 +142,65 @@ static CanardRxTransfer makeNodeStatusTransfer( 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) * ========================================================================= */ @@ -180,8 +242,6 @@ TEST_F(DroneCANNodeTableTest, NewNodeAddedOnFirstStatus) EXPECT_EQ(node->mode, UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL); EXPECT_EQ(node->uptime_sec, 100u); EXPECT_EQ(node->vendor_status_code, 0xABCDu); - EXPECT_EQ(node->name_len, 0u); - EXPECT_EQ(node->name[0], '\0'); } /* GAP-N1 (second node): Two distinct IDs → two separate entries */ @@ -357,6 +417,45 @@ TEST(DroneCANShouldAcceptTransfer, RejectsUnknownResponseId) 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) * @@ -374,6 +473,8 @@ class DroneCANDispatchTest : public ::testing::Test { 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); @@ -381,15 +482,26 @@ class DroneCANDispatchTest : public ::testing::Test { } }; -/* GAP-S2: GetNodeInfo response → handler populates name and version fields */ -TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry) +/* 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 so the table has a slot for it */ + /* 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)); @@ -405,9 +517,8 @@ TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry) resp.hardware_version.major = 2; resp.hardware_version.minor = 0; - for (int i = 0; i < 16; i++) { + 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); @@ -425,23 +536,318 @@ TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry) onTransferReceived(&ins, &xfer); - /* Verify the node table entry was populated */ + /* 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); +} - EXPECT_EQ(node->name_len, (uint8_t)strlen(name)); - EXPECT_EQ(0, memcmp(node->name, name, node->name_len)); +/* ========================================================================= + * 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. + * ========================================================================= */ - EXPECT_EQ(node->sw_major, 1u); - EXPECT_EQ(node->sw_minor, 7u); - EXPECT_EQ(node->sw_optional_field_flags, 1u); - EXPECT_EQ(node->sw_vcs_commit, 0xDEADBEEFu); +/* 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; - EXPECT_EQ(node->hw_major, 2u); - EXPECT_EQ(node->hw_minor, 0u); - for (int i = 0; i < 16; i++) { - EXPECT_EQ(node->hw_unique_id[i], (uint8_t)(0xA0 + i)) - << "unique_id mismatch at byte " << i; - } + 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); } From b056d2ae74924156d56aedd1125a1aef0e81fec1 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sat, 15 Aug 2026 22:27:59 -0700 Subject: [PATCH 3/6] refactor(dronecan): extract async param/service client into its own module Split dronecanAsyncRequest() and the response handler (GetNodeInfo, ParamGetSet, ExecuteOpcode, RestartNode - a single shared slot serialising all on-demand service requests) out of dronecan.c into dronecan_async.c/.h. dronecanAsyncSlot's definition and the response handler move too; dronecan.h keeps declaring dronecanAsyncRequest()/ dronecanAsyncSlot since fc_msp.c is an external caller of both. dronecan.c's onTransferReceived() now calls dronecanAsyncHandleServiceResponse() (renamed from the static handle_AsyncServiceResponse for external linkage), and STATE_DRONECAN_NORMAL calls the new dronecanAsyncCheckTimeout() instead of carrying the timeout-expiry check inline. Also flips the file-scope `canard` CanardInstance from static to plain external linkage, since dronecan_async.c needs `extern CanardInstance canard` to reach it. (On the branch this was originally authored on, that linkage change had already landed earlier, as part of the DNA server work - rebasing this extraction back to sit directly on param-getset instead means picking it up here.) Cherry-picked from feature/dronecan-actuator-control (original commit e577393b6) onto feature/dronecan-param-getset: this is general dronecan.c restructuring in async-request/GetNodeInfo/ParamGetSet/ ExecuteOpcode/RestartNode territory - this branch's own scope - not actuator-control-specific, so it belongs here rather than riding along with unrelated actuator-output work. Full unit test suite (29 tests in dronecan_application_unittest, full suite otherwise unchanged) passes. SITL builds clean with -Werror. --- src/main/CMakeLists.txt | 2 + src/main/drivers/dronecan/dronecan.c | 266 +------------------ src/main/drivers/dronecan/dronecan_async.c | 294 +++++++++++++++++++++ src/main/drivers/dronecan/dronecan_async.h | 16 ++ src/test/unit/CMakeLists.txt | 1 + 5 files changed, 317 insertions(+), 262 deletions(-) create mode 100644 src/main/drivers/dronecan/dronecan_async.c create mode 100644 src/main/drivers/dronecan/dronecan_async.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index da19602209d..37bcb71a61f 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 diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 77ce978fde1..9fa5a0434c6 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,7 +42,6 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, ); static dronecanState_e dronecanState = STATE_DRONECAN_INIT; -dronecanAsyncSlot_t dronecanAsyncSlot = { .state = DRONECAN_ASYNC_IDLE }; #ifdef UNIT_TEST uint8_t activeNodeCount = 0; @@ -145,11 +145,7 @@ void dronecanUpdate(timeUs_t currentTimeUs) case STATE_DRONECAN_NORMAL: processCanardTxQueueSafe(); - // 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; - } + dronecanAsyncCheckTimeout(); for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) { @@ -360,260 +356,6 @@ const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) { return findNodeByID(nodeID); } - -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; -} - -/* - 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. -*/ -static void handle_AsyncServiceResponse(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; - } -} - // Canard Handlers and Senders @@ -921,7 +663,7 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } if (transfer->transfer_type == CanardTransferTypeResponse) { - handle_AsyncServiceResponse(&canard, transfer); + dronecanAsyncHandleServiceResponse(&canard, transfer); } if (transfer->transfer_type == CanardTransferTypeBroadcast) { 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/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 3a4f3798d9e..80bd535875c 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -71,6 +71,7 @@ set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY definitions USE_DR # 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" From caac38ef3bbc52d0af807ff4e5424ddf1802112f Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 21 Aug 2026 17:51:51 -0700 Subject: [PATCH 4/6] docs(dronecan): document on-demand async service client, fix stale docs Add coverage for the async client added by this branch (GetNodeInfo, ParamGetSet, ExecuteOpcode, RestartNode via a shared request slot) and the MSP2_INAV_DRONECAN_ASYNC_REQUEST/RESULT messages that replace the old 0x2043 NODE_INFO meaning. Also corrects drift found while updating: dronecanNodeInfo_t no longer carries name/name_len, MSP2_INAV_DRONECAN_NODES is 13 bytes/node (not 30) with last_seen_ms as an elapsed delta rather than an absolute timestamp, and the documented settings (dronecan_mode/dronecan_baudrate) don't match the real ones (dronecan_node_id/dronecan_bitrate_kbps). DroneCAN.md's Parameter Get/Set feature entry flips from Planned to Supported, with a new section on the configurator DroneCAN tab workflow, including a note that RestartNode can report failure even when the node restarted successfully (peripheral-side ack-before-reset race, not an INAV bug). --- docs/DroneCAN-Driver.md | 179 +++++++++++++++++++++++++++++++++------- docs/DroneCAN.md | 27 +++++- 2 files changed, 170 insertions(+), 36 deletions(-) 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 From fffef76c86b81df263b1dd34f4f7a6db9b754769 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sat, 22 Aug 2026 09:55:35 -0700 Subject: [PATCH 5/6] fix(dronecan): check async request timeout after draining RX FIFO dronecanUpdate() checked dronecanAsyncCheckTimeout() before processing this tick's received CAN frames, so a response arriving exactly at the timeout deadline could be marked ERROR before it was read, silently discarding an on-time reply. Adds a regression test that drives dronecanUpdate() directly, building a real response frame with libcanard's own encoder from a throwaway peer node, correctly handling the multi-frame transfer the payload needs. Verified red (fails against the old call order) before restoring the fix (green). --- src/main/drivers/dronecan/dronecan.c | 7 +- .../unit/dronecan_application_unittest.cc | 139 +++++++++++++++++- 2 files changed, 142 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 9fa5a0434c6..682423802b9 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -145,8 +145,6 @@ void dronecanUpdate(timeUs_t currentTimeUs) case STATE_DRONECAN_NORMAL: processCanardTxQueueSafe(); - dronecanAsyncCheckTimeout(); - for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) { timestamp = millis() * 1000ULL; @@ -166,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; diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index 41ab67b1ffd..34788606a8d 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -27,6 +27,7 @@ extern "C" { #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" @@ -50,6 +51,13 @@ extern "C" { 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, @@ -94,14 +102,30 @@ void dronecanBatterySensorReceiveInfo(struct uavcan_equipment_power_BatteryInfo /* STM32 CAN driver stubs */ int16_t canardSTM32CAN1_Init(uint32_t b) { (void)b; return CANARD_OK; } -int16_t canardSTM32Receive(CanardCANFrame *f) { (void)f; return 0; } 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)); } -int32_t canardSTM32GetRxFifoFillLevel(void) { return 0; } 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"; @@ -851,3 +875,114 @@ TEST_F(DroneCANDispatchTest, AsyncRequest_RejectedWhilePending) 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); +} From c3bbcc5fe30edc10c9206f48b8e1dcabb7933f6d Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sat, 22 Aug 2026 09:55:43 -0700 Subject: [PATCH 6/6] fix(dronecan): extract DroneCAN MSP handling out of fc_msp.c fc_msp.c's MSP2_INAV_DRONECAN_ASYNC_REQUEST handler skipped each value's byte-read when the payload was too short but dispatched the request anyway, sending a real UAVCAN write with a zeroed value or truncated param name instead of rejecting it. Moves all DroneCAN-specific MSP command handling (PARAM_GETSET request parsing, node-table serialization, async request/result handling) into new files fc_msp_dronecan.c/.h, following the existing fc_msp_box.c precedent for splitting self-contained MSP logic out of this file. Each case in fc_msp.c's command switch is now a single function call. The extracted parser (mspParseDronecanParamGetSetRequest) rejects any truncated read immediately instead of the old behavior of silently proceeding with zeroed defaults. 13 unit tests cover complete and truncated INT/FLOAT/BOOL/STRING values, the EMPTY-on-write case, and truncated/complete name fields. --- src/main/CMakeLists.txt | 2 + src/main/fc/fc_msp.c | 190 +---------------- src/main/fc/fc_msp_dronecan.c | 244 ++++++++++++++++++++++ src/main/fc/fc_msp_dronecan.h | 49 +++++ src/test/unit/CMakeLists.txt | 6 + src/test/unit/fc_msp_dronecan_unittest.cc | 239 +++++++++++++++++++++ 6 files changed, 545 insertions(+), 185 deletions(-) create mode 100644 src/main/fc/fc_msp_dronecan.c create mode 100644 src/main/fc/fc_msp_dronecan.h create mode 100644 src/test/unit/fc_msp_dronecan_unittest.cc diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 37bcb71a61f..8f177ef227e 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -311,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/fc/fc_msp.c b/src/main/fc/fc_msp.c index db86f748f34..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); - 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); - } - } + mspSerializeDronecanNodes(dst); break; #endif @@ -4621,177 +4606,12 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #ifdef USE_DRONECAN case MSP2_INAV_DRONECAN_ASYNC_REQUEST: - { - if (sbufBytesRemaining(src) < 3) { - *ret = MSP_RESULT_ERROR; - break; - } - 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; - break; - } - - bool accepted = false; - if (service_id == DRONECAN_SERVICE_GETNODEINFO) { - accepted = dronecanAsyncRequest(service_id, nodeID, NULL); - } else if (service_id == DRONECAN_SERVICE_PARAM_GETSET) { - if (sbufBytesRemaining(src) < 3) { // index(2) + is_write(1) minimum - *ret = MSP_RESULT_ERROR; - break; - } - dronecanParamRequest_t req; - memset(&req, 0, sizeof(req)); - req.index = sbufReadU16(src); - req.is_write = sbufReadU8(src); - if (req.is_write && sbufBytesRemaining(src) >= 1) { - req.value_type = sbufReadU8(src); - switch (req.value_type) { - case DRONECAN_PARAM_TYPE_INT: - if (sbufBytesRemaining(src) >= 8) { - 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) { - uint32_t raw = sbufReadU32(src); - memcpy(&req.value_float, &raw, 4); - } - break; - case DRONECAN_PARAM_TYPE_BOOL: - if (sbufBytesRemaining(src) >= 1) - req.value_bool = sbufReadU8(src); - break; - case DRONECAN_PARAM_TYPE_STRING: - if (sbufBytesRemaining(src) >= 1) { - 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) { - sbufReadData(src, req.value_str, req.value_str_len); - sbufAdvance(src, req.value_str_len); - } - } - break; - } - } - 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) { - sbufReadData(src, req.req_name, req.req_name_len); - sbufAdvance(src, req.req_name_len); - } - } - accepted = dronecanAsyncRequest(service_id, nodeID, &req); - } else if (service_id == DRONECAN_SERVICE_EXECUTE_OPCODE) { - if (sbufBytesRemaining(src) < 1) { - *ret = MSP_RESULT_ERROR; - break; - } - 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; - } + mspHandleDronecanAsyncRequest(src, dst, ret); break; case MSP2_INAV_DRONECAN_ASYNC_RESULT: - { - 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; - } - *ret = MSP_RESULT_ACK; - } + 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/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 80bd535875c..1009df17ee5 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -101,6 +101,12 @@ 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/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))); +}