diff --git a/boards/auterion/fmu-v6x/default.px4board b/boards/auterion/fmu-v6x/default.px4board index e8ae6e7e8bf..aac9785c11d 100644 --- a/boards/auterion/fmu-v6x/default.px4board +++ b/boards/auterion/fmu-v6x/default.px4board @@ -50,6 +50,7 @@ CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FAILURE_INJECTION_MANAGER=n CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y @@ -84,6 +85,7 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_FAILURE=n CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_LED_CONTROL=y diff --git a/docs/en/debug/failure_injection.md b/docs/en/debug/failure_injection.md index 9d31a2891bb..2d990d6a36b 100644 --- a/docs/en/debug/failure_injection.md +++ b/docs/en/debug/failure_injection.md @@ -28,6 +28,7 @@ A `—` means the module still accepts the command, but no consumer applies it i | `traffic` | `off` | `off` | `off` | `off` | | `motor` | `off` | `off` | `off` | `off` | | `esc` | `off`, `wrong` | `off`, `wrong` | `off`, `wrong` | `off`, `wrong` | +| `can` | — | — | — | `off` | [SIH]: ../sim_sih/index.md [Gazebo]: ../sim_gazebo_gz/index.md @@ -42,6 +43,7 @@ A `—` means the module still accepts the command, but no consumer applies it i - `motor off` also requires [CA_FAILURE_MODE](../advanced_config/parameter_reference.md#CA_FAILURE_MODE). - `esc off` reports the addressed ESC as offline and blanks its telemetry; `esc wrong` keeps it online but reports implausible telemetry (voltage and current at 10% of the real value, RPM 10x). ESCs are addressed by motor instance (the ESC's actuator function), so `-i 1` targets the ESC driving motor 1. - On hardware, ESC injection is applied only by the UAVCAN (DroneCAN) ESC driver; the other ESC drivers (DShot, Cyphal, VOXL, TAP ESC) publish their telemetry unmodified. +- `can off` takes the addressed CAN bus offline entirely, so every node on it stops responding; the instance selects the bus. Only applied on fmu-v6x-class hardware. ::: @@ -91,6 +93,7 @@ where: - `traffic`: Traffic avoidance (ADS-B/transponder) - `rc_signal`: RC Signal - `mavlink_signal`: MAVLink data telemetry connection + - `can`: CAN bus. The instance selects the bus. Supported on fmu-v6x-class hardware. - _failure_type_: - `ok`: Publish as normal (Disable failure injection) - `off`: Stop publishing diff --git a/msg/FailureInjection.msg b/msg/FailureInjection.msg index 5a4bf5b588c..c253be945ca 100644 --- a/msg/FailureInjection.msg +++ b/msg/FailureInjection.msg @@ -31,6 +31,11 @@ uint8 FAILURE_UNIT_SYSTEM_RC_SIGNAL = 104 uint8 FAILURE_UNIT_SYSTEM_MAVLINK_SIGNAL = 105 uint8 FAILURE_UNIT_SYSTEM_ESC = 106 uint8 FAILURE_UNIT_SYSTEM_TRAFFIC_AVOIDANCE = 107 +uint8 FAILURE_UNIT_DATALINK_LTE = 150 +uint8 FAILURE_UNIT_DATALINK_WIFI = 151 +uint8 FAILURE_UNIT_DATALINK_TELEM_RADIO = 152 +uint8 FAILURE_UNIT_BUS_CAN = 200 +uint8 FAILURE_UNIT_BUS_I2C = 201 uint16[4] instance_mask # Bit i targets instance (i+1); 0xFFFF = all instances diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 2452888c4ed..3da4e8b9292 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -174,6 +174,11 @@ uint8 FAILURE_UNIT_SYSTEM_RC_SIGNAL = 104 uint8 FAILURE_UNIT_SYSTEM_MAVLINK_SIGNAL = 105 uint8 FAILURE_UNIT_SYSTEM_ESC = 106 uint8 FAILURE_UNIT_SYSTEM_TRAFFIC_AVOIDANCE = 107 +uint8 FAILURE_UNIT_DATALINK_LTE = 150 +uint8 FAILURE_UNIT_DATALINK_WIFI = 151 +uint8 FAILURE_UNIT_DATALINK_TELEM_RADIO = 152 +uint8 FAILURE_UNIT_BUS_CAN = 200 +uint8 FAILURE_UNIT_BUS_I2C = 201 uint8 FAILURE_TYPE_OK = 0 uint8 FAILURE_TYPE_OFF = 1 diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp index b8a28789a4a..f0865a5e7e4 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp @@ -174,6 +174,21 @@ public: */ int init(const uavcan::uint32_t bitrate, const OperatingMode mode); + /** + * Failure injection: take this interface off the CAN bus / bring it back. + * setOffline() requests FDCAN Init mode (CCCR.INIT): the controller stops + * transmitting, receiving and acknowledging, i.e. it electrically disappears + * from the bus. setOnline() clears Init mode; the bit timings configured by + * init() are preserved, so no reconfiguration is needed to rejoin. + * Do not cache the requested state, re-check isInInitMode(): handleBusOff() + * clears INIT from ISR context. + */ + void setOffline(); + void setOnline(); + + /** True while held in FDCAN Init mode (CCCR.INIT), i.e. off the bus. */ + bool isInInitMode() const { return (can_->CCCR & FDCAN_CCCR_INIT) != 0; } + void handleTxInterrupt(uavcan::uint64_t utc_usec); void handleRxInterrupt(uavcan::uint8_t fifo_index); diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp index 2a8ff73fc90..f274ca42239 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp @@ -854,6 +854,23 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) return 0; } +void CanIface::setOffline() +{ + // Request Init mode: stops TX/RX and, crucially, ACK generation, so this node + // electrically leaves the bus (failure injection). Only the INIT bit is touched; + // bit timings and message-RAM configuration are left intact for setOnline(). + CriticalSectionLocker lock; + can_->CCCR |= FDCAN_CCCR_INIT; +} + +void CanIface::setOnline() +{ + // Clear Init mode; the controller re-synchronises and rejoins the bus using the + // timings configured by init(). + CriticalSectionLocker lock; + can_->CCCR &= ~FDCAN_CCCR_INIT; +} + void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec) { // Update counters for successful transmissions diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 3e90f6c46d7..f8dd1903216 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -797,6 +797,8 @@ UavcanNode::Run() _node.spinOnce(); // expected to be non-blocking + apply_can_failure_injection(); + publish_can_interface_statuses(); publish_node_statuses(); @@ -1047,6 +1049,39 @@ UavcanNode::Run() } } +void UavcanNode::apply_can_failure_injection() +{ +#if defined(CONFIG_MODULES_FAILURE_INJECTION_MANAGER) && defined(UAVCAN_STM32H7_NUTTX) + // FAILURE_UNIT_BUS_CAN: instance i+1 selects CAN interface i. FAILURE_TYPE_OFF holds + // the FDCAN peripheral in Init mode so the node leaves the bus entirely (no TX/RX/ACK); + // FAILURE_TYPE_OK rejoins it. No-op unless the failure-injection manager is built. + _failure_config.update(); + + for (uint8_t i = 0; i < can->driver.getNumIfaces(); i++) { + UAVCAN_DRIVER::CanIface *iface = can->driver.getIface(i); + + if (iface == nullptr) { + continue; + } + + const bool off = _failure_config.mode(failure_injection_s::FAILURE_UNIT_BUS_CAN, i + 1) + == failure_injection::Mode::Off; + + if (off == iface->isInInitMode()) { + continue; + } + + if (off) { + iface->setOffline(); + + } else { + iface->setOnline(); + } + } + +#endif // CONFIG_MODULES_FAILURE_INJECTION_MANAGER && UAVCAN_STM32H7_NUTTX +} + void UavcanNode::publish_can_interface_statuses() { constexpr hrt_abstime status_pub_interval = 100_ms; diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 76286f76c51..be7892beb80 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -86,6 +86,7 @@ #include "uavcan_servers.hpp" #include +#include #include #include @@ -242,6 +243,8 @@ private: void publish_can_interface_statuses(); void publish_node_statuses(); + void apply_can_failure_injection(); + int print_params(uavcan::protocol::param::GetSet::Response &resp); int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req); void update_params(); @@ -301,6 +304,8 @@ private: perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + failure_injection::Config _failure_config; ///< active failure-injection config + void handle_time_sync(const uavcan::TimerEvent &); typedef uavcan::MethodBinder TimerCallback; diff --git a/src/modules/failure_injection_manager/FailureInjectionManagerTest.cpp b/src/modules/failure_injection_manager/FailureInjectionManagerTest.cpp index 699d9056551..cd4457d33db 100644 --- a/src/modules/failure_injection_manager/FailureInjectionManagerTest.cpp +++ b/src/modules/failure_injection_manager/FailureInjectionManagerTest.cpp @@ -50,6 +50,7 @@ constexpr uint8_t GPS = failure_injection_s::FAILURE_UNIT_SENSOR_GPS; constexpr uint8_t MOTOR = failure_injection_s::FAILURE_UNIT_SYSTEM_MOTOR; constexpr uint8_t ESC = failure_injection_s::FAILURE_UNIT_SYSTEM_ESC; constexpr uint8_t TRAFFIC = failure_injection_s::FAILURE_UNIT_SYSTEM_TRAFFIC_AVOIDANCE; +constexpr uint8_t CAN = failure_injection_s::FAILURE_UNIT_BUS_CAN; constexpr uint8_t OK = failure_injection_s::FAILURE_TYPE_OK; constexpr uint8_t OFF = failure_injection_s::FAILURE_TYPE_OFF; @@ -81,8 +82,15 @@ TEST(FailureTable, SupportedCatalogueMatchesInventory) EXPECT_TRUE(FailureTable::isSupported(TRAFFIC, OFF)); EXPECT_FALSE(FailureTable::isSupported(TRAFFIC, STUCK)); EXPECT_FALSE(FailureTable::isSupported(TRAFFIC, WRONG)); + // CAN bus: blackout on/off only. + EXPECT_TRUE(FailureTable::isSupported(CAN, OFF)); + EXPECT_TRUE(FailureTable::isSupported(CAN, OK)); + EXPECT_FALSE(FailureTable::isSupported(CAN, STUCK)); + EXPECT_FALSE(FailureTable::isSupported(CAN, WRONG)); // Unimplemented units. EXPECT_FALSE(FailureTable::isSupported(failure_injection_s::FAILURE_UNIT_SYSTEM_RC_SIGNAL, OFF)); + EXPECT_FALSE(FailureTable::isSupported(failure_injection_s::FAILURE_UNIT_BUS_I2C, OFF)); + EXPECT_FALSE(FailureTable::isSupported(failure_injection_s::FAILURE_UNIT_DATALINK_LTE, OFF)); } TEST(FailureTable, TrafficStuckIsRejectedWithoutChange) diff --git a/src/modules/failure_injection_manager/FailureTable.cpp b/src/modules/failure_injection_manager/FailureTable.cpp index 26d1838b7b0..ab795ad8eea 100644 --- a/src/modules/failure_injection_manager/FailureTable.cpp +++ b/src/modules/failure_injection_manager/FailureTable.cpp @@ -62,6 +62,7 @@ bool FailureTable::isSupported(uint8_t unit, uint8_t type) case failure_injection_s::FAILURE_UNIT_SENSOR_VIO: case failure_injection_s::FAILURE_UNIT_SYSTEM_MOTOR: case failure_injection_s::FAILURE_UNIT_SYSTEM_TRAFFIC_AVOIDANCE: // multi-target queue: no single value to freeze + case failure_injection_s::FAILURE_UNIT_BUS_CAN: return type == failure_injection_s::FAILURE_TYPE_OK || type == failure_injection_s::FAILURE_TYPE_OFF; diff --git a/src/modules/failure_injection_manager/failure_injection_manager_params.yaml b/src/modules/failure_injection_manager/failure_injection_manager_params.yaml index d42ec3ecee6..369761d750f 100644 --- a/src/modules/failure_injection_manager/failure_injection_manager_params.yaml +++ b/src/modules/failure_injection_manager/failure_injection_manager_params.yaml @@ -52,6 +52,7 @@ parameters: 100: Battery 101: Motor 107: Traffic avoidance + 200: CAN bus SYS_FAIL_RC_MODE: description: short: Failure type applied by the RC switch diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 83231e16050..1b1e896aa3b 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 83231e16050b3782e6fef639a0842cb6c337380c +Subproject commit 1b1e896aa3be0d591166e086e6b9d8613bd03716 diff --git a/src/systemcmds/failure/failure.cpp b/src/systemcmds/failure/failure.cpp index 08afa59a5b8..302ce49f62b 100644 --- a/src/systemcmds/failure/failure.cpp +++ b/src/systemcmds/failure/failure.cpp @@ -70,6 +70,7 @@ static constexpr FailureUnit failure_units[] = { { "rc_signal", vehicle_command_s::FAILURE_UNIT_SYSTEM_RC_SIGNAL}, { "mavlink_signal", vehicle_command_s::FAILURE_UNIT_SYSTEM_MAVLINK_SIGNAL}, { "traffic", vehicle_command_s::FAILURE_UNIT_SYSTEM_TRAFFIC_AVOIDANCE}, + { "can", vehicle_command_s::FAILURE_UNIT_BUS_CAN}, }; struct FailureType {