diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index fafcf5d4762..6231331dc7a 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -418,7 +418,8 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: [8256 - MSP2_INAV_ESC_RPM](#msp2_inav_esc_rpm) [8257 - MSP2_INAV_ESC_TELEM](#msp2_inav_esc_telem) [8258 - MSP2_INAV_DRONECAN_NODES](#msp2_inav_dronecan_nodes) -[8259 - MSP2_INAV_DRONECAN_NODE_INFO](#msp2_inav_dronecan_node_info) +[8259 - MSP2_INAV_DRONECAN_ASYNC_REQUEST](#msp2_inav_dronecan_async_request) +[8260 - MSP2_INAV_DRONECAN_ASYNC_RESULT](#msp2_inav_dronecan_async_result) [8264 - MSP2_INAV_LED_STRIP_CONFIG_EX](#msp2_inav_led_strip_config_ex) [8265 - MSP2_INAV_SET_LED_STRIP_CONFIG_EX](#msp2_inav_set_led_strip_config_ex) [8266 - MSP2_INAV_FW_APPROACH](#msp2_inav_fw_approach) @@ -4165,34 +4166,49 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Request Payload:** **None** **Reply Payload:** -|Field|C Type|Size (Bytes)|Description| -|---|---|---|---| -| `nodeCount` | `uint8_t` | 1 | Number of detected DroneCAN nodes | -| `nodeData` | `dronecanNodeStatus_t[]` | array | Array of per-node status records, one per detected node. Each record: nodeID(1)+health(1)+mode(1)+last_seen_ms(4) = 7 bytes. Full detail available via MSP2_INAV_DRONECAN_NODE_INFO. | +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `nodeCount` | `uint8_t` | 1 | - | Number of detected DroneCAN nodes | +| `nodeID` | `uint8_t[]` | array | - | [per node] DroneCAN node ID (1-127) | +| `health` | `uint8_t` | 1 | - | [per node] Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL | +| `mode` | `uint8_t` | 1 | - | [per node] Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE | +| `last_seen_ms` | `uint32_t` | 4 | ms | [per node] Milliseconds since this node was last seen (FC-local timestamp delta) | +| `uptime_sec` | `uint32_t` | 4 | s | [per node] Node uptime in seconds (from NodeStatus broadcast) | +| `vendor_status_code` | `uint16_t` | 2 | - | [per node] Vendor-specific status code | -**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4). Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO. +**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 13 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4)+uptime_sec(4)+vendor_status_code(2). Maximum payload 1 + (DRONECAN_MAX_NODES * 13) = 417 bytes. For full node detail (name, SW/HW version, unique ID) use MSP2_INAV_DRONECAN_ASYNC_REQUEST with service_id=DRONECAN_SERVICE_GETNODEINFO(1). -## `MSP2_INAV_DRONECAN_NODE_INFO (8259 / 0x2043)` -**Description:** Returns full status detail for a single DroneCAN node by ID. +## `MSP2_INAV_DRONECAN_ASYNC_REQUEST (8259 / 0x2043)` +**Description:** Initiates an asynchronous DroneCAN service request (GetNodeInfo, ParamGetSet, ExecuteOpcode, RestartNode) to a specific node. Result retrieved via MSP2_INAV_DRONECAN_ASYNC_RESULT. **Request Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| -| `nodeID` | `uint8_t` | 1 | DroneCAN node ID to query (1-127) | +| `service_id` | `uint16_t` | 2 | Service to invoke: 1=GETNODEINFO, 5=RESTART_NODE, 10=EXECUTE_OPCODE, 11=PARAM_GETSET. Transmitted as u16 for MSP alignment; only low 8 bits used. | +| `nodeID` | `uint8_t` | 1 | Target DroneCAN node ID (1-127) | **Reply Payload:** -|Field|C Type|Size (Bytes)|Units|Description| -|---|---|---|---|---| -| `nodeID` | `uint8_t` | 1 | - | DroneCAN node ID | -| `health` | `uint8_t` | 1 | - | Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL | -| `mode` | `uint8_t` | 1 | - | Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE | -| `uptime_sec` | `uint32_t` | 4 | s | Node uptime in seconds | -| `vendor_status_code` | `uint16_t` | 2 | - | Vendor-specific status code | -| `last_seen_ms` | `uint32_t` | 4 | ms | FC millisecond timestamp when this node was last seen | -| `name_len` | `uint8_t` | 1 | - | Length of node name string (0 if unknown) | -| `name` | `char[32]` | 32 | - | Node name up to 32 bytes, zero-padded | +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `accepted` | `uint8_t` | 1 | 0=request accepted; 1=busy (slot in use) or unrecognised service_id; 0xFF=bus not in STATE_DRONECAN_NORMAL (not ready) | +| `seq` | `uint8_t` | 1 | Sequence number; correlate with MSP2_INAV_DRONECAN_ASYNC_RESULT to verify the result belongs to this request | + +**Notes:** Requires `USE_DRONECAN`. Initiates an async DroneCAN service request; poll MSP2_INAV_DRONECAN_ASYNC_RESULT at ~100ms intervals until state=READY(2) or ERROR(3). Only one request in-flight at a time. Service-specific request fields follow the common header in the request payload: EXECUTE_OPCODE appends opcode(u8); PARAM_GETSET appends index(u16)+is_write(u8) and optionally value_type(u8)+value(variable) for writes, then req_name_len(u8)+req_name(bytes) for named lookup. Param value encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data. Requests time out after DRONECAN_ASYNC_TIMEOUT_MS (2000ms). If bus is not in STATE_DRONECAN_NORMAL, returns accepted=0xFF without dispatching. + +## `MSP2_INAV_DRONECAN_ASYNC_RESULT (8260 / 0x2044)` +**Description:** Polls the result of the most recent MSP2_INAV_DRONECAN_ASYNC_REQUEST. Poll at ~100ms intervals until state is READY(2) or ERROR(3). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `state` | `uint8_t` | 1 | Async slot state: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR | +| `seq` | `uint8_t` | 1 | Sequence number matching the originating MSP2_INAV_DRONECAN_ASYNC_REQUEST reply | +| `service_id` | `uint16_t` | 2 | Service ID of the in-flight or just-completed request | +| `node_id` | `uint8_t` | 1 | Node ID of the target | -**Notes:** Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. +**Notes:** Requires `USE_DRONECAN`. When state=READY(2), service-specific result fields follow the 5-byte common header. GETNODEINFO: name_len(u8)+name(bytes)+sw_major(u8)+sw_minor(u8)+sw_optional_field_flags(u8)+sw_vcs_commit(u32)+hw_major(u8)+hw_minor(u8)+hw_unique_id(u8[16]). PARAM_GETSET: name_len(u8)+name(bytes)+type(u8)+value(variable)+min_type(u8)+min(variable)+max_type(u8)+max(variable); value/min/max encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data; EMPTY(0) min/max type means no bound is present. EXECUTE_OPCODE and RESTART_NODE: ok(u8) where 1=success. Reading result when state=READY transitions slot back to IDLE. ## `MSP2_INAV_LED_STRIP_CONFIG_EX (8264 / 0x2048)` **Description:** Retrieves the full configuration for each LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_LED_STRIP_CONFIG`. diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index e8f64d0cb93..ac70180150e 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9830,86 +9830,123 @@ "units": "" }, { - "name": "nodeData", - "desc": "Array of per-node status records, one per detected node. Each record: nodeID(1)+health(1)+mode(1)+last_seen_ms(4) = 7 bytes. Full detail available via MSP2_INAV_DRONECAN_NODE_INFO.", - "ctype": "dronecanNodeStatus_t", + "name": "nodeID", + "ctype": "uint8_t", + "desc": "[per node] DroneCAN node ID (1-127)", + "units": "", "array": true, - "array_size": 0, + "array_size": 0 + }, + { + "name": "health", + "ctype": "uint8_t", + "desc": "[per node] Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL", + "units": "" + }, + { + "name": "mode", + "ctype": "uint8_t", + "desc": "[per node] Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE", + "units": "" + }, + { + "name": "last_seen_ms", + "ctype": "uint32_t", + "desc": "[per node] Milliseconds since this node was last seen (FC-local timestamp delta)", + "units": "ms" + }, + { + "name": "uptime_sec", + "ctype": "uint32_t", + "desc": "[per node] Node uptime in seconds (from NodeStatus broadcast)", + "units": "s" + }, + { + "name": "vendor_status_code", + "ctype": "uint16_t", + "desc": "[per node] Vendor-specific status code", "units": "" } ] }, "variable_len": true, - "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4). Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.", + "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 13 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4)+uptime_sec(4)+vendor_status_code(2). Maximum payload 1 + (DRONECAN_MAX_NODES * 13) = 417 bytes. For full node detail (name, SW/HW version, unique ID) use MSP2_INAV_DRONECAN_ASYNC_REQUEST with service_id=DRONECAN_SERVICE_GETNODEINFO(1).", "description": "Returns the list of all detected DroneCAN nodes with their current status." - }, - "MSP2_INAV_DRONECAN_NODE_INFO": { - "code": 8259, - "mspv": 2, + }, + "MSP2_INAV_DRONECAN_ASYNC_REQUEST": { + "code": 8259, + "mspv": 2, "request": { - "payload": [ - { + "payload": [ + { + "name": "service_id", + "ctype": "uint16_t", + "desc": "Service to invoke: 1=GETNODEINFO, 5=RESTART_NODE, 10=EXECUTE_OPCODE, 11=PARAM_GETSET. Transmitted as u16 for MSP alignment; only low 8 bits used.", + "units": "" + }, + { "name": "nodeID", "ctype": "uint8_t", - "desc": "DroneCAN node ID to query (1-127)", - "units": "" + "desc": "Target DroneCAN node ID (1-127)", + "units": "" } - ] - }, + ] + }, "reply": { "payload": [ { - "name": "nodeID", - "ctype": "uint8_t", - "desc": "DroneCAN node ID", - "units": "" - }, + "name": "accepted", + "ctype": "uint8_t", + "desc": "0=request accepted; 1=busy (slot in use) or unrecognised service_id; 0xFF=bus not in STATE_DRONECAN_NORMAL (not ready)", + "units": "" + }, { - "name": "health", + "name": "seq", + "ctype": "uint8_t", + "desc": "Sequence number; correlate with MSP2_INAV_DRONECAN_ASYNC_RESULT to verify the result belongs to this request", + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_DRONECAN`. Initiates an async DroneCAN service request; poll MSP2_INAV_DRONECAN_ASYNC_RESULT at ~100ms intervals until state=READY(2) or ERROR(3). Only one request in-flight at a time. Service-specific request fields follow the common header in the request payload: EXECUTE_OPCODE appends opcode(u8); PARAM_GETSET appends index(u16)+is_write(u8) and optionally value_type(u8)+value(variable) for writes, then req_name_len(u8)+req_name(bytes) for named lookup. Param value encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data. Requests time out after DRONECAN_ASYNC_TIMEOUT_MS (2000ms). If bus is not in STATE_DRONECAN_NORMAL, returns accepted=0xFF without dispatching.", + "description": "Initiates an asynchronous DroneCAN service request (GetNodeInfo, ParamGetSet, ExecuteOpcode, RestartNode) to a specific node. Result retrieved via MSP2_INAV_DRONECAN_ASYNC_RESULT." + }, + "MSP2_INAV_DRONECAN_ASYNC_RESULT": { + "code": 8260, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "state", "ctype": "uint8_t", - "desc": "Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL", + "desc": "Async slot state: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR", "units": "" }, { - "name": "mode", - "ctype": "uint8_t", - "desc": "Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE", - "units": "" - }, - { - "name": "uptime_sec", - "ctype": "uint32_t", - "desc": "Node uptime in seconds", - "units": "s" + "name": "seq", + "ctype": "uint8_t", + "desc": "Sequence number matching the originating MSP2_INAV_DRONECAN_ASYNC_REQUEST reply", + "units": "" }, { - "name": "vendor_status_code", + "name": "service_id", "ctype": "uint16_t", - "desc": "Vendor-specific status code", - "units": "" + "desc": "Service ID of the in-flight or just-completed request", + "units": "" }, { - "name": "last_seen_ms", - "ctype": "uint32_t", - "desc": "FC millisecond timestamp when this node was last seen", - "units": "ms" - }, - { - "name": "name_len", - "ctype": "uint8_t", - "desc": "Length of node name string (0 if unknown)", - "units": "" - }, - { - "name": "name", - "ctype": "char[32]", - "desc": "Node name up to 32 bytes, zero-padded", - "units": "" - } - ] - }, - "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table.", - "description": "Returns full status detail for a single DroneCAN node by ID." + "name": "node_id", + "ctype": "uint8_t", + "desc": "Node ID of the target", + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_DRONECAN`. When state=READY(2), service-specific result fields follow the 5-byte common header. GETNODEINFO: name_len(u8)+name(bytes)+sw_major(u8)+sw_minor(u8)+sw_optional_field_flags(u8)+sw_vcs_commit(u32)+hw_major(u8)+hw_minor(u8)+hw_unique_id(u8[16]). PARAM_GETSET: name_len(u8)+name(bytes)+type(u8)+value(variable)+min_type(u8)+min(variable)+max_type(u8)+max(variable); value/min/max encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data; EMPTY(0) min/max type means no bound is present. EXECUTE_OPCODE and RESTART_NODE: ok(u8) where 1=success. Reading result when state=READY transitions slot back to IDLE.", + "description": "Polls the result of the most recent MSP2_INAV_DRONECAN_ASYNC_REQUEST. Poll at ~100ms intervals until state is READY(2) or ERROR(3)." }, "MSP2_INAV_LED_STRIP_CONFIG_EX": { "code": 8264, diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index a96e6a75ba1..8619068381e 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -39,10 +39,19 @@ 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]; +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 #if defined(STM32H7) static inline void dronecanMaskTxISR(void) { NVIC_DisableIRQ(FDCAN1_IT0_IRQn); } @@ -59,8 +68,14 @@ static inline void dronecanUnmaskTxISR(void) {} 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 ------------------------------------------------------------- @@ -139,6 +154,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; @@ -189,11 +210,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 +347,275 @@ 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); +} + + +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 + dronecanMaskTxISR(); + int16_t res = canardRequestOrRespond(&canard, node_id, signature, service_id, + &dronecanAsyncSlot.transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, CanardRequest, + buf_ptr, len); + dronecanUnmaskTxISR(); + + if (res < 0) { + LOG_DEBUG(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_DEBUG(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_DEBUG(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_DEBUG(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_DEBUG(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 + + /* 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 @@ -374,6 +671,16 @@ static void process1HzTasks(timeUs_t timestamp_usec) canardCleanupStaleTransfers(&canard, timestamp_usec); dronecanUnmaskTxISR(); + // 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 */ @@ -389,66 +696,74 @@ static void process1HzTasks(timeUs_t timestamp_usec) This function must fill in the out_data_type_signature to be the signature of the message. */ +#ifdef UNIT_TEST +bool shouldAcceptTransfer(const CanardInstance *ins, +#else static bool shouldAcceptTransfer(const CanardInstance *ins, +#endif uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id) { - UNUSED(ins); + UNUSED(ins); UNUSED(source_node_id); if (transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; - return true; - } - } - } - if (transfer_type == CanardTransferTypeResponse) { - // check if we want to handle a specific service request - switch (data_type_id) { - } - } - if (transfer_type == CanardTransferTypeBroadcast) { - // see if we want to handle a specific broadcast packet - switch (data_type_id) { - - case UAVCAN_PROTOCOL_NODESTATUS_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE; - return true; - } - case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: { - *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE; + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; return true; } - case UAVCAN_EQUIPMENT_GNSS_FIX_ID: { - *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE; + } + if (transfer_type == CanardTransferTypeResponse) { + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE; + return true; + case UAVCAN_PROTOCOL_PARAM_GETSET_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE; + return true; + case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE; + return true; + case UAVCAN_PROTOCOL_RESTARTNODE_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; return true; } - case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: { + } + if (transfer_type == CanardTransferTypeBroadcast) { + switch (data_type_id) { + case UAVCAN_PROTOCOL_NODESTATUS_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE; + return true; + case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: + *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE; + return true; + case UAVCAN_EQUIPMENT_GNSS_FIX_ID: + *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE; + return true; + case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE; return true; - } - case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: { + case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE; return true; - } - case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: { + case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE; return true; } - } - } - // we don't want any other messages - return false; + } + return false; } // Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) +#ifdef UNIT_TEST +void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { +#else static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); +#endif + UNUSED(ins); struct uavcan_protocol_NodeStatus nodeStatus; if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { @@ -457,30 +772,29 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { } uint8_t nodeId = transfer->source_node_id; - for (uint8_t i = 0; i < activeNodeCount; i++) { - if (nodeTable[i].nodeID == nodeId) { - // update health, mode, uptime, vendor_status_code, last_seen_ms - nodeTable[i].health = nodeStatus.health; - nodeTable[i].mode = nodeStatus.mode; - nodeTable[i].uptime_sec = nodeStatus.uptime_sec; - nodeTable[i].vendor_status_code = nodeStatus.vendor_specific_status_code; - nodeTable[i].last_seen_ms = millis(); - return; - } + dronecanNodeInfo_t *node = findNodeByID(nodeId); + if (node) { + node->health = nodeStatus.health; + node->mode = nodeStatus.mode; + node->uptime_sec = nodeStatus.uptime_sec; + node->vendor_status_code = nodeStatus.vendor_specific_status_code; + node->last_seen_ms = millis(); + return; } // new node if (activeNodeCount < DRONECAN_MAX_NODES) { + memset(&nodeTable[activeNodeCount], 0, sizeof(dronecanNodeInfo_t)); nodeTable[activeNodeCount].nodeID = nodeId; nodeTable[activeNodeCount].health = nodeStatus.health; nodeTable[activeNodeCount].mode = nodeStatus.mode; nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec; nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code; - nodeTable[activeNodeCount].name_len = 0; - nodeTable[activeNodeCount].name[0] = 0; nodeTable[activeNodeCount].last_seen_ms = millis(); activeNodeCount++; - } + } else { + LOG_DEBUG(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeId); + } } static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -587,7 +901,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 @@ -598,10 +916,11 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } } } - if (transfer->transfer_type == CanardTransferTypeResponse) { - switch (transfer->data_type_id) { - } - } + + 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) { @@ -633,4 +952,5 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } } } + #endif diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index c69981b9692..8722bccb916 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -28,23 +28,100 @@ typedef struct dronecanConfig_s { } dronecanConfig_t; typedef struct dronecanNodeInfo_s { - uint8_t nodeID; - uint8_t health; - uint8_t mode; + uint8_t nodeID; + uint8_t health; + uint8_t mode; uint32_t uptime_sec; uint16_t vendor_status_code; uint32_t last_seen_ms; - uint8_t name_len; - char name[32]; } dronecanNodeInfo_t; -// Wire format for MSP2_INAV_DRONECAN_NODES records (7 bytes each, packed). -typedef struct dronecanNodeStatus_s { - uint8_t nodeID; - uint8_t health; - uint8_t mode; - uint32_t last_seen_ms; -} __attribute__((packed)) dronecanNodeStatus_t; +typedef enum { + DRONECAN_ASYNC_IDLE = 0, + DRONECAN_ASYNC_PENDING, + DRONECAN_ASYNC_READY, + DRONECAN_ASYNC_ERROR, +} dronecanAsyncState_e; + +#define DRONECAN_SERVICE_GETNODEINFO 1 +#define DRONECAN_SERVICE_RESTART_NODE 5 +#define DRONECAN_SERVICE_EXECUTE_OPCODE 10 +#define DRONECAN_SERVICE_PARAM_GETSET 11 + +#define DRONECAN_ASYNC_TIMEOUT_MS 2000 +#define DRONECAN_NODE_STALE_TIMEOUT_MS 10000 // Remove node from table if no NodeStatus received for this long +#define DRONECAN_STATE_NOT_READY 0xFF // MSP sentinel: bus not in STATE_NORMAL; outside dronecanAsyncState_e range + +#define DRONECAN_PARAM_TYPE_EMPTY 0 +#define DRONECAN_PARAM_TYPE_INT 1 +#define DRONECAN_PARAM_TYPE_FLOAT 2 +#define DRONECAN_PARAM_TYPE_BOOL 3 +#define DRONECAN_PARAM_TYPE_STRING 4 + +typedef struct dronecanParamRequest_s { + uint16_t index; + uint8_t is_write; + uint8_t value_type; + int64_t value_int; + float value_float; + uint8_t value_bool; + uint8_t value_str_len; + char value_str[128]; + uint8_t req_name_len; + char req_name[92]; +} dronecanParamRequest_t; + +typedef struct dronecanGetNodeInfoResult_s { + uint8_t sw_major; + uint8_t sw_minor; + uint8_t sw_optional_field_flags; + uint32_t sw_vcs_commit; + uint8_t hw_major; + uint8_t hw_minor; + uint8_t hw_unique_id[16]; + uint8_t name_len; + char name[81]; // 80 bytes max + null terminator +} dronecanGetNodeInfoResult_t; + +typedef struct dronecanParamResult_s { + uint8_t type; + int64_t value_int; + float value_float; + uint8_t value_bool; + uint8_t value_str_len; + char value_str[128]; + uint8_t name_len; + char name[93]; // 92 bytes max per UAVCAN param.GetSet DSDL + null terminator + // NumericValue range from the GetSet response; DRONECAN_PARAM_TYPE_EMPTY means not provided. + // Only INT and FLOAT variants are valid — BOOL and STRING have no numeric range. + uint8_t min_type; + int64_t min_int; + float min_float; + uint8_t max_type; + int64_t max_int; + float max_float; +} dronecanParamResult_t; + +typedef struct dronecanSimpleResult_s { + bool ok; +} dronecanSimpleResult_t; + +typedef struct dronecanAsyncSlot_s { + dronecanAsyncState_e state; + uint8_t seq; + uint8_t service_id; + uint8_t node_id; + uint8_t transfer_id; + uint32_t requested_at_ms; + union { + dronecanGetNodeInfoResult_t node_info; + dronecanParamResult_t param; + dronecanSimpleResult_t simple; + } result; +} dronecanAsyncSlot_t; + +extern dronecanAsyncSlot_t dronecanAsyncSlot; +bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload); void dronecanInit(void); void dronecanUpdate(timeUs_t currentTimeUs); @@ -54,5 +131,6 @@ uint32_t dronecanGetBitrateKbps(void); const dronecanNodeInfo_t *dronecanGetNode(uint8_t index); uint32_t dronecanGetBusOffCount(void); CanardPoolAllocatorStatistics dronecanGetPoolStats(void); +const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID); PG_DECLARE(dronecanConfig_t, dronecanConfig); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 9da0b461562..b8b238c0177 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1895,12 +1895,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; @@ -4586,39 +4586,177 @@ 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 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; + 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); } - 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; + } + 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); } - if (!found) { - *ret = MSP_RESULT_ERROR; + + 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; } + *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 180597c4527..a2967a69f98 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -97,7 +97,8 @@ #define MSP2_INAV_ESC_TELEM 0x2041 #define MSP2_INAV_DRONECAN_NODES 0x2042 -#define MSP2_INAV_DRONECAN_NODE_INFO 0x2043 +#define MSP2_INAV_DRONECAN_ASYNC_REQUEST 0x2043 +#define MSP2_INAV_DRONECAN_ASYNC_RESULT 0x2044 #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index e9e2eb7d8a9..dfccb50294b 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -53,6 +53,53 @@ 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" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.ExecuteOpcode_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.ExecuteOpcode_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Value.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.NumericValue.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Empty.c") +set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_includes + "../../lib/main/Dronecan/dsdlc_generated/include") +set_property(SOURCE dronecan_application_unittest.cc PROPERTY definitions + USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0 + FC_VERSION_MAJOR=10 FC_VERSION_MINOR=0 FC_VERSION_PATCH_LEVEL=0) + # bxCAN timing algorithm tests - self-contained, no driver or HAL dependencies # Keep in sync with canard_stm32f7xx_driver.c:canardSTM32ComputeTimings diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc new file mode 100644 index 00000000000..57af83fd229 --- /dev/null +++ b/src/test/unit/dronecan_application_unittest.cc @@ -0,0 +1,844 @@ +/** + * DroneCAN Application-Layer Unit Tests + * + * Tests node table management and transfer acceptance filter using the real + * dronecan.c compiled against INAV stubs. The UNIT_TEST build makes + * activeNodeCount and nodeTable non-static so tests can reset state in SetUp. + * + * Coverage: + * GAP-N1 New node ID → added to table; no slot if table full + * GAP-N2 Subsequent NodeStatus from same node → fields updated in place + * GAP-N3 last_seen_ms follows controllable millis() value + * GAP-N4 33rd unique node → table overflow rejected, count stays at 32 + * GAP-S1 shouldAcceptTransfer: NodeStatus ✓, GetNodeInfo request ✓, + * GetNodeInfo response ✓, unknown ID ✗ + */ + +#include "gtest/gtest.h" + +extern "C" { +#include +#include +#include + +#include "platform.h" + +/* DSDL types used by dronecan.c handlers */ +#include "uavcan.protocol.NodeStatus.h" +#include "uavcan.protocol.GetNodeInfo.h" +#include "uavcan.protocol.param.GetSet_res.h" +#include "uavcan.protocol.param.ExecuteOpcode_res.h" +#include "uavcan.protocol.RestartNode_res.h" + +/* Canard core and STM32 driver declarations */ +#include "drivers/dronecan/libcanard/canard.h" +#include "drivers/dronecan/libcanard/canard_stm32_driver.h" + +/* INAV headers pulled in by dronecan.c — included here so the types are + available when we define stub globals below. */ +#include "io/gps.h" +#include "sensors/battery_sensor_dronecan.h" +#include "fc/runtime_config.h" +#include "sensors/diagnostics.h" +#include "build/version.h" + +/* 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; } + +/* 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; +} + +/* ========================================================================= + * Helpers: encode response structs and build CanardRxTransfer objects. + * ========================================================================= */ + +static CanardRxTransfer makeParamGetSetTransfer( + uint8_t source_node_id, uint8_t transfer_id, + struct uavcan_protocol_param_GetSetResponse *resp, + uint8_t *buf) +{ + uint32_t len = uavcan_protocol_param_GetSetResponse_encode(resp, buf); + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_ID; + xfer.source_node_id = source_node_id; + xfer.transfer_id = transfer_id; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + +static CanardRxTransfer makeExecuteOpcodeTransfer( + uint8_t source_node_id, uint8_t transfer_id, + bool ok, uint8_t *buf) +{ + struct uavcan_protocol_param_ExecuteOpcodeResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.ok = ok; + uint32_t len = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&resp, buf); + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_ID; + xfer.source_node_id = source_node_id; + xfer.transfer_id = transfer_id; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + +static CanardRxTransfer makeRestartNodeTransfer( + uint8_t source_node_id, uint8_t transfer_id, + bool ok, uint8_t *buf) +{ + struct uavcan_protocol_RestartNodeResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.ok = ok; + uint32_t len = uavcan_protocol_RestartNodeResponse_encode(&resp, buf); + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_ID; + xfer.source_node_id = source_node_id; + xfer.transfer_id = transfer_id; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + +/* ========================================================================= + * Node table tests (GAP-N1 … GAP-N4) + * ========================================================================= */ + +class DroneCANNodeTableTest : public ::testing::Test { +protected: + CanardInstance ins; + uint8_t memory_pool[4096]; /* generous pool: 32 nodes × 1 frame each */ + uint8_t buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4]; + + void SetUp() override { + activeNodeCount = 0; + memset(nodeTable, 0, sizeof(dronecanNodeInfo_t) * DRONECAN_MAX_NODES); + mock_time_ms = 0; + canardInit(&ins, memory_pool, sizeof(memory_pool), + onTransferReceived, shouldAcceptTransfer, NULL); + canardSetLocalNodeID(&ins, 1); /* FC node ID required for canardRequestOrRespond */ + } +}; + +/* GAP-N1: First NodeStatus from an unseen node ID → entry added to table */ +TEST_F(DroneCANNodeTableTest, NewNodeAddedOnFirstStatus) +{ + ASSERT_EQ(dronecanGetNodeCount(), 0u); + + CanardRxTransfer xfer = makeNodeStatusTransfer( + 10, 100, + UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK, + UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL, + 0xABCD, buf); + handle_NodeStatus(&ins, &xfer); + + EXPECT_EQ(dronecanGetNodeCount(), 1u); + + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->nodeID, 10u); + EXPECT_EQ(node->health, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK); + EXPECT_EQ(node->mode, UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL); + EXPECT_EQ(node->uptime_sec, 100u); + EXPECT_EQ(node->vendor_status_code, 0xABCDu); +} + +/* GAP-N1 (second node): Two distinct IDs → two separate entries */ +TEST_F(DroneCANNodeTableTest, TwoDistinctNodesStoredSeparately) +{ + CanardRxTransfer x1 = makeNodeStatusTransfer(10, 100, 0, 0, 0, buf); + handle_NodeStatus(&ins, &x1); + CanardRxTransfer x2 = makeNodeStatusTransfer(20, 200, 0, 0, 0, buf); + handle_NodeStatus(&ins, &x2); + + EXPECT_EQ(dronecanGetNodeCount(), 2u); + EXPECT_EQ(dronecanGetNode(0)->nodeID, 10u); + EXPECT_EQ(dronecanGetNode(1)->nodeID, 20u); +} + +/* GAP-N2: Second NodeStatus from the same node → fields updated, no new entry */ +TEST_F(DroneCANNodeTableTest, ExistingNodeUpdatedInPlace) +{ + CanardRxTransfer x1 = makeNodeStatusTransfer( + 10, 100, + UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK, + UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL, + 0x0000, buf); + handle_NodeStatus(&ins, &x1); + ASSERT_EQ(dronecanGetNodeCount(), 1u); + + CanardRxTransfer x2 = makeNodeStatusTransfer( + 10, 500, + UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING, + UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE, + 0xBEEF, buf); + handle_NodeStatus(&ins, &x2); + + EXPECT_EQ(dronecanGetNodeCount(), 1u); /* still one node */ + + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->health, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING); + EXPECT_EQ(node->mode, UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE); + EXPECT_EQ(node->uptime_sec, 500u); + EXPECT_EQ(node->vendor_status_code, 0xBEEFu); +} + +/* GAP-N3: last_seen_ms is set from millis() at the time of each call */ +TEST_F(DroneCANNodeTableTest, LastSeenMsFollowsMillis) +{ + mock_time_ms = 1000; + CanardRxTransfer x1 = makeNodeStatusTransfer(20, 10, 0, 0, 0, buf); + handle_NodeStatus(&ins, &x1); + + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->last_seen_ms, 1000u); + + mock_time_ms = 2500; + CanardRxTransfer x2 = makeNodeStatusTransfer(20, 20, 0, 0, 0, buf); + handle_NodeStatus(&ins, &x2); + + EXPECT_EQ(node->last_seen_ms, 2500u); +} + +/* GAP-N3: last_seen_ms for a new node also uses current millis() */ +TEST_F(DroneCANNodeTableTest, LastSeenMsSetOnInsert) +{ + mock_time_ms = 9999; + CanardRxTransfer xfer = makeNodeStatusTransfer(5, 0, 0, 0, 0, buf); + handle_NodeStatus(&ins, &xfer); + + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->last_seen_ms, 9999u); +} + +/* GAP-N4: Fill the table to DRONECAN_MAX_NODES, then a 33rd node is silently + dropped — count stays at 32 and the overflow ID is not present. */ +TEST_F(DroneCANNodeTableTest, TableFullNodeRejected) +{ + for (uint8_t i = 1; i <= DRONECAN_MAX_NODES; i++) { + CanardRxTransfer xfer = makeNodeStatusTransfer(i, 0, 0, 0, 0, buf); + handle_NodeStatus(&ins, &xfer); + } + ASSERT_EQ(dronecanGetNodeCount(), (uint8_t)DRONECAN_MAX_NODES); + + /* Try to add a 33rd node (ID 100, not in 1..32) */ + CanardRxTransfer overflow = makeNodeStatusTransfer(100, 0, 0, 0, 0, buf); + handle_NodeStatus(&ins, &overflow); + + EXPECT_EQ(dronecanGetNodeCount(), (uint8_t)DRONECAN_MAX_NODES); + + for (uint8_t i = 0; i < DRONECAN_MAX_NODES; i++) { + const dronecanNodeInfo_t *n = dronecanGetNode(i); + ASSERT_NE(n, nullptr); + EXPECT_NE(n->nodeID, 100u) << "overflow node ID 100 should not be in slot " << (int)i; + } +} + +/* GAP-N4 boundary: dronecanGetNode at index == DRONECAN_MAX_NODES returns NULL */ +TEST_F(DroneCANNodeTableTest, GetNodeOutOfBoundsReturnsNull) +{ + EXPECT_EQ(dronecanGetNode(DRONECAN_MAX_NODES), nullptr); + EXPECT_EQ(dronecanGetNode(255), nullptr); +} + +/* ========================================================================= + * shouldAcceptTransfer tests (GAP-S1) + * ========================================================================= */ + +/* shouldAcceptTransfer does not use the CanardInstance — pass NULL. */ + +TEST(DroneCANShouldAcceptTransfer, AcceptsNodeStatusBroadcast) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_NODESTATUS_ID, + CanardTransferTypeBroadcast, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsGetNodeInfoRequest) +{ + /* The FC handles incoming GetNodeInfo requests and sends a response */ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + CanardTransferTypeRequest, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsGetNodeInfoResponse) +{ + /* Phase 3: FC now accepts GetNodeInfo responses so handle_GetNodeInfoResponse + can populate the node table with name and version data. */ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, RejectsUnknownBroadcastId) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + 0xFFFF, /* not a real UAVCAN data type ID */ + CanardTransferTypeBroadcast, + 42); + + EXPECT_FALSE(accept); +} + +TEST(DroneCANShouldAcceptTransfer, RejectsUnknownResponseId) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + 0xFFFF, + CanardTransferTypeResponse, + 42); + + EXPECT_FALSE(accept); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsParamGetSetResponse) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsExecuteOpcodeResponse) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsRestartNodeResponse) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_SIGNATURE); +} + +/* ========================================================================= + * onTransferReceived dispatch test (GAP-S2) + * + * Verifies that a GetNodeInfo response transfer is dispatched to + * handle_GetNodeInfoResponse and populates the node table entry. + * Written before Phase 4 — fails until the handler is implemented. + * ========================================================================= */ + +class DroneCANDispatchTest : public ::testing::Test { +protected: + CanardInstance ins; + uint8_t memory_pool[4096]; + uint8_t buf[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE + 16]; + + void SetUp() override { + activeNodeCount = 0; + memset(nodeTable, 0, sizeof(dronecanNodeInfo_t) * DRONECAN_MAX_NODES); + memset(&dronecanAsyncSlot, 0, sizeof(dronecanAsyncSlot)); + dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE; + mock_time_ms = 0; + canardInit(&ins, memory_pool, sizeof(memory_pool), + onTransferReceived, shouldAcceptTransfer, NULL); + canardSetLocalNodeID(&ins, 1); + } +}; + +/* GAP-S2: GetNodeInfo response → handler populates async slot result. + * The node table (dronecanNodeInfo_t) holds only NodeStatus-level fields since + * commit 96f8a4bd9 stripped the GetNodeInfo fields to save ~3.5 KB RAM and + * replaced auto-fetch with the on-demand async slot pattern. */ +TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesAsyncSlot) +{ + /* Pre-insert node 42 via a NodeStatus (node table is independent of async slot) */ + uint8_t ns_buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4]; + CanardRxTransfer ns_xfer = makeNodeStatusTransfer(42, 10, 0, 0, 0, ns_buf); + handle_NodeStatus(&ins, &ns_xfer); + ASSERT_EQ(dronecanGetNodeCount(), 1u); + + /* Prime the async slot — handle_AsyncServiceResponse guards on state, service_id, + * node_id, and transfer_id. The guard checks transfer_id == (slot.transfer_id-1)&0x1F, + * so set transfer_id=1 so the expected in-flight id is 0 (matching xfer.transfer_id). */ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_GETNODEINFO; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + /* Build a GetNodeInfo response from node 42 */ + struct uavcan_protocol_GetNodeInfoResponse resp; + memset(&resp, 0, sizeof(resp)); + + resp.status.uptime_sec = 10; + resp.status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + resp.status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + + resp.software_version.major = 1; + resp.software_version.minor = 7; + resp.software_version.optional_field_flags = 1; /* vcs_commit valid */ + resp.software_version.vcs_commit = 0xDEADBEEF; + + resp.hardware_version.major = 2; + resp.hardware_version.minor = 0; + for (int i = 0; i < 16; i++) + resp.hardware_version.unique_id[i] = (uint8_t)(0xA0 + i); + + const char *name = "com.example.gps"; + resp.name.len = (uint8_t)strlen(name); + memcpy(resp.name.data, name, resp.name.len); + + uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&resp, buf); + + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_GETNODEINFO_ID; + xfer.source_node_id = 42; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)encoded_len; + + onTransferReceived(&ins, &xfer); + + /* Slot must now be READY */ + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + + /* Result fields populated from the GetNodeInfo response */ + const dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info; + EXPECT_EQ(r->name_len, (uint8_t)strlen(name)); + EXPECT_EQ(0, memcmp(r->name, name, r->name_len)); + + EXPECT_EQ(r->sw_major, 1u); + EXPECT_EQ(r->sw_minor, 7u); + EXPECT_EQ(r->sw_optional_field_flags, 1u); + EXPECT_EQ(r->sw_vcs_commit, 0xDEADBEEFu); + + EXPECT_EQ(r->hw_major, 2u); + EXPECT_EQ(r->hw_minor, 0u); + for (int i = 0; i < 16; i++) + EXPECT_EQ(r->hw_unique_id[i], (uint8_t)(0xA0 + i)) + << "unique_id mismatch at byte " << i; + + /* Node table entry still exists (populated by the preceding NodeStatus) */ + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->nodeID, 42u); +} + +/* ========================================================================= + * Async service response guard rejection tests (GAP-S3) + * + * handle_AsyncServiceResponse has four guards before decoding the payload. + * Each test confirms a mismatched guard leaves the slot state unchanged. + * ========================================================================= */ + +/* GAP-S3a: Slot in IDLE state → response silently ignored */ +TEST_F(DroneCANDispatchTest, AsyncSlot_IdleState_IgnoresParamGetSetResponse) +{ + /* slot stays IDLE (SetUp default); send a valid PARAM_GETSET response */ + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + resp.value.integer_value = 7; + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_IDLE); +} + +/* GAP-S3b: Slot PENDING but response comes from the wrong node ID */ +TEST_F(DroneCANDispatchTest, AsyncSlot_WrongNodeId_IgnoresResponse) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; /* guard expects in-flight id (1-1)&0x1F = 0 */ + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + + /* source_node_id = 99, not 42 */ + CanardRxTransfer xfer = makeParamGetSetTransfer(99, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING); +} + +/* GAP-S3c: Slot PENDING but transfer_id does not match the in-flight id */ +TEST_F(DroneCANDispatchTest, AsyncSlot_WrongTransferId_IgnoresResponse) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; /* guard expects xfer.transfer_id == 0 */ + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + + /* xfer.transfer_id = 5, which != (1-1)&0x1F = 0 */ + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 5, &resp, buf); + onTransferReceived(&ins, &xfer); + + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING); +} + +/* GAP-S3d: Slot PENDING for GETNODEINFO; a PARAM_GETSET response arrives → + * data_type_id mismatch rejects it before any decode. */ +TEST_F(DroneCANDispatchTest, AsyncSlot_WrongServiceId_IgnoresResponse) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_GETNODEINFO; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + + /* xfer.data_type_id == PARAM_GETSET(11) != slot.service_id(GETNODEINFO=1) */ + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING); +} + +/* ========================================================================= + * PARAM_GETSET response decode tests (GAP-S4) + * ========================================================================= */ + +/* GAP-S4a: Integer value with integer min/max range */ +TEST_F(DroneCANDispatchTest, ParamGetSetIntResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + resp.value.integer_value = 42; + resp.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; + resp.min_value.integer_value = 0; + resp.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE; + resp.max_value.integer_value = 100; + const char *name = "MOT_SPIN_MIN"; + resp.name.len = (uint8_t)strlen(name); + memcpy(resp.name.data, name, resp.name.len); + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + EXPECT_EQ(r->type, (uint8_t)DRONECAN_PARAM_TYPE_INT); + EXPECT_EQ(r->value_int, 42); + EXPECT_EQ(r->name_len, (uint8_t)strlen(name)); + EXPECT_EQ(0, memcmp(r->name, name, r->name_len)); + EXPECT_EQ(r->min_type, (uint8_t)DRONECAN_PARAM_TYPE_INT); + EXPECT_EQ(r->min_int, 0); + EXPECT_EQ(r->max_type, (uint8_t)DRONECAN_PARAM_TYPE_INT); + EXPECT_EQ(r->max_int, 100); +} + +/* GAP-S4b: Float value with float min/max range */ +TEST_F(DroneCANDispatchTest, ParamGetSetFloatResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE; + resp.value.real_value = 3.14f; + resp.min_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE; + resp.min_value.real_value = 0.0f; + resp.max_value.union_tag = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE; + resp.max_value.real_value = 10.0f; + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + EXPECT_EQ(r->type, (uint8_t)DRONECAN_PARAM_TYPE_FLOAT); + EXPECT_FLOAT_EQ(r->value_float, 3.14f); + EXPECT_EQ(r->min_type, (uint8_t)DRONECAN_PARAM_TYPE_FLOAT); + EXPECT_FLOAT_EQ(r->min_float, 0.0f); + EXPECT_EQ(r->max_type, (uint8_t)DRONECAN_PARAM_TYPE_FLOAT); + EXPECT_FLOAT_EQ(r->max_float, 10.0f); +} + +/* GAP-S4c: Boolean value (no numeric range) */ +TEST_F(DroneCANDispatchTest, ParamGetSetBoolResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE; + resp.value.boolean_value = 1; + /* min/max remain EMPTY (memset to 0 = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_EMPTY) */ + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + EXPECT_EQ(r->type, (uint8_t)DRONECAN_PARAM_TYPE_BOOL); + EXPECT_EQ(r->value_bool, 1u); + EXPECT_EQ(r->min_type, (uint8_t)DRONECAN_PARAM_TYPE_EMPTY); + EXPECT_EQ(r->max_type, (uint8_t)DRONECAN_PARAM_TYPE_EMPTY); +} + +/* GAP-S4d: String value */ +TEST_F(DroneCANDispatchTest, ParamGetSetStringResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE; + const char *str = "hello"; + resp.value.string_value.len = (uint8_t)strlen(str); + memcpy(resp.value.string_value.data, str, resp.value.string_value.len); + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param; + EXPECT_EQ(r->type, (uint8_t)DRONECAN_PARAM_TYPE_STRING); + EXPECT_EQ(r->value_str_len, (uint8_t)strlen(str)); + EXPECT_EQ(0, memcmp(r->value_str, str, r->value_str_len)); +} + +/* GAP-S4e: Empty value (unknown union_tag) → type forced to DRONECAN_PARAM_TYPE_EMPTY */ +TEST_F(DroneCANDispatchTest, ParamGetSetEmptyResponse_SetsEmptyType) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + struct uavcan_protocol_param_GetSetResponse resp; + memset(&resp, 0, sizeof(resp)); + /* union_tag == 0 == UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY */ + resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY; + + CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + EXPECT_EQ(dronecanAsyncSlot.result.param.type, (uint8_t)DRONECAN_PARAM_TYPE_EMPTY); +} + +/* ========================================================================= + * EXECUTE_OPCODE response decode tests (GAP-S5) + * ========================================================================= */ + +/* GAP-S5a: ok=true */ +TEST_F(DroneCANDispatchTest, ExecuteOpcodeOkResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_EXECUTE_OPCODE; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + CanardRxTransfer xfer = makeExecuteOpcodeTransfer(42, 0, true, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + EXPECT_TRUE(dronecanAsyncSlot.result.simple.ok); +} + +/* GAP-S5b: ok=false */ +TEST_F(DroneCANDispatchTest, ExecuteOpcodeFailResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_EXECUTE_OPCODE; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + CanardRxTransfer xfer = makeExecuteOpcodeTransfer(42, 0, false, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + EXPECT_FALSE(dronecanAsyncSlot.result.simple.ok); +} + +/* ========================================================================= + * RESTART_NODE response decode test (GAP-S6) + * ========================================================================= */ + +/* GAP-S6: ok=true */ +TEST_F(DroneCANDispatchTest, RestartNodeOkResponse_PopulatesSlot) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_RESTART_NODE; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + + CanardRxTransfer xfer = makeRestartNodeTransfer(42, 0, true, buf); + onTransferReceived(&ins, &xfer); + + ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); + EXPECT_TRUE(dronecanAsyncSlot.result.simple.ok); +} + +/* ========================================================================= + * dronecanAsyncRequest re-entry guard test (GAP-S7) + * ========================================================================= */ + +/* GAP-S7: A second async request is rejected while one is already in flight. + * Uses RESTART_NODE (no null-payload check) so the re-entry guard is the only + * reason dronecanAsyncRequest returns false. Slot PENDING with + * requested_at_ms=0 and mock_time_ms=0 keeps the timeout condition satisfied + * (0 < DRONECAN_ASYNC_TIMEOUT_MS), so the guard fires before touching the bus. */ +TEST_F(DroneCANDispatchTest, AsyncRequest_RejectedWhilePending) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.requested_at_ms = 0; + mock_time_ms = 0; + + EXPECT_FALSE(dronecanAsyncRequest(DRONECAN_SERVICE_RESTART_NODE, 42, nullptr)); + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING); +} 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); +}