Failsafe: validate dead reckoning timeout param

Signed-off-by: Silvan <silvan@auterion.com>

msg/FailsafeFlags.msg

@@ -52,6 +52,7 @@ bool wind_limit_exceeded              # Wind limit exceeded
 bool flight_time_limit_exceeded       # Maximum flight time exceeded
 bool position_accuracy_low            # Position estimate has dropped below threshold, but is currently still declared valid
 bool navigator_failure        	      # Navigator failed to execute a mode
+bool dead_reckoning_invalid           # Postion estimate based on dead reckoning is too old to be trusted
 
 # Failure detector
 bool fd_critical_failure              # Critical failure (attitude/altitude limit exceeded, or external ATS)

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -131,6 +131,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 
 
 	lowPositionAccuracy(context, reporter, lpos);
+	deadReckoningTimeout(context, reporter, lpos);
 }
 
 void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter,
@@ -645,6 +646,54 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
 	reporter.failsafeFlags().position_accuracy_low = position_valid_but_low_accuracy;
 }
 
+void EstimatorChecks::deadReckoningTimeout(const Context &context, Report &reporter,
+		const vehicle_local_position_s &lpos)
+{
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	vehicle_land_detected_s vehicle_land_detected;
+
+	if (!lpos.dead_reckoning) {
+		_last_not_dead_reckoning_time_us = now;
+	}
+
+	if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+
+		if (!vehicle_land_detected.landed && ((reporter.failsafeFlags().mode_req_global_position
+						       && !reporter.failsafeFlags().global_position_invalid) ||
+						      (reporter.failsafeFlags().mode_req_global_position_relaxed
+						       && !reporter.failsafeFlags().global_position_invalid_relaxed) ||
+						      (reporter.failsafeFlags().mode_req_local_position && !reporter.failsafeFlags().local_position_invalid))) {
+
+			reporter.failsafeFlags().dead_reckoning_invalid = (_last_not_dead_reckoning_time_us != 0
+					&& _param_com_dead_reckoning_tout_t.get() > FLT_EPSILON
+					&& now > _last_not_dead_reckoning_time_us + _param_com_dead_reckoning_tout_t.get() * 1_s);
+		}
+	}
+
+	if (reporter.failsafeFlags().dead_reckoning_invalid && _param_com_dead_reckoning_tout_act.get()) {
+
+		// only report if armed
+		if (context.isArmed()) {
+			/* EVENT
+			 * @description Position estimates based on dead reckoning has surpassed the timeout for being trusted. Warn user.
+			 *
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_DR_TOUT_T</param> and <param>COM_DR_TOUT_ACT</param> parameters.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
+						    events::ID("check_estimator_dead_reckoning_invalid"),
+						    events::Log::Error, "Dead reckoning is too old to be trusted");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_warning(reporter.mavlink_log_pub(), "Dead reckoning is too old to be trusted\t");
+			}
+		}
+	}
+}
+
 void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
 		bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz,
 		const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags,

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -43,6 +43,7 @@
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/sensor_gps.h>
 
@@ -72,6 +73,7 @@ private:
 				       const vehicle_local_position_s &lpos);
 	void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
 	void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const;
+	void deadReckoningTimeout(const Context &context, Report &reporter, const vehicle_local_position_s &lpos);
 
 	void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
 				     bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz,
@@ -94,12 +96,14 @@ private:
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
 	hrt_abstime	_last_gpos_fail_time_us{0};	///< Last time that the global position validity recovery check failed (usec)
 	hrt_abstime	_last_gpos_relaxed_fail_time_us{0};	///< Last time that the global position relaxed validity recovery check failed (usec)
 	hrt_abstime	_last_lpos_fail_time_us{0};	///< Last time that the local position validity recovery check failed (usec)
 	hrt_abstime	_last_lpos_relaxed_fail_time_us{0};	///< Last time that the relaxed local position validity recovery check failed (usec)
 	hrt_abstime	_last_lvel_fail_time_us{0};	///< Last time that the local velocity validity recovery check failed (usec)
+	hrt_abstime	_last_not_dead_reckoning_time_us{0};	///< Last time that the dead reckoning initiated (usec)
 
 	bool _gps_was_fused{false};
 	bool _gnss_spoofed{false};
@@ -114,6 +118,8 @@ private:
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
 					(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph,
-					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
+					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act,
+					(ParamInt<px4::params::COM_DR_TOUT_ACT>) _param_com_dead_reckoning_tout_act,
+					(ParamFloat<px4::params::COM_DR_TOUT_T>) _param_com_dead_reckoning_tout_t
 				       )
 };

src/modules/commander/commander_params.c

@@ -967,6 +967,40 @@ PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f);
  */
 PARAM_DEFINE_INT32(COM_POS_LOW_ACT, 3);
 
+/**
+ * Dead reckoning timeout failsafe threshold
+ *
+ * This triggers the action specified in COM_DR_TOUT_ACT if the dead reckoning has been active longer than this threshold.
+ *
+ * Set to -1 to disable.
+ *
+ * @min -1
+ * @max 1000
+ * @group Commander
+ * @unit s
+ */
+PARAM_DEFINE_FLOAT(COM_DR_TOUT_T, -1.0f);
+
+/**
+ * Dead reckoning timeout action
+ *
+ * Action the system takes when the dead reckoning has active for a specified time.
+ * See COM_DR_TOUT_ACT to set the failsafe threshold.
+ * The failsafe action is only executed if the vehicle is in auto mission,
+ * otherwise it is only a warning.
+ *
+ * @group Commander
+ *
+ * @value 0 None
+ * @value 1 Warning
+ * @value 2 Hold
+ * @value 3 Return
+ * @value 4 Terminate
+ * @value 5 Land
+ * @increment 1
+ */
+PARAM_DEFINE_INT32(COM_DR_TOUT_ACT, 1);
+
 /**
  * Flag to allow arming
  *

src/modules/commander/failsafe/failsafe.cpp

@@ -549,6 +549,12 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 		CHECK_FAILSAFE(status_flags, position_accuracy_low, fromPosLowActParam(_param_com_pos_low_act.get()));
 	}
 
+	// trigger dead reckoning Timeout Failsafe (only in auto mission)
+	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
+	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
+		CHECK_FAILSAFE(status_flags, dead_reckoning_invalid, fromPosLowActParam(_param_com_dead_reckoning_tout_act.get()));
+	}
+
 	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
 	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
 		CHECK_FAILSAFE(status_flags, navigator_failure,

src/modules/commander/failsafe/failsafe.h

@@ -213,7 +213,8 @@ private:
 					(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
 					(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act,
 					(ParamInt<px4::params::COM_FLTT_LOW_ACT>) _param_com_fltt_low_act,
-					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
+					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act,
+					(ParamInt<px4::params::COM_DR_TOUT_ACT>) _param_com_dead_reckoning_tout_act
 				       );
 
 };