diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg index 98aa90e7e3..191f1ef28a 100644 --- a/msg/actuator_armed.msg +++ b/msg/actuator_armed.msg @@ -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 diff --git a/msg/rate_ctrl_status.msg b/msg/rate_ctrl_status.msg index 145bf25a5c..9773a2f379 100644 --- a/msg/rate_ctrl_status.msg +++ b/msg/rate_ctrl_status.msg @@ -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] diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 85b45cf43e..6a4e234953 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 09572856c7..af14350db8 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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); + } } } } diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 9b8fa5a49e..6b82a9094f 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -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; + } +} diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 959b6c145d..5f213f2350 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -43,30 +43,28 @@ #pragma once -#include -#include +#include +#include +#include #include -#include - // subscriptions #include -#include -#include -#include #include #include +#include +#include +#include -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) _param_fd_fail_p, @@ -100,6 +101,7 @@ private: (ParamFloat) _param_fd_fail_p_ttri, (ParamBool) _param_fd_ext_ats_en, (ParamInt) _param_fd_ext_ats_trig, - (ParamInt) _param_escs_en + (ParamInt) _param_escs_en, + (ParamBool) _param_fd_rate_ctrl_en ) }; diff --git a/src/modules/commander/failure_detector/failure_detector_params.c b/src/modules/commander/failure_detector/failure_detector_params.c index 1cdf726062..3ce70ea59a 100644 --- a/src/modules/commander/failure_detector/failure_detector_params.c +++ b/src/modules/commander/failure_detector/failure_detector_params.c @@ -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); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6d440b4303..4d7a8dccab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -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; } } } diff --git a/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp b/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp index ca5a00cc78..034217c3a2 100644 --- a/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp +++ b/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp @@ -72,7 +72,7 @@ private: mavlink_flight_information_t flight_info{}; flight_info.flight_uuid = static_cast(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); } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index dc97d17dbb..ece7fbc9a8 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -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); diff --git a/src/modules/mc_rate_control/RateControl/RateControl.cpp b/src/modules/mc_rate_control/RateControl/RateControl.cpp index 37a2525506..84c60cd388 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.cpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.cpp @@ -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); } diff --git a/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/modules/mc_rate_control/RateControl/RateControl.hpp index 6f0783a349..7f0070c7e1 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.hpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.hpp @@ -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{}; };