navigator initialization and mode change improvements

src/modules/navigator/land.cpp

@@ -56,7 +56,7 @@ Land::on_activation()
 	reset_mission_item_reached();
 
 	/* convert mission item to current setpoint */
-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->previous.valid = false;
 	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
@@ -73,19 +73,22 @@ Land::on_active()
 	/* for VTOL update landing location during back transition */
 	if (_navigator->get_vstatus()->is_vtol &&
 	    _navigator->get_vstatus()->in_transition_mode) {
-		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
 		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+
 		_navigator->set_position_setpoint_triplet_updated();
 	}
 
 
 	if (_navigator->get_land_detected()->landed) {
+
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
 		set_idle_item(&_mission_item);
 
-		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		_navigator->set_position_setpoint_triplet_updated();
 	}

src/modules/navigator/loiter.cpp

@@ -57,7 +57,7 @@ Loiter::on_inactive()
 void
 Loiter::on_activation()
 {
-	if (_navigator->get_reposition_triplet()->current.valid) {
+	if (_reposition.valid) {
 		reposition();
 
 	} else {
@@ -68,7 +68,7 @@ Loiter::on_activation()
 void
 Loiter::on_active()
 {
-	if (_navigator->get_reposition_triplet()->current.valid) {
+	if (_reposition.valid) {
 		reposition();
 	}
 
@@ -98,8 +98,6 @@ Loiter::set_loiter_position()
 		return;
 	}
 
-	_loiter_pos_set = true;
-
 	// set current mission item to loiter
 	set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());
 
@@ -113,6 +111,8 @@ Loiter::set_loiter_position()
 
 	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
 	_navigator->set_position_setpoint_triplet_updated();
+
+	_loiter_pos_set = true;
 }
 
 void
@@ -123,25 +123,29 @@ Loiter::reposition()
 		return;
 	}
 
-	struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet();
-
-	if (rep->current.valid) {
+	if (_reposition.valid) {
 		// set loiter position based on reposition command
 
-		// convert mission item to current setpoint
-		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		pos_sp_triplet->current.velocity_valid = false;
-		pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+		// previous: current position
 		pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
 		pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
 		pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt;
-		memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
+		pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
+		pos_sp_triplet->previous.valid = true;
+
+		// current: use reposition
+		pos_sp_triplet->current = _reposition;
+		pos_sp_triplet->current.velocity_valid = false;
+
+		// next: invalid
 		pos_sp_triplet->next.valid = false;
 
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
 		_navigator->set_position_setpoint_triplet_updated();
 
-		// mark this as done
-		memset(rep, 0, sizeof(*rep));
+		// clear the reposition to mark as done
+		_reposition = position_setpoint_s{};
 	}
 }

src/modules/navigator/loiter.h

@@ -55,14 +55,7 @@ class Loiter : public MissionBlock, public ModuleParams
 	void on_activation() override;
 	void on_active() override;
 
-	// TODO: share this with mission
-	enum mission_yaw_mode {
-		MISSION_YAWMODE_NONE = 0,
-		MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
-		MISSION_YAWMODE_FRONT_TO_HOME = 2,
-		MISSION_YAWMODE_BACK_TO_HOME = 3,
-		MISSION_YAWMODE_MAX = 4
-	};
+	position_setpoint_s &get_reposition() { return _reposition; }
 
 private:
 	/**
@@ -76,5 +69,7 @@ class Loiter : public MissionBlock, public ModuleParams
 	 */
 	void set_loiter_position();
 
+	position_setpoint_s			_reposition{};	/**< position setpoint for non-mission direct position command */
+
 	bool _loiter_pos_set{false};
 };

src/modules/navigator/mission.h

@@ -80,14 +80,6 @@ class Mission : public MissionBlock, public ModuleParams
 		MISSION_ALTMODE_FOH = 1
 	};
 
-	enum mission_yaw_mode {
-		MISSION_YAWMODE_NONE = 0,
-		MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
-		MISSION_YAWMODE_FRONT_TO_HOME = 2,
-		MISSION_YAWMODE_BACK_TO_HOME = 3,
-		MISSION_YAWMODE_MAX = 4
-	};
-
 	bool set_current_offboard_mission_index(uint16_t index);
 
 	bool land_start();

src/modules/navigator/mission_block.cpp

@@ -59,6 +59,14 @@ using matrix::wrap_pi;
 MissionBlock::MissionBlock(Navigator *navigator) :
 	NavigatorMode(navigator)
 {
+	_mission_item.lat = (double)NAN;
+	_mission_item.lon = (double)NAN;
+	_mission_item.yaw = NAN;
+	_mission_item.loiter_radius = _navigator->get_loiter_radius();
+	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+	_mission_item.time_inside = 0.0f;
+	_mission_item.autocontinue = true;
+	_mission_item.origin = ORIGIN_ONBOARD;
 }
 
 bool
