mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
feat(failure_injection): add CAN bus failure injection (#28140)
This commit is contained in:
1 parent
ad6cf079f0
commit
6bfe60507d
13 files changed
+99
-1
No files matched your search
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -86,6 +86,7 @@
|
||||
#include "uavcan_servers.hpp"
|
||||
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
#include <lib/failure_injection/FailureInjection.hpp>
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
@@ -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<UavcanNode *, void (UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback;
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
Submodule src/modules/mavlink/mavlink updated: 83231e1605...1b1e896aa3.
@@ -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 {
|
||||
|
||||
Reference in new issue
Block a user