rate controller failure detector at takeoff

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

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]

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_NONE      =  0
-uint8 FAILURE_ROLL = 1 	        # (1 << 0)
+uint8 FAILURE_ROLL      =  1  # (1 << 0)
-uint8 FAILURE_PITCH = 2	        # (1 << 1)
+uint8 FAILURE_PITCH     =  2  # (1 << 1)
-uint8 FAILURE_ALT = 4 	        # (1 << 2)
+uint8 FAILURE_ALT       =  4  # (1 << 2)
-uint8 FAILURE_EXT = 8 	        # (1 << 3)
+uint8 FAILURE_EXT       =  8  # (1 << 3)
-uint8 FAILURE_ARM_ESC = 16      # (1 << 4)
+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

@@ -2451,19 +2451,17 @@ Commander::run()
 			_status.failure_detector_status = _failure_detector.getStatus();
 			_status_changed = true;
 
-			if (_armed.armed) {
+			if (_armed.armed && (_failure_detector.getStatus() != FAILURE_NONE)) {
-				if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
+				if (_status.failure_detector_status & FAILURE_ARM_ESCS) {
-					const hrt_abstime time_at_arm = _armed.armed_time_ms * 1000;
-
 					// 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 |
+				if (_status.failure_detector_status & (FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT | FAILURE_RATE_CTRL)) {
-								       vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) {
+
 					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 &&
+					} else if (_status.failure_detector_status & (FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT)) {
-						   !_flight_termination_triggered && !_lockdown_triggered) {
 
-						_armed.force_failsafe = true;
+						if (!_status_flags.circuit_breaker_flight_termination_disabled && !_flight_termination_triggered
-						_flight_termination_triggered = true;
+						    && !_lockdown_triggered) {
-						mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight");
+
-						set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
+							_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,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

@@ -43,30 +43,28 @@
 
 #pragma once
 
-#include <matrix/matrix/math.hpp>
+#include <lib/matrix/matrix/math.hpp>
-#include <mathlib/mathlib.h>
+#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_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC,
-} failure_detector_bitmak;
+	FAILURE_RATE_CTRL = vehicle_status_s::FAILURE_RATE_CTRL,
-
+};
-using uORB::SubscriptionData;
 
 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

@@ -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

@@ -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

@@ -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

@@ -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_ctrl_status_s &rate_ctrl_status = _rate_control.getRateControlStatus();
-			_rate_control.getRateControlStatus(rate_ctrl_status);
+
+			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

@@ -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.rollspeed_integ = _rate_int(0);
+	_rate_ctrl_status.yawspeed_integ = _rate_int(2);
-	rate_ctrl_status.pitchspeed_integ = _rate_int(1);
-	rate_ctrl_status.yawspeed_integ = _rate_int(2);
 }

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{};
 };