mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
219 lines
6.9 KiB
C++
219 lines
6.9 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#include "FailureTable.hpp"
|
|
|
|
#include <cstring>
|
|
|
|
namespace failure_injection
|
|
{
|
|
|
|
bool FailureTable::isSupported(uint8_t unit, uint8_t type)
|
|
{
|
|
// Capability catalogue: the (unit, type) combinations implemented today.
|
|
// FAILURE_TYPE_OK is always accepted for a known unit (it clears the unit).
|
|
switch (unit) {
|
|
case failure_injection_s::FAILURE_UNIT_SENSOR_GYRO:
|
|
case failure_injection_s::FAILURE_UNIT_SENSOR_ACCEL:
|
|
case failure_injection_s::FAILURE_UNIT_SENSOR_MAG:
|
|
case failure_injection_s::FAILURE_UNIT_SENSOR_BARO:
|
|
case failure_injection_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR:
|
|
return type == failure_injection_s::FAILURE_TYPE_OK
|
|
|| type == failure_injection_s::FAILURE_TYPE_OFF
|
|
|| type == failure_injection_s::FAILURE_TYPE_STUCK;
|
|
|
|
case failure_injection_s::FAILURE_UNIT_SENSOR_GPS:
|
|
case failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED:
|
|
return type == failure_injection_s::FAILURE_TYPE_OK
|
|
|| type == failure_injection_s::FAILURE_TYPE_OFF
|
|
|| type == failure_injection_s::FAILURE_TYPE_STUCK
|
|
|| type == failure_injection_s::FAILURE_TYPE_WRONG;
|
|
|
|
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;
|
|
|
|
case failure_injection_s::FAILURE_UNIT_SYSTEM_ESC:
|
|
case failure_injection_s::FAILURE_UNIT_SYSTEM_BATTERY:
|
|
return type == failure_injection_s::FAILURE_TYPE_OK
|
|
|| type == failure_injection_s::FAILURE_TYPE_OFF
|
|
|| type == failure_injection_s::FAILURE_TYPE_WRONG;
|
|
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
uint16_t FailureTable::instanceToMask(uint8_t instance)
|
|
{
|
|
if (instance == 0) {
|
|
return 0xFFFF; // all instances
|
|
}
|
|
|
|
if (instance > 16) {
|
|
return 0; // out of range, addressable instances are 1..16
|
|
}
|
|
|
|
return static_cast<uint16_t>(1u << (instance - 1));
|
|
}
|
|
|
|
int FailureTable::findEntry(uint8_t unit, uint8_t type) const
|
|
{
|
|
for (uint8_t i = 0; i < _count; i++) {
|
|
if (_entries[i].unit == unit && _entries[i].type == type) {
|
|
return i;
|
|
}
|
|
}
|
|
|
|
return -1;
|
|
}
|
|
|
|
void FailureTable::clearInstances(uint8_t unit, uint16_t mask, uint8_t keep_type)
|
|
{
|
|
for (uint8_t i = 0; i < _count; i++) {
|
|
if (_entries[i].unit == unit && _entries[i].type != keep_type
|
|
&& (_entries[i].instance_mask & mask) != 0) {
|
|
_entries[i].instance_mask &= ~mask;
|
|
_changed = true;
|
|
}
|
|
}
|
|
|
|
compact();
|
|
}
|
|
|
|
void FailureTable::compact()
|
|
{
|
|
uint8_t write = 0;
|
|
|
|
for (uint8_t read = 0; read < _count; read++) {
|
|
if (_entries[read].instance_mask != 0) {
|
|
if (write != read) {
|
|
_entries[write] = _entries[read];
|
|
}
|
|
|
|
write++;
|
|
}
|
|
}
|
|
|
|
_count = write;
|
|
|
|
// Zero the unused tail so dropped entries leave no stale state behind.
|
|
for (uint8_t i = _count; i < failure_injection_s::MAX_FAILURES; i++) {
|
|
_entries[i] = Entry{};
|
|
}
|
|
}
|
|
|
|
FailureTable::AckResult FailureTable::inject(uint8_t unit, uint8_t type, uint8_t instance)
|
|
{
|
|
return injectMask(unit, type, instanceToMask(instance));
|
|
}
|
|
|
|
FailureTable::AckResult FailureTable::injectMask(uint8_t unit, uint8_t type, uint16_t mask)
|
|
{
|
|
// Validate the request against the capability catalogue.
|
|
if (!isSupported(unit, type)) {
|
|
return AckResult::Unsupported;
|
|
}
|
|
|
|
if (mask == 0) {
|
|
// Out-of-range instance: nothing addressable to do.
|
|
return AckResult::Accepted;
|
|
}
|
|
|
|
// Snapshot for rollback if the request would overflow the table. _changed is
|
|
// set at the mutation sites below, so it has to be restored on rollback too.
|
|
Entry saved_entries[failure_injection_s::MAX_FAILURES];
|
|
uint8_t saved_count = _count;
|
|
bool saved_changed = _changed;
|
|
memcpy(saved_entries, _entries, sizeof(_entries));
|
|
|
|
if (type == failure_injection_s::FAILURE_TYPE_OK) {
|
|
// Clear the addressed instances from every failure mode of this unit.
|
|
clearInstances(unit, mask, 0xFF);
|
|
|
|
} else {
|
|
// An instance is in at most one mode per unit: drop these instances
|
|
// from the unit's other modes before applying the requested one.
|
|
clearInstances(unit, mask, type);
|
|
|
|
int idx = findEntry(unit, type);
|
|
|
|
if (idx < 0) {
|
|
if (_count >= failure_injection_s::MAX_FAILURES) {
|
|
// Would exceed the cap: roll back and reject.
|
|
memcpy(_entries, saved_entries, sizeof(_entries));
|
|
_count = saved_count;
|
|
_changed = saved_changed;
|
|
return AckResult::Rejected;
|
|
}
|
|
|
|
idx = _count++;
|
|
_entries[idx].unit = unit;
|
|
_entries[idx].type = type;
|
|
_entries[idx].instance_mask = 0;
|
|
_changed = true;
|
|
}
|
|
|
|
// Only a mask that actually adds instances counts as a change (dedup).
|
|
if ((_entries[idx].instance_mask & mask) != mask) {
|
|
_entries[idx].instance_mask |= mask;
|
|
_changed = true;
|
|
}
|
|
}
|
|
|
|
return AckResult::Accepted;
|
|
}
|
|
|
|
void FailureTable::fill(failure_injection_s &msg) const
|
|
{
|
|
msg.count = _count;
|
|
|
|
for (uint8_t i = 0; i < failure_injection_s::MAX_FAILURES; i++) {
|
|
if (i < _count) {
|
|
msg.unit[i] = _entries[i].unit;
|
|
msg.instance_mask[i] = _entries[i].instance_mask;
|
|
msg.failure_type[i] = _entries[i].type;
|
|
|
|
} else {
|
|
msg.unit[i] = 0;
|
|
msg.instance_mask[i] = 0;
|
|
msg.failure_type[i] = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace failure_injection
|