fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 00:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: split out failure injection class into its own file

Browse Source
This commit is contained in:
Matthias Grob
2025-09-30 11:23:09 +02:00
parent bacb30650c
commit 7f2a67a588
5 changed files with 210 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failure_detector/CMakeLists.txt
+1
View File
@@ -33,4 +33,5 @@
px4_add_library(failure_detector px4_add_library(failure_detector
FailureDetector.cpp FailureDetector.cpp
FailureInjector.cpp
) )
src/modules/commander/failure_detector/FailureDetector.cpp
-117
View File
@@ -42,123 +42,6 @@
using namespace time_literals; using namespace time_literals;
void FailureInjector::update()
{
vehicle_command_s vehicle_command;
while (_vehicle_command_sub.update(&vehicle_command)) {
if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) {
continue;
}
bool handled = false;
bool supported = false;
const int failure_unit = static_cast<int>(vehicle_command.param1 + 0.5f);
const int failure_type = static_cast<int>(vehicle_command.param2 + 0.5f);
const int instance = static_cast<int>(vehicle_command.param3 + 0.5f);
if (failure_unit == vehicle_command_s::FAILURE_UNIT_SYSTEM_MOTOR) {
handled = true;
if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
PX4_INFO("CMD_INJECT_FAILURE, motors ok");
supported = false;
// 0 to signal all
if (instance == 0) {
supported = true;
for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", i);
_esc_blocked &= ~(1 << i);
_esc_wrong &= ~(1 << i);
}
} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
supported = true;
PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", instance - 1);
_esc_blocked &= ~(1 << (instance - 1));
_esc_wrong &= ~(1 << (instance - 1));
}
}
else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
PX4_WARN("CMD_INJECT_FAILURE, motors off");
supported = true;
// 0 to signal all
if (instance == 0) {
for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
PX4_INFO("CMD_INJECT_FAILURE, motor %d off", i);
_esc_blocked |= 1 << i;
}
} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
PX4_INFO("CMD_INJECT_FAILURE, motor %d off", instance - 1);
_esc_blocked |= 1 << (instance - 1);
}
}
else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
PX4_INFO("CMD_INJECT_FAILURE, motors wrong");
supported = true;
// 0 to signal all
if (instance == 0) {
for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
PX4_INFO("CMD_INJECT_FAILURE, motor %d wrong", i);
_esc_wrong |= 1 << i;
}
} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
PX4_INFO("CMD_INJECT_FAILURE, motor %d wrong", instance - 1);
_esc_wrong |= 1 << (instance - 1);
}
}
}
if (handled) {
vehicle_command_ack_s ack{};
ack.command = vehicle_command.command;
ack.from_external = false;
ack.result = supported ?
vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED :
vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
ack.timestamp = hrt_absolute_time();
_command_ack_pub.publish(ack);
}
}
}
void FailureInjector::manipulateEscStatus(esc_status_s &status)
{
if (_esc_blocked != 0 || _esc_wrong != 0) {
unsigned offline = 0;
for (int i = 0; i < status.esc_count; i++) {
const unsigned i_esc = status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
if (_esc_blocked & (1 << i_esc)) {
unsigned function = status.esc[i].actuator_function;
memset(&status.esc[i], 0, sizeof(status.esc[i]));
status.esc[i].actuator_function = function;
offline |= 1 << i;
} else if (_esc_wrong & (1 << i_esc)) {
// Create wrong rerport for this motor by scaling key values up and down
status.esc[i].esc_voltage *= 0.1f;
status.esc[i].esc_current *= 0.1f;
status.esc[i].esc_rpm *= 10.0f;
}
}
status.esc_online_flags &= ~offline;
}
}
FailureDetector::FailureDetector(ModuleParams *parent) : FailureDetector::FailureDetector(ModuleParams *parent) :
ModuleParams(parent) ModuleParams(parent)
{ {
src/modules/commander/failure_detector/FailureDetector.hpp
+2 -18
View File
@@ -1,4 +1,3 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2018 PX4 Development Team. All rights reserved. * Copyright (c) 2018 PX4 Development Team. All rights reserved.
@@ -43,6 +42,8 @@
#pragma once #pragma once
#include "FailureInjector.hpp"
#include <lib/hysteresis/hysteresis.h> #include <lib/hysteresis/hysteresis.h>
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp> #include <lib/mathlib/math/filter/AlphaFilter.hpp>
@@ -56,12 +57,9 @@
#include <uORB/topics/sensor_selection.h> #include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_imu_status.h> #include <uORB/topics/vehicle_imu_status.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/pwm_input.h> #include <uORB/topics/pwm_input.h>
union failure_detector_status_u { union failure_detector_status_u {
@@ -80,20 +78,6 @@ union failure_detector_status_u {
using uORB::SubscriptionData; using uORB::SubscriptionData;
class FailureInjector
{
public:
void update();
void manipulateEscStatus(esc_status_s &status);
private:
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
uint32_t _esc_blocked{};
uint32_t _esc_wrong{};
};
class FailureDetector : public ModuleParams class FailureDetector : public ModuleParams
{ {
public: public:
src/modules/commander/failure_detector/FailureInjector.cpp
+153
View File
@@ -0,0 +1,153 @@
/****************************************************************************
*
* Copyright (c) 2025 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 "FailureInjector.hpp"
#include <uORB/topics/actuator_motors.h>
void FailureInjector::update()
{
vehicle_command_s vehicle_command;
while (_vehicle_command_sub.update(&vehicle_command)) {
if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) {
continue;
}
bool handled = false;
bool supported = false;
const int failure_unit = static_cast<int>(vehicle_command.param1 + 0.5f);
const int failure_type = static_cast<int>(vehicle_command.param2 + 0.5f);
const int instance = static_cast<int>(vehicle_command.param3 + 0.5f);
if (failure_unit == vehicle_command_s::FAILURE_UNIT_SYSTEM_MOTOR) {
handled = true;
if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
PX4_INFO("CMD_INJECT_FAILURE, motors ok");
supported = false;
// 0 to signal all
if (instance == 0) {
supported = true;
for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", i);
_esc_blocked &= ~(1 << i);
_esc_wrong &= ~(1 << i);
}
} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
supported = true;
PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", instance - 1);
_esc_blocked &= ~(1 << (instance - 1));
_esc_wrong &= ~(1 << (instance - 1));
}
}
else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
PX4_WARN("CMD_INJECT_FAILURE, motors off");
supported = true;
// 0 to signal all
if (instance == 0) {
for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
PX4_INFO("CMD_INJECT_FAILURE, motor %d off", i);
_esc_blocked |= 1 << i;
}
} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
PX4_INFO("CMD_INJECT_FAILURE, motor %d off", instance - 1);
_esc_blocked |= 1 << (instance - 1);
}
}
else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
PX4_INFO("CMD_INJECT_FAILURE, motors wrong");
supported = true;
// 0 to signal all
if (instance == 0) {
for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
PX4_INFO("CMD_INJECT_FAILURE, motor %d wrong", i);
_esc_wrong |= 1 << i;
}
} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
PX4_INFO("CMD_INJECT_FAILURE, motor %d wrong", instance - 1);
_esc_wrong |= 1 << (instance - 1);
}
}
}
if (handled) {
vehicle_command_ack_s ack{};
ack.command = vehicle_command.command;
ack.from_external = false;
ack.result = supported ?
vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED :
vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
ack.timestamp = hrt_absolute_time();
_command_ack_pub.publish(ack);
}
}
}
void FailureInjector::manipulateEscStatus(esc_status_s &status)
{
if (_esc_blocked != 0 || _esc_wrong != 0) {
unsigned offline = 0;
for (int i = 0; i < status.esc_count; i++) {
const unsigned i_esc = status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
if (_esc_blocked & (1 << i_esc)) {
unsigned function = status.esc[i].actuator_function;
memset(&status.esc[i], 0, sizeof(status.esc[i]));
status.esc[i].actuator_function = function;
offline |= 1 << i;
} else if (_esc_wrong & (1 << i_esc)) {
// Create wrong rerport for this motor by scaling key values up and down
status.esc[i].esc_voltage *= 0.1f;
status.esc[i].esc_current *= 0.1f;
status.esc[i].esc_rpm *= 10.0f;
}
}
status.esc_online_flags &= ~offline;
}
}
src/modules/commander/failure_detector/FailureInjector.hpp
+54
View File
@@ -0,0 +1,54 @@
/****************************************************************************
*
* Copyright (c) 2025 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.
*
****************************************************************************/
#pragma once
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_command.h>
class FailureInjector
{
public:
void update();
void manipulateEscStatus(esc_status_s &status);
private:
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
uint32_t _esc_blocked{};
uint32_t _esc_wrong{};
};