diff --git a/docs/DroneCAN-Driver.md b/docs/DroneCAN-Driver.md index 128195dfd71..f152bc362d4 100644 --- a/docs/DroneCAN-Driver.md +++ b/docs/DroneCAN-Driver.md @@ -637,6 +637,168 @@ CAN bus must have 120Ω termination at each end. Without proper termination: --- +## Error Recovery and Graceful Disable + +### Safe Initialization + +The DroneCAN driver initialization sequence in `dronecanInit()` and `dronecanSTM32Initializexx()` has been designed with failure safety in mind: + +**Initialization Order:** +1. Memory pool allocation +2. CAN hardware setup (GPIO, clock, HAL initialization) +3. CAN filter configuration +4. RX message queue setup +5. **Interrupt enable** ← Moved to END of init sequence + +**Critical Safety Fix:** The CAN interrupt (e.g., `CAN1_RX0_IRQn`) is enabled ONLY after all initialization steps complete successfully. If any step fails (steps 1-4), the function returns an error code without enabling interrupts, preventing spurious interrupts on unconfigured hardware. + +### Graceful Disable Behavior + +If DroneCAN is disabled via CLI (`set dronecan_enabled = 0`), the driver cleanly disables the CAN interface: + +```c +void dronecanDisable(void) { + // Disable interrupt first (prevents spurious interrupts) + HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn); + + // Clean up TX queue (discard pending messages) + while (canardPeekTxQueue(&canard) != NULL) { + canardPopTxQueue(&canard); + } + + // Stop CAN hardware (enters sleep mode) + HAL_CAN_Stop(&hcan1); + + // Mark as disabled + dronecanInitialized = false; +} +``` + +**Benefits:** +- No messages stuck in TX queue +- No sporadic CAN activity after disable +- CAN bus remains stable for other potential users +- Can re-enable without hardware issues + +### Error Recovery + +The driver implements automatic error recovery for common CAN bus faults: + +#### Bus-Off Recovery + +When the CAN bus enters "bus-off" state (due to excessive errors), the STM32 CAN controller automatically recovers after transmitting 128 error frames. The driver detects this: + +```c +void dronecanUpdate(void) { + // Check for bus-off condition + if (hcan1.State == HAL_CAN_STATE_BUS_OFF) { + // Bus-off detected - wait for automatic recovery + // No manual intervention needed + LOG_INFO(CAN, "Bus-off detected, recovering..."); + } +} +``` + +**Typical Causes:** +- Wiring problems (CAN_H/CAN_L swapped) +- Termination resistor issues +- Transceiver failures +- High electrical noise + +**Prevention:** +1. Verify CAN bus wiring and termination +2. Check transceiver power supply (3.3V) +3. Reduce message rates if experiencing high error rates +4. Electrically isolate CAN bus from noisy power supplies + +#### Incomplete Initialization Handling + +If hardware initialization fails partway through (e.g., GPIO allocation fails, clock not enabled), the function safely aborts: + +```c +int8_t dronecanInit(...) { + // Setup step 1 + if (setupGPIO() != 0) { + return DRONECAN_INIT_FAILED; // Abort - no interrupt enabled + } + + // Setup step 2 + if (setupHALCAN() != 0) { + return DRONECAN_INIT_FAILED; // Abort - no interrupt enabled + } + + // ... more setup steps ... + + // Enable interrupt ONLY after all steps succeed + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); + + return DRONECAN_INIT_OK; +} +``` + +**Why This Matters:** +- If enabled early, interrupt fires on unconfigured hardware → crash +- If enabled late (after failure), flight controller continues safely +- Previous versions had race condition → now fixed + +#### Runtime Error Handling + +The `dronecanUpdate()` function handles runtime errors gracefully: + +```c +void dronecanUpdate(void) { + if (!dronecanInitialized) { + return; // Not initialized, skip + } + + // Process RX messages + canardUpdate(&canard); // May encounter decode errors + + // Process TX queue + processCanardTxQueue(); + + // If bus goes offline, driver continues + // Waits for automatic recovery or manual restart +} +``` + +**Fault Tolerance:** +- Single message decode error doesn't affect others +- TX queue backpressure handled (graceful wait) +- Bus-off state doesn't crash flight controller +- Can survive transceiver power loss and recovery + +### Node Status Broadcasting + +The driver broadcasts periodic node status to other CAN nodes, enabling network monitoring: + +```c +void broadcastNodeStatus(void) { + struct uavcan_protocol_NodeStatus status; + status.uptime_sec = ++uptime; + status.health = getHealthStatus(); + status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + + // Broadcast every 1 second + canardBroadcast(&canard, + UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); +} +``` + +**Enables:** +- Network topology visualization +- Health monitoring of flight controller +- DroneCAN GUI integration +- Future dynamic node ID assignment + +--- + ## References ### UAVCAN Protocol Resources @@ -675,6 +837,7 @@ CAN bus must have 120Ω termination at each end. Without proper termination: | Date | Version | Changes | |---|---|---| +| 2026-02-18 | 1.1 | Added error recovery, graceful disable behavior, and safe initialization documentation | | 2026-02-16 | 1.0 | Initial version - handler-based architecture documentation | --- diff --git a/docs/DroneCAN.md b/docs/DroneCAN.md index 54f660d4cbc..2e1b4df9b44 100644 --- a/docs/DroneCAN.md +++ b/docs/DroneCAN.md @@ -73,6 +73,228 @@ save Both voltage and current come from the same DroneCAN BatteryInfo message, so they update together when using a DroneCAN battery monitor. +## Configuration Examples + +### Example 1: GPS Only Setup + +Use a DroneCAN GPS without battery monitoring: + +``` +# Enable DroneCAN interface +set dronecan_node_id = 10 +set dronecan_bitrate = 1000KBPS + +# Configure GPS +set gps_provider = DRONECAN + +# Disable battery monitoring from DroneCAN +set bat_voltage_src = ADC +set current_meter_type = NONE + +save +``` + +**Result:** Flight controller receives GPS data from DroneCAN GPS receiver on node ID 1-127 (auto-detected). + +--- + +### Example 2: Battery Monitoring Only + +Use DroneCAN for battery voltage and current without GPS: + +``` +# Enable DroneCAN interface +set dronecan_node_id = 10 +set dronecan_bitrate = 1000KBPS + +# Configure battery monitoring +set bat_voltage_src = CAN +feature CURRENT_METER +set current_meter_type = CAN + +# Keep GPS on other source +set gps_provider = UBLOX + +save +``` + +**Result:** Flight controller receives battery voltage and current from DroneCAN BatteryInfo messages. + +--- + +### Example 3: GPS + Battery Combined + +Use DroneCAN for both GPS and battery monitoring: + +``` +# Enable DroneCAN interface +set dronecan_node_id = 10 +set dronecan_bitrate = 1000KBPS + +# Configure GPS +set gps_provider = DRONECAN + +# Configure battery monitoring +set bat_voltage_src = CAN +feature CURRENT_METER +set current_meter_type = CAN + +save +``` + +**Result:** Single CAN bus provides both GPS and battery data. Simplifies hardware setup significantly. + +**Note:** The GPS and battery monitor must be configured as separate DroneCAN nodes with different node IDs (e.g., GPS on node 1, battery monitor on node 2). + +--- + +### Example 4: Multi-Node DroneCAN Network + +Setting up multiple DroneCAN peripherals on a single CAN bus: + +``` +# Flight Controller Configuration +set dronecan_node_id = 10 # Flight controller = node 10 +set dronecan_bitrate = 1000KBPS + +# Configure GPS (from node 1) +set gps_provider = DRONECAN + +# Configure battery monitor (from node 2) +set bat_voltage_src = CAN +feature CURRENT_METER +set current_meter_type = CAN + +save +``` + +**Peripheral Configuration (using dronecan_gui or similar tool):** +- **GPS Receiver:** Node ID = 1, Bitrate = 1000 KBPS +- **Battery Monitor:** Node ID = 2, Bitrate = 1000 KBPS +- **Potential Future Peripheral:** Node ID = 3, etc. + +**CAN Bus Layout:** +``` + [FC: Node 10] + | + +-- CAN_H/CAN_L + | + [GPS: Node 1] ----+---- [Battery: Node 2] + | | + 120R 120R + (termination) (termination) +``` + +**Important:** Each node must have a unique ID (1-127). Don't assign two devices the same node ID. + +--- + +### Example 5: SITL Simulation + +Testing DroneCAN configuration without hardware: + +```bash +# Build and run SITL +make SITL_TARGET=F405_OHMINIV2 + +# In SITL console, configure DroneCAN: +set dronecan_node_id = 10 +set dronecan_bitrate = 1000KBPS +set gps_provider = DRONECAN +set bat_voltage_src = CAN +save +``` + +**Note:** SITL includes a virtual CAN bus for testing. To send simulated DroneCAN messages: + +1. Connect SITL to external DroneCAN simulator or +2. Use `candump` and `cansend` tools to inject test messages +3. Monitor responses with `candump can0` + +--- + +### Example 6: Hardware-Specific Setup for MATEKH743 + +The MATEKH743 has integrated FDCAN peripheral: + +``` +# MATEKH743 with DroneCAN +set dronecan_node_id = 10 +set dronecan_bitrate = 1000KBPS + +# GPS configuration +set gps_provider = DRONECAN + +# Battery monitoring (if using DroneCAN BMS) +set bat_voltage_src = CAN +feature CURRENT_METER +set current_meter_type = CAN + +# Save and reboot +save +``` + +**Wiring for MATEKH743:** +- Use the **CAN 1** port on the board +- Connect to DroneCAN GPS on CAN_H/CAN_L +- Connect to DroneCAN battery monitor on same CAN bus +- Add 120Ω termination resistors at both ends + +--- + +### Example 7: Hardware-Specific Setup for MATEKF765SE + +The MATEKF765SE has STM32F765 with bxCAN: + +``` +# MATEKF765SE with DroneCAN +set dronecan_node_id = 10 +set dronecan_bitrate = 1000KBPS + +# Note: F7 boards may have bitrate limitations +# If you experience issues, try 500KBPS +# set dronecan_bitrate = 500KBPS + +# GPS configuration +set gps_provider = DRONECAN + +# Battery monitoring +set bat_voltage_src = CAN +feature CURRENT_METER +set current_meter_type = CAN + +save +``` + +**Wiring for MATEKF765SE:** +- Use the **CAN 1** port on the board +- F7 boards support both 500 KBPS and 1000 KBPS +- If you have range issues, try lower bitrate + +--- + +### Configuration Verification + +After setting up DroneCAN, verify configuration: + +``` +# Check DroneCAN settings +set dronecan_node_id +set dronecan_bitrate +set gps_provider +set bat_voltage_src +set current_meter_type +``` + +**Expected Output for GPS + Battery:** +``` +dronecan_node_id = 10 +dronecan_bitrate = 1000KBPS +gps_provider = DRONECAN +bat_voltage_src = CAN +current_meter_type = CAN +``` + ## Hardware Setup ### Wiring diff --git a/src/test/unit/dronecan_messages_unittest.cc b/src/test/unit/dronecan_messages_unittest.cc index 380e637cda0..44392db25a9 100644 --- a/src/test/unit/dronecan_messages_unittest.cc +++ b/src/test/unit/dronecan_messages_unittest.cc @@ -531,3 +531,260 @@ TEST(DroneCANConstants, DataTypeIDs) EXPECT_EQ(UAVCAN_EQUIPMENT_GNSS_FIX2_ID, 1063); EXPECT_EQ(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID, 1092); } + +// =========================================================================== +// Error Handling Tests (Enhanced Coverage) +// =========================================================================== + +TEST_F(DroneCANMessageTest, GNSSFix2_ZeroPayload) +{ + // Test with empty transfer (zero payload length) + CanardRxTransfer transfer = makeTransfer(0); + struct uavcan_equipment_gnss_Fix2 rx_msg; + memset(&rx_msg, 0, sizeof(rx_msg)); + + // Decode should fail gracefully with zero payload + bool result = uavcan_equipment_gnss_Fix2_decode(&transfer, &rx_msg); + + // Result may be true (failed decode) or false depending on decoder implementation + // The important thing is it doesn't crash + (void)result; +} + +TEST_F(DroneCANMessageTest, GNSSFix2_TruncatedBuffer) +{ + // Create a valid message, then truncate the encoded payload + struct uavcan_equipment_gnss_Fix2 tx_msg; + memset(&tx_msg, 0, sizeof(tx_msg)); + + tx_msg.latitude_deg_1e8 = 377749000; + tx_msg.longitude_deg_1e8 = -1224194000; + tx_msg.height_msl_mm = 16000; + tx_msg.sats_used = 12; + tx_msg.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + + uint32_t full_encoded_len = uavcan_equipment_gnss_Fix2_encode(&tx_msg, buffer +#if CANARD_ENABLE_TAO_OPTION + , false +#endif + ); + EXPECT_GT(full_encoded_len, 5u); + + // Test with each truncation point + for (uint32_t len = 1; len < full_encoded_len; len++) { + CanardRxTransfer transfer = makeTransfer(len); + struct uavcan_equipment_gnss_Fix2 rx_msg; + memset(&rx_msg, 0, sizeof(rx_msg)); + + // Should not crash, may return true/false + bool result = uavcan_equipment_gnss_Fix2_decode(&transfer, &rx_msg); + (void)result; + } +} + +TEST_F(DroneCANMessageTest, BatteryInfo_NegativeCurrentAndTemperature) +{ + // Test with negative current (discharging) and cold temperatures + struct uavcan_equipment_power_BatteryInfo tx_msg; + memset(&tx_msg, 0, sizeof(tx_msg)); + + tx_msg.voltage = 14.0f; + tx_msg.current = -50.0f; // Negative current (discharging) + tx_msg.temperature = -40.0f; // Cold ambient temperature + tx_msg.state_of_charge_pct = 50; + tx_msg.battery_id = 0; + + uint32_t encoded_len = uavcan_equipment_power_BatteryInfo_encode(&tx_msg, buffer +#if CANARD_ENABLE_TAO_OPTION + , false +#endif + ); + + EXPECT_GT(encoded_len, 0u); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_equipment_power_BatteryInfo rx_msg; + memset(&rx_msg, 0, sizeof(rx_msg)); + + bool result = uavcan_equipment_power_BatteryInfo_decode(&transfer, &rx_msg); + EXPECT_FALSE(result); + + // Verify sign is preserved (approximately) + EXPECT_LT(rx_msg.current, 0.0f); + EXPECT_LT(rx_msg.temperature, 0.0f); +} + +TEST_F(DroneCANMessageTest, BatteryInfo_StateOfChargePercentBoundaries) +{ + // Test SOC percentage boundaries (0%, 50%, 100%, max 127 for 7-bit field) + uint8_t soc_values[] = {0, 1, 50, 100, 127}; + + for (uint8_t soc : soc_values) { + struct uavcan_equipment_power_BatteryInfo tx_msg; + memset(&tx_msg, 0, sizeof(tx_msg)); + + tx_msg.voltage = 12.0f; + tx_msg.state_of_charge_pct = soc; + tx_msg.state_of_health_pct = 100; + + memset(buffer, 0, sizeof(buffer)); + uint32_t encoded_len = uavcan_equipment_power_BatteryInfo_encode(&tx_msg, buffer +#if CANARD_ENABLE_TAO_OPTION + , false +#endif + ); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_equipment_power_BatteryInfo rx_msg; + memset(&rx_msg, 0, sizeof(rx_msg)); + + EXPECT_FALSE(uavcan_equipment_power_BatteryInfo_decode(&transfer, &rx_msg)); + EXPECT_EQ(rx_msg.state_of_charge_pct, soc); + } +} + +TEST_F(DroneCANMessageTest, NodeStatus_UptimeMaxValues) +{ + // Test with maximum uptime value (32-bit field) + struct uavcan_protocol_NodeStatus tx_msg; + memset(&tx_msg, 0, sizeof(tx_msg)); + + tx_msg.uptime_sec = 0xFFFFFFFFu; // Maximum uint32 value + tx_msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + tx_msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + tx_msg.vendor_specific_status_code = 0xFFFF; // Maximum vendor status + + uint32_t encoded_len = uavcan_protocol_NodeStatus_encode(&tx_msg, buffer +#if CANARD_ENABLE_TAO_OPTION + , false +#endif + ); + + EXPECT_GT(encoded_len, 0u); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_protocol_NodeStatus rx_msg; + memset(&rx_msg, 0, sizeof(rx_msg)); + + bool result = uavcan_protocol_NodeStatus_decode(&transfer, &rx_msg); + EXPECT_FALSE(result); + EXPECT_EQ(rx_msg.uptime_sec, 0xFFFFFFFFu); + EXPECT_EQ(rx_msg.vendor_specific_status_code, 0xFFFF); +} + +TEST_F(DroneCANMessageTest, GNSSFix_ZeroCovarianceLength) +{ + // Test Fix message with covariance length field edge cases + struct uavcan_equipment_gnss_Fix tx_msg; + memset(&tx_msg, 0, sizeof(tx_msg)); + + tx_msg.status = UAVCAN_EQUIPMENT_GNSS_FIX_STATUS_3D_FIX; + tx_msg.sats_used = 5; + tx_msg.position_covariance.len = 0; + tx_msg.velocity_covariance.len = 0; + + uint32_t encoded_len = uavcan_equipment_gnss_Fix_encode(&tx_msg, buffer +#if CANARD_ENABLE_TAO_OPTION + , false +#endif + ); + + EXPECT_GT(encoded_len, 0u); + EXPECT_LE(encoded_len, UAVCAN_EQUIPMENT_GNSS_FIX_MAX_SIZE); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_equipment_gnss_Fix rx_msg; + memset(&rx_msg, 0, sizeof(rx_msg)); + + EXPECT_FALSE(uavcan_equipment_gnss_Fix_decode(&transfer, &rx_msg)); + EXPECT_EQ(rx_msg.position_covariance.len, 0); + EXPECT_EQ(rx_msg.velocity_covariance.len, 0); +} + +TEST_F(DroneCANMessageTest, GNSSAuxiliary_ZeroDOPValues) +{ + // Test Auxiliary message with all zero DOP values (perfect geometry) + struct uavcan_equipment_gnss_Auxiliary tx_msg; + memset(&tx_msg, 0, sizeof(tx_msg)); + + tx_msg.gdop = 0.0f; + tx_msg.pdop = 0.0f; + tx_msg.hdop = 0.0f; + tx_msg.vdop = 0.0f; + tx_msg.tdop = 0.0f; + tx_msg.ndop = 0.0f; + tx_msg.edop = 0.0f; + tx_msg.sats_visible = 0; + tx_msg.sats_used = 0; + + uint32_t encoded_len = uavcan_equipment_gnss_Auxiliary_encode(&tx_msg, buffer +#if CANARD_ENABLE_TAO_OPTION + , false +#endif + ); + + EXPECT_GT(encoded_len, 0u); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_equipment_gnss_Auxiliary rx_msg; + memset(&rx_msg, 0, sizeof(rx_msg)); + + EXPECT_FALSE(uavcan_equipment_gnss_Auxiliary_decode(&transfer, &rx_msg)); + EXPECT_EQ(rx_msg.sats_visible, 0); + EXPECT_EQ(rx_msg.sats_used, 0); +} + +TEST_F(DroneCANMessageTest, GNSSFix2_MaxSatellites) +{ + // Test with maximum satellites count (63 for 6-bit field) + struct uavcan_equipment_gnss_Fix2 tx_msg; + memset(&tx_msg, 0, sizeof(tx_msg)); + + tx_msg.latitude_deg_1e8 = 0; + tx_msg.longitude_deg_1e8 = 0; + tx_msg.sats_used = 63; // Maximum for 6-bit field + tx_msg.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + + uint32_t encoded_len = uavcan_equipment_gnss_Fix2_encode(&tx_msg, buffer +#if CANARD_ENABLE_TAO_OPTION + , false +#endif + ); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_equipment_gnss_Fix2 rx_msg; + memset(&rx_msg, 0, sizeof(rx_msg)); + + EXPECT_FALSE(uavcan_equipment_gnss_Fix2_decode(&transfer, &rx_msg)); + EXPECT_EQ(rx_msg.sats_used, 63); +} + +TEST_F(DroneCANMessageTest, TransferBufferRoundtrip) +{ + // Test that multiple consecutive encodes/decodes work correctly + // This tests buffer reuse and any potential state issues + for (int i = 0; i < 10; i++) { + struct uavcan_equipment_gnss_Fix2 tx_msg; + memset(&tx_msg, 0, sizeof(tx_msg)); + + tx_msg.latitude_deg_1e8 = 377749000 + i * 1000; + tx_msg.longitude_deg_1e8 = -1224194000 + i * 1000; + tx_msg.sats_used = 5 + (i % 15); // Vary between 5-20 + tx_msg.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + + memset(buffer, 0xFF, sizeof(buffer)); // Fill with pattern + uint32_t encoded_len = uavcan_equipment_gnss_Fix2_encode(&tx_msg, buffer +#if CANARD_ENABLE_TAO_OPTION + , false +#endif + ); + + CanardRxTransfer transfer = makeTransfer(encoded_len); + struct uavcan_equipment_gnss_Fix2 rx_msg; + memset(&rx_msg, 0, sizeof(rx_msg)); + + EXPECT_FALSE(uavcan_equipment_gnss_Fix2_decode(&transfer, &rx_msg)); + EXPECT_EQ(rx_msg.latitude_deg_1e8, tx_msg.latitude_deg_1e8); + EXPECT_EQ(rx_msg.sats_used, tx_msg.sats_used); + } +}