@@ -469,10 +477,14 @@ MissionBlock::issue_command(const mission_item_s &item)
 }
 
 float
-MissionBlock::get_time_inside(const struct mission_item_s &item)
+MissionBlock::get_time_inside(const mission_item_s &item) const
 {
-	if (item.nav_cmd != NAV_CMD_TAKEOFF) {
-		return item.time_inside;
+	if ((item.nav_cmd == NAV_CMD_WAYPOINT && _navigator->get_vstatus()->is_rotary_wing) ||
+	    item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	    item.nav_cmd == NAV_CMD_DELAY) {
+
+		// TODO: set appropriate upper limit
+		return math::constrain(item.time_inside, 0.0f, 3600.0f);
 	}
 
 	return 0.0f;
@@ -502,7 +514,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 
 	sp->lat = item.lat;
 	sp->lon = item.lon;
-	sp->alt = item.altitude_is_relative ? item.altitude + _navigator->get_home_position()->alt : item.altitude;
+	sp->alt = get_absolute_altitude_for_item(item);
 	sp->yaw = item.yaw;
 	sp->yaw_valid = PX4_ISFINITE(item.yaw);
 	sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
@@ -728,7 +740,7 @@ MissionBlock::mission_apply_limitation(mission_item_s &item)
 }
 
 float
-MissionBlock::get_absolute_altitude_for_item(struct mission_item_s &mission_item) const
+MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const
 {
 	if (mission_item.altitude_is_relative) {
 		return mission_item.altitude + _navigator->get_home_position()->alt;

src/modules/navigator/mission_block.h

@@ -115,13 +115,13 @@ class MissionBlock : public NavigatorMode
 	/**
 	 * General function used to adjust the mission item based on vehicle specific limitations
 	 */
-	void	mission_apply_limitation(mission_item_s &item);
+	void mission_apply_limitation(mission_item_s &item);
 
 	void issue_command(const mission_item_s &item);
 
-	float get_time_inside(const struct mission_item_s &item);
+	float get_time_inside(const mission_item_s &item) const ;
 
-	float get_absolute_altitude_for_item(struct mission_item_s &mission_item) const;
+	float get_absolute_altitude_for_item(const mission_item_s &mission_item) const;
 
 	mission_item_s _mission_item{};
 

src/modules/navigator/mission_params.c

@@ -122,7 +122,7 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 1);
 /**
 * Enable yaw control of the mount. (Only affects multicopters and ROI mission items)
 *
-* If enabled, yaw commands will be sent to the mount and the vehicle will follow its heading mode as specified by MIS_YAWMODE.
+* If enabled, yaw commands will be sent to the mount and the vehicle will follow its heading mode
 * If disabled, the vehicle will yaw towards the ROI.
 *
 * @value 0 Disable

src/modules/navigator/navigator.h

@@ -85,7 +85,8 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 {
 public:
 	Navigator();
-	virtual ~Navigator() = default;
+	~Navigator() override;
+
 	Navigator(const Navigator &) = delete;
 	Navigator operator=(const Navigator &) = delete;
 
@@ -150,8 +151,6 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 	struct home_position_s *get_home_position() { return &_home_pos; }
 	struct mission_result_s *get_mission_result() { return &_mission_result; }
 	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_global_position_s *get_global_position() { return &_global_pos; }
 	struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
 	struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
@@ -325,8 +324,6 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 	// Publications
 	geofence_result_s				_geofence_result{};
 	position_setpoint_triplet_s			_pos_sp_triplet{};	/**< triplet of position setpoints */
-	position_setpoint_triplet_s			_reposition_triplet{};	/**< triplet for non-mission direct position command */
-	position_setpoint_triplet_s			_takeoff_triplet{};	/**< triplet for non-mission direct takeoff command */
 	vehicle_roi_s					_vroi{};		/**< vehicle ROI */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
@@ -336,7 +333,6 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 
 	bool		_can_loiter_at_sp{false};			/**< flags if current position SP can be used to loiter */
 	bool		_pos_sp_triplet_updated{false};		/**< flags if position SP triplet needs to be published */
-	bool 		_pos_sp_triplet_published_invalid_once{false};	/**< flags if position SP triplet has been published once to UORB */
 	bool		_mission_result_updated{false};		/**< flags if mission result has seen an update */
 
 	NavigatorMode	*_navigation_mode{nullptr};		/**< abstract pointer to current navigation mode class */