Navigator resurrect FW GPS failure navigation (#7762)

src/modules/navigator/gpsfailure.cpp

@@ -37,24 +37,21 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <string.h>
-#include <stdlib.h>
-#include <math.h>
-#include <fcntl.h>
+#include "gpsfailure.h"
+#include "navigator.h"
 
 #include <systemlib/mavlink_log.h>
-#include <systemlib/err.h>
 #include <geo/geo.h>
 #include <navigator/navigation.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/home_position.h>
+#include <mathlib/mathlib.h>
 
-#include "navigator.h"
-#include "gpsfailure.h"
+using matrix::Eulerf;
+using matrix::Quatf;
 
-#define DELAY_SIGMA	0.01f
+static constexpr float DELAY_SIGMA = 0.01f;
 
 GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
@@ -67,14 +64,11 @@ GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
 {
 	/* load initial params */
 	updateParams();
+
 	/* initial reset */
 	on_inactive();
 }
 
-GpsFailure::~GpsFailure()
-{
-}
-
 void
 GpsFailure::on_inactive()
 {
@@ -96,24 +90,34 @@ GpsFailure::on_activation()
 void
 GpsFailure::on_active()
 {
-
 	switch (_gpsf_state) {
 	case GPSF_STATE_LOITER: {
 			/* Position controller does not run in this mode:
 			 * navigator has to publish an attitude setpoint */
-			_navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
-			_navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
-			_navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
-			_navigator->publish_att_sp();
+			vehicle_attitude_setpoint_s att_sp = {};
+			att_sp.timestamp = hrt_absolute_time();
+			att_sp.roll_body = math::radians(_param_openlooploiter_roll.get());
+			att_sp.pitch_body = math::radians(_param_openlooploiter_pitch.get());
+			att_sp.thrust = _param_openlooploiter_thrust.get();
+
+			Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f));
+			q.copyTo(att_sp.q_d);
+			att_sp.q_d_valid = true;
+
+			if (_att_sp_pub != nullptr) {
+				/* publish att sp*/
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
+
+			} else {
+				/* advertise and publish */
+				_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+			}
 
 			/* Measure time */
-			hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
-
-			//warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
-			//_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
-			if (elapsed > _param_loitertime.get() * 1e6f) {
-				/* no recovery, adavance the state machine */
-				warnx("gps not recovered, switch to next state");
+			if ((_param_loitertime.get() > FLT_EPSILON) &&
+			    (hrt_elapsed_time(&_timestamp_activation) > _param_loitertime.get() * 1e6f)) {
+				/* no recovery, advance the state machine */
+				PX4_WARN("GPS not recovered, switching to next failure state");
 				advance_gpsf();
 			}
 
@@ -142,17 +146,17 @@ GpsFailure::set_gpsf_item()
 
 	switch (_gpsf_state) {
 	case GPSF_STATE_TERMINATE: {
-			/* Request flight termination from the commander */
+			/* Request flight termination from commander */
 			_navigator->get_mission_result()->flight_termination = true;
 			_navigator->set_mission_result_updated();
-			warnx("gps fail: request flight termination");
+			PX4_WARN("GPS failure: request flight termination");
 		}
+		break;
 
 	default:
 		break;
 	}
 
-	reset_mission_item_reached();
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -162,20 +166,18 @@ GpsFailure::advance_gpsf()
 	switch (_gpsf_state) {
 	case GPSF_STATE_NONE:
 		_gpsf_state = GPSF_STATE_LOITER;
-		warnx("gpsf loiter");
-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "GPS failed: open loop loiter");
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: fixed bank loiter");
 		break;
 
 	case GPSF_STATE_LOITER:
 		_gpsf_state = GPSF_STATE_TERMINATE;
-		warnx("gpsf terminate");
-		mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no gps recovery, termination");
-		warnx("mavlink sent");
+		mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no GPS recovery, terminating flight");
 		break;
 
 	case GPSF_STATE_TERMINATE:
-		warnx("gpsf end");
+		PX4_WARN("terminate");
 		_gpsf_state = GPSF_STATE_END;
+		break;
 
 	default:
 		break;

src/modules/navigator/gpsfailure.h

@@ -47,9 +47,6 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 
-#include <drivers/drv_hrt.h>
-
-#include "navigator_mode.h"
 #include "mission_block.h"
 
 class Navigator;
@@ -58,14 +55,11 @@ class GpsFailure : public MissionBlock
 {
 public:
 	GpsFailure(Navigator *navigator, const char *name);
+	~GpsFailure() = default;
 
-	~GpsFailure();
-
-	virtual void on_inactive();
-
-	virtual void on_activation();
-
-	virtual void on_active();
+	void on_inactive() override;
+	void on_activation() override;
+	void on_active() override;
 
 private:
 	/* Params */
@@ -79,9 +73,11 @@ private:
 		GPSF_STATE_LOITER = 1,
 		GPSF_STATE_TERMINATE = 2,
 		GPSF_STATE_END = 3,
-	} _gpsf_state;
+	} _gpsf_state{GPSF_STATE_NONE};
 
-	hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */
+	hrt_abstime _timestamp_activation{0}; //*< timestamp when this mode was activated */
+
+	orb_advert_t	_att_sp_pub{nullptr};
 
 	/**
 	 * Set the GPSF item

src/modules/navigator/gpsfailure_params.c

@@ -36,18 +36,13 @@
  *
  * Parameters for GPSF navigation mode
  *
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
-
-/*
- * GPS Failure Navigation Mode parameters, accessible via MAVLink
  */
 
 /**
  * Loiter time
  *
- * The amount of time in seconds the system should do open loop loiter and wait for gps recovery
- * before it goes into flight termination.
+ * The time in seconds the system should do open loop loiter and wait for GPS recovery
+ * before it goes into flight termination. Set to 0 to disable.
  *
  * @unit s
  * @min 0.0
@@ -56,12 +51,12 @@
  * @increment 1
  * @group GPS Failure Navigation
  */
-PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
+PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 0.0f);
 
 /**
- * Open loop loiter roll
+ * Fixed bank angle
  *
- * Roll in degrees during the open loop loiter
+ * Roll in degrees during the loiter
  *
  * @unit deg
  * @min 0.0
@@ -73,7 +68,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
 PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
 
 /**
- * Open loop loiter pitch
+ * Fixed pitch angle
  *
  * Pitch in degrees during the open loop loiter
  *
@@ -87,7 +82,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
 PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
 
 /**
- * Open loop loiter thrust
+ * Thrust
  *
  * Thrust value which is set during the open loop loiter
  *
@@ -98,4 +93,4 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
  * @increment 0.05
  * @group GPS Failure Navigation
  */
-PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);
+PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.0f);

