commander move remaining position failsafe params to class and cleanup

src/modules/commander/Commander.hpp

@@ -39,6 +39,7 @@
 #include <controllib/blocks.hpp>
 #include <px4_module.h>
 #include <px4_module_params.h>
+#include <mathlib/mathlib.h>
 
 // publications
 #include <uORB/Publication.hpp>
@@ -58,9 +59,12 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 
+using math::constrain;
 using uORB::Publication;
 using uORB::Subscription;
 
+using namespace time_literals;
+
 class Commander : public ModuleBase<Commander>, public ModuleParams
 {
 public:
@@ -95,35 +99,51 @@ private:
 		(ParamFloat<px4::params::COM_POS_FS_EPH>) _eph_threshold,
 		(ParamFloat<px4::params::COM_POS_FS_EPV>) _epv_threshold,
 		(ParamFloat<px4::params::COM_VEL_FS_EVH>) _evh_threshold,
+
+		(ParamInt<px4::params::COM_POS_FS_DELAY>) _failsafe_pos_delay,
+		(ParamInt<px4::params::COM_POS_FS_PROB>) _failsafe_pos_probation,
+		(ParamInt<px4::params::COM_POS_FS_GAIN>) _failsafe_pos_gain
 	)
 
-	bool handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd,
-			    actuator_armed_s *armed_local, home_position_s *home, const vehicle_global_position_s &global_pos,
-			    const vehicle_local_position_s &local_pos, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed);
+	static constexpr int64_t sec_to_usec = (1000 * 1000);
+	const int64_t POSVEL_PROBATION_MIN = 1 * sec_to_usec;	/**< minimum probation duration (usec) */
+	const int64_t POSVEL_PROBATION_MAX = 100 * sec_to_usec;	/**< maximum probation duration (usec) */
+
+	hrt_abstime	_last_gpos_fail_time_us{0};	/**< Last time that the global position 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_lvel_fail_time_us{0};	/**< Last time that the local velocity validity recovery check failed (usec) */
+
+	// Probation times for position and velocity validity checks to pass if failed
+	hrt_abstime	_gpos_probation_time_us = POSVEL_PROBATION_MIN;
+	hrt_abstime	_lpos_probation_time_us = POSVEL_PROBATION_MIN;
+	hrt_abstime	_lvel_probation_time_us = POSVEL_PROBATION_MIN;
 
-	bool set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition,
-			       const vehicle_global_position_s &globalPosition, bool set_alt_only_to_lpos_ref);
+	bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd,
+			    actuator_armed_s *armed, home_position_s *home, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed);
+
+	bool set_home_position(orb_advert_t &homePub, home_position_s &home, bool set_alt_only_to_lpos_ref);
 
 	// Set the main system state based on RC and override device inputs
-	transition_result_t set_main_state(const vehicle_status_s &status, const vehicle_global_position_s &global_position,
-					   const vehicle_local_position_s &local_position, bool *changed);
+	transition_result_t set_main_state(const vehicle_status_s &status, bool *changed);
 
 	// Enable override (manual reversion mode) on the system
-	transition_result_t set_main_state_override_on(const vehicle_status_s &status_local, bool *changed);
+	transition_result_t set_main_state_override_on(const vehicle_status_s &status, bool *changed);
 
 	// Set the system main state based on the current RC inputs
-	transition_result_t set_main_state_rc(const vehicle_status_s &status_local,
-					      const vehicle_global_position_s &global_position, const vehicle_local_position_s &local_position, bool *changed);
+	transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed);
+
+	// Set the main system state based on RC and override device inputs
+	transition_result_t set_main_state(vehicle_status_s *status, bool *changed);
+	transition_result_t set_main_state_override_on(vehicle_status_s *status, bool *changed);
+	transition_result_t set_main_state_rc(vehicle_status_s *status, bool *changed);
 
-	void check_valid(const hrt_abstime &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out,
-			 bool *changed);
+	void check_valid(const hrt_abstime &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, bool *changed);
 
 	bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
 				   const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state,
 				   bool *validity_changed);
 
-	void reset_posvel_validity(const vehicle_global_position_s &global_position,
-				   const vehicle_local_position_s &local_position, bool *changed);
+	void reset_posvel_validity(bool *changed);
 
 	void mission_init();
 
@@ -152,8 +172,10 @@ private:
 	// publisher
 	orb_advert_t _vehicle_cmd_pub = nullptr;
 
-	// subscriptions
-	Subscription<mission_result_s> _mission_result_sub;
+	// Subscriptions
+	Subscription<mission_result_s>			_mission_result_sub;
+	Subscription<vehicle_global_position_s>		_global_position_sub;
+	Subscription<vehicle_local_position_s>		_local_position_sub;
 };
 
 #endif /* COMMANDER_HPP_ */