From 67c787d513156f20c8b21e3d477bce605508b14e Mon Sep 17 00:00:00 2001 From: daijoubu Date: Mon, 1 Jun 2026 13:36:04 -0700 Subject: [PATCH 01/42] test(dronecan): add GetNodeInfo, SoftwareVersion, HardwareVersion, RTCMStream tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Closes coverage gaps identified in audit 2026-06-01: GAP-D1: GetNodeInfoResponse encode/decode round-trip, empty name, 80-char max-length name GAP-D2: RTCMStream basic encode/decode, empty payload GAP-D3: SoftwareVersion optional_field_flags behaviour — vcs_commit/image_crc are always on the wire; flags are app-level hint only. Tests document this and verify correct flag-checking is needed in handle_GetNodeInfoResponse. HardwareVersion unique_id[16] round-trip and zero case. Constants: GetNodeInfo and RTCMStream signatures, IDs, max sizes. Node table logic (GAP-N1..N4), MSP byte-layout (GAP-M1..M2), and shouldAcceptTransfer dispatch (GAP-S1..S3) require dronecan.c with mocked INAV dependencies — tracked for dronecan_application_unittest.cc. 13 tests, 0 failures. --- src/test/unit/CMakeLists.txt | 14 + .../unit/dronecan_getnodeinfo_unittest.cc | 362 ++++++++++++++++++ 2 files changed, 376 insertions(+) create mode 100644 src/test/unit/dronecan_getnodeinfo_unittest.cc diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index e9e2eb7d8a9..7880d0e9eb8 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -53,6 +53,20 @@ 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) + # 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_getnodeinfo_unittest.cc b/src/test/unit/dronecan_getnodeinfo_unittest.cc new file mode 100644 index 00000000000..f9a8e5fa7ea --- /dev/null +++ b/src/test/unit/dronecan_getnodeinfo_unittest.cc @@ -0,0 +1,362 @@ +/** + * DroneCAN GetNodeInfo and Service Message Unit Tests + * + * Covers coverage gaps identified in audit 2026-06-01: + * GAP-D1 GetNodeInfoResponse encode/decode round-trip + * GAP-D2 RTCMStream encode/decode + * GAP-D3 SoftwareVersion optional_field_flags wire behaviour + * (vcs_commit/image_crc are ALWAYS encoded; flags are app-level hint) + * + * Node table logic (GAP-N1..N4), MSP byte-layout (GAP-M1..M2), and + * shouldAcceptTransfer dispatch (GAP-S1..S3) require dronecan.c to be + * compiled with mocked INAV dependencies. That infrastructure belongs in a + * separate dronecan_application_unittest.cc — tracked in the project todo. + */ + +#include +#include + +extern "C" { +#include "drivers/dronecan/libcanard/canard.h" +#include "uavcan.protocol.GetNodeInfo.h" +#include "uavcan.protocol.GetNodeInfo_res.h" +#include "uavcan.protocol.GetNodeInfo_req.h" +#include "uavcan.protocol.SoftwareVersion.h" +#include "uavcan.protocol.HardwareVersion.h" +#include "uavcan.protocol.NodeStatus.h" +#include "uavcan.equipment.gnss.RTCMStream.h" +} + +#include "gtest/gtest.h" + +class DroneCANGetNodeInfoTest : public ::testing::Test { +protected: + void SetUp() override { + memset(buffer, 0, sizeof(buffer)); + } + + CanardRxTransfer makeTransfer(uint32_t len) { + CanardRxTransfer transfer; + memset(&transfer, 0, sizeof(transfer)); + transfer.payload_len = len; + transfer.payload_head = buffer; + transfer.payload_middle = NULL; + transfer.payload_tail = NULL; + return transfer; + } + + // Buffer large enough for the largest GetNodeInfo response (377 bytes). + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE + 16]; +}; + +// =========================================================================== +// GetNodeInfoResponse encode/decode (GAP-D1) +// =========================================================================== + +TEST_F(DroneCANGetNodeInfoTest, GetNodeInfoResponse_RoundTrip) +{ + struct uavcan_protocol_GetNodeInfoResponse tx; + memset(&tx, 0, sizeof(tx)); + + // NodeStatus + tx.status.uptime_sec = 12345; + tx.status.health = 1; // WARNING + tx.status.mode = 0; // OPERATIONAL + tx.status.vendor_specific_status_code = 0xABCD; + + // SoftwareVersion + tx.software_version.major = 1; + tx.software_version.minor = 7; + tx.software_version.optional_field_flags = 1; // vcs_commit valid + tx.software_version.vcs_commit = 0xDEADBEEF; + tx.software_version.image_crc = 0; // not flagged + + // HardwareVersion + tx.hardware_version.major = 2; + tx.hardware_version.minor = 0; + for (int i = 0; i < 16; i++) { + tx.hardware_version.unique_id[i] = (uint8_t)(0x10 + i); + } + tx.hardware_version.certificate_of_authenticity.len = 0; + + // Name + const char *name = "com.example.sensor"; + tx.name.len = (uint8_t)strlen(name); + memcpy(tx.name.data, name, tx.name.len); + + uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&tx, buffer); + + EXPECT_GT(encoded_len, 0u); + EXPECT_LE(encoded_len, (uint32_t)UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_GetNodeInfoResponse rx; + memset(&rx, 0, sizeof(rx)); + bool decode_failed = uavcan_protocol_GetNodeInfoResponse_decode(&transfer, &rx); + + EXPECT_FALSE(decode_failed); + + EXPECT_EQ(rx.status.uptime_sec, tx.status.uptime_sec); + EXPECT_EQ(rx.status.health, tx.status.health); + EXPECT_EQ(rx.status.mode, tx.status.mode); + EXPECT_EQ(rx.status.vendor_specific_status_code, tx.status.vendor_specific_status_code); + + EXPECT_EQ(rx.software_version.major, tx.software_version.major); + EXPECT_EQ(rx.software_version.minor, tx.software_version.minor); + EXPECT_EQ(rx.software_version.optional_field_flags, tx.software_version.optional_field_flags); + EXPECT_EQ(rx.software_version.vcs_commit, tx.software_version.vcs_commit); + + EXPECT_EQ(rx.hardware_version.major, tx.hardware_version.major); + EXPECT_EQ(rx.hardware_version.minor, tx.hardware_version.minor); + for (int i = 0; i < 16; i++) { + EXPECT_EQ(rx.hardware_version.unique_id[i], tx.hardware_version.unique_id[i]) + << "unique_id mismatch at byte " << i; + } + + EXPECT_EQ(rx.name.len, tx.name.len); + EXPECT_EQ(0, memcmp(rx.name.data, tx.name.data, tx.name.len)); +} + +TEST_F(DroneCANGetNodeInfoTest, GetNodeInfoResponse_EmptyName) +{ + // TAO-encoded name length is inferred from remaining payload when len=0. + // A zero-length name must decode without error and name.len must be 0. + struct uavcan_protocol_GetNodeInfoResponse tx; + memset(&tx, 0, sizeof(tx)); + tx.name.len = 0; + + uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&tx, buffer); + EXPECT_GT(encoded_len, 0u); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_GetNodeInfoResponse rx; + memset(&rx, 0xFF, sizeof(rx)); + bool decode_failed = uavcan_protocol_GetNodeInfoResponse_decode(&transfer, &rx); + + EXPECT_FALSE(decode_failed); + EXPECT_EQ(rx.name.len, 0u); +} + +TEST_F(DroneCANGetNodeInfoTest, GetNodeInfoResponse_MaxLengthName) +{ + struct uavcan_protocol_GetNodeInfoResponse tx; + memset(&tx, 0, sizeof(tx)); + tx.name.len = 80; + for (int i = 0; i < 80; i++) { + tx.name.data[i] = (uint8_t)('a' + (i % 26)); + } + + uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&tx, buffer); + EXPECT_LE(encoded_len, (uint32_t)UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_GetNodeInfoResponse rx; + memset(&rx, 0, sizeof(rx)); + bool decode_failed = uavcan_protocol_GetNodeInfoResponse_decode(&transfer, &rx); + + EXPECT_FALSE(decode_failed); + EXPECT_EQ(rx.name.len, 80u); + EXPECT_EQ(0, memcmp(rx.name.data, tx.name.data, 80)); +} + +// =========================================================================== +// SoftwareVersion optional_field_flags (GAP-D3) +// +// The DSDL-generated encoder writes vcs_commit and image_crc unconditionally +// (always 15 bytes on the wire). optional_field_flags is an app-level hint +// that tells the receiver which fields are meaningful — it does NOT gate the +// wire encoding. Tests here document this behaviour and ensure that +// handle_GetNodeInfoResponse checks the flag before storing vcs_commit. +// =========================================================================== + +TEST_F(DroneCANGetNodeInfoTest, SoftwareVersion_AlwaysEncodesAllFields) +{ + // Even with flags=0, vcs_commit and image_crc bytes are present on wire. + // Verify that a non-zero vcs_commit set with flags=0 still survives the + // round-trip — the application must use the flag to decide whether to use + // the value, not rely on the decoder zeroing it out. + struct uavcan_protocol_SoftwareVersion tx; + memset(&tx, 0, sizeof(tx)); + tx.major = 3; + tx.minor = 1; + tx.optional_field_flags = 0; // neither field is flagged as valid + tx.vcs_commit = 0xCAFEBABE; // present on wire, but not flagged + tx.image_crc = 0; + + uint32_t encoded_len = uavcan_protocol_SoftwareVersion_encode(&tx, buffer); + EXPECT_GT(encoded_len, 0u); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_SoftwareVersion rx; + memset(&rx, 0, sizeof(rx)); + bool decode_failed = uavcan_protocol_SoftwareVersion_decode(&transfer, &rx); + + EXPECT_FALSE(decode_failed); + EXPECT_EQ(rx.major, tx.major); + EXPECT_EQ(rx.minor, tx.minor); + EXPECT_EQ(rx.optional_field_flags, 0u); + // vcs_commit IS decoded (wire is always 15 bytes) but flags=0 means + // the application must NOT trust it — assert the flag is checked: + EXPECT_EQ(rx.optional_field_flags & 1u, 0u) << "vcs_commit flag must not be set"; +} + +TEST_F(DroneCANGetNodeInfoTest, SoftwareVersion_VCSCommitFlaggedAndValid) +{ + struct uavcan_protocol_SoftwareVersion tx; + memset(&tx, 0, sizeof(tx)); + tx.major = 1; + tx.minor = 5; + tx.optional_field_flags = 1; // VCS_COMMIT valid + tx.vcs_commit = 0xDEADBEEF; + tx.image_crc = 0; + + uint32_t encoded_len = uavcan_protocol_SoftwareVersion_encode(&tx, buffer); + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_SoftwareVersion rx; + memset(&rx, 0, sizeof(rx)); + + EXPECT_FALSE(uavcan_protocol_SoftwareVersion_decode(&transfer, &rx)); + EXPECT_EQ(rx.optional_field_flags & 1u, 1u); + EXPECT_EQ(rx.vcs_commit, 0xDEADBEEFu); +} + +TEST_F(DroneCANGetNodeInfoTest, SoftwareVersion_BothOptionalFieldsFlagged) +{ + struct uavcan_protocol_SoftwareVersion tx; + memset(&tx, 0, sizeof(tx)); + tx.major = 2; + tx.minor = 0; + tx.optional_field_flags = 3; // VCS_COMMIT and IMAGE_CRC both valid + tx.vcs_commit = 0x12345678; + tx.image_crc = 0xABCDEF0123456789ULL; + + uint32_t encoded_len = uavcan_protocol_SoftwareVersion_encode(&tx, buffer); + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_SoftwareVersion rx; + memset(&rx, 0, sizeof(rx)); + + EXPECT_FALSE(uavcan_protocol_SoftwareVersion_decode(&transfer, &rx)); + EXPECT_EQ(rx.optional_field_flags, 3u); + EXPECT_EQ(rx.vcs_commit, 0x12345678u); + EXPECT_EQ(rx.image_crc, 0xABCDEF0123456789ULL); +} + +// =========================================================================== +// HardwareVersion unique_id (part of GAP-D1) +// =========================================================================== + +TEST_F(DroneCANGetNodeInfoTest, HardwareVersion_UniqueIdRoundTrip) +{ + struct uavcan_protocol_HardwareVersion tx; + memset(&tx, 0, sizeof(tx)); + tx.major = 1; + tx.minor = 0; + for (int i = 0; i < 16; i++) { + tx.unique_id[i] = (uint8_t)(0xA0 + i); + } + tx.certificate_of_authenticity.len = 0; + + uint32_t encoded_len = uavcan_protocol_HardwareVersion_encode(&tx, buffer); + EXPECT_GT(encoded_len, 0u); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_HardwareVersion rx; + memset(&rx, 0, sizeof(rx)); + + EXPECT_FALSE(uavcan_protocol_HardwareVersion_decode(&transfer, &rx)); + EXPECT_EQ(rx.major, tx.major); + EXPECT_EQ(rx.minor, tx.minor); + for (int i = 0; i < 16; i++) { + EXPECT_EQ(rx.unique_id[i], tx.unique_id[i]) << "unique_id mismatch at byte " << i; + } + EXPECT_EQ(rx.certificate_of_authenticity.len, 0u); +} + +TEST_F(DroneCANGetNodeInfoTest, HardwareVersion_ZeroUniqueId) +{ + struct uavcan_protocol_HardwareVersion tx; + memset(&tx, 0, sizeof(tx)); + // All unique_id bytes zero — valid for nodes that don't implement unique ID. + + uint32_t encoded_len = uavcan_protocol_HardwareVersion_encode(&tx, buffer); + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_HardwareVersion rx; + memset(&rx, 0xFF, sizeof(rx)); + + EXPECT_FALSE(uavcan_protocol_HardwareVersion_decode(&transfer, &rx)); + for (int i = 0; i < 16; i++) { + EXPECT_EQ(rx.unique_id[i], 0u) << "unique_id byte " << i << " should be zero"; + } +} + +// =========================================================================== +// RTCMStream encode/decode (GAP-D2) +// =========================================================================== + +TEST_F(DroneCANGetNodeInfoTest, RTCMStream_BasicEncodeDecode) +{ + struct uavcan_equipment_gnss_RTCMStream tx; + memset(&tx, 0, sizeof(tx)); + tx.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3; + + const uint8_t payload[] = {0xD3, 0x00, 0x13, 0x3E, 0xD0, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x70}; + tx.data.len = sizeof(payload); + memcpy(tx.data.data, payload, sizeof(payload)); + + uint32_t encoded_len = uavcan_equipment_gnss_RTCMStream_encode(&tx, buffer); + + EXPECT_GT(encoded_len, 0u); + EXPECT_LE(encoded_len, (uint32_t)UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_MAX_SIZE); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_equipment_gnss_RTCMStream rx; + memset(&rx, 0, sizeof(rx)); + + EXPECT_FALSE(uavcan_equipment_gnss_RTCMStream_decode(&transfer, &rx)); + EXPECT_EQ(rx.protocol_id, UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3); + EXPECT_EQ(rx.data.len, tx.data.len); + EXPECT_EQ(0, memcmp(rx.data.data, tx.data.data, tx.data.len)); +} + +TEST_F(DroneCANGetNodeInfoTest, RTCMStream_EmptyPayload) +{ + struct uavcan_equipment_gnss_RTCMStream tx; + memset(&tx, 0, sizeof(tx)); + tx.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM2; + tx.data.len = 0; + + uint32_t encoded_len = uavcan_equipment_gnss_RTCMStream_encode(&tx, buffer); + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_equipment_gnss_RTCMStream rx; + memset(&rx, 0xFF, sizeof(rx)); + + EXPECT_FALSE(uavcan_equipment_gnss_RTCMStream_decode(&transfer, &rx)); + EXPECT_EQ(rx.protocol_id, UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM2); + EXPECT_EQ(rx.data.len, 0u); +} + +// =========================================================================== +// Constants (extend GAP-S2: signatures must match DSDL spec) +// =========================================================================== + +TEST(DroneCANGetNodeInfoConstants, Signatures) +{ + EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, 0xEE468A8121C46A9EULL); + EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE, 0xEE468A8121C46A9EULL); + EXPECT_EQ(UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE, 0x1F56030ECB171501ULL); +} + +TEST(DroneCANGetNodeInfoConstants, IDs) +{ + EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_ID, 1u); + EXPECT_EQ(UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID, 1062u); +} + +TEST(DroneCANGetNodeInfoConstants, MessageSizes) +{ + // Response max size accounts for 80-char name + all nested structs. + EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE, 377); + EXPECT_EQ(UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_MAX_SIZE, 130); +} From 4b8ae4375d4cbf03c017f75684af93e2a622bf08 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Mon, 1 Jun 2026 16:19:22 -0700 Subject: [PATCH 02/42] test(dronecan): add node table and shouldAcceptTransfer unit tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 12 tests covering GAP-N1..N4 (node table insert, update, last_seen_ms, overflow rejection) and GAP-S1 (shouldAcceptTransfer accept/reject logic). dronecan.c: activeNodeCount and nodeTable are non-static in UNIT_TEST builds so SetUp() can reset state between test cases. RejectsGetNodeInfoResponseBeforePhase3 is intentionally a canary — it documents current behaviour and will fail when Phase 3 wires up the response handler, signalling a required test update. --- src/main/drivers/dronecan/dronecan.c | 5 + src/test/unit/CMakeLists.txt | 24 ++ .../unit/dronecan_application_unittest.cc | 342 ++++++++++++++++++ 3 files changed, 371 insertions(+) create mode 100644 src/test/unit/dronecan_application_unittest.cc diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index a96e6a75ba1..e58085b9600 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -39,10 +39,15 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, ); static dronecanState_e dronecanState = STATE_DRONECAN_INIT; +#ifdef UNIT_TEST +uint8_t activeNodeCount = 0; +dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; +#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); } diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 7880d0e9eb8..f7ad66653bd 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -67,6 +67,30 @@ set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY extra_includes "../../lib/main/Dronecan/dsdlc_generated/include") set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY definitions USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0) +# DroneCAN application-layer tests - compiles dronecan.c with INAV stubs. +# UNIT_TEST exposes activeNodeCount and nodeTable as non-static for SetUp reset. +set_property(SOURCE dronecan_application_unittest.cc PROPERTY depends + "drivers/dronecan/dronecan.c" + "drivers/dronecan/libcanard/canard.c") +set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.NodeStatus.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.SoftwareVersion.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.HardwareVersion.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix2.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Auxiliary.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.power.BatteryInfo.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.RTCMStream.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.Timestamp.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.ECEFPositionVelocity.c") +set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_includes + "../../lib/main/Dronecan/dsdlc_generated/include") +set_property(SOURCE dronecan_application_unittest.cc PROPERTY definitions + USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0 + FC_VERSION_MAJOR=10 FC_VERSION_MINOR=0 FC_VERSION_PATCH_LEVEL=0) + # 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..980ba07748f --- /dev/null +++ b/src/test/unit/dronecan_application_unittest.cc @@ -0,0 +1,342 @@ +/** + * 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 ✗ (Phase 3 will change this), unknown ID ✗ + */ + +#include "gtest/gtest.h" + +extern "C" { +#include +#include +#include + +#include "platform.h" + +/* DSDL types used by dronecan.c handlers */ +#include "uavcan.protocol.NodeStatus.h" +#include "uavcan.protocol.GetNodeInfo.h" + +/* Canard core and STM32 driver declarations */ +#include "drivers/dronecan/libcanard/canard.h" +#include "drivers/dronecan/libcanard/canard_stm32_driver.h" + +/* INAV headers pulled in by dronecan.c — included here so the types are + available when we define stub globals below. */ +#include "io/gps.h" +#include "sensors/battery_sensor_dronecan.h" +#include "fc/runtime_config.h" +#include "sensors/diagnostics.h" +#include "build/version.h" + +/* 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); + +/* ========================================================================= + * 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 canardSTM32Recieve(CanardCANFrame *f) { (void)f; return 0; } +int16_t canardSTM32Transmit(const CanardCANFrame *f) { (void)f; return 1; } +void canardSTM32GetProtocolStatus(canardProtocolStatus_t *s) { memset(s, 0, sizeof(*s)); } +int32_t canardSTM32GetRxFifoFillLevel(void) { return 0; } +void canardSTM32RecoverFromBusOff(void) {} +void canardSTM32GetUniqueID(uint8_t id[16]) { memset(id, 0, 16); } + +/* Version strings declared in build/version.h */ +const char* const shortGitRevision = "00000000"; +const char* const compilerVersion = "test"; +const char* const targetName = "TEST"; +const char* const buildDate = "Jan 01 2026"; +const char* const buildTime = "00:00:00"; + +} /* extern "C" */ + +/* ========================================================================= + * Helper: encode a NodeStatus and build a single-frame CanardRxTransfer. + * buf must be at least UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE bytes. + * ========================================================================= */ +static CanardRxTransfer makeNodeStatusTransfer( + uint8_t nodeId, + uint32_t uptime_sec, + uint8_t health, + uint8_t mode, + uint16_t vendor_code, + uint8_t *buf) +{ + struct uavcan_protocol_NodeStatus ns; + memset(&ns, 0, sizeof(ns)); + ns.uptime_sec = uptime_sec; + ns.health = health; + ns.mode = mode; + ns.vendor_specific_status_code = vendor_code; + + uint32_t len = uavcan_protocol_NodeStatus_encode(&ns, buf); + + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeBroadcast; + xfer.data_type_id = UAVCAN_PROTOCOL_NODESTATUS_ID; + xfer.source_node_id = nodeId; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + +/* ========================================================================= + * Node table tests (GAP-N1 … GAP-N4) + * ========================================================================= */ + +class DroneCANNodeTableTest : public ::testing::Test { +protected: + 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; + } +}; + +/* 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(nullptr, &xfer); + + EXPECT_EQ(dronecanGetNodeCount(), 1u); + + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->nodeID, 10u); + EXPECT_EQ(node->health, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK); + EXPECT_EQ(node->mode, UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL); + EXPECT_EQ(node->uptime_sec, 100u); + EXPECT_EQ(node->vendor_status_code, 0xABCDu); + EXPECT_EQ(node->name_len, 0u); + EXPECT_EQ(node->name[0], '\0'); +} + +/* GAP-N1 (second node): Two distinct IDs → two separate entries */ +TEST_F(DroneCANNodeTableTest, TwoDistinctNodesStoredSeparately) +{ + CanardRxTransfer x1 = makeNodeStatusTransfer(10, 100, 0, 0, 0, buf); + CanardRxTransfer x2 = makeNodeStatusTransfer(20, 200, 0, 0, 0, buf); + handle_NodeStatus(nullptr, &x1); + handle_NodeStatus(nullptr, &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(nullptr, &x1); + ASSERT_EQ(dronecanGetNodeCount(), 1u); + + CanardRxTransfer x2 = makeNodeStatusTransfer( + 10, 500, + UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING, + UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE, + 0xBEEF, buf); + handle_NodeStatus(nullptr, &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(nullptr, &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(nullptr, &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(nullptr, &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(nullptr, &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(nullptr, &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, RejectsGetNodeInfoResponseBeforePhase3) +{ + /* Phase 3 will add response handling — until then this must return false + so the test documents the expected change and will fail when Phase 3 + wires up the response handler, prompting an update. */ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_FALSE(accept); +} + +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); +} From 265d965e5ab341c717bc6710faebdca66b115e6e Mon Sep 17 00:00:00 2001 From: daijoubu Date: Mon, 1 Jun 2026 17:06:37 -0700 Subject: [PATCH 03/42] =?UTF-8?q?test(dronecan):=20fix=20fixture=20for=20P?= =?UTF-8?q?hase=202=20=E2=80=94=20pass=20real=20CanardInstance=20to=20hand?= =?UTF-8?q?le=5FNodeStatus?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Phase 2 removed UNUSED(ins) and now calls canardRequestOrRespond(ins, ...) on new node discovery. Tests were passing nullptr which caused a segfault on the second node in any test that added two nodes. Fix: DroneCANNodeTableTest fixture initialises a real CanardInstance with a 4096-byte pool and local node ID 1. All handle_NodeStatus calls now pass &ins. --- .../unit/dronecan_application_unittest.cc | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index 980ba07748f..32a60f9a827 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -53,6 +53,7 @@ bool shouldAcceptTransfer(const CanardInstance *ins, 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 @@ -134,12 +135,17 @@ static CanardRxTransfer makeNodeStatusTransfer( 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 */ } }; @@ -153,7 +159,7 @@ TEST_F(DroneCANNodeTableTest, NewNodeAddedOnFirstStatus) UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK, UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL, 0xABCD, buf); - handle_NodeStatus(nullptr, &xfer); + handle_NodeStatus(&ins, &xfer); EXPECT_EQ(dronecanGetNodeCount(), 1u); @@ -173,8 +179,8 @@ TEST_F(DroneCANNodeTableTest, TwoDistinctNodesStoredSeparately) { CanardRxTransfer x1 = makeNodeStatusTransfer(10, 100, 0, 0, 0, buf); CanardRxTransfer x2 = makeNodeStatusTransfer(20, 200, 0, 0, 0, buf); - handle_NodeStatus(nullptr, &x1); - handle_NodeStatus(nullptr, &x2); + handle_NodeStatus(&ins, &x1); + handle_NodeStatus(&ins, &x2); EXPECT_EQ(dronecanGetNodeCount(), 2u); EXPECT_EQ(dronecanGetNode(0)->nodeID, 10u); @@ -189,7 +195,7 @@ TEST_F(DroneCANNodeTableTest, ExistingNodeUpdatedInPlace) UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK, UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL, 0x0000, buf); - handle_NodeStatus(nullptr, &x1); + handle_NodeStatus(&ins, &x1); ASSERT_EQ(dronecanGetNodeCount(), 1u); CanardRxTransfer x2 = makeNodeStatusTransfer( @@ -197,7 +203,7 @@ TEST_F(DroneCANNodeTableTest, ExistingNodeUpdatedInPlace) UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING, UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE, 0xBEEF, buf); - handle_NodeStatus(nullptr, &x2); + handle_NodeStatus(&ins, &x2); EXPECT_EQ(dronecanGetNodeCount(), 1u); /* still one node */ @@ -214,7 +220,7 @@ TEST_F(DroneCANNodeTableTest, LastSeenMsFollowsMillis) { mock_time_ms = 1000; CanardRxTransfer x1 = makeNodeStatusTransfer(20, 10, 0, 0, 0, buf); - handle_NodeStatus(nullptr, &x1); + handle_NodeStatus(&ins, &x1); const dronecanNodeInfo_t *node = dronecanGetNode(0); ASSERT_NE(node, nullptr); @@ -222,7 +228,7 @@ TEST_F(DroneCANNodeTableTest, LastSeenMsFollowsMillis) mock_time_ms = 2500; CanardRxTransfer x2 = makeNodeStatusTransfer(20, 20, 0, 0, 0, buf); - handle_NodeStatus(nullptr, &x2); + handle_NodeStatus(&ins, &x2); EXPECT_EQ(node->last_seen_ms, 2500u); } @@ -232,7 +238,7 @@ TEST_F(DroneCANNodeTableTest, LastSeenMsSetOnInsert) { mock_time_ms = 9999; CanardRxTransfer xfer = makeNodeStatusTransfer(5, 0, 0, 0, 0, buf); - handle_NodeStatus(nullptr, &xfer); + handle_NodeStatus(&ins, &xfer); const dronecanNodeInfo_t *node = dronecanGetNode(0); ASSERT_NE(node, nullptr); @@ -245,13 +251,13 @@ 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(nullptr, &xfer); + 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(nullptr, &overflow); + handle_NodeStatus(&ins, &overflow); EXPECT_EQ(dronecanGetNodeCount(), (uint8_t)DRONECAN_MAX_NODES); From f408fdf4ea666184b3e4d3c02417c3ec80d62227 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Mon, 1 Jun 2026 18:55:05 -0700 Subject: [PATCH 04/42] =?UTF-8?q?feat(dronecan):=20Phase=201-3=20=E2=80=94?= =?UTF-8?q?=20extend=20node=20struct,=20request=20and=20accept=20GetNodeIn?= =?UTF-8?q?fo?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Phase 1: Add sw/hw version fields to dronecanNodeInfo_t; zero-initialise on node insert so table is valid before the GetNodeInfo response arrives. Phase 2: Send GetNodeInfo request to each newly discovered node from handle_NodeStatus via canardRequestOrRespond. Phase 3: Accept GetNodeInfo responses in shouldAcceptTransfer so the response handler wired in Phase 4 will receive them. Tests: update AcceptsGetNodeInfoResponse (was canary RejectsBeforePhase3). --- src/test/unit/dronecan_application_unittest.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index 32a60f9a827..e7606b485cc 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -308,11 +308,10 @@ TEST(DroneCANShouldAcceptTransfer, AcceptsGetNodeInfoRequest) EXPECT_EQ(signature, UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE); } -TEST(DroneCANShouldAcceptTransfer, RejectsGetNodeInfoResponseBeforePhase3) +TEST(DroneCANShouldAcceptTransfer, AcceptsGetNodeInfoResponse) { - /* Phase 3 will add response handling — until then this must return false - so the test documents the expected change and will fail when Phase 3 - wires up the response handler, prompting an update. */ + /* 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, @@ -320,7 +319,8 @@ TEST(DroneCANShouldAcceptTransfer, RejectsGetNodeInfoResponseBeforePhase3) CanardTransferTypeResponse, 42); - EXPECT_FALSE(accept); + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE); } TEST(DroneCANShouldAcceptTransfer, RejectsUnknownBroadcastId) From b820b9526270ae510e89c8d95e44f22c0ea01158 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Mon, 1 Jun 2026 18:58:06 -0700 Subject: [PATCH 05/42] test(dronecan): add GAP-S2 dispatch test for GetNodeInfo response (red) Verifies onTransferReceived dispatches GetNodeInfo responses to handle_GetNodeInfoResponse and the node table entry is populated with name, sw version, hw version, and unique_id. Fails until Phase 4 implements the handler. --- .../unit/dronecan_application_unittest.cc | 89 +++++++++++++++++++ 1 file changed, 89 insertions(+) diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index e7606b485cc..a01058a10dc 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -346,3 +346,92 @@ TEST(DroneCANShouldAcceptTransfer, RejectsUnknownResponseId) EXPECT_FALSE(accept); } + +/* ========================================================================= + * onTransferReceived dispatch test (GAP-S2) + * + * Verifies that a GetNodeInfo response transfer is dispatched to + * handle_GetNodeInfoResponse and populates the node table entry. + * Written before Phase 4 — fails until the handler is implemented. + * ========================================================================= */ + +class DroneCANDispatchTest : public ::testing::Test { +protected: + CanardInstance ins; + uint8_t memory_pool[4096]; + uint8_t buf[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE + 16]; + + void SetUp() override { + activeNodeCount = 0; + memset(nodeTable, 0, sizeof(dronecanNodeInfo_t) * DRONECAN_MAX_NODES); + mock_time_ms = 0; + canardInit(&ins, memory_pool, sizeof(memory_pool), + onTransferReceived, shouldAcceptTransfer, NULL); + canardSetLocalNodeID(&ins, 1); + } +}; + +/* GAP-S2: GetNodeInfo response → handler populates name and version fields */ +TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry) +{ + /* Pre-insert node 42 via a NodeStatus so the table has a slot for it */ + uint8_t ns_buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4]; + CanardRxTransfer ns_xfer = makeNodeStatusTransfer(42, 10, 0, 0, 0, ns_buf); + handle_NodeStatus(&ins, &ns_xfer); + ASSERT_EQ(dronecanGetNodeCount(), 1u); + + /* Build a GetNodeInfo response from node 42 */ + struct uavcan_protocol_GetNodeInfoResponse resp; + memset(&resp, 0, sizeof(resp)); + + resp.status.uptime_sec = 10; + resp.status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + resp.status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + + resp.software_version.major = 1; + resp.software_version.minor = 7; + resp.software_version.optional_field_flags = 1; /* vcs_commit valid */ + resp.software_version.vcs_commit = 0xDEADBEEF; + + resp.hardware_version.major = 2; + resp.hardware_version.minor = 0; + for (int i = 0; i < 16; i++) { + resp.hardware_version.unique_id[i] = (uint8_t)(0xA0 + i); + } + + const char *name = "com.example.gps"; + resp.name.len = (uint8_t)strlen(name); + memcpy(resp.name.data, name, resp.name.len); + + uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&resp, buf); + + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_GETNODEINFO_ID; + xfer.source_node_id = 42; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)encoded_len; + + onTransferReceived(&ins, &xfer); + + /* Verify the node table entry was populated */ + const dronecanNodeInfo_t *node = dronecanGetNode(0); + ASSERT_NE(node, nullptr); + EXPECT_EQ(node->nodeID, 42u); + + EXPECT_EQ(node->name_len, (uint8_t)strlen(name)); + EXPECT_EQ(0, memcmp(node->name, name, node->name_len)); + + EXPECT_EQ(node->sw_major, 1u); + EXPECT_EQ(node->sw_minor, 7u); + EXPECT_EQ(node->sw_optional_field_flags, 1u); + EXPECT_EQ(node->sw_vcs_commit, 0xDEADBEEFu); + + EXPECT_EQ(node->hw_major, 2u); + EXPECT_EQ(node->hw_minor, 0u); + for (int i = 0; i < 16; i++) { + EXPECT_EQ(node->hw_unique_id[i], (uint8_t)(0xA0 + i)) + << "unique_id mismatch at byte " << i; + } +} From 0b73657ba7f25a96fc0cc17a0b4795200c409ea0 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Mon, 1 Jun 2026 19:30:55 -0700 Subject: [PATCH 06/42] refactor(dronecan): standardise on nodeID for local variables MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Rename local variable nodeId → nodeID in handle_NodeStatus, handle_GetNodeInfoResponse, and the MSP2_INAV_DRONECAN_NODE_INFO handler to match the struct field naming used throughout dronecanNodeInfo_t, dronecanNodeStatus_t, and dronecanConfig_t. --- src/main/fc/fc_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 9da0b461562..d5d327be31d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4592,12 +4592,12 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ERROR; break; } - uint8_t nodeId = sbufReadU8(src); + 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) { + if (node->nodeID == nodeID) { found = true; if (sbufBytesRemaining(dst) < 46) { *ret = MSP_RESULT_ERROR; From df5360481039bd64b33b836056eb2c1a54746300 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Mon, 1 Jun 2026 20:23:22 -0700 Subject: [PATCH 07/42] =?UTF-8?q?feat(dronecan):=20Phase=205=20=E2=80=94?= =?UTF-8?q?=20extend=20MSP2=5FINAV=5FDRONECAN=5FNODE=5FINFO=20to=2071=20by?= =?UTF-8?q?tes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Append sw_major, sw_minor, sw_optional_field_flags, sw_vcs_commit, hw_major, hw_minor, and hw_unique_id after the existing 46-byte payload. Update size guard from 46 to 71 bytes. Remove duplicate found = true. --- src/main/fc/fc_msp.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d5d327be31d..e2cc5df6264 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4599,7 +4599,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu const dronecanNodeInfo_t *node = dronecanGetNode(i); if (node->nodeID == nodeID) { found = true; - if (sbufBytesRemaining(dst) < 46) { + if (sbufBytesRemaining(dst) < 71) { *ret = MSP_RESULT_ERROR; break; } @@ -4611,9 +4611,16 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu sbufWriteU32(dst, millis() - node->last_seen_ms); sbufWriteU8(dst, node->name_len); sbufWriteDataSafe(dst, node->name, 32); - found = true; + sbufWriteU8(dst, node->sw_major); + sbufWriteU8(dst, node->sw_minor); + sbufWriteU8(dst, node->sw_optional_field_flags); + sbufWriteU32(dst, node->sw_vcs_commit); + sbufWriteU8(dst, node->hw_major); + sbufWriteU8(dst, node->hw_minor); + sbufWriteData(dst, node->hw_unique_id, 16); *ret = MSP_RESULT_ACK; break; + } } if (!found) { From af02bfbd11de536cd3f68f87eaf2afb8282d7b79 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Mon, 1 Jun 2026 22:14:59 -0700 Subject: [PATCH 08/42] docs(dronecan): update msp_messages.json for 71-byte NODE_INFO response Add sw_major, sw_minor, sw_optional_field_flags, sw_vcs_commit, hw_major, hw_minor, hw_unique_id fields to MSP2_INAV_DRONECAN_NODE_INFO reply payload. Update notes to document 71-byte response size and conditional availability depending on GetNodeInfo response receipt. --- docs/development/msp/msp_messages.json | 52 +++++++++++++++++++++++--- 1 file changed, 47 insertions(+), 5 deletions(-) diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index e8f64d0cb93..601d7888391 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9903,13 +9903,55 @@ { "name": "name", "ctype": "char[32]", - "desc": "Node name up to 32 bytes, zero-padded", + "desc": "Node name up to 32 bytes, zero-padded", "units": "" - } - ] + }, + { + "name": "sw_major", + "ctype": "uint8_t", + "desc": "Software version major (from GetNodeInfo response)", + "units": "" + }, + { + "name": "sw_minor", + "ctype": "uint8_t", + "desc": "Software version minor (from GetNodeInfo response)", + "units": "" + }, + { + "name": "sw_optional_field_flags", + "ctype": "uint8_t", + "desc": "UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid", + "units": "" + }, + { + "name": "sw_vcs_commit", + "ctype": "uint32_t", + "desc": "Git commit hash (valid when sw_optional_field_flags bit 0 is set)", + "units": "" + }, + { + "name": "hw_major", + "ctype": "uint8_t", + "desc": "Hardware version major (from GetNodeInfo response)", + "units": "" + }, + { + "name": "hw_minor", + "ctype": "uint8_t", + "desc": "Hardware version minor (from GetNodeInfo response)", + "units": "" + }, + { + "name": "hw_unique_id", + "ctype": "uint8_t[16]", + "desc": "128-bit hardware unique ID (from GetNodeInfo response)", + "units": "" + } + ] }, - "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table.", - "description": "Returns full status detail for a single DroneCAN node by ID." + "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is 71 bytes when GetNodeInfo data has been received from the node (46 bytes if not yet received).", + "description": "Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service." }, "MSP2_INAV_LED_STRIP_CONFIG_EX": { "code": 8264, From cb818646c01fce4a8d0e2686e3f239dfc3806a3f Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 06:07:06 -0700 Subject: [PATCH 09/42] fix(dronecan): commit missing dronecan.h struct extension The sw/hw version fields added to dronecanNodeInfo_t were only in the working tree and never staged. A clean checkout of this branch would fail to compile all subsequent commits that write or read those fields. --- src/main/drivers/dronecan/dronecan.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index c69981b9692..4d7bc18c15c 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -36,6 +36,15 @@ typedef struct dronecanNodeInfo_s { uint32_t last_seen_ms; uint8_t name_len; char name[32]; + /* Software version (from GetNodeInfo response)*/ + uint8_t sw_major; + uint8_t sw_minor; + uint8_t sw_optional_field_flags; + uint32_t sw_vcs_commit; + /* Hardware version (from GetNodeInfo response)*/ + uint8_t hw_major; + uint8_t hw_minor; + uint8_t hw_unique_id[16]; } dronecanNodeInfo_t; // Wire format for MSP2_INAV_DRONECAN_NODES records (7 bytes each, packed). From beac9c20f108e97b91421fe03104c229da28665d Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 06:07:14 -0700 Subject: [PATCH 10/42] fix(dronecan): address code review findings - Use static transfer_id for GetNodeInfo requests; per Canard API the pointer must outlive the call so the library can increment it across successive transfers. Stack-local was a protocol violation. Add TODO for retry-on-failure (currently a single attempt at discovery). - Fix log message typo: "GetNodeInfoRespones" -> "GetNodeInfoResponse" - Use UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT named constant instead of magic literal 1 - Use sbufWriteDataSafe for hw_unique_id, consistent with name field --- src/main/fc/fc_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e2cc5df6264..1b014c0c4b7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4617,7 +4617,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu sbufWriteU32(dst, node->sw_vcs_commit); sbufWriteU8(dst, node->hw_major); sbufWriteU8(dst, node->hw_minor); - sbufWriteData(dst, node->hw_unique_id, 16); + sbufWriteDataSafe(dst, node->hw_unique_id, 16); *ret = MSP_RESULT_ACK; break; From 9a26e3dbeda8ab5c041c5e8c1b81ff4c700a8125 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 06:17:29 -0700 Subject: [PATCH 11/42] chore(dronecan): strip trailing whitespace from dronecan.c/.h, tidy case block Remove trailing spaces/tabs throughout dronecan.c and dronecan.h. Remove spurious blank line inside MSP2_INAV_DRONECAN_NODE_INFO case block in fc_msp.c. No functional change. --- src/main/fc/fc_msp.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 1b014c0c4b7..d7451c2945a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4620,7 +4620,6 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu sbufWriteDataSafe(dst, node->hw_unique_id, 16); *ret = MSP_RESULT_ACK; break; - } } if (!found) { From d002ffc6033b57b520918d7e13fd091789077693 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 14:03:20 -0700 Subject: [PATCH 12/42] docs(msp): correct last_seen_ms field semantics in DRONECAN messages The field carries elapsed milliseconds (millis() - last_seen_ms at request time), not an absolute FC timestamp. Update the NODE_INFO field description and the NODES notes string to reflect the actual wire semantics. --- docs/development/msp/msp_messages.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 601d7888391..18f9839d850 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9840,7 +9840,7 @@ ] }, "variable_len": true, - "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4). Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.", + "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.", "description": "Returns the list of all detected DroneCAN nodes with their current status." }, "MSP2_INAV_DRONECAN_NODE_INFO": { @@ -9890,8 +9890,8 @@ }, { "name": "last_seen_ms", - "ctype": "uint32_t", - "desc": "FC millisecond timestamp when this node was last seen", + "ctype": "uint32_t", + "desc": "Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request)", "units": "ms" }, { From 8d367b014374b4d3b3e08bd752a3188b3c0ddfc1 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 14:06:42 -0700 Subject: [PATCH 13/42] fix(dronecan): use per-node transfer_id for GetNodeInfo requests Canard requires a separate transfer_id counter per (data_type_id, dst_node_id) descriptor. The previous static local was shared across all destination nodes, violating the protocol and risking mismatched or dropped responses when multiple nodes are discovered simultaneously. Add getNodeInfo_transfer_id to dronecanNodeInfo_t, zero-init it at node insertion, and pass its address to canardRequestOrRespond. --- src/main/drivers/dronecan/dronecan.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index 4d7bc18c15c..7a2359be700 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -45,6 +45,9 @@ typedef struct dronecanNodeInfo_s { uint8_t hw_major; uint8_t hw_minor; uint8_t hw_unique_id[16]; + /* Canard transfer ID for outgoing GetNodeInfo requests to this node. + * Must be per-node: Canard forbids sharing a counter across different dst_node_id. */ + uint8_t getNodeInfo_transfer_id; } dronecanNodeInfo_t; // Wire format for MSP2_INAV_DRONECAN_NODES records (7 bytes each, packed). From 98fc56b4c1c37df186ceb21e625748e8e61594a4 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 14:58:19 -0700 Subject: [PATCH 14/42] fix(msp): replace magic 71 with documented field sum in NODE_INFO guard The buffer size check for MSP2_INAV_DRONECAN_NODE_INFO now uses an explicit per-field sum matching the writes that follow, so the check stays in sync when fields are added or removed. --- src/main/fc/fc_msp.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d7451c2945a..43a856c01eb 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4599,7 +4599,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu const dronecanNodeInfo_t *node = dronecanGetNode(i); if (node->nodeID == nodeID) { found = true; - if (sbufBytesRemaining(dst) < 71) { + // nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) + // +name_len(1)+name(32)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) + // +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 71 + if (sbufBytesRemaining(dst) < (1+1+1+4+2+4+1+32+1+1+1+4+1+1+16)) { *ret = MSP_RESULT_ERROR; break; } From 72289e03857bd1fc2b92911655dc851ecac5ae47 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 15:32:27 -0700 Subject: [PATCH 15/42] fix(dronecan): limit bus-off recovery to 50 attempts before entering FAILED Previously busoffTimeUs was reset unconditionally, causing endless 20ms recovery attempts on a permanently-faulted bus with no exit path. After 50 attempts (~1 second at 20ms cadence) without clearing BusOff, transition to STATE_DRONECAN_FAILED and log the failure. The retry counter resets on successful recovery so a transient fault is handled correctly. --- src/main/drivers/dronecan/dronecan.c | 42 ++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index e58085b9600..4b17fae8582 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -194,11 +194,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; @@ -638,4 +645,39 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } } } + + // Transmitting + for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;) + { + const int16_t tx_res = canardSTM32Transmit(tx_frame); + + if (tx_res < 0) { + LOG_DEBUG(CAN, "Transmit error %d", tx_res); + canardPopTxQueue(&canard); // Error - discard frame + } else if (tx_res > 0) { + canardPopTxQueue(&canard); // Success - remove from queue + } else { + // tx_res == 0: TX FIFO full, retry later + break; + } + } + +} + +/* + This function is called at 1 Hz rate from the main loop. +*/ +void process1HzTasks(timeUs_t timestamp_usec) +{ + /* + Purge transfers that are no longer transmitted. This can free up some memory + */ + canardCleanupStaleTransfers(&canard, timestamp_usec); + + /* + Transmit the node status message + */ + send_NodeStatus(); +} + #endif From 76d8ac701c770ee1df4acbfb04680c8d6e0b2781 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 15:52:22 -0700 Subject: [PATCH 16/42] refactor(dronecan): add dronecanGetNodeByID() and eliminate duplicate lookups Add a private findNodeByID() helper in dronecan.c and a public dronecanGetNodeByID() accessor in the driver API. Use them to consolidate three independent linear-search loops that all performed the same nodeID lookup: - handle_NodeStatus: replace loop with findNodeByID() - handle_GetNodeInfoResponse: replace loop with findNodeByID() - MSP2_INAV_DRONECAN_NODE_INFO handler: replace loop+found flag with dronecanGetNodeByID(), simplifying to a flat early-exit structure --- src/main/drivers/dronecan/dronecan.c | 88 ++++++++++++++++++++++++---- src/main/drivers/dronecan/dronecan.h | 1 + src/main/fc/fc_msp.c | 57 ++++++++---------- 3 files changed, 102 insertions(+), 44 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 4b17fae8582..3e6fccf04f3 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -331,6 +331,53 @@ static void processCanardTxQueueSafe(void) { // NOTE: All canard handlers and senders are based on this reference: https://dronecan.github.io/Specification/7._List_of_standard_data_types/ // Alternatively, you can look at the corresponding generated header file in the dsdlc_generated folder +static dronecanNodeInfo_t *findNodeByID(uint8_t nodeID) { + for (uint8_t i = 0; i < activeNodeCount; i++) { + if (nodeTable[i].nodeID == nodeID) { + return &nodeTable[i]; + } + } + return NULL; +} + +const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) { + return findNodeByID(nodeID); +} + +static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *transfer) +{ + UNUSED(ins); + struct uavcan_protocol_GetNodeInfoResponse resp; + + if (uavcan_protocol_GetNodeInfoResponse_decode(transfer, &resp)) { + LOG_DEBUG(CAN, "GetNodeInfoResponse decode failed"); + return; + } + + uint8_t nodeID = transfer->source_node_id; + dronecanNodeInfo_t *node = findNodeByID(nodeID); + if (!node) { + LOG_DEBUG(CAN, "GetNodeInfoResponse from unknown node %u", nodeID); + return; + } + + uint8_t len = resp.name.len < 32 ? resp.name.len : 32; + node->name_len = len; + memcpy(node->name, resp.name.data, len); + + node->sw_major = resp.software_version.major; + node->sw_minor = resp.software_version.minor; + node->sw_optional_field_flags = resp.software_version.optional_field_flags; + node->sw_vcs_commit = (resp.software_version.optional_field_flags & UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT) + ? resp.software_version.vcs_commit : 0; + + node->hw_major = resp.hardware_version.major; + node->hw_minor = resp.hardware_version.minor; + memcpy(node->hw_unique_id, resp.hardware_version.unique_id, 16); +} +// Canard 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 @@ -460,7 +507,6 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, // Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); struct uavcan_protocol_NodeStatus nodeStatus; if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { @@ -469,16 +515,14 @@ 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) { @@ -490,9 +534,26 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { nodeTable[activeNodeCount].name_len = 0; nodeTable[activeNodeCount].name[0] = 0; nodeTable[activeNodeCount].last_seen_ms = millis(); + nodeTable[activeNodeCount].sw_major = 0; + nodeTable[activeNodeCount].sw_minor = 0; + nodeTable[activeNodeCount].sw_optional_field_flags = 0; + nodeTable[activeNodeCount].sw_vcs_commit = 0; + nodeTable[activeNodeCount].hw_major = 0; + nodeTable[activeNodeCount].hw_minor = 0; + memset(nodeTable[activeNodeCount].hw_unique_id, 0, 16); + nodeTable[activeNodeCount].getNodeInfo_transfer_id = 0; activeNodeCount++; - } + dronecanMaskTxISR(); + const int16_t res = canardRequestOrRespond(ins, nodeId, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID, + &nodeTable[activeNodeCount - 1].getNodeInfo_transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, CanardRequest, NULL, 0); + dronecanUnmaskTxISR(); + if (res < 0) { + LOG_DEBUG(CAN, "GetNodeInfo request failed for node %u: %d", nodeId, res); + } + } } static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -612,6 +673,9 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } if (transfer->transfer_type == CanardTransferTypeResponse) { switch (transfer->data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + handle_GetNodeInfoResponse(ins, transfer); + break; } } if (transfer->transfer_type == CanardTransferTypeBroadcast) { diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index 7a2359be700..ddd74e21584 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -66,5 +66,6 @@ uint32_t dronecanGetBitrateKbps(void); const dronecanNodeInfo_t *dronecanGetNode(uint8_t index); uint32_t dronecanGetBusOffCount(void); CanardPoolAllocatorStatistics dronecanGetPoolStats(void); +const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID); PG_DECLARE(dronecanConfig_t, dronecanConfig); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 43a856c01eb..8f5f97a3c2f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4593,41 +4593,34 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu 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; - // nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) - // +name_len(1)+name(32)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) - // +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 71 - if (sbufBytesRemaining(dst) < (1+1+1+4+2+4+1+32+1+1+1+4+1+1+16)) { - *ret = MSP_RESULT_ERROR; - break; - } - sbufWriteU8(dst, node->nodeID); - sbufWriteU8(dst, node->health); - sbufWriteU8(dst, node->mode); - sbufWriteU32(dst, node->uptime_sec); - sbufWriteU16(dst, node->vendor_status_code); - sbufWriteU32(dst, millis() - node->last_seen_ms); - sbufWriteU8(dst, node->name_len); - sbufWriteDataSafe(dst, node->name, 32); - sbufWriteU8(dst, node->sw_major); - sbufWriteU8(dst, node->sw_minor); - sbufWriteU8(dst, node->sw_optional_field_flags); - sbufWriteU32(dst, node->sw_vcs_commit); - sbufWriteU8(dst, node->hw_major); - sbufWriteU8(dst, node->hw_minor); - sbufWriteDataSafe(dst, node->hw_unique_id, 16); - *ret = MSP_RESULT_ACK; - break; - } + const dronecanNodeInfo_t *node = dronecanGetNodeByID(nodeID); + if (!node) { + *ret = MSP_RESULT_ERROR; + break; } - if (!found) { + // nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) + // +name_len(1)+name(32)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) + // +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 71 + if (sbufBytesRemaining(dst) < (1+1+1+4+2+4+1+32+1+1+1+4+1+1+16)) { *ret = MSP_RESULT_ERROR; + break; } + sbufWriteU8(dst, node->nodeID); + sbufWriteU8(dst, node->health); + sbufWriteU8(dst, node->mode); + sbufWriteU32(dst, node->uptime_sec); + sbufWriteU16(dst, node->vendor_status_code); + sbufWriteU32(dst, millis() - node->last_seen_ms); + sbufWriteU8(dst, node->name_len); + sbufWriteDataSafe(dst, node->name, 32); + sbufWriteU8(dst, node->sw_major); + sbufWriteU8(dst, node->sw_minor); + sbufWriteU8(dst, node->sw_optional_field_flags); + sbufWriteU32(dst, node->sw_vcs_commit); + sbufWriteU8(dst, node->hw_major); + sbufWriteU8(dst, node->hw_minor); + sbufWriteDataSafe(dst, node->hw_unique_id, 16); + *ret = MSP_RESULT_ACK; } break; #endif From 8940597a4a2ec497f25a50c1674907fc573ba0e3 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 15:55:22 -0700 Subject: [PATCH 17/42] refactor(msp): replace inline field sum with MSP2_DRONECAN_NODE_INFO_SIZE constant Define the constant near the DroneCAN include with a breakdown comment showing each field's contribution. The check site now reads clearly and the single definition is the one place to update when the wire format changes. --- src/main/fc/fc_msp.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8f5f97a3c2f..6500d65d9bd 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -146,6 +146,11 @@ #ifdef USE_DRONECAN #include "drivers/dronecan/dronecan.h" +// MSP2_INAV_DRONECAN_NODE_INFO reply size: +// nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) +// +name_len(1)+name(32)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) +// +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 71 +#define MSP2_DRONECAN_NODE_INFO_SIZE 71 #endif extern timeDelta_t cycleTime; // FIXME dependency on mw.c @@ -4598,10 +4603,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ERROR; break; } - // nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) - // +name_len(1)+name(32)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) - // +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 71 - if (sbufBytesRemaining(dst) < (1+1+1+4+2+4+1+32+1+1+1+4+1+1+16)) { + if (sbufBytesRemaining(dst) < MSP2_DRONECAN_NODE_INFO_SIZE) { *ret = MSP_RESULT_ERROR; break; } From 2d72d1e91870a59c7d9821e49341846707349a6d Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 15:56:30 -0700 Subject: [PATCH 18/42] refactor(msp): move MSP2_DRONECAN_NODE_INFO_SIZE to msp_protocol_v2_inav.h The size constant belongs alongside the message code definition, not inside the implementation file. Removed from fc_msp.c, added with field breakdown comment next to MSP2_INAV_DRONECAN_NODE_INFO. --- src/main/fc/fc_msp.c | 5 ----- src/main/msp/msp_protocol_v2_inav.h | 7 ++++++- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6500d65d9bd..683572aa882 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -146,11 +146,6 @@ #ifdef USE_DRONECAN #include "drivers/dronecan/dronecan.h" -// MSP2_INAV_DRONECAN_NODE_INFO reply size: -// nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) -// +name_len(1)+name(32)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) -// +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 71 -#define MSP2_DRONECAN_NODE_INFO_SIZE 71 #endif extern timeDelta_t cycleTime; // FIXME dependency on mw.c diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 180597c4527..49b9a0f76c8 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -97,7 +97,12 @@ #define MSP2_INAV_ESC_TELEM 0x2041 #define MSP2_INAV_DRONECAN_NODES 0x2042 -#define MSP2_INAV_DRONECAN_NODE_INFO 0x2043 +#define MSP2_INAV_DRONECAN_NODE_INFO 0x2043 +// MSP2_INAV_DRONECAN_NODE_INFO reply size: +// nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) +// +name_len(1)+name(32)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) +// +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 71 +#define MSP2_DRONECAN_NODE_INFO_SIZE 71 #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 From b0837e6833b2cfa6edaae753141fc160f75d407f Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 15:58:18 -0700 Subject: [PATCH 19/42] fix(dronecan): use memset for new-node slot init; fix stale test comment MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace eight individual zero-assignments with memset() on the whole dronecanNodeInfo_t slot before setting non-zero fields. Future struct additions are zeroed automatically. Update coverage comment in dronecan_application_unittest.cc — GetNodeInfo response acceptance has been implemented since Phase 3; remove the stale 'Phase 3 will change this' note. --- src/main/drivers/dronecan/dronecan.c | 11 +---------- src/test/unit/dronecan_application_unittest.cc | 2 +- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 3e6fccf04f3..5e6205d752d 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -526,22 +526,13 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { } // 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(); - nodeTable[activeNodeCount].sw_major = 0; - nodeTable[activeNodeCount].sw_minor = 0; - nodeTable[activeNodeCount].sw_optional_field_flags = 0; - nodeTable[activeNodeCount].sw_vcs_commit = 0; - nodeTable[activeNodeCount].hw_major = 0; - nodeTable[activeNodeCount].hw_minor = 0; - memset(nodeTable[activeNodeCount].hw_unique_id, 0, 16); - nodeTable[activeNodeCount].getNodeInfo_transfer_id = 0; activeNodeCount++; dronecanMaskTxISR(); diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index a01058a10dc..a05d8bd6ed4 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -11,7 +11,7 @@ * 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 ✗ (Phase 3 will change this), unknown ID ✗ + * GetNodeInfo response ✓, unknown ID ✗ */ #include "gtest/gtest.h" From b8f036a7bc469c90c0f8d7f32b24d2c6b107edd4 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 17:06:17 -0700 Subject: [PATCH 20/42] fix(dronecan): extend node name to 80 bytes; log table overflow - Extend dronecanNodeInfo_t name field from 32 to 80 bytes to match the full UAVCAN GetNodeInfo name length (spec allows up to 80 bytes) - Use sizeof(node->name) in truncation logic so it tracks the struct - Update MSP2_INAV_DRONECAN_NODE_INFO wire format: 71 -> 119 bytes - Log when node table is full and a new node is silently dropped --- docs/development/msp/msp_messages.json | 6 +++--- src/main/drivers/dronecan/dronecan.c | 7 ++++--- src/main/drivers/dronecan/dronecan.h | 2 +- src/main/fc/fc_msp.c | 2 +- src/main/msp/msp_protocol_v2_inav.h | 6 +++--- 5 files changed, 12 insertions(+), 11 deletions(-) diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 18f9839d850..4dc50b37189 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9902,8 +9902,8 @@ }, { "name": "name", - "ctype": "char[32]", - "desc": "Node name up to 32 bytes, zero-padded", + "ctype": "char[80]", + "desc": "Node name up to 80 bytes, zero-padded", "units": "" }, { @@ -9950,7 +9950,7 @@ } ] }, - "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is 71 bytes when GetNodeInfo data has been received from the node (46 bytes if not yet received).", + "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes.", "description": "Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service." }, "MSP2_INAV_LED_STRIP_CONFIG_EX": { diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 5e6205d752d..1ae91d09d77 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -344,8 +344,7 @@ const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) { return findNodeByID(nodeID); } -static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *transfer) -{ +static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *transfer) { UNUSED(ins); struct uavcan_protocol_GetNodeInfoResponse resp; @@ -361,7 +360,7 @@ static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *tr return; } - uint8_t len = resp.name.len < 32 ? resp.name.len : 32; + uint8_t len = resp.name.len < sizeof(node->name) ? resp.name.len : sizeof(node->name); node->name_len = len; memcpy(node->name, resp.name.data, len); @@ -544,6 +543,8 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { if (res < 0) { LOG_DEBUG(CAN, "GetNodeInfo request failed for node %u: %d", nodeId, res); } + } else { + LOG_DEBUG(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeId); } } diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index ddd74e21584..c4029cb0244 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -35,7 +35,7 @@ typedef struct dronecanNodeInfo_s { uint16_t vendor_status_code; uint32_t last_seen_ms; uint8_t name_len; - char name[32]; + char name[80]; /* Software version (from GetNodeInfo response)*/ uint8_t sw_major; uint8_t sw_minor; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 683572aa882..e24faa3b1c8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4609,7 +4609,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu sbufWriteU16(dst, node->vendor_status_code); sbufWriteU32(dst, millis() - node->last_seen_ms); sbufWriteU8(dst, node->name_len); - sbufWriteDataSafe(dst, node->name, 32); + sbufWriteDataSafe(dst, node->name, 80); sbufWriteU8(dst, node->sw_major); sbufWriteU8(dst, node->sw_minor); sbufWriteU8(dst, node->sw_optional_field_flags); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 49b9a0f76c8..c34ca2229ce 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -100,9 +100,9 @@ #define MSP2_INAV_DRONECAN_NODE_INFO 0x2043 // MSP2_INAV_DRONECAN_NODE_INFO reply size: // nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) -// +name_len(1)+name(32)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) -// +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 71 -#define MSP2_DRONECAN_NODE_INFO_SIZE 71 +// +name_len(1)+name(80)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) +// +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 119 +#define MSP2_DRONECAN_NODE_INFO_SIZE 119 #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 From e91fb80c6899ee255347b8fd81a03e042ae6ea05 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 25 Jun 2026 21:23:36 -0700 Subject: [PATCH 21/42] fixup: remove orphaned TX loop and duplicate process1HzTasks from rebase artifact Orphaned blob (old non-static TX for-loop + stray closing brace + duplicate non-static process1HzTasks) slipped through the 27-commit getnodeinfo rebase. The correct versions already exist: processCanardTxQueue() for ISR-driven TX and static process1HzTasks() at line 426 with NVIC masking. --- src/main/drivers/dronecan/dronecan.c | 33 ---------------------------- 1 file changed, 33 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 1ae91d09d77..90062b18d44 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -702,38 +702,5 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } } - // Transmitting - for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;) - { - const int16_t tx_res = canardSTM32Transmit(tx_frame); - - if (tx_res < 0) { - LOG_DEBUG(CAN, "Transmit error %d", tx_res); - canardPopTxQueue(&canard); // Error - discard frame - } else if (tx_res > 0) { - canardPopTxQueue(&canard); // Success - remove from queue - } else { - // tx_res == 0: TX FIFO full, retry later - break; - } - } - -} - -/* - This function is called at 1 Hz rate from the main loop. -*/ -void process1HzTasks(timeUs_t timestamp_usec) -{ - /* - Purge transfers that are no longer transmitted. This can free up some memory - */ - canardCleanupStaleTransfers(&canard, timestamp_usec); - - /* - Transmit the node status message - */ - send_NodeStatus(); -} #endif From 5f53f6a14ea63350b6dc7aa59b40bf5be1e597d6 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 26 Jun 2026 08:00:30 -0700 Subject: [PATCH 22/42] fix(dronecan): restore shouldAcceptTransfer response case; expose test symbols MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Two fixes found by running unit tests after the getnodeinfo rebase: 1. shouldAcceptTransfer response block was empty — GetNodeInfo responses were silently dropped by canard before handle_GetNodeInfoResponse could run. Restores the UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE case that was present on the pre-rebase branch but clobbered during conflict resolution. 2. UNIT_TEST build: expose handle_NodeStatus, shouldAcceptTransfer, and onTransferReceived as non-static so dronecan_application_unittest.cc can call them directly. Add txErrCount/busOffCount to UNIT_TEST block. Fix stub typo canardSTM32Recieve→canardSTM32Receive and add missing canardSTM32GetAndClearRxDropCount stub. All 13 dronecan_application_unittest + 13 dronecan_getnodeinfo_unittest pass. --- src/main/drivers/dronecan/dronecan.c | 25 ++++++++++++++++++- .../unit/dronecan_application_unittest.cc | 3 ++- 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 90062b18d44..b0375a60332 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -42,6 +42,8 @@ static dronecanState_e dronecanState = STATE_DRONECAN_INIT; #ifdef UNIT_TEST uint8_t activeNodeCount = 0; dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; +static volatile uint32_t txErrCount = 0; +static uint32_t busOffCount = 0; #else static uint8_t activeNodeCount = 0; static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; @@ -64,8 +66,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 ------------------------------------------------------------- @@ -447,7 +455,11 @@ static void process1HzTasks(timeUs_t timestamp_usec) This function must fill in the out_data_type_signature to be the signature of the message. */ +#ifdef UNIT_TEST +bool shouldAcceptTransfer(const CanardInstance *ins, +#else static bool shouldAcceptTransfer(const CanardInstance *ins, +#endif uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, @@ -465,8 +477,11 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, } } if (transfer_type == CanardTransferTypeResponse) { - // check if we want to handle a specific service request switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE; + return true; + } } } if (transfer_type == CanardTransferTypeBroadcast) { @@ -505,7 +520,11 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, // Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) +#ifdef UNIT_TEST +void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { +#else static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { +#endif struct uavcan_protocol_NodeStatus nodeStatus; if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { @@ -652,7 +671,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 diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index a05d8bd6ed4..e602491d82c 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -82,7 +82,8 @@ void dronecanBatterySensorReceiveInfo(struct uavcan_equipment_power_BatteryInfo /* STM32 CAN driver stubs */ int16_t canardSTM32CAN1_Init(uint32_t b) { (void)b; return CANARD_OK; } -int16_t canardSTM32Recieve(CanardCANFrame *f) { (void)f; return 0; } +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; } From 2d73d754a5e8afefc608079bf92899ece382f81f Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 1 Jul 2026 09:38:48 -0700 Subject: [PATCH 23/42] fix(dronecan): address code review findings on getnodeinfo branch - Add transfer ID guard in handle_GetNodeInfoResponse to reject stale or replayed frames (uses per-node getNodeInfo_transfer_id counter) - Use sizeof(node->hw_unique_id) instead of magic number 16 in memcpy - Fix comment label: "Canard Senders" -> "Canard Handlers and Senders" - Remove extra blank line before #endif at end of file - Rename MSP field last_seen_ms -> elapsed_ms to match notes wording - Fix TwoDistinctNodesStoredSeparately test: interleave encode+dispatch so node 10 is not inserted with node 20's payload data --- docs/development/msp/README.md | 19 +++++++++++++------ docs/development/msp/msp_messages.json | 4 ++-- src/main/drivers/dronecan/dronecan.c | 10 +++++++--- .../unit/dronecan_application_unittest.cc | 2 +- 4 files changed, 23 insertions(+), 12 deletions(-) diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index fafcf5d4762..56d177ffbad 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -4170,10 +4170,10 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `nodeCount` | `uint8_t` | 1 | Number of detected DroneCAN nodes | | `nodeData` | `dronecanNodeStatus_t[]` | array | Array of per-node status records, one per detected node. Each record: nodeID(1)+health(1)+mode(1)+last_seen_ms(4) = 7 bytes. Full detail available via MSP2_INAV_DRONECAN_NODE_INFO. | -**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4). Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO. +**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO. ## `MSP2_INAV_DRONECAN_NODE_INFO (8259 / 0x2043)` -**Description:** Returns full status detail for a single DroneCAN node by ID. +**Description:** Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service. **Request Payload:** |Field|C Type|Size (Bytes)|Description| @@ -4188,11 +4188,18 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `mode` | `uint8_t` | 1 | - | Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE | | `uptime_sec` | `uint32_t` | 4 | s | Node uptime in seconds | | `vendor_status_code` | `uint16_t` | 2 | - | Vendor-specific status code | -| `last_seen_ms` | `uint32_t` | 4 | ms | FC millisecond timestamp when this node was last seen | +| `elapsed_ms` | `uint32_t` | 4 | ms | Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request) | | `name_len` | `uint8_t` | 1 | - | Length of node name string (0 if unknown) | -| `name` | `char[32]` | 32 | - | Node name up to 32 bytes, zero-padded | - -**Notes:** Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. +| `name` | `char[80]` | 80 | - | Node name up to 80 bytes, zero-padded | +| `sw_major` | `uint8_t` | 1 | - | Software version major (from GetNodeInfo response) | +| `sw_minor` | `uint8_t` | 1 | - | Software version minor (from GetNodeInfo response) | +| `sw_optional_field_flags` | `uint8_t` | 1 | - | UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid | +| `sw_vcs_commit` | `uint32_t` | 4 | - | Git commit hash (valid when sw_optional_field_flags bit 0 is set) | +| `hw_major` | `uint8_t` | 1 | - | Hardware version major (from GetNodeInfo response) | +| `hw_minor` | `uint8_t` | 1 | - | Hardware version minor (from GetNodeInfo response) | +| `hw_unique_id` | `uint8_t[16]` | 16 | - | 128-bit hardware unique ID (from GetNodeInfo response) | + +**Notes:** Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes. ## `MSP2_INAV_LED_STRIP_CONFIG_EX (8264 / 0x2048)` **Description:** Retrieves the full configuration for each LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_LED_STRIP_CONFIG`. diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 4dc50b37189..e7feea80c36 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9889,10 +9889,10 @@ "units": "" }, { - "name": "last_seen_ms", + "name": "elapsed_ms", "ctype": "uint32_t", "desc": "Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request)", - "units": "ms" + "units": "ms" }, { "name": "name_len", diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index b0375a60332..d006695ccd2 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -368,6 +368,11 @@ static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *tr return; } + if (transfer->transfer_id != ((node->getNodeInfo_transfer_id - 1) & 0x1F)) { + LOG_DEBUG(CAN, "GetNodeInfoResponse from node %u: stale tid %u", nodeID, transfer->transfer_id); + return; + } + uint8_t len = resp.name.len < sizeof(node->name) ? resp.name.len : sizeof(node->name); node->name_len = len; memcpy(node->name, resp.name.data, len); @@ -380,9 +385,9 @@ static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *tr node->hw_major = resp.hardware_version.major; node->hw_minor = resp.hardware_version.minor; - memcpy(node->hw_unique_id, resp.hardware_version.unique_id, 16); + memcpy(node->hw_unique_id, resp.hardware_version.unique_id, sizeof(node->hw_unique_id)); } -// Canard Senders +// Canard Handlers and Senders /* @@ -725,5 +730,4 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } } - #endif diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index e602491d82c..2b2fad29b24 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -179,8 +179,8 @@ TEST_F(DroneCANNodeTableTest, NewNodeAddedOnFirstStatus) TEST_F(DroneCANNodeTableTest, TwoDistinctNodesStoredSeparately) { CanardRxTransfer x1 = makeNodeStatusTransfer(10, 100, 0, 0, 0, buf); - CanardRxTransfer x2 = makeNodeStatusTransfer(20, 200, 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); From 42dabdc1ae57c322dccad9440f36a0547cc739be Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 2 Jun 2026 22:23:04 -0700 Subject: [PATCH 24/42] feat(dronecan): shared async slot for on-demand GetNodeInfo and GetSet MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace auto-fetch-on-discovery with a single shared async slot that serves any DroneCAN service request (GetNodeInfo, param GetSet) via two new MSP commands: MSP2_INAV_DRONECAN_ASYNC_REQUEST (0x2043) — fires any service request MSP2_INAV_DRONECAN_ASYNC_RESULT (0x2044) — polls result with seq guard Design changes: - Strip node table to runtime fields only (nodeID, health, mode, uptime_sec, vendor_status_code, last_seen_ms); saves ~3.5 KB RAM - Remove automatic GetNodeInfo fetch on node discovery - Add dronecanAsyncRequest() as single entry point for all service calls - Add handle_AsyncServiceResponse() dispatching on service_id - Add 2-second timeout with seq generation counter to detect stale polls - Add UAVCAN_PROTOCOL_PARAM_GETSET_ID to shouldAcceptTransfer response block - Extend MSP2_INAV_DRONECAN_NODES wire format with uptime_sec and vendor_status_code (13 bytes/record, was 7) --- src/main/drivers/dronecan/dronecan.c | 217 +++++++++++++++++++++------ src/main/drivers/dronecan/dronecan.h | 92 +++++++++--- src/main/fc/fc_msp.c | 153 +++++++++++++++---- src/main/msp/msp_protocol_v2_inav.h | 8 +- 4 files changed, 365 insertions(+), 105 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index d006695ccd2..82cbc6ac315 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -39,6 +39,8 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, ); static dronecanState_e dronecanState = STATE_DRONECAN_INIT; +dronecanAsyncSlot_t dronecanAsyncSlot = { .state = DRONECAN_ASYNC_IDLE }; + #ifdef UNIT_TEST uint8_t activeNodeCount = 0; dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; @@ -152,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 > 2000) { + dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR; + } + for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) { timestamp = millis() * 1000ULL; @@ -352,41 +360,165 @@ const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) { return findNodeByID(nodeID); } -static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_protocol_GetNodeInfoResponse resp; - if (uavcan_protocol_GetNodeInfoResponse_decode(transfer, &resp)) { - LOG_DEBUG(CAN, "GetNodeInfoResponse decode failed"); - return; +bool dronecanAsyncRequest(uint16_t service_id, uint8_t node_id, const void *payload, uint8_t payload_len) +{ + UNUSED(payload_len); + + if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING && + millis() - dronecanAsyncSlot.requested_at_ms <= 2000) { + return false; } - uint8_t nodeID = transfer->source_node_id; - dronecanNodeInfo_t *node = findNodeByID(nodeID); - if (!node) { - LOG_DEBUG(CAN, "GetNodeInfoResponse from unknown node %u", nodeID); - return; + uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_MAX_SIZE]; + 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: + getset.value.string_value.len = req->value_str_len; + memcpy(getset.value.string_value.data, req->value_str, req->value_str_len); + break; + default: + getset.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY; + break; + } + } + len = uavcan_protocol_param_GetSetRequest_encode(&getset, buffer); + buf_ptr = buffer; + signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE; + break; + } + + default: + return false; } - if (transfer->transfer_id != ((node->getNodeInfo_transfer_id - 1) & 0x1F)) { - LOG_DEBUG(CAN, "GetNodeInfoResponse from node %u: stale tid %u", nodeID, transfer->transfer_id); - return; + int16_t res = canardRequestOrRespond(&canard, node_id, signature, (uint8_t)service_id, + &dronecanAsyncSlot.transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, CanardRequest, + buf_ptr, len); + + if (res < 0) { + LOG_DEBUG(CAN, "dronecanAsyncRequest: service %u node %u failed: %d", service_id, node_id, res); + return false; } - uint8_t len = resp.name.len < sizeof(node->name) ? resp.name.len : sizeof(node->name); - node->name_len = len; - memcpy(node->name, resp.name.data, len); + 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 a response to an asynchronous service request such as + a configuration parameter on the node info structur +*/ +static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *transfer) +{ + UNUSED(ins); - node->sw_major = resp.software_version.major; - node->sw_minor = resp.software_version.minor; - node->sw_optional_field_flags = resp.software_version.optional_field_flags; - node->sw_vcs_commit = (resp.software_version.optional_field_flags & UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT) - ? resp.software_version.vcs_commit : 0; + if (dronecanAsyncSlot.state != DRONECAN_ASYNC_PENDING) // timed out or already received + return; + if (transfer->data_type_id != dronecanAsyncSlot.service_id) // Data does not match the requested parameter + return; + if (transfer->source_node_id != dronecanAsyncSlot.node_id) // response received for different node_id + 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) ? resp.name.len : sizeof(r->name); + r->name_len = len; + memcpy(r->name, resp.name.data, len); + 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; + } - node->hw_major = resp.hardware_version.major; - node->hw_minor = resp.hardware_version.minor; - memcpy(node->hw_unique_id, resp.hardware_version.unique_id, sizeof(node->hw_unique_id)); + 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; + } + dronecanAsyncSlot.state = DRONECAN_ASYNC_READY; + break; + } + + default: + break; + } } + // Canard Handlers and Senders @@ -483,10 +615,15 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, } 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_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; + } } } if (transfer_type == CanardTransferTypeBroadcast) { @@ -530,6 +667,7 @@ void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { #else static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { #endif + UNUSED(ins); struct uavcan_protocol_NodeStatus nodeStatus; if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { @@ -558,15 +696,6 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { nodeTable[activeNodeCount].last_seen_ms = millis(); activeNodeCount++; - dronecanMaskTxISR(); - const int16_t res = canardRequestOrRespond(ins, nodeId, - UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID, - &nodeTable[activeNodeCount - 1].getNodeInfo_transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, CanardRequest, NULL, 0); - dronecanUnmaskTxISR(); - if (res < 0) { - LOG_DEBUG(CAN, "GetNodeInfo request failed for node %u: %d", nodeId, res); - } } else { LOG_DEBUG(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeId); } @@ -691,13 +820,11 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } } } - if (transfer->transfer_type == CanardTransferTypeResponse) { - switch (transfer->data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: - handle_GetNodeInfoResponse(ins, transfer); - break; - } - } + + if (transfer->transfer_type == CanardTransferTypeResponse) { + handle_AsyncServiceResponse(&canard, transfer); + } + if (transfer->transfer_type == CanardTransferTypeBroadcast) { // check if we want to handle a specific broadcast message switch (transfer->data_type_id) { diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index c4029cb0244..e86f1675018 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -28,35 +28,79 @@ typedef struct dronecanConfig_s { } dronecanConfig_t; typedef struct dronecanNodeInfo_s { - uint8_t nodeID; - uint8_t health; - uint8_t mode; + uint8_t nodeID; + uint8_t health; + uint8_t mode; uint32_t uptime_sec; uint16_t vendor_status_code; uint32_t last_seen_ms; - uint8_t name_len; - char name[80]; - /* Software version (from GetNodeInfo response)*/ - uint8_t sw_major; - uint8_t sw_minor; - uint8_t sw_optional_field_flags; - uint32_t sw_vcs_commit; - /* Hardware version (from GetNodeInfo response)*/ - uint8_t hw_major; - uint8_t hw_minor; - uint8_t hw_unique_id[16]; - /* Canard transfer ID for outgoing GetNodeInfo requests to this node. - * Must be per-node: Canard forbids sharing a counter across different dst_node_id. */ - uint8_t getNodeInfo_transfer_id; } dronecanNodeInfo_t; -// Wire format for MSP2_INAV_DRONECAN_NODES records (7 bytes each, packed). -typedef struct dronecanNodeStatus_s { - uint8_t nodeID; - uint8_t health; - uint8_t mode; - uint32_t last_seen_ms; -} __attribute__((packed)) dronecanNodeStatus_t; +typedef enum { + DRONECAN_ASYNC_IDLE = 0, + DRONECAN_ASYNC_PENDING, + DRONECAN_ASYNC_READY, + DRONECAN_ASYNC_ERROR, +} dronecanAsyncState_e; + +#define DRONECAN_SERVICE_GETNODEINFO 1 +#define DRONECAN_SERVICE_PARAM_GETSET 11 + +#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[64]; +} 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[80]; +} 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[64]; + uint8_t name_len; + char name[93]; +} dronecanParamResult_t; + +typedef struct dronecanAsyncSlot_s { + dronecanAsyncState_e state; + uint8_t seq; + uint16_t service_id; + uint8_t node_id; + uint8_t transfer_id; + uint32_t requested_at_ms; + union { + dronecanGetNodeInfoResult_t node_info; + dronecanParamResult_t param; + } result; +} dronecanAsyncSlot_t; + +extern dronecanAsyncSlot_t dronecanAsyncSlot; +bool dronecanAsyncRequest(uint16_t service_id, uint8_t node_id, const void *payload, uint8_t payload_len); void dronecanInit(void); void dronecanUpdate(timeUs_t currentTimeUs); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e24faa3b1c8..cdd1a1f39ec 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,41 +4586,134 @@ 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; } + uint16_t service_id = sbufReadU16(src); uint8_t nodeID = sbufReadU8(src); - const dronecanNodeInfo_t *node = dronecanGetNodeByID(nodeID); - if (!node) { - *ret = MSP_RESULT_ERROR; + + if (dronecanGetState() != STATE_DRONECAN_NORMAL) { + sbufWriteU8(dst, 2); // not ready + sbufWriteU8(dst, 0); + *ret = MSP_RESULT_ACK; break; } - if (sbufBytesRemaining(dst) < MSP2_DRONECAN_NODE_INFO_SIZE) { - *ret = MSP_RESULT_ERROR; - break; + + bool accepted = false; + if (service_id == DRONECAN_SERVICE_GETNODEINFO) { + accepted = dronecanAsyncRequest(service_id, nodeID, NULL, 0); + } else if (service_id == DRONECAN_SERVICE_PARAM_GETSET) { + if (sbufBytesRemaining(src) < 2) { + *ret = MSP_RESULT_ERROR; + break; + } + dronecanParamRequest_t req; + memset(&req, 0, sizeof(req)); + req.index = sbufReadU16(src); + req.is_write = sbufBytesRemaining(src) > 0 ? sbufReadU8(src) : 0; + 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) { + uint32_t lo = sbufReadU32(src); + uint32_t hi = sbufReadU32(src); + req.value_int = (int64_t)lo | ((int64_t)hi << 32); + } + 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); + } + break; + } + } + accepted = dronecanAsyncRequest(service_id, nodeID, &req, sizeof(req)); } - sbufWriteU8(dst, node->nodeID); - sbufWriteU8(dst, node->health); - sbufWriteU8(dst, node->mode); - sbufWriteU32(dst, node->uptime_sec); - sbufWriteU16(dst, node->vendor_status_code); - sbufWriteU32(dst, millis() - node->last_seen_ms); - sbufWriteU8(dst, node->name_len); - sbufWriteDataSafe(dst, node->name, 80); - sbufWriteU8(dst, node->sw_major); - sbufWriteU8(dst, node->sw_minor); - sbufWriteU8(dst, node->sw_optional_field_flags); - sbufWriteU32(dst, node->sw_vcs_commit); - sbufWriteU8(dst, node->hw_major); - sbufWriteU8(dst, node->hw_minor); - sbufWriteDataSafe(dst, node->hw_unique_id, 16); + + sbufWriteU8(dst, accepted ? 0 : 1); // 0=accepted, 1=busy + sbufWriteU8(dst, dronecanAsyncSlot.seq); *ret = MSP_RESULT_ACK; } break; -#endif + + 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: { + uint32_t lo = (uint32_t)(r->value_int & 0xFFFFFFFF); + uint32_t hi = (uint32_t)((r->value_int >> 32) & 0xFFFFFFFF); + sbufWriteU32(dst, lo); + sbufWriteU32(dst, hi); + 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; + } + break; + } + } + dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE; + } + *ret = MSP_RESULT_ACK; + } + break; +#endif #if defined(USE_FLASHFS) case MSP_DATAFLASH_READ: diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index c34ca2229ce..a2967a69f98 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -97,12 +97,8 @@ #define MSP2_INAV_ESC_TELEM 0x2041 #define MSP2_INAV_DRONECAN_NODES 0x2042 -#define MSP2_INAV_DRONECAN_NODE_INFO 0x2043 -// MSP2_INAV_DRONECAN_NODE_INFO reply size: -// nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4) -// +name_len(1)+name(80)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1) -// +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 119 -#define MSP2_DRONECAN_NODE_INFO_SIZE 119 +#define MSP2_INAV_DRONECAN_ASYNC_REQUEST 0x2043 +#define MSP2_INAV_DRONECAN_ASYNC_RESULT 0x2044 #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 From caf93e43ffff03e9fbfd4001322c7149ab92dcdb Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 3 Jun 2026 09:28:12 -0700 Subject: [PATCH 25/42] feat(dronecan): ExecuteOpcode, RestartNode, GetSet write fixes, node expiry - Add ExecuteOpcode (service 10) and RestartNode (service 5) to async slot framework: request builder, response decoder, shouldAccept filter, MSP ASYNC_REQUEST/RESULT handlers - Fix GetSet write: zero-initialise encode buffer so reserved bits are not contaminated by stack garbage - Fix GetSet write: include parameter name in request; ArduPilot nodes require the name field to accept writes - Prune node table entries silent for >10s so stale nodes (e.g. after a node ID change) are removed automatically --- src/main/drivers/dronecan/dronecan.h | 13 +++++++++++-- src/main/fc/fc_msp.c | 20 ++++++++++++++++++++ 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index e86f1675018..c7bb5678fb3 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -43,8 +43,10 @@ typedef enum { DRONECAN_ASYNC_ERROR, } dronecanAsyncState_e; -#define DRONECAN_SERVICE_GETNODEINFO 1 -#define DRONECAN_SERVICE_PARAM_GETSET 11 +#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_PARAM_TYPE_EMPTY 0 #define DRONECAN_PARAM_TYPE_INT 1 @@ -61,6 +63,8 @@ typedef struct dronecanParamRequest_s { uint8_t value_bool; uint8_t value_str_len; char value_str[64]; + uint8_t req_name_len; + char req_name[92]; } dronecanParamRequest_t; typedef struct dronecanGetNodeInfoResult_s { @@ -86,6 +90,10 @@ typedef struct dronecanParamResult_s { char name[93]; } dronecanParamResult_t; +typedef struct dronecanSimpleResult_s { + bool ok; +} dronecanSimpleResult_t; + typedef struct dronecanAsyncSlot_s { dronecanAsyncState_e state; uint8_t seq; @@ -96,6 +104,7 @@ typedef struct dronecanAsyncSlot_s { union { dronecanGetNodeInfoResult_t node_info; dronecanParamResult_t param; + dronecanSimpleResult_t simple; } result; } dronecanAsyncSlot_t; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cdd1a1f39ec..e9449799cbd 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4645,7 +4645,23 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu 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); + } accepted = dronecanAsyncRequest(service_id, nodeID, &req, sizeof(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, sizeof(opcode)); + } else if (service_id == DRONECAN_SERVICE_RESTART_NODE) { + accepted = dronecanAsyncRequest(service_id, nodeID, NULL, 0); } sbufWriteU8(dst, accepted ? 0 : 1); // 0=accepted, 1=busy @@ -4707,6 +4723,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } 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; } From 8e7fb521aa43790f80872f450c665cdb2adaac21 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 3 Jun 2026 12:14:14 -0700 Subject: [PATCH 26/42] =?UTF-8?q?fix(dronecan):=20code=20review=20cleanup?= =?UTF-8?q?=20=E2=80=94=20types,=20comments,=20indentation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Change service_id to uint8_t throughout (slot, function sig, fc_msp local) - Replace obvious buffer zero-init comment with explanation of why (UAVCAN reserved bits) - Fix extra indentation on timeout expiry assignment - MSP reads service_id as u16 for protocol compat; casts to u8 on use --- src/main/drivers/dronecan/dronecan.c | 81 +++++++++++++++++++++++++--- src/main/drivers/dronecan/dronecan.h | 10 ++-- src/main/fc/fc_msp.c | 12 ++--- 3 files changed, 86 insertions(+), 17 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 82cbc6ac315..50ffb589177 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -361,16 +361,16 @@ const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) { } -bool dronecanAsyncRequest(uint16_t service_id, uint8_t node_id, const void *payload, uint8_t payload_len) +bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload) { - UNUSED(payload_len); - if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING && - millis() - dronecanAsyncSlot.requested_at_ms <= 2000) { + 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; @@ -408,17 +408,42 @@ bool dronecanAsyncRequest(uint16_t service_id, uint8_t node_id, const void *payl break; } } + getset.name.len = req->req_name_len; + memcpy(getset.name.data, req->req_name, req->req_name_len); 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; } - int16_t res = canardRequestOrRespond(&canard, node_id, signature, (uint8_t)service_id, + int16_t res = canardRequestOrRespond(&canard, node_id, signature, service_id, &dronecanAsyncSlot.transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, CanardRequest, buf_ptr, len); @@ -437,7 +462,7 @@ bool dronecanAsyncRequest(uint16_t service_id, uint8_t node_id, const void *payl /* Handle a response to an asynchronous service request such as - a configuration parameter on the node info structur + a configuration parameter on the node info structure */ static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -514,6 +539,28 @@ static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *t break; } + case DRONECAN_SERVICE_EXECUTE_OPCODE: { + struct uavcan_protocol_param_ExecuteOpcodeResponse resp; + if (uavcan_protocol_param_ExecuteOpcodeResponse_decode(transfer, &resp)) { + 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)) { + dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR; + return; + } + dronecanAsyncSlot.result.simple.ok = resp.ok; + dronecanAsyncSlot.state = DRONECAN_ASYNC_READY; + break; + } + default: break; } @@ -577,6 +624,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 > 10000) { + nodeTable[i] = nodeTable[activeNodeCount - 1]; + activeNodeCount--; + } else { + i++; + } + } + /* Transmit the node status message */ @@ -624,6 +681,16 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, *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; + } } } if (transfer_type == CanardTransferTypeBroadcast) { diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index c7bb5678fb3..d56cfaba0b9 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -48,6 +48,8 @@ typedef enum { #define DRONECAN_SERVICE_EXECUTE_OPCODE 10 #define DRONECAN_SERVICE_PARAM_GETSET 11 +#define DRONECAN_ASYNC_TIMEOUT_MS 2000 + #define DRONECAN_PARAM_TYPE_EMPTY 0 #define DRONECAN_PARAM_TYPE_INT 1 #define DRONECAN_PARAM_TYPE_FLOAT 2 @@ -62,7 +64,7 @@ typedef struct dronecanParamRequest_s { float value_float; uint8_t value_bool; uint8_t value_str_len; - char value_str[64]; + char value_str[128]; uint8_t req_name_len; char req_name[92]; } dronecanParamRequest_t; @@ -85,7 +87,7 @@ typedef struct dronecanParamResult_s { float value_float; uint8_t value_bool; uint8_t value_str_len; - char value_str[64]; + char value_str[128]; uint8_t name_len; char name[93]; } dronecanParamResult_t; @@ -97,7 +99,7 @@ typedef struct dronecanSimpleResult_s { typedef struct dronecanAsyncSlot_s { dronecanAsyncState_e state; uint8_t seq; - uint16_t service_id; + uint8_t service_id; uint8_t node_id; uint8_t transfer_id; uint32_t requested_at_ms; @@ -109,7 +111,7 @@ typedef struct dronecanAsyncSlot_s { } dronecanAsyncSlot_t; extern dronecanAsyncSlot_t dronecanAsyncSlot; -bool dronecanAsyncRequest(uint16_t service_id, uint8_t node_id, const void *payload, uint8_t payload_len); +bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload); void dronecanInit(void); void dronecanUpdate(timeUs_t currentTimeUs); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e9449799cbd..c93e3f45816 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4592,11 +4592,11 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ERROR; break; } - uint16_t service_id = sbufReadU16(src); + 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, 2); // not ready + sbufWriteU8(dst, 0xFF); // not ready - outside async enum range sbufWriteU8(dst, 0); *ret = MSP_RESULT_ACK; break; @@ -4604,7 +4604,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu bool accepted = false; if (service_id == DRONECAN_SERVICE_GETNODEINFO) { - accepted = dronecanAsyncRequest(service_id, nodeID, NULL, 0); + accepted = dronecanAsyncRequest(service_id, nodeID, NULL); } else if (service_id == DRONECAN_SERVICE_PARAM_GETSET) { if (sbufBytesRemaining(src) < 2) { *ret = MSP_RESULT_ERROR; @@ -4652,16 +4652,16 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (sbufBytesRemaining(src) >= req.req_name_len) sbufReadData(src, req.req_name, req.req_name_len); } - accepted = dronecanAsyncRequest(service_id, nodeID, &req, sizeof(req)); + 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, sizeof(opcode)); + accepted = dronecanAsyncRequest(service_id, nodeID, &opcode); } else if (service_id == DRONECAN_SERVICE_RESTART_NODE) { - accepted = dronecanAsyncRequest(service_id, nodeID, NULL, 0); + accepted = dronecanAsyncRequest(service_id, nodeID, NULL); } sbufWriteU8(dst, accepted ? 0 : 1); // 0=accepted, 1=busy From 481140f7c969cc7f35881409a46bc20462791636 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 3 Jun 2026 13:46:30 -0700 Subject: [PATCH 27/42] =?UTF-8?q?fix(dronecan):=20code=20review=20pass=202?= =?UTF-8?q?=20=E2=80=94=20null=20termination,=20named=20constant,=20commen?= =?UTF-8?q?ts?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - GetNodeInfo name buffer: clamp to sizeof-1, add null terminator (matches param result pattern); grow name[80] -> name[81] to hold the terminator - Add DRONECAN_STATE_NOT_READY 0xFF constant to header; use it in fc_msp.c instead of a bare magic number - Update accepted/busy comment to note 1 also covers unrecognised service_id --- src/main/drivers/dronecan/dronecan.c | 3 ++- src/main/drivers/dronecan/dronecan.h | 5 +++-- src/main/fc/fc_msp.c | 4 ++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 50ffb589177..772bfaed9d4 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -484,9 +484,10 @@ static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *t return; } dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info; - uint8_t len = resp.name.len < sizeof(r->name) ? resp.name.len : sizeof(r->name); + 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; diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index d56cfaba0b9..8bf49f46e8f 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -48,7 +48,8 @@ typedef enum { #define DRONECAN_SERVICE_EXECUTE_OPCODE 10 #define DRONECAN_SERVICE_PARAM_GETSET 11 -#define DRONECAN_ASYNC_TIMEOUT_MS 2000 +#define DRONECAN_ASYNC_TIMEOUT_MS 2000 +#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 @@ -78,7 +79,7 @@ typedef struct dronecanGetNodeInfoResult_s { uint8_t hw_minor; uint8_t hw_unique_id[16]; uint8_t name_len; - char name[80]; + char name[81]; // 80 bytes max + null terminator } dronecanGetNodeInfoResult_t; typedef struct dronecanParamResult_s { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c93e3f45816..ae3b9215800 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4596,7 +4596,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu uint8_t nodeID = sbufReadU8(src); if (dronecanGetState() != STATE_DRONECAN_NORMAL) { - sbufWriteU8(dst, 0xFF); // not ready - outside async enum range + sbufWriteU8(dst, DRONECAN_STATE_NOT_READY); sbufWriteU8(dst, 0); *ret = MSP_RESULT_ACK; break; @@ -4664,7 +4664,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu accepted = dronecanAsyncRequest(service_id, nodeID, NULL); } - sbufWriteU8(dst, accepted ? 0 : 1); // 0=accepted, 1=busy + sbufWriteU8(dst, accepted ? 0 : 1); // 0=accepted, 1=busy or unrecognised service_id sbufWriteU8(dst, dronecanAsyncSlot.seq); *ret = MSP_RESULT_ACK; } From 15355b9e7ef467181df3204ab60974154c2bcf4e Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 3 Jun 2026 14:01:59 -0700 Subject: [PATCH 28/42] =?UTF-8?q?fix(dronecan):=20code=20review=20pass=203?= =?UTF-8?q?=20=E2=80=94=20transfer=5Fid=20guard,=20buf=5Fptr=20comment,=20?= =?UTF-8?q?logging?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add UAVCAN-spec transfer_id match in handle_AsyncServiceResponse to reject stale frames after bus-off recovery (in-flight id is (transfer_id-1) & 0x1F) - Add comment clarifying buf_ptr=NULL is intentional for zero-length payloads - Add LOG_DEBUG for ExecuteOpcodeResponse decode failure (matches GetNodeInfo and ParamGetSet pattern) --- src/main/drivers/dronecan/dronecan.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 772bfaed9d4..c6f4776b468 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -443,6 +443,7 @@ bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *paylo return false; } + // buf_ptr remains NULL for zero-length payloads (GETNODEINFO, RESTART_NODE with no payload); libcanard accepts NULL with len=0 int16_t res = canardRequestOrRespond(&canard, node_id, signature, service_id, &dronecanAsyncSlot.transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, CanardRequest, buf_ptr, len); @@ -474,6 +475,10 @@ static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *t 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: { @@ -543,6 +548,7 @@ static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *t 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; } From b5bfa9ffbf9a9571345496cd97ebe745f4d2e6fe Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 3 Jun 2026 15:39:42 -0700 Subject: [PATCH 29/42] =?UTF-8?q?fix(dronecan):=20code=20review=20pass=205?= =?UTF-8?q?=20=E2=80=94=20guard,=20comment,=20modeLabel?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - PARAM_GETSET minimum-size guard: < 2 → < 3 (index+is_write required); remove fallback ternary for is_write - handle_AsyncServiceResponse: update stale comment to name service_id specifically rather than vague "parameter" --- src/main/drivers/dronecan/dronecan.c | 5 +++-- src/main/fc/fc_msp.c | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index c6f4776b468..c20366f2a80 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -443,7 +443,7 @@ bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *paylo return false; } - // buf_ptr remains NULL for zero-length payloads (GETNODEINFO, RESTART_NODE with no payload); libcanard accepts NULL with len=0 + // buf_ptr remains NULL only for GETNODEINFO (zero-length request); libcanard accepts NULL with len=0 int16_t res = canardRequestOrRespond(&canard, node_id, signature, service_id, &dronecanAsyncSlot.transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, CanardRequest, buf_ptr, len); @@ -471,7 +471,7 @@ static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *t if (dronecanAsyncSlot.state != DRONECAN_ASYNC_PENDING) // timed out or already received return; - if (transfer->data_type_id != dronecanAsyncSlot.service_id) // Data does not match the requested parameter + 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; @@ -560,6 +560,7 @@ static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *t 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; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ae3b9215800..2900dfbb4ee 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4606,14 +4606,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (service_id == DRONECAN_SERVICE_GETNODEINFO) { accepted = dronecanAsyncRequest(service_id, nodeID, NULL); } else if (service_id == DRONECAN_SERVICE_PARAM_GETSET) { - if (sbufBytesRemaining(src) < 2) { + 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 = sbufBytesRemaining(src) > 0 ? sbufReadU8(src) : 0; + req.is_write = sbufReadU8(src); if (req.is_write && sbufBytesRemaining(src) >= 1) { req.value_type = sbufReadU8(src); switch (req.value_type) { From 639febe96b5e43c9e8ae9a170dd33e94317acb5d Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 3 Jun 2026 16:18:15 -0700 Subject: [PATCH 30/42] feat(dronecan): expose param GetSet min/max range through MSP Decode NumericValue min_value and max_value from UAVCAN param.GetSet response and thread them to the configurator. The FC now serializes min/max type and value bytes after the param value in the async result MSP payload. --- src/main/drivers/dronecan/dronecan.c | 22 ++++++++++++++++++++++ src/main/drivers/dronecan/dronecan.h | 7 +++++++ src/main/fc/fc_msp.c | 16 ++++++++++++++++ 3 files changed, 45 insertions(+) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index c20366f2a80..08eb7ccea25 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -541,6 +541,28 @@ static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *t r->type = DRONECAN_PARAM_TYPE_EMPTY; break; } + r->min_type = (uint8_t)resp.min_value.union_tag; + switch (resp.min_value.union_tag) { + case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE: + r->min_int = resp.min_value.integer_value; + break; + case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE: + r->min_float = resp.min_value.real_value; + break; + default: + break; + } + r->max_type = (uint8_t)resp.max_value.union_tag; + switch (resp.max_value.union_tag) { + case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE: + r->max_int = resp.max_value.integer_value; + break; + case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE: + r->max_float = resp.max_value.real_value; + break; + default: + break; + } dronecanAsyncSlot.state = DRONECAN_ASYNC_READY; break; } diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index 8bf49f46e8f..3f279008b8e 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -91,6 +91,13 @@ typedef struct dronecanParamResult_s { char value_str[128]; uint8_t name_len; char name[93]; + // NumericValue range from the GetSet response (type 0=EMPTY means not provided by node) + 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 { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 2900dfbb4ee..1b525b41087 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4721,6 +4721,22 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu default: break; } + sbufWriteU8(dst, r->min_type); + if (r->min_type == 1) { + sbufWriteU32(dst, (uint32_t)(r->min_int & 0xFFFFFFFF)); + sbufWriteU32(dst, (uint32_t)((r->min_int >> 32) & 0xFFFFFFFF)); + } else if (r->min_type == 2) { + uint32_t raw; memcpy(&raw, &r->min_float, 4); + sbufWriteU32(dst, raw); + } + sbufWriteU8(dst, r->max_type); + if (r->max_type == 1) { + sbufWriteU32(dst, (uint32_t)(r->max_int & 0xFFFFFFFF)); + sbufWriteU32(dst, (uint32_t)((r->max_int >> 32) & 0xFFFFFFFF)); + } else if (r->max_type == 2) { + uint32_t raw; memcpy(&raw, &r->max_float, 4); + sbufWriteU32(dst, raw); + } break; } case DRONECAN_SERVICE_EXECUTE_OPCODE: From 7489f51012f5e543610e34a5a541d02480911ed0 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 3 Jun 2026 17:23:35 -0700 Subject: [PATCH 31/42] =?UTF-8?q?fix(dronecan):=20code=20review=20pass=20?= =?UTF-8?q?=E2=80=94=20explicit=20NumericValue=20mapping,=20stale=20field?= =?UTF-8?q?=20zeroing,=20named=20constants,=20struct=20alignment?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - dronecan.c: map NUMERICVALUE_* → DRONECAN_PARAM_TYPE_* explicitly in min/max decode switch; zero inactive fields before each switch to prevent stale values across consecutive GetSet calls - dronecan.h: reorder min/max struct fields (int64_t first) to eliminate 7-byte ARM alignment padding per min/max group - fc_msp.c: replace bare 1/2 with DRONECAN_PARAM_TYPE_INT/FLOAT; add braces to else-if blocks in min/max serializer --- src/main/drivers/dronecan/dronecan.c | 16 ++++++++++++---- src/main/drivers/dronecan/dronecan.h | 7 ++++--- src/main/fc/fc_msp.c | 14 ++++++++------ 3 files changed, 24 insertions(+), 13 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 08eb7ccea25..2a8581b204a 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -541,23 +541,31 @@ static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *t r->type = DRONECAN_PARAM_TYPE_EMPTY; break; } - r->min_type = (uint8_t)resp.min_value.union_tag; + 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_int = resp.min_value.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 = (uint8_t)resp.max_value.union_tag; + 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_int = resp.max_value.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: diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index 3f279008b8e..8d88cd22fee 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -91,13 +91,14 @@ typedef struct dronecanParamResult_s { char value_str[128]; uint8_t name_len; char name[93]; - // NumericValue range from the GetSet response (type 0=EMPTY means not provided by node) - uint8_t min_type; + // NumericValue range from the GetSet response (DRONECAN_PARAM_TYPE_EMPTY=0 means not provided by node; + // only INT and FLOAT variants are valid — BOOL and STRING have no numeric range) int64_t min_int; float min_float; - uint8_t max_type; + uint8_t min_type; int64_t max_int; float max_float; + uint8_t max_type; } dronecanParamResult_t; typedef struct dronecanSimpleResult_s { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 1b525b41087..77daf729543 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4722,19 +4722,21 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; } sbufWriteU8(dst, r->min_type); - if (r->min_type == 1) { + if (r->min_type == DRONECAN_PARAM_TYPE_INT) { sbufWriteU32(dst, (uint32_t)(r->min_int & 0xFFFFFFFF)); sbufWriteU32(dst, (uint32_t)((r->min_int >> 32) & 0xFFFFFFFF)); - } else if (r->min_type == 2) { - uint32_t raw; memcpy(&raw, &r->min_float, 4); + } 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 == 1) { + if (r->max_type == DRONECAN_PARAM_TYPE_INT) { sbufWriteU32(dst, (uint32_t)(r->max_int & 0xFFFFFFFF)); sbufWriteU32(dst, (uint32_t)((r->max_int >> 32) & 0xFFFFFFFF)); - } else if (r->max_type == 2) { - uint32_t raw; memcpy(&raw, &r->max_float, 4); + } else if (r->max_type == DRONECAN_PARAM_TYPE_FLOAT) { + uint32_t raw; + memcpy(&raw, &r->max_float, 4); sbufWriteU32(dst, raw); } break; From 61b3d3abc56f8e2a6f04eb5f3a4b663e37c1bbb2 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 3 Jun 2026 17:40:51 -0700 Subject: [PATCH 32/42] fix(dronecan): correct struct field order and comment in dronecanParamResult_t MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Reorder min/max fields to match existing value field convention (type, int64, float). Fix comment that incorrectly claimed the prior ordering eliminated ARM alignment padding — both orderings produce identical struct size. --- src/main/drivers/dronecan/dronecan.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index 8d88cd22fee..460e25274a9 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -91,14 +91,14 @@ typedef struct dronecanParamResult_s { char value_str[128]; uint8_t name_len; char name[93]; - // NumericValue range from the GetSet response (DRONECAN_PARAM_TYPE_EMPTY=0 means not provided by node; - // only INT and FLOAT variants are valid — BOOL and STRING have no numeric range) + // 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 min_type; + uint8_t max_type; int64_t max_int; float max_float; - uint8_t max_type; } dronecanParamResult_t; typedef struct dronecanSimpleResult_s { From ab87ddac0af51b12cbfadafda1368ab58e2eba2f Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sat, 6 Jun 2026 20:15:42 -0700 Subject: [PATCH 33/42] docs(msp): update DroneCAN MSP docs for async request/result API MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace the removed MSP2_INAV_DRONECAN_NODE_INFO (0x2043) entry with documentation for the new async slot pattern introduced in the dronecan-param-getset feature: - MSP2_INAV_DRONECAN_NODES (0x2042): fix per-node record size 7→13 bytes (added uptime_sec u32 + vendor_status_code u16) - MSP2_INAV_DRONECAN_ASYNC_REQUEST (0x2043): document all four services (GetNodeInfo, RestartNode, ExecuteOpcode, ParamGetSet read/write) including param type encoding and sequence number correlation - MSP2_INAV_DRONECAN_ASYNC_RESULT (0x2044): document state machine, all service-specific result payloads, and slot-reset behaviour --- docs/development/msp/README.md | 150 +++++++++++++++++++++++++++------ 1 file changed, 123 insertions(+), 27 deletions(-) diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index 56d177ffbad..bcdb5dd3702 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) @@ -4168,38 +4169,133 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: |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. | +| `nodeData` | (repeated) | 13 × nodeCount | Array of per-node status records (see below) | -**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO. +Each node record is 13 bytes in the following order: -## `MSP2_INAV_DRONECAN_NODE_INFO (8259 / 0x2043)` -**Description:** Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service. - -**Request Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| -| `nodeID` | `uint8_t` | 1 | DroneCAN node ID to query (1-127) | - +| `nodeID` | `uint8_t` | 1 | DroneCAN node ID (1-127) | +| `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 | +| `last_seen_ms` | `uint32_t` | 4 | Milliseconds since this node was last seen (FC-local timestamp delta) | +| `uptime_sec` | `uint32_t` | 4 | Node uptime in seconds (from NodeStatus broadcast) | +| `vendor_status_code` | `uint16_t` | 2 | Vendor-specific status code | + +**Notes:** Requires `USE_DRONECAN`. Maximum payload is 1 + (DRONECAN_MAX_NODES × 13) = 417 bytes. For full node detail (name, software/hardware version, unique ID) use `MSP2_INAV_DRONECAN_ASYNC_REQUEST` with `service_id = DRONECAN_SERVICE_GETNODEINFO (1)`. + +## `MSP2_INAV_DRONECAN_ASYNC_REQUEST (8259 / 0x2043)` +**Description:** Initiates an asynchronous DroneCAN service request to a specific node. The result is retrieved by polling `MSP2_INAV_DRONECAN_ASYNC_RESULT`. Only one request may be in-flight at a time; a new request while one is PENDING returns `accepted=1` (busy). + +**Service IDs:** + +| Constant | Value | Description | +|---|---|---| +| `DRONECAN_SERVICE_GETNODEINFO` | 1 | Retrieve node name, SW/HW version, unique ID | +| `DRONECAN_SERVICE_RESTART_NODE` | 5 | Restart the target node | +| `DRONECAN_SERVICE_EXECUTE_OPCODE` | 10 | Execute a maintenance opcode (e.g. erase config) | +| `DRONECAN_SERVICE_PARAM_GETSET` | 11 | Read or write a node parameter | + +**Request Payload (common header):** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `service_id` | `uint16_t` | 2 | Service to invoke (see table above; transmitted as u16 for MSP alignment, only low 8 bits used) | +| `nodeID` | `uint8_t` | 1 | Target DroneCAN node ID (1-127) | + +Additional fields follow the common header depending on `service_id`: + +**`DRONECAN_SERVICE_GETNODEINFO` (1) — no additional fields.** + +**`DRONECAN_SERVICE_RESTART_NODE` (5) — no additional fields.** + +**`DRONECAN_SERVICE_EXECUTE_OPCODE` (10):** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `opcode` | `uint8_t` | 1 | Opcode to execute (e.g. 0=save config, 1=erase config) | + +**`DRONECAN_SERVICE_PARAM_GETSET` (11) — read:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `index` | `uint16_t` | 2 | Parameter index to read | +| `is_write` | `uint8_t` | 1 | 0 = read | +| `req_name_len` | `uint8_t` | 1 | Length of optional name hint (0 if not provided) | +| `req_name` | `uint8_t[]` | req_name_len | Optional parameter name hint | + +**`DRONECAN_SERVICE_PARAM_GETSET` (11) — write:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `index` | `uint16_t` | 2 | Parameter index to write | +| `is_write` | `uint8_t` | 1 | 1 = write | +| `value_type` | `uint8_t` | 1 | Parameter type: 1=INT, 2=FLOAT, 3=BOOL, 4=STRING | +| `value` | (type-dependent) | varies | Value to write — see param type encoding table below | +| `req_name_len` | `uint8_t` | 1 | Length of optional name hint (0 if not provided) | +| `req_name` | `uint8_t[]` | req_name_len | Optional parameter name hint | + +Param value encoding: + +| `value_type` | Encoding | +|---|---| +| INT (1) | `uint32_t lo` + `uint32_t hi` — 64-bit little-endian split | +| FLOAT (2) | `uint32_t raw` — IEEE 754 single-precision bit pattern | +| BOOL (3) | `uint8_t` — 0=false, 1=true | +| STRING (4) | `uint8_t len` + `uint8_t data[len]` — length-prefixed, not null-terminated | + **Reply Payload:** -|Field|C Type|Size (Bytes)|Units|Description| -|---|---|---|---|---| -| `nodeID` | `uint8_t` | 1 | - | DroneCAN node ID | -| `health` | `uint8_t` | 1 | - | Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL | -| `mode` | `uint8_t` | 1 | - | Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE | -| `uptime_sec` | `uint32_t` | 4 | s | Node uptime in seconds | -| `vendor_status_code` | `uint16_t` | 2 | - | Vendor-specific status code | -| `elapsed_ms` | `uint32_t` | 4 | ms | Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request) | -| `name_len` | `uint8_t` | 1 | - | Length of node name string (0 if unknown) | -| `name` | `char[80]` | 80 | - | Node name up to 80 bytes, zero-padded | -| `sw_major` | `uint8_t` | 1 | - | Software version major (from GetNodeInfo response) | -| `sw_minor` | `uint8_t` | 1 | - | Software version minor (from GetNodeInfo response) | -| `sw_optional_field_flags` | `uint8_t` | 1 | - | UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid | -| `sw_vcs_commit` | `uint32_t` | 4 | - | Git commit hash (valid when sw_optional_field_flags bit 0 is set) | -| `hw_major` | `uint8_t` | 1 | - | Hardware version major (from GetNodeInfo response) | -| `hw_minor` | `uint8_t` | 1 | - | Hardware version minor (from GetNodeInfo response) | -| `hw_unique_id` | `uint8_t[16]` | 16 | - | 128-bit hardware unique ID (from GetNodeInfo response) | +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `accepted` | `uint8_t` | 1 | 0 = request accepted; 1 = busy (slot in use) or unrecognised service_id | +| `seq` | `uint8_t` | 1 | Sequence number; use to correlate this request with the matching `MSP2_INAV_DRONECAN_ASYNC_RESULT` response | + +**Notes:** Requires `USE_DRONECAN`. If the DroneCAN bus is not in `STATE_DRONECAN_NORMAL`, the reply returns `accepted=1` and `seq=0` without dispatching any request. Requests time out after `DRONECAN_ASYNC_TIMEOUT_MS` (2000 ms); after timeout the slot returns to IDLE and the result is discarded. + +## `MSP2_INAV_DRONECAN_ASYNC_RESULT (8260 / 0x2044)` +**Description:** Polls the result of the most recent `MSP2_INAV_DRONECAN_ASYNC_REQUEST`. The caller should poll at ~100 ms intervals until `state` is `DRONECAN_ASYNC_READY (2)` or `DRONECAN_ASYNC_ERROR (3)`. + +**Request Payload:** **None** + +**Reply Payload (common header):** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `state` | `uint8_t` | 1 | Slot state: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR | +| `seq` | `uint8_t` | 1 | Sequence number matching the originating `MSP2_INAV_DRONECAN_ASYNC_REQUEST` response | +| `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 | + +When `state == DRONECAN_ASYNC_READY (2)`, service-specific result fields follow: + +**`DRONECAN_SERVICE_GETNODEINFO` (1):** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `name_len` | `uint8_t` | 1 | Length of node name | +| `name` | `uint8_t[]` | name_len | Node name (not null-terminated) | +| `sw_major` | `uint8_t` | 1 | Software version major | +| `sw_minor` | `uint8_t` | 1 | Software version minor | +| `sw_optional_field_flags` | `uint8_t` | 1 | Bitmask indicating which optional SW fields are present | +| `sw_vcs_commit` | `uint32_t` | 4 | VCS commit hash (if `sw_optional_field_flags` bit 0 set) | +| `hw_major` | `uint8_t` | 1 | Hardware version major | +| `hw_minor` | `uint8_t` | 1 | Hardware version minor | +| `hw_unique_id` | `uint8_t[16]` | 16 | 128-bit hardware unique ID | + +**`DRONECAN_SERVICE_PARAM_GETSET` (11):** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `name_len` | `uint8_t` | 1 | Length of parameter name | +| `name` | `uint8_t[]` | name_len | Parameter name (not null-terminated; max 92 bytes per UAVCAN spec) | +| `type` | `uint8_t` | 1 | Parameter type: 0=EMPTY, 1=INT, 2=FLOAT, 3=BOOL, 4=STRING | +| `value` | (type-dependent) | varies | Current parameter value — same encoding as request write value | +| `min_type` | `uint8_t` | 1 | Type of min bound (0=EMPTY means no bound) | +| `min` | (type-dependent) | varies | Minimum value — present only if `min_type` is INT or FLOAT | +| `max_type` | `uint8_t` | 1 | Type of max bound (0=EMPTY means no bound) | +| `max` | (type-dependent) | varies | Maximum value — present only if `max_type` is INT or FLOAT | + +Min/max encoding follows the same INT (8-byte lo/hi split) and FLOAT (4-byte raw) patterns as the write value. + +**`DRONECAN_SERVICE_EXECUTE_OPCODE` (10) and `DRONECAN_SERVICE_RESTART_NODE` (5):** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `ok` | `uint8_t` | 1 | 1 = success, 0 = failure | -**Notes:** Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes. +**Notes:** Requires `USE_DRONECAN`. Reading the result when `state == DRONECAN_ASYNC_READY` transitions the slot back to `DRONECAN_ASYNC_IDLE`. If `state` is IDLE or PENDING, no result fields are present beyond the 5-byte common header. ## `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`. From 9fc518f167f8b7fee90652982d1bfeb813526ddd Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sat, 6 Jun 2026 20:21:03 -0700 Subject: [PATCH 34/42] docs(msp): update DroneCAN entries in msp_messages.json for async API MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace stale MSP2_INAV_DRONECAN_NODE_INFO entry and fix MSP2_INAV_DRONECAN_NODES record size to match the current dronecan-param-getset implementation: - MSP2_INAV_DRONECAN_NODES (8258): expand per-node record from 7 to 13 bytes — add uptime_sec(u32) and vendor_status_code(u16) fields - MSP2_INAV_DRONECAN_ASYNC_REQUEST (8259): replace removed NODE_INFO entry; documents common header + service-specific request fields for all four services (GetNodeInfo, RestartNode, ExecuteOpcode, ParamGetSet) - MSP2_INAV_DRONECAN_ASYNC_RESULT (8260): new entry documenting the 5-byte common header and all service-specific result payloads --- docs/development/msp/msp_messages.json | 153 ++++++++++++------------- 1 file changed, 74 insertions(+), 79 deletions(-) diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index e7feea80c36..0da2ccb6fd4 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9830,128 +9830,123 @@ "units": "" }, { - "name": "nodeData", - "desc": "Array of per-node status records, one per detected node. Each record: nodeID(1)+health(1)+mode(1)+last_seen_ms(4) = 7 bytes. Full detail available via MSP2_INAV_DRONECAN_NODE_INFO.", - "ctype": "dronecanNodeStatus_t", - "array": true, - "array_size": 0, - "units": "" - } - ] - }, - "variable_len": true, - "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.", - "description": "Returns the list of all detected DroneCAN nodes with their current status." - }, - "MSP2_INAV_DRONECAN_NODE_INFO": { - "code": 8259, - "mspv": 2, - "request": { - "payload": [ - { "name": "nodeID", "ctype": "uint8_t", - "desc": "DroneCAN node ID to query (1-127)", - "units": "" - } - ] - }, - "reply": { - "payload": [ - { - "name": "nodeID", - "ctype": "uint8_t", - "desc": "DroneCAN node ID", - "units": "" - }, + "desc": "[per node] DroneCAN node ID (1-127)", + "units": "", + "array": true, + "array_size": 0 + }, { "name": "health", "ctype": "uint8_t", - "desc": "Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL", + "desc": "[per node] Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL", "units": "" }, { "name": "mode", - "ctype": "uint8_t", - "desc": "Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE", - "units": "" - }, - { - "name": "uptime_sec", - "ctype": "uint32_t", - "desc": "Node uptime in seconds", - "units": "s" + "ctype": "uint8_t", + "desc": "[per node] Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE", + "units": "" }, { - "name": "vendor_status_code", - "ctype": "uint16_t", - "desc": "Vendor-specific status code", - "units": "" + "name": "last_seen_ms", + "ctype": "uint32_t", + "desc": "[per node] Milliseconds since this node was last seen (FC-local timestamp delta)", + "units": "ms" }, { - "name": "elapsed_ms", + "name": "uptime_sec", "ctype": "uint32_t", - "desc": "Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request)", - "units": "ms" - }, + "desc": "[per node] Node uptime in seconds (from NodeStatus broadcast)", + "units": "s" + }, { - "name": "name_len", - "ctype": "uint8_t", - "desc": "Length of node name string (0 if unknown)", - "units": "" - }, + "name": "vendor_status_code", + "ctype": "uint16_t", + "desc": "[per node] Vendor-specific status code", + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 13 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4)+uptime_sec(4)+vendor_status_code(2). Maximum payload 1 + (DRONECAN_MAX_NODES * 13) = 417 bytes. For full node detail (name, SW/HW version, unique ID) use MSP2_INAV_DRONECAN_ASYNC_REQUEST with service_id=DRONECAN_SERVICE_GETNODEINFO(1).", + "description": "Returns the list of all detected DroneCAN nodes with their current status." + }, + "MSP2_INAV_DRONECAN_ASYNC_REQUEST": { + "code": 8259, + "mspv": 2, + "request": { + "payload": [ { - "name": "name", - "ctype": "char[80]", - "desc": "Node name up to 80 bytes, zero-padded", + "name": "service_id", + "ctype": "uint16_t", + "desc": "Service to invoke: 1=GETNODEINFO, 5=RESTART_NODE, 10=EXECUTE_OPCODE, 11=PARAM_GETSET. Transmitted as u16 for MSP alignment; only low 8 bits used.", "units": "" }, { - "name": "sw_major", + "name": "nodeID", "ctype": "uint8_t", - "desc": "Software version major (from GetNodeInfo response)", + "desc": "Target DroneCAN node ID (1-127)", "units": "" - }, + } + ] + }, + "reply": { + "payload": [ { - "name": "sw_minor", + "name": "accepted", "ctype": "uint8_t", - "desc": "Software version minor (from GetNodeInfo response)", + "desc": "0=request accepted; 1=busy (slot in use) or unrecognised service_id", "units": "" }, { - "name": "sw_optional_field_flags", + "name": "seq", "ctype": "uint8_t", - "desc": "UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid", + "desc": "Sequence number; correlate with MSP2_INAV_DRONECAN_ASYNC_RESULT to verify the result belongs to this request", "units": "" - }, + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_DRONECAN`. Initiates an async DroneCAN service request; poll MSP2_INAV_DRONECAN_ASYNC_RESULT at ~100ms intervals until state=READY(2) or ERROR(3). Only one request in-flight at a time. Service-specific request fields follow the common header in the request payload: EXECUTE_OPCODE appends opcode(u8); PARAM_GETSET appends index(u16)+is_write(u8) and optionally value_type(u8)+value(variable) for writes, then req_name_len(u8)+req_name(bytes) for named lookup. Param value encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data. Requests time out after DRONECAN_ASYNC_TIMEOUT_MS (2000ms). If bus is not in STATE_DRONECAN_NORMAL, returns accepted=1 without dispatching.", + "description": "Initiates an asynchronous DroneCAN service request (GetNodeInfo, ParamGetSet, ExecuteOpcode, RestartNode) to a specific node. Result retrieved via MSP2_INAV_DRONECAN_ASYNC_RESULT." + }, + "MSP2_INAV_DRONECAN_ASYNC_RESULT": { + "code": 8260, + "mspv": 2, + "request": null, + "reply": { + "payload": [ { - "name": "sw_vcs_commit", - "ctype": "uint32_t", - "desc": "Git commit hash (valid when sw_optional_field_flags bit 0 is set)", + "name": "state", + "ctype": "uint8_t", + "desc": "Async slot state: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR", "units": "" }, { - "name": "hw_major", + "name": "seq", "ctype": "uint8_t", - "desc": "Hardware version major (from GetNodeInfo response)", + "desc": "Sequence number matching the originating MSP2_INAV_DRONECAN_ASYNC_REQUEST reply", "units": "" }, { - "name": "hw_minor", - "ctype": "uint8_t", - "desc": "Hardware version minor (from GetNodeInfo response)", + "name": "service_id", + "ctype": "uint16_t", + "desc": "Service ID of the in-flight or just-completed request", "units": "" }, { - "name": "hw_unique_id", - "ctype": "uint8_t[16]", - "desc": "128-bit hardware unique ID (from GetNodeInfo response)", + "name": "node_id", + "ctype": "uint8_t", + "desc": "Node ID of the target", "units": "" } ] }, - "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes.", - "description": "Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service." + "variable_len": true, + "notes": "Requires `USE_DRONECAN`. When state=READY(2), service-specific result fields follow the 5-byte common header. GETNODEINFO: name_len(u8)+name(bytes)+sw_major(u8)+sw_minor(u8)+sw_optional_field_flags(u8)+sw_vcs_commit(u32)+hw_major(u8)+hw_minor(u8)+hw_unique_id(u8[16]). PARAM_GETSET: name_len(u8)+name(bytes)+type(u8)+value(variable)+min_type(u8)+min(variable)+max_type(u8)+max(variable); value/min/max encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data; EMPTY(0) min/max type means no bound is present. EXECUTE_OPCODE and RESTART_NODE: ok(u8) where 1=success. Reading result when state=READY transitions slot back to IDLE.", + "description": "Polls the result of the most recent MSP2_INAV_DRONECAN_ASYNC_REQUEST. Poll at ~100ms intervals until state is READY(2) or ERROR(3)." }, "MSP2_INAV_LED_STRIP_CONFIG_EX": { "code": 8264, From 98583d5a1ce830528cef5f98d52663a3c9e0e00a Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sat, 6 Jun 2026 20:23:11 -0700 Subject: [PATCH 35/42] docs(msp): regenerate README.md from msp_messages.json Re-run gen_msp_md.py after updating msp_messages.json to keep the generated Markdown in sync with the JSON source of truth. --- docs/development/msp/README.md | 139 ++++++--------------------------- 1 file changed, 26 insertions(+), 113 deletions(-) diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index bcdb5dd3702..bef62a83772 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -4166,136 +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` | (repeated) | 13 × nodeCount | Array of per-node status records (see below) | - -Each node record is 13 bytes in the following order: - -|Field|C Type|Size (Bytes)|Description| -|---|---|---|---| -| `nodeID` | `uint8_t` | 1 | DroneCAN node ID (1-127) | -| `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 | -| `last_seen_ms` | `uint32_t` | 4 | Milliseconds since this node was last seen (FC-local timestamp delta) | -| `uptime_sec` | `uint32_t` | 4 | Node uptime in seconds (from NodeStatus broadcast) | -| `vendor_status_code` | `uint16_t` | 2 | Vendor-specific status code | +|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`. Maximum payload is 1 + (DRONECAN_MAX_NODES × 13) = 417 bytes. For full node detail (name, software/hardware version, unique ID) use `MSP2_INAV_DRONECAN_ASYNC_REQUEST` with `service_id = DRONECAN_SERVICE_GETNODEINFO (1)`. +**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_ASYNC_REQUEST (8259 / 0x2043)` -**Description:** Initiates an asynchronous DroneCAN service request to a specific node. The result is retrieved by polling `MSP2_INAV_DRONECAN_ASYNC_RESULT`. Only one request may be in-flight at a time; a new request while one is PENDING returns `accepted=1` (busy). - -**Service IDs:** - -| Constant | Value | Description | -|---|---|---| -| `DRONECAN_SERVICE_GETNODEINFO` | 1 | Retrieve node name, SW/HW version, unique ID | -| `DRONECAN_SERVICE_RESTART_NODE` | 5 | Restart the target node | -| `DRONECAN_SERVICE_EXECUTE_OPCODE` | 10 | Execute a maintenance opcode (e.g. erase config) | -| `DRONECAN_SERVICE_PARAM_GETSET` | 11 | Read or write a node parameter | - -**Request Payload (common header):** +**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| |---|---|---|---| -| `service_id` | `uint16_t` | 2 | Service to invoke (see table above; transmitted as u16 for MSP alignment, only low 8 bits used) | +| `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) | - -Additional fields follow the common header depending on `service_id`: - -**`DRONECAN_SERVICE_GETNODEINFO` (1) — no additional fields.** - -**`DRONECAN_SERVICE_RESTART_NODE` (5) — no additional fields.** - -**`DRONECAN_SERVICE_EXECUTE_OPCODE` (10):** -|Field|C Type|Size (Bytes)|Description| -|---|---|---|---| -| `opcode` | `uint8_t` | 1 | Opcode to execute (e.g. 0=save config, 1=erase config) | - -**`DRONECAN_SERVICE_PARAM_GETSET` (11) — read:** -|Field|C Type|Size (Bytes)|Description| -|---|---|---|---| -| `index` | `uint16_t` | 2 | Parameter index to read | -| `is_write` | `uint8_t` | 1 | 0 = read | -| `req_name_len` | `uint8_t` | 1 | Length of optional name hint (0 if not provided) | -| `req_name` | `uint8_t[]` | req_name_len | Optional parameter name hint | - -**`DRONECAN_SERVICE_PARAM_GETSET` (11) — write:** -|Field|C Type|Size (Bytes)|Description| -|---|---|---|---| -| `index` | `uint16_t` | 2 | Parameter index to write | -| `is_write` | `uint8_t` | 1 | 1 = write | -| `value_type` | `uint8_t` | 1 | Parameter type: 1=INT, 2=FLOAT, 3=BOOL, 4=STRING | -| `value` | (type-dependent) | varies | Value to write — see param type encoding table below | -| `req_name_len` | `uint8_t` | 1 | Length of optional name hint (0 if not provided) | -| `req_name` | `uint8_t[]` | req_name_len | Optional parameter name hint | - -Param value encoding: - -| `value_type` | Encoding | -|---|---| -| INT (1) | `uint32_t lo` + `uint32_t hi` — 64-bit little-endian split | -| FLOAT (2) | `uint32_t raw` — IEEE 754 single-precision bit pattern | -| BOOL (3) | `uint8_t` — 0=false, 1=true | -| STRING (4) | `uint8_t len` + `uint8_t data[len]` — length-prefixed, not null-terminated | - + **Reply Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| -| `accepted` | `uint8_t` | 1 | 0 = request accepted; 1 = busy (slot in use) or unrecognised service_id | -| `seq` | `uint8_t` | 1 | Sequence number; use to correlate this request with the matching `MSP2_INAV_DRONECAN_ASYNC_RESULT` response | +| `accepted` | `uint8_t` | 1 | 0=request accepted; 1=busy (slot in use) or unrecognised service_id | +| `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`. If the DroneCAN bus is not in `STATE_DRONECAN_NORMAL`, the reply returns `accepted=1` and `seq=0` without dispatching any request. Requests time out after `DRONECAN_ASYNC_TIMEOUT_MS` (2000 ms); after timeout the slot returns to IDLE and the result is discarded. +**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=1 without dispatching. ## `MSP2_INAV_DRONECAN_ASYNC_RESULT (8260 / 0x2044)` -**Description:** Polls the result of the most recent `MSP2_INAV_DRONECAN_ASYNC_REQUEST`. The caller should poll at ~100 ms intervals until `state` is `DRONECAN_ASYNC_READY (2)` or `DRONECAN_ASYNC_ERROR (3)`. +**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 (common header):** +**Request Payload:** **None** + +**Reply Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| -| `state` | `uint8_t` | 1 | Slot state: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR | -| `seq` | `uint8_t` | 1 | Sequence number matching the originating `MSP2_INAV_DRONECAN_ASYNC_REQUEST` response | -| `service_id` | `uint16_t` | 2 | Service ID of the in-flight (or just-completed) request | +| `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 | -When `state == DRONECAN_ASYNC_READY (2)`, service-specific result fields follow: - -**`DRONECAN_SERVICE_GETNODEINFO` (1):** -|Field|C Type|Size (Bytes)|Description| -|---|---|---|---| -| `name_len` | `uint8_t` | 1 | Length of node name | -| `name` | `uint8_t[]` | name_len | Node name (not null-terminated) | -| `sw_major` | `uint8_t` | 1 | Software version major | -| `sw_minor` | `uint8_t` | 1 | Software version minor | -| `sw_optional_field_flags` | `uint8_t` | 1 | Bitmask indicating which optional SW fields are present | -| `sw_vcs_commit` | `uint32_t` | 4 | VCS commit hash (if `sw_optional_field_flags` bit 0 set) | -| `hw_major` | `uint8_t` | 1 | Hardware version major | -| `hw_minor` | `uint8_t` | 1 | Hardware version minor | -| `hw_unique_id` | `uint8_t[16]` | 16 | 128-bit hardware unique ID | - -**`DRONECAN_SERVICE_PARAM_GETSET` (11):** -|Field|C Type|Size (Bytes)|Description| -|---|---|---|---| -| `name_len` | `uint8_t` | 1 | Length of parameter name | -| `name` | `uint8_t[]` | name_len | Parameter name (not null-terminated; max 92 bytes per UAVCAN spec) | -| `type` | `uint8_t` | 1 | Parameter type: 0=EMPTY, 1=INT, 2=FLOAT, 3=BOOL, 4=STRING | -| `value` | (type-dependent) | varies | Current parameter value — same encoding as request write value | -| `min_type` | `uint8_t` | 1 | Type of min bound (0=EMPTY means no bound) | -| `min` | (type-dependent) | varies | Minimum value — present only if `min_type` is INT or FLOAT | -| `max_type` | `uint8_t` | 1 | Type of max bound (0=EMPTY means no bound) | -| `max` | (type-dependent) | varies | Maximum value — present only if `max_type` is INT or FLOAT | - -Min/max encoding follows the same INT (8-byte lo/hi split) and FLOAT (4-byte raw) patterns as the write value. - -**`DRONECAN_SERVICE_EXECUTE_OPCODE` (10) and `DRONECAN_SERVICE_RESTART_NODE` (5):** -|Field|C Type|Size (Bytes)|Description| -|---|---|---|---| -| `ok` | `uint8_t` | 1 | 1 = success, 0 = failure | - -**Notes:** Requires `USE_DRONECAN`. Reading the result when `state == DRONECAN_ASYNC_READY` transitions the slot back to `DRONECAN_ASYNC_IDLE`. If `state` is IDLE or PENDING, no result fields are present beyond the 5-byte common header. +**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`. From 62c9d4a971d28c75f3bcae883bce9dea9e0b486c Mon Sep 17 00:00:00 2001 From: daijoubu Date: Mon, 29 Jun 2026 19:21:34 -0700 Subject: [PATCH 36/42] fix(dronecan): mask TX ISR around dronecanAsyncRequest canardRequestOrRespond dronecanAsyncRequest() is called from the main loop (via MSP handler). canardRequestOrRespond() mutates the libcanard TX queue, which is also accessed by the ISR-driven processCanardTxQueue(). Without masking, a TX-complete ISR could corrupt the queue mid-enqueue. --- src/main/drivers/dronecan/dronecan.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 2a8581b204a..d36fc0d62e8 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -444,9 +444,11 @@ bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *paylo } // 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); From 8eab76bac1f92592ed8c431825365078c74ce7e5 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Tue, 30 Jun 2026 21:00:38 -0700 Subject: [PATCH 37/42] test(dronecan): update GAP-S2 test for async slot architecture The struct dronecanNodeInfo_t was stripped of GetNodeInfo fields in 96f8a4bd9 (shared async slot) to save ~3.5 KB RAM. GetNodeInfo results now go to dronecanAsyncSlot.result.node_info (DRONECAN_ASYNC_READY) instead of the node table. Update the test to: - Prime the async slot (PENDING + service_id + node_id) before dispatch, since handle_AsyncServiceResponse guards on all three - Assert slot state transitions to DRONECAN_ASYNC_READY - Assert result fields via dronecanAsyncSlot.result.node_info - Reset dronecanAsyncSlot in SetUp() for test isolation --- src/test/unit/CMakeLists.txt | 11 +++- .../unit/dronecan_application_unittest.cc | 58 ++++++++++++------- 2 files changed, 46 insertions(+), 23 deletions(-) diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index f7ad66653bd..dfccb50294b 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -84,7 +84,16 @@ set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.power.BatteryInfo.c" "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.RTCMStream.c" "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.Timestamp.c" - "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.ECEFPositionVelocity.c") + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.ECEFPositionVelocity.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.ExecuteOpcode_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.ExecuteOpcode_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_req.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_res.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Value.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.NumericValue.c" + "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Empty.c") set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_includes "../../lib/main/Dronecan/dsdlc_generated/include") set_property(SOURCE dronecan_application_unittest.cc PROPERTY definitions diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index 2b2fad29b24..c65e959435b 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -171,8 +171,6 @@ TEST_F(DroneCANNodeTableTest, NewNodeAddedOnFirstStatus) EXPECT_EQ(node->mode, UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL); EXPECT_EQ(node->uptime_sec, 100u); EXPECT_EQ(node->vendor_status_code, 0xABCDu); - EXPECT_EQ(node->name_len, 0u); - EXPECT_EQ(node->name[0], '\0'); } /* GAP-N1 (second node): Two distinct IDs → two separate entries */ @@ -365,6 +363,8 @@ class DroneCANDispatchTest : public ::testing::Test { void SetUp() override { activeNodeCount = 0; memset(nodeTable, 0, sizeof(dronecanNodeInfo_t) * DRONECAN_MAX_NODES); + memset(&dronecanAsyncSlot, 0, sizeof(dronecanAsyncSlot)); + dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE; mock_time_ms = 0; canardInit(&ins, memory_pool, sizeof(memory_pool), onTransferReceived, shouldAcceptTransfer, NULL); @@ -372,15 +372,26 @@ class DroneCANDispatchTest : public ::testing::Test { } }; -/* GAP-S2: GetNodeInfo response → handler populates name and version fields */ -TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry) +/* GAP-S2: GetNodeInfo response → handler populates async slot result. + * The node table (dronecanNodeInfo_t) holds only NodeStatus-level fields since + * commit 96f8a4bd9 stripped the GetNodeInfo fields to save ~3.5 KB RAM and + * replaced auto-fetch with the on-demand async slot pattern. */ +TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesAsyncSlot) { - /* Pre-insert node 42 via a NodeStatus so the table has a slot for it */ + /* Pre-insert node 42 via a NodeStatus (node table is independent of async slot) */ uint8_t ns_buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4]; CanardRxTransfer ns_xfer = makeNodeStatusTransfer(42, 10, 0, 0, 0, ns_buf); handle_NodeStatus(&ins, &ns_xfer); ASSERT_EQ(dronecanGetNodeCount(), 1u); + /* Prime the async slot — handle_AsyncServiceResponse guards on state, service_id, + * node_id, and transfer_id. The guard checks transfer_id == (slot.transfer_id-1)&0x1F, + * so set transfer_id=1 so the expected in-flight id is 0 (matching xfer.transfer_id). */ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.service_id = DRONECAN_SERVICE_GETNODEINFO; + dronecanAsyncSlot.node_id = 42; + dronecanAsyncSlot.transfer_id = 1; + /* Build a GetNodeInfo response from node 42 */ struct uavcan_protocol_GetNodeInfoResponse resp; memset(&resp, 0, sizeof(resp)); @@ -396,9 +407,8 @@ TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry) resp.hardware_version.major = 2; resp.hardware_version.minor = 0; - for (int i = 0; i < 16; i++) { + for (int i = 0; i < 16; i++) resp.hardware_version.unique_id[i] = (uint8_t)(0xA0 + i); - } const char *name = "com.example.gps"; resp.name.len = (uint8_t)strlen(name); @@ -416,23 +426,27 @@ TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry) onTransferReceived(&ins, &xfer); - /* Verify the node table entry was populated */ - const dronecanNodeInfo_t *node = dronecanGetNode(0); - ASSERT_NE(node, nullptr); - EXPECT_EQ(node->nodeID, 42u); + /* Slot must now be READY */ + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY); - EXPECT_EQ(node->name_len, (uint8_t)strlen(name)); - EXPECT_EQ(0, memcmp(node->name, name, node->name_len)); + /* 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(node->sw_major, 1u); - EXPECT_EQ(node->sw_minor, 7u); - EXPECT_EQ(node->sw_optional_field_flags, 1u); - EXPECT_EQ(node->sw_vcs_commit, 0xDEADBEEFu); + EXPECT_EQ(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(node->hw_major, 2u); - EXPECT_EQ(node->hw_minor, 0u); - for (int i = 0; i < 16; i++) { - EXPECT_EQ(node->hw_unique_id[i], (uint8_t)(0xA0 + i)) + 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); } From e9bc83251ee012440c7985d24bda4cc85ff75298 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 1 Jul 2026 10:30:50 -0700 Subject: [PATCH 38/42] fix(dronecan): address code review findings on param-getset branch - Replace hardcoded 2000 with DRONECAN_ASYNC_TIMEOUT_MS in timeout check and use >= to align with dronecanAsyncRequest guard - Add DRONECAN_NODE_STALE_TIMEOUT_MS constant; replace magic 10000 - Fix double-indented body in timeout check - Fix brace alignment in shouldAcceptTransfer response cases - Clear async slot to IDLE after reading ERROR state in MSP result handler - Add comment to dronecanParamResult_t.name[93] explaining DSDL capacity - Document 0xFF (bus not ready) sentinel in MSP async request JSON/README --- docs/development/msp/README.md | 4 ++-- docs/development/msp/msp_messages.json | 4 ++-- src/main/drivers/dronecan/dronecan.c | 16 ++++++++-------- src/main/drivers/dronecan/dronecan.h | 7 ++++--- src/main/fc/fc_msp.c | 2 ++ 5 files changed, 18 insertions(+), 15 deletions(-) diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index bef62a83772..6231331dc7a 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -4190,10 +4190,10 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Reply Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| -| `accepted` | `uint8_t` | 1 | 0=request accepted; 1=busy (slot in use) or unrecognised service_id | +| `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=1 without dispatching. +**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). diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 0da2ccb6fd4..ac70180150e 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9897,7 +9897,7 @@ { "name": "accepted", "ctype": "uint8_t", - "desc": "0=request accepted; 1=busy (slot in use) or unrecognised service_id", + "desc": "0=request accepted; 1=busy (slot in use) or unrecognised service_id; 0xFF=bus not in STATE_DRONECAN_NORMAL (not ready)", "units": "" }, { @@ -9909,7 +9909,7 @@ ] }, "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=1 without dispatching.", + "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": { diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index d36fc0d62e8..da340f27831 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -156,9 +156,9 @@ void dronecanUpdate(timeUs_t currentTimeUs) // Check for and expire any pending async requests that have timed out. if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING && - millis() - dronecanAsyncSlot.requested_at_ms > 2000) { - dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR; - } + millis() - dronecanAsyncSlot.requested_at_ms >= DRONECAN_ASYNC_TIMEOUT_MS) { + dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR; + } for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) { @@ -666,7 +666,7 @@ static void process1HzTasks(timeUs_t timestamp_usec) // Remove nodes that have stopped broadcasting NodeStatus for (uint8_t i = 0; i < activeNodeCount; ) { - if (millis() - nodeTable[i].last_seen_ms > 10000) { + if (millis() - nodeTable[i].last_seen_ms > DRONECAN_NODE_STALE_TIMEOUT_MS) { nodeTable[i] = nodeTable[activeNodeCount - 1]; activeNodeCount--; } else { @@ -715,22 +715,22 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, 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; - } + } } } if (transfer_type == CanardTransferTypeBroadcast) { diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h index 460e25274a9..8722bccb916 100644 --- a/src/main/drivers/dronecan/dronecan.h +++ b/src/main/drivers/dronecan/dronecan.h @@ -48,8 +48,9 @@ typedef enum { #define DRONECAN_SERVICE_EXECUTE_OPCODE 10 #define DRONECAN_SERVICE_PARAM_GETSET 11 -#define DRONECAN_ASYNC_TIMEOUT_MS 2000 -#define DRONECAN_STATE_NOT_READY 0xFF // MSP sentinel: bus not in STATE_NORMAL; outside dronecanAsyncState_e range +#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 @@ -90,7 +91,7 @@ typedef struct dronecanParamResult_s { uint8_t value_str_len; char value_str[128]; uint8_t name_len; - char name[93]; + 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; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 77daf729543..bfafb01a98f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4747,6 +4747,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; } dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE; + } else if (dronecanAsyncSlot.state == DRONECAN_ASYNC_ERROR) { + dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE; } *ret = MSP_RESULT_ACK; } From 41b1d5e712b0f04d95900356b9187bc520436c5b Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 1 Jul 2026 14:37:00 -0700 Subject: [PATCH 39/42] test(dronecan): add unit tests for PARAM_GETSET, EXECUTE_OPCODE, RESTART_NODE response decode MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Extends dronecan_application_unittest.cc with 13 new tests covering: - handle_AsyncServiceResponse guard rejections (IDLE state, wrong node_id, wrong transfer_id, wrong service_id) - PARAM_GETSET response decode for all five Value types (INT, FLOAT, BOOL, STRING, EMPTY) including integer and float min/max range fields - EXECUTE_OPCODE response decode (ok=true and ok=false) - RESTART_NODE response decode (ok=true) - dronecanAsyncRequest re-entry guard (slot PENDING → returns false) 26/26 tests pass. --- .../unit/dronecan_application_unittest.cc | 352 ++++++++++++++++++ 1 file changed, 352 insertions(+) diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index c65e959435b..694168fe27f 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -26,6 +26,9 @@ extern "C" { /* DSDL types used by dronecan.c handlers */ #include "uavcan.protocol.NodeStatus.h" #include "uavcan.protocol.GetNodeInfo.h" +#include "uavcan.protocol.param.GetSet_res.h" +#include "uavcan.protocol.param.ExecuteOpcode_res.h" +#include "uavcan.protocol.RestartNode_res.h" /* Canard core and STM32 driver declarations */ #include "drivers/dronecan/libcanard/canard.h" @@ -130,6 +133,65 @@ static CanardRxTransfer makeNodeStatusTransfer( return xfer; } +/* ========================================================================= + * Helpers: encode response structs and build CanardRxTransfer objects. + * ========================================================================= */ + +static CanardRxTransfer makeParamGetSetTransfer( + uint8_t source_node_id, uint8_t transfer_id, + struct uavcan_protocol_param_GetSetResponse *resp, + uint8_t *buf) +{ + uint32_t len = uavcan_protocol_param_GetSetResponse_encode(resp, buf); + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_ID; + xfer.source_node_id = source_node_id; + xfer.transfer_id = transfer_id; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + +static CanardRxTransfer makeExecuteOpcodeTransfer( + uint8_t source_node_id, uint8_t transfer_id, + bool ok, uint8_t *buf) +{ + struct uavcan_protocol_param_ExecuteOpcodeResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.ok = ok; + uint32_t len = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&resp, buf); + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_ID; + xfer.source_node_id = source_node_id; + xfer.transfer_id = transfer_id; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + +static CanardRxTransfer makeRestartNodeTransfer( + uint8_t source_node_id, uint8_t transfer_id, + bool ok, uint8_t *buf) +{ + struct uavcan_protocol_RestartNodeResponse resp; + memset(&resp, 0, sizeof(resp)); + resp.ok = ok; + uint32_t len = uavcan_protocol_RestartNodeResponse_encode(&resp, buf); + CanardRxTransfer xfer; + memset(&xfer, 0, sizeof(xfer)); + xfer.transfer_type = CanardTransferTypeResponse; + xfer.data_type_id = UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_ID; + xfer.source_node_id = source_node_id; + xfer.transfer_id = transfer_id; + xfer.payload_head = buf; + xfer.payload_len = (uint16_t)len; + return xfer; +} + /* ========================================================================= * Node table tests (GAP-N1 … GAP-N4) * ========================================================================= */ @@ -450,3 +512,293 @@ TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesAsyncSlot) 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. + * Slot PENDING with requested_at_ms=0 and mock_time_ms=0 satisfies the + * timeout guard (0 < DRONECAN_ASYNC_TIMEOUT_MS), so the function returns false + * without touching the internal CAN driver. */ +TEST_F(DroneCANDispatchTest, AsyncRequest_RejectedWhilePending) +{ + dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING; + dronecanAsyncSlot.requested_at_ms = 0; + mock_time_ms = 0; + + EXPECT_FALSE(dronecanAsyncRequest(DRONECAN_SERVICE_PARAM_GETSET, 42, nullptr)); + EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING); +} From 1f59089b0abdd2dff766da7b83ba132eea28d8f4 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 1 Jul 2026 16:27:01 -0700 Subject: [PATCH 40/42] fix(dronecan): address code review findings from param-getset pass 2 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - dronecan.c: clamp name and string-value lengths before memcpy in dronecanAsyncRequest() — safe today because MSP pre-clamps, but the function is part of a public API and must not rely on caller preconditions - dronecan.c: update stale block comment on handle_AsyncServiceResponse - fc_msp.c: replace manual 32-bit split/join of int64 wire values with memcpy through uint64_t, eliminating signed left-shift and implementation-defined right-shift on negative values - fc_msp.c: remove trailing whitespace on USE_DRONECAN #endif - unittest: narrow GAP-S7 re-entry guard test to RESTART_NODE so the null-payload check cannot mask a missing re-entry guard --- src/main/drivers/dronecan/dronecan.c | 19 ++++++++++++------- src/main/fc/fc_msp.c | 16 ++++++++-------- .../unit/dronecan_application_unittest.cc | 9 +++++---- 3 files changed, 25 insertions(+), 19 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index da340f27831..d597397f23b 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -399,17 +399,22 @@ bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *paylo case DRONECAN_PARAM_TYPE_BOOL: getset.value.boolean_value = req->value_bool; break; - case DRONECAN_PARAM_TYPE_STRING: - getset.value.string_value.len = req->value_str_len; - memcpy(getset.value.string_value.data, req->value_str, req->value_str_len); + 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; } } - getset.name.len = req->req_name_len; - memcpy(getset.name.data, req->req_name, req->req_name_len); + 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; @@ -464,8 +469,8 @@ bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *paylo } /* - Handle a response to an asynchronous service request such as - a configuration parameter on the node info structure + Handle responses for any pending async service request + (GETNODEINFO, PARAM_GETSET, EXECUTE_OPCODE, RESTART_NODE). */ static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *transfer) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bfafb01a98f..7de70dbdfa0 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4619,9 +4619,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu switch (req.value_type) { case DRONECAN_PARAM_TYPE_INT: if (sbufBytesRemaining(src) >= 8) { - uint32_t lo = sbufReadU32(src); - uint32_t hi = sbufReadU32(src); - req.value_int = (int64_t)lo | ((int64_t)hi << 32); + uint64_t tmp; + sbufReadData(src, &tmp, sizeof(tmp)); + sbufAdvance(src, sizeof(tmp)); + req.value_int = (int64_t)tmp; } break; case DRONECAN_PARAM_TYPE_FLOAT: @@ -4699,10 +4700,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu sbufWriteU8(dst, r->type); switch (r->type) { case DRONECAN_PARAM_TYPE_INT: { - uint32_t lo = (uint32_t)(r->value_int & 0xFFFFFFFF); - uint32_t hi = (uint32_t)((r->value_int >> 32) & 0xFFFFFFFF); - sbufWriteU32(dst, lo); - sbufWriteU32(dst, hi); + uint64_t tmp; + memcpy(&tmp, &r->value_int, sizeof(tmp)); + sbufWriteData(dst, &tmp, sizeof(tmp)); break; } case DRONECAN_PARAM_TYPE_FLOAT: { @@ -4753,7 +4753,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ACK; } break; -#endif +#endif #if defined(USE_FLASHFS) case MSP_DATAFLASH_READ: diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index 694168fe27f..dc96e4c4b97 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -790,15 +790,16 @@ TEST_F(DroneCANDispatchTest, RestartNodeOkResponse_PopulatesSlot) * ========================================================================= */ /* GAP-S7: A second async request is rejected while one is already in flight. - * Slot PENDING with requested_at_ms=0 and mock_time_ms=0 satisfies the - * timeout guard (0 < DRONECAN_ASYNC_TIMEOUT_MS), so the function returns false - * without touching the internal CAN driver. */ + * 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_PARAM_GETSET, 42, nullptr)); + EXPECT_FALSE(dronecanAsyncRequest(DRONECAN_SERVICE_RESTART_NODE, 42, nullptr)); EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING); } From 219e4676139108e8b0979da7ebaac87f08cee71a Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 1 Jul 2026 17:36:55 -0700 Subject: [PATCH 41/42] fix(dronecan): address code review findings from param-getset pass 3 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - fc_msp.c: add missing sbufAdvance after sbufReadData in STRING write arm of PARAM_GETSET decoder — without it, req_name_len read the first byte of the string value, silently targeting the wrong param on the node - fc_msp.c: add sbufAdvance after name read for consistency with INT path - fc_msp.c: replace signed right-shift with memcpy/uint64_t for min_int and max_int range encoding, consistent with the value_int fix in pass 2 - dronecan.c, fc_msp.c: remove trailing whitespace on blank lines in new code --- src/main/drivers/dronecan/dronecan.c | 4 ++-- src/main/fc/fc_msp.c | 20 +++++++++++++------- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index d597397f23b..94497f06fd8 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -374,7 +374,7 @@ bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *paylo 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; @@ -420,7 +420,7 @@ bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *paylo signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE; break; } - + case DRONECAN_SERVICE_EXECUTE_OPCODE: { if (!payload) return false; const uint8_t *opcode = (const uint8_t *)payload; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7de70dbdfa0..b8b238c0177 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4601,7 +4601,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ACK; break; } - + bool accepted = false; if (service_id == DRONECAN_SERVICE_GETNODEINFO) { accepted = dronecanAsyncRequest(service_id, nodeID, NULL); @@ -4640,8 +4640,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu 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) + if (sbufBytesRemaining(src) >= req.value_str_len) { sbufReadData(src, req.value_str, req.value_str_len); + sbufAdvance(src, req.value_str_len); + } } break; } @@ -4650,8 +4652,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu 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) + if (sbufBytesRemaining(src) >= req.req_name_len) { sbufReadData(src, req.req_name, req.req_name_len); + sbufAdvance(src, req.req_name_len); + } } accepted = dronecanAsyncRequest(service_id, nodeID, &req); } else if (service_id == DRONECAN_SERVICE_EXECUTE_OPCODE) { @@ -4723,8 +4727,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } sbufWriteU8(dst, r->min_type); if (r->min_type == DRONECAN_PARAM_TYPE_INT) { - sbufWriteU32(dst, (uint32_t)(r->min_int & 0xFFFFFFFF)); - sbufWriteU32(dst, (uint32_t)((r->min_int >> 32) & 0xFFFFFFFF)); + 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); @@ -4732,8 +4737,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } sbufWriteU8(dst, r->max_type); if (r->max_type == DRONECAN_PARAM_TYPE_INT) { - sbufWriteU32(dst, (uint32_t)(r->max_int & 0xFFFFFFFF)); - sbufWriteU32(dst, (uint32_t)((r->max_int >> 32) & 0xFFFFFFFF)); + 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); From b2f2507cafc3e3029703489e144e633afb95409d Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 1 Jul 2026 18:57:40 -0700 Subject: [PATCH 42/42] fix(dronecan): address code review findings from param-getset pass 4 - dronecan.c: expand handle_AsyncServiceResponse comment to explain the single-slot design rationale - dronecan.c: normalise shouldAcceptTransfer to consistent 4-space indentation (was mixed tabs/spaces from prior edits) - unittest: add shouldAcceptTransfer tests for PARAM_GETSET, EXECUTE_OPCODE, and RESTART_NODE response acceptance (29 tests total) --- src/main/drivers/dronecan/dronecan.c | 72 ++++++++----------- .../unit/dronecan_application_unittest.cc | 39 ++++++++++ 2 files changed, 67 insertions(+), 44 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 94497f06fd8..8619068381e 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -471,6 +471,8 @@ bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *paylo /* 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) { @@ -704,72 +706,54 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, CanardTransferType transfer_type, uint8_t source_node_id) { - UNUSED(ins); + UNUSED(ins); UNUSED(source_node_id); if (transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; - return true; - } - } - } - if (transfer_type == CanardTransferTypeResponse) { - switch (data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE; + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; return true; } - - case UAVCAN_PROTOCOL_PARAM_GETSET_ID: { + } + 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: { + case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE; return true; - } - - case UAVCAN_PROTOCOL_RESTARTNODE_ID: { + case UAVCAN_PROTOCOL_RESTARTNODE_ID: *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; return true; } - } - } - if (transfer_type == CanardTransferTypeBroadcast) { - // see if we want to handle a specific broadcast packet - switch (data_type_id) { - - case UAVCAN_PROTOCOL_NODESTATUS_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE; - return true; - } - case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: { + } + 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: { + case UAVCAN_EQUIPMENT_GNSS_FIX_ID: *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE; return true; - } - case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: { + 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 ) diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc index dc96e4c4b97..57af83fd229 100644 --- a/src/test/unit/dronecan_application_unittest.cc +++ b/src/test/unit/dronecan_application_unittest.cc @@ -408,6 +408,45 @@ TEST(DroneCANShouldAcceptTransfer, RejectsUnknownResponseId) EXPECT_FALSE(accept); } +TEST(DroneCANShouldAcceptTransfer, AcceptsParamGetSetResponse) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsExecuteOpcodeResponse) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_SIGNATURE); +} + +TEST(DroneCANShouldAcceptTransfer, AcceptsRestartNodeResponse) +{ + uint64_t signature = 0; + bool accept = shouldAcceptTransfer( + nullptr, &signature, + UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_ID, + CanardTransferTypeResponse, + 42); + + EXPECT_TRUE(accept); + EXPECT_EQ(signature, UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_SIGNATURE); +} + /* ========================================================================= * onTransferReceived dispatch test (GAP-S2) *