src/modules/navigator/navigator.h

@@ -130,7 +130,6 @@ public:
 	struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
 	struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
-	struct vehicle_attitude_setpoint_s *get_att_sp() { return &_att_sp; }
 	struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
 	struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
 	struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
@@ -239,7 +238,6 @@ private:
 	int		_vehicle_command_sub{-1};	/**< vehicle commands (onboard and offboard) */
 	int		_vstatus_sub{-1};		/**< vehicle status subscription */
 
-	orb_advert_t	_att_sp_pub{nullptr};
 	orb_advert_t	_geofence_result_pub{nullptr};
 	orb_advert_t	_mission_result_pub{nullptr};
 	orb_advert_t	_pos_sp_triplet_pub{nullptr};

src/modules/navigator/navigator_main.cpp

@@ -57,14 +57,9 @@
 #include <px4_tasks.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
-
 #include <sys/types.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/systemlib.h>
-#include <drivers/device/device.h>
-#include <arch/board/board.h>
-
-#include <uORB/uORB.h>
 #include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
@@ -267,13 +262,11 @@ Navigator::task_main()
 	px4_pollfd_struct_t fds[1] = {};
 
 	/* Setup of loop */
-	fds[0].fd = _global_pos_sub;
+	fds[0].fd = _local_pos_sub;
 	fds[0].events = POLLIN;
 
-	bool global_pos_available_once = false;
-
-	/* rate-limit global pos subscription to 20 Hz / 50 ms */
-	orb_set_interval(_global_pos_sub, 49);
+	/* rate-limit position subscription to 20 Hz / 50 ms */
+	orb_set_interval(_local_pos_sub, 50);
 
 	while (!_task_should_exit) {
 
@@ -281,11 +274,6 @@ Navigator::task_main()
 		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
 
 		if (pret == 0) {
-			/* timed out - periodic check for _task_should_exit, etc. */
-			if (global_pos_available_once) {
-				global_pos_available_once = false;
-			}
-
 			/* Let the loop run anyway, don't do `continue` here. */
 
 		} else if (pret < 0) {
@@ -295,16 +283,9 @@ Navigator::task_main()
 			continue;
 
 		} else {
-
 			if (fds[0].revents & POLLIN) {
-				/* success, global pos is available */
-				global_position_update();
-
-				if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
-					have_geofence_position_data = true;
-				}
-
-				global_pos_available_once = true;
+				/* success, local pos is available */
+				local_position_update();
 			}
 		}
 
@@ -323,11 +304,15 @@ Navigator::task_main()
 			}
 		}
 
-		/* local position updated */
-		orb_check(_local_pos_sub, &updated);
+		/* global position updated */
+		orb_check(_global_pos_sub, &updated);
 
 		if (updated) {
-			local_position_update();
+			global_position_update();
+
+			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
+				have_geofence_position_data = true;
+			}
 		}
 
 		/* sensors combined updated */
@@ -372,6 +357,7 @@ Navigator::task_main()
 			home_position_update();
 		}
 
+		/* vehicle_command updated */
 		orb_check(_vehicle_command_sub, &updated);
 
 		if (updated) {
@@ -509,9 +495,6 @@ Navigator::task_main()
 					_mission.set_current_offboard_mission_index(cmd.param1);
 				}
 
-			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
-				warnx("navigator: got pause/continue command");
-
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
 				if (cmd.param2 > FLT_EPSILON) {
 					// XXX not differentiating ground and airspeed yet
@@ -680,29 +663,17 @@ Navigator::task_main()
 	}
 
 	orb_unsubscribe(_global_pos_sub);
-	_global_pos_sub = -1;
 	orb_unsubscribe(_local_pos_sub);
-	_local_pos_sub = -1;
 	orb_unsubscribe(_gps_pos_sub);
-	_gps_pos_sub = -1;
 	orb_unsubscribe(_sensor_combined_sub);
-	_sensor_combined_sub = -1;
 	orb_unsubscribe(_fw_pos_ctrl_status_sub);
-	_fw_pos_ctrl_status_sub = -1;
 	orb_unsubscribe(_vstatus_sub);
-	_vstatus_sub = -1;
 	orb_unsubscribe(_land_detected_sub);
-	_land_detected_sub = -1;
 	orb_unsubscribe(_home_pos_sub);
-	_home_pos_sub = -1;
 	orb_unsubscribe(_onboard_mission_sub);
-	_onboard_mission_sub = -1;
 	orb_unsubscribe(_offboard_mission_sub);
-	_offboard_mission_sub = -1;
 	orb_unsubscribe(_param_update_sub);
-	_param_update_sub = -1;
 	orb_unsubscribe(_vehicle_command_sub);
-	_vehicle_command_sub = -1;
 
 	PX4_INFO("exiting");
 
@@ -985,20 +956,6 @@ Navigator::publish_geofence_result()
 	}
 }
 
-void
-Navigator::publish_att_sp()
-{
-	/* lazily publish the attitude sp only once available */
-	if (_att_sp_pub != nullptr) {
-		/* publish att sp*/
-		orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
-
-	} else {
-		/* advertise and publish */
-		_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
-	}
-}
-
 void
 Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd)
 {