Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

rate controller failure detector at takeoff

Browse Source
This commit is contained in:
Daniel Agar
2020-09-14 12:57:34 -04:00
parent e51000c5be
commit 6c5af97868
12 changed files with 121 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/actuator_armed.msg
+2 -1
View File
@@ -1,6 +1,7 @@
uint64 timestamp # time since system start (microseconds)
uint32 armed_time_ms # Arming timestamp
uint64 armed_time # Arming timestamp
bool armed # Set to true if system is armed
bool prearmed # Set to true if the actuator safety is disabled but motors are not armed
bool ready_to_arm # Set to true if system is ready to be armed
msg/rate_ctrl_status.msg
+3
View File
@@ -5,3 +5,6 @@ float32 rollspeed_integ
float32 pitchspeed_integ
float32 yawspeed_integ
float32 additional_integ1 # FW: wheel rate integrator (optional)
float32[3] error # rate error [radians/s]
float32[3] error_integrated # rate error integrated [radians]
msg/vehicle_status.msg
+7 -6
View File
@@ -11,12 +11,13 @@ uint8 ARMING_STATE_IN_AIR_RESTORE = 5
uint8 ARMING_STATE_MAX = 6
# FailureDetector status
uint8 FAILURE_NONE = 0
uint8 FAILURE_ROLL = 1 # (1 << 0)
uint8 FAILURE_PITCH = 2 # (1 << 1)
uint8 FAILURE_ALT = 4 # (1 << 2)
uint8 FAILURE_EXT = 8 # (1 << 3)
uint8 FAILURE_ARM_ESC = 16 # (1 << 4)
uint8 FAILURE_NONE = 0
uint8 FAILURE_ROLL = 1 # (1 << 0)
uint8 FAILURE_PITCH = 2 # (1 << 1)
uint8 FAILURE_ALT = 4 # (1 << 2)
uint8 FAILURE_EXT = 8 # (1 << 3)
uint8 FAILURE_ARM_ESC = 16 # (1 << 4)
uint8 FAILURE_RATE_CTRL = 32 # (1 << 5)
# HIL
uint8 HIL_STATE_OFF = 0
src/modules/commander/Commander.cpp
+14 -13
View File
@@ -2451,19 +2451,17 @@ Commander::run()
_status.failure_detector_status = _failure_detector.getStatus();
_status_changed = true;
if (_armed.armed) {
if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
const hrt_abstime time_at_arm = _armed.armed_time_ms * 1000;
if (_armed.armed && (_failure_detector.getStatus() != FAILURE_NONE)) {
if (_status.failure_detector_status & FAILURE_ARM_ESCS) {
// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
if (hrt_elapsed_time(&time_at_arm) < 500_ms) {
if (hrt_elapsed_time(&_armed.armed_time) < 500_ms) {
arm_disarm(false, true, arm_disarm_reason_t::FAILURE_DETECTOR);
mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request");
}
}
if (_status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH |
vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) {
if (_status.failure_detector_status & (FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT | FAILURE_RATE_CTRL)) {
const bool is_second_after_takeoff = hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get());
if (is_second_after_takeoff && !_lockdown_triggered) {
@@ -2472,13 +2470,16 @@ Commander::run()
_lockdown_triggered = true;
mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: lockdown");
} else if (!_status_flags.circuit_breaker_flight_termination_disabled &&
!_flight_termination_triggered && !_lockdown_triggered) {
} else if (_status.failure_detector_status & (FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT)) {
_armed.force_failsafe = true;
_flight_termination_triggered = true;
mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight");
set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
if (!_status_flags.circuit_breaker_flight_termination_disabled && !_flight_termination_triggered
&& !_lockdown_triggered) {
_armed.force_failsafe = true;
_flight_termination_triggered = true;
mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight");
set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
}
}
}
}
src/modules/commander/failure_detector/FailureDetector.cpp
+45 -7
View File
@@ -45,6 +45,9 @@ using namespace time_literals;
FailureDetector::FailureDetector(ModuleParams *parent) :
ModuleParams(parent)
{
_ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_rate_ctrl_failure_hysteresis.set_hysteresis_time_from(false, 100_ms);
}
bool FailureDetector::update(const vehicle_status_s &vehicle_status)
@@ -54,18 +57,22 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status)
if (isAttitudeStabilized(vehicle_status)) {
updateAttitudeStatus();
if (_param_fd_ext_ats_en.get()) {
updateExternalAtsStatus();
}
} else {
_status &= ~(FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT);
}
if (_param_fd_ext_ats_en.get()) {
updateExternalAtsStatus();
}
if (_param_escs_en.get()) {
updateEscsStatus(vehicle_status);
}
if (_param_fd_rate_ctrl_en.get()) {
updateRateControlStatus(vehicle_status);
}
return _status != previous_status;
}
@@ -136,10 +143,9 @@ void FailureDetector::updateExternalAtsStatus()
uint32_t pulse_width = pwm_input.pulse_width;
bool ats_trigger_status = (pulse_width >= (uint32_t)_param_fd_ext_ats_trig.get()) && (pulse_width < 3_ms);
hrt_abstime time_now = hrt_absolute_time();
const hrt_abstime time_now = hrt_absolute_time();
// Update hysteresis
_ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz
_ext_ats_failure_hysteresis.set_state_and_update(ats_trigger_status, time_now);
_status &= ~FAILURE_EXT;
@@ -160,7 +166,6 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
if (_esc_status_sub.update(&esc_status)) {
int all_escs_armed = (1 << esc_status.esc_count) - 1;
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
if (_esc_failure_hysteresis.get_state()) {
@@ -174,3 +179,36 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
_status &= ~FAILURE_ARM_ESCS;
}
}
void FailureDetector::updateRateControlStatus(const vehicle_status_s &vehicle_status)
{
rate_ctrl_status_s rcs;
if (_rate_ctrl_status_sub.update(&rcs) && (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
static constexpr float rate_error_lim = math::radians(30.f); // 30 degrees/second
static constexpr float angle_error_lim = math::radians(60.f); // 60 degrees
// roll rate error
const bool rr_pos_err = (rcs.error[0] > rate_error_lim) && (rcs.error_integrated[0] > angle_error_lim);
const bool rr_neg_err = (rcs.error[0] < -rate_error_lim) && (rcs.error_integrated[0] < -angle_error_lim);
// pitch rate error
const bool pr_pos_err = (rcs.error[1] > rate_error_lim) && (rcs.error_integrated[1] > angle_error_lim);
const bool pr_neg_err = (rcs.error[1] < -rate_error_lim) && (rcs.error_integrated[1] < -angle_error_lim);
const bool fail = rr_pos_err || rr_neg_err || pr_pos_err || pr_neg_err;
_rate_ctrl_failure_hysteresis.set_state_and_update(fail, rcs.timestamp);
if (_rate_ctrl_failure_hysteresis.get_state()) {
_status |= FAILURE_RATE_CTRL;
} else {
_status &= ~FAILURE_RATE_CTRL;
}
} else {
_rate_ctrl_failure_hysteresis.set_state_and_update(false, hrt_absolute_time());
_status &= ~FAILURE_RATE_CTRL;
}
}
src/modules/commander/failure_detector/FailureDetector.hpp
+18 -16
View File
@@ -43,30 +43,28 @@
#pragma once
#include <matrix/matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/hysteresis/hysteresis.h>
#include <px4_platform_common/module_params.h>
#include <hysteresis/hysteresis.h>
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/pwm_input.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
typedef enum {
enum failure_detector_bitmak {
FAILURE_NONE = vehicle_status_s::FAILURE_NONE,
FAILURE_ROLL = vehicle_status_s::FAILURE_ROLL,
FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH,
FAILURE_ALT = vehicle_status_s::FAILURE_ALT,
FAILURE_EXT = vehicle_status_s::FAILURE_EXT,
FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC
} failure_detector_bitmak;
using uORB::SubscriptionData;
FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC,
FAILURE_RATE_CTRL = vehicle_status_s::FAILURE_RATE_CTRL,
};
class FailureDetector : public ModuleParams
{
@@ -81,17 +79,20 @@ private:
void updateAttitudeStatus();
void updateExternalAtsStatus();
void updateEscsStatus(const vehicle_status_s &vehicle_status);
void updateRateControlStatus(const vehicle_status_s &vehicle_status);
uint8_t _status{FAILURE_NONE};
systemlib::Hysteresis _esc_failure_hysteresis{false};
systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
systemlib::Hysteresis _rate_ctrl_failure_hysteresis{false};
systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false};
systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
systemlib::Hysteresis _esc_failure_hysteresis{false};
uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
uORB::Subscription _rate_ctrl_status_sub{ORB_ID(rate_ctrl_status)};
uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
@@ -100,6 +101,7 @@ private:
(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en
(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
(ParamBool<px4::params::FD_RATE_CTRL_EN>) _param_fd_rate_ctrl_en
)
};
src/modules/commander/failure_detector/failure_detector_params.c
+10
View File
@@ -141,3 +141,13 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
/**
* Enable rate controller checks at takeoff.
* If enabled the failure detector will monitor the rate controller at takeoff and immediately disarm if there's an error.
*
* @boolean
*
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_RATE_CTRL_EN, 0);
src/modules/commander/state_machine_helper.cpp
+2 -2
View File
@@ -231,10 +231,10 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
}
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
armed->armed_time_ms = hrt_absolute_time() / 1000;
armed->armed_time = hrt_absolute_time();
} else {
armed->armed_time_ms = 0;
armed->armed_time = 0;
}
}
}
src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp
+1 -1
View File
@@ -72,7 +72,7 @@ private:
mavlink_flight_information_t flight_info{};
flight_info.flight_uuid = static_cast<uint64_t>(flight_uuid);
flight_info.arming_time_utc = flight_info.takeoff_time_utc = actuator_armed.armed_time_ms;
flight_info.arming_time_utc = flight_info.takeoff_time_utc = actuator_armed.armed_time / 1000;
flight_info.time_boot_ms = hrt_absolute_time() / 1000;
mavlink_msg_flight_information_send_struct(_mavlink->get_channel(), &flight_info);
}
src/modules/mc_rate_control/MulticopterRateControl.cpp
+7 -2
View File
@@ -243,8 +243,13 @@ MulticopterRateControl::Run()
const Vector3f att_control = _rate_control.update(rates, _rates_sp, angular_accel, dt, _maybe_landed || _landed);
// publish rate controller status
rate_ctrl_status_s rate_ctrl_status{};
_rate_control.getRateControlStatus(rate_ctrl_status);
rate_ctrl_status_s &rate_ctrl_status = _rate_control.getRateControlStatus();
if (!_v_control_mode.flag_armed || _landed) {
Vector3f().copyTo(rate_ctrl_status.error);
Vector3f().copyTo(rate_ctrl_status.error_integrated);
}
rate_ctrl_status.timestamp = hrt_absolute_time();
_controller_status_pub.publish(rate_ctrl_status);
src/modules/mc_rate_control/RateControl/RateControl.cpp
+9 -6
View File
@@ -71,6 +71,12 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
updateIntegral(rate_error, dt);
}
rate_error.copyTo(_rate_ctrl_status.error);
_rate_ctrl_status.error_integrated[0] += rate_error(0) * dt;
_rate_ctrl_status.error_integrated[1] += rate_error(1) * dt;
_rate_ctrl_status.error_integrated[2] += rate_error(2) * dt;
return torque;
}
@@ -104,11 +110,8 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
_rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i));
}
}
}
void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
{
rate_ctrl_status.rollspeed_integ = _rate_int(0);
rate_ctrl_status.pitchspeed_integ = _rate_int(1);
rate_ctrl_status.yawspeed_integ = _rate_int(2);
_rate_ctrl_status.rollspeed_integ = _rate_int(0);
_rate_ctrl_status.pitchspeed_integ = _rate_int(1);
_rate_ctrl_status.yawspeed_integ = _rate_int(2);
}
src/modules/mc_rate_control/RateControl/RateControl.hpp
+3 -1
View File
@@ -98,7 +98,7 @@ public:
* Get status message of controller for logging/debugging
* @param rate_ctrl_status status message to fill with internal states
*/
void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status);
rate_ctrl_status_s &getRateControlStatus() { return _rate_ctrl_status; }
private:
void updateIntegral(matrix::Vector3f &rate_error, const float dt);
@@ -115,4 +115,6 @@ private:
bool _mixer_saturation_positive[3] {};
bool _mixer_saturation_negative[3] {};
rate_ctrl_status_s _rate_ctrl_status{};
};