Navigator/FlightTaskAuto yaw handling improvements/simplifications (#22532)

* PositionSetpoint: remove yaw_valid field

* Navigator: set yaw setpoint to NAN for Takeoff

Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset.
The yaw setpoint generation is handled by FlightTaskAuto.

* PositionSetpoint.msg: remove disable_weather_vane and instead only use the yaw field

Strictly follow the concept that if the position_setpoint.yaw is set, then
follow it the controller, and otherwise let the controller set it as it
thinks it's best.

* Navigator: remove logic that sets yaw to be accepted in TAKEOFF

No longer needed as during Takeoff we anyway don't set a yaw setpoint.

* PositionSetpoint.msg: remove yawspeed_valid

* PositionSetpoint.msg: remove yawspeed

* Navigator: set yaw setpoint to NAN instead of current

In set_takeoff and set_land_item, as well as for VTOL transition.
The flight tasks then set the yaw corresponding to the current yaw.

* Navigator: change get_yaw_acceptance into a bool

* PositionSetpoint.msg: improve comment for yaw

* MissionBlock: remove unnecessary code from set_vtol_transition_item

* Navigator: clean up calculate_breaking_stop(), set yaw to NAN

* Navigator: set yaw to NAN in variouls places where not specifc setpoint is desired

* Navigator: set yaw to NAN in reset_position_setpoint()

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>

msg/PositionSetpoint.msg

@@ -22,11 +22,7 @@ float32 vz			# local velocity setpoint in m/s in NED
 float64 lat			# latitude, in deg
 float64 lon			# longitude, in deg
 float32 alt			# altitude AMSL, in m
-float32 yaw			# yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
+float32 yaw			# yaw (only in hover), in rad [-PI..PI), NaN = leave to flight task
-bool yaw_valid			# true if yaw setpoint valid
-
-float32 yawspeed		# yawspeed (only for multirotors, in rad/s)
-bool yawspeed_valid		# true if yawspeed setpoint valid
 
 float32 loiter_radius		# loiter major axis radius in m
 float32 loiter_minor_radius	# loiter minor axis radius (used for non-circular loiter shapes) in m
@@ -39,5 +35,3 @@ float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoi
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
-
-bool disable_weather_vane   # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -466,7 +466,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 	}
 
 	// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
-	_weathervane.setNavigatorForceDisabled(_sub_triplet_setpoint.get().current.disable_weather_vane);
+	_weathervane.setNavigatorForceDisabled(PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw));
 
 	// Calculate the current vehicle state and check if it has updated.
 	State previous_state = _current_state;
@@ -481,7 +481,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
 				_triplet_next_wp,
 				_sub_triplet_setpoint.get().next.yaw,
-				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
+				(float)NAN,
 				_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
 		_obstacle_avoidance.checkAvoidanceProgress(
 			_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
@@ -509,13 +509,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_yaw_setpoint = NAN;
 			_yawspeed_setpoint = 0.f;
 
-		} else if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane)
+		} else if (PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw)) {
-			   && _sub_triplet_setpoint.get().current.yaw_valid) {
-			// Use the yaw computed in Navigator except during takeoff because
-			// Navigator is not handling the yaw reset properly.
-			// But: use if from Navigator during takeoff if disable_weather_vane is true,
-			// because we're then aligning to the transition waypoint.
-			// TODO: fix in navigator
 			_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
 			_yawspeed_setpoint = NAN;
 

src/modules/navigator/mission.cpp

@@ -322,8 +322,7 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
 
 		_mission_item.lat = _global_pos_sub.get().lat;
 		_mission_item.lon = _global_pos_sub.get().lon;
-		/* hold heading for takeoff items */
+		_mission_item.yaw = NAN; // FlightTaskAuto handles yaw directly
-		_mission_item.yaw = _navigator->get_local_position()->heading;
 		_mission_item.altitude = _mission_init_climb_altitude_amsl;
 		_mission_item.altitude_is_relative = false;
 		_mission_item.autocontinue = true;
@@ -366,9 +365,6 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
 	    _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
 	    !_land_detected_sub.get().landed) {
 
-		/* disable weathervane before front transition for allowing yaw to align */
-		pos_sp_triplet->current.disable_weather_vane = true;
-
 		/* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */
 		_mission_item.yaw = get_bearing_to_next_waypoint(
 					    _global_pos_sub.get().lat, _global_pos_sub.get().lon,
@@ -389,16 +385,13 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
 	    _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
 	    !_land_detected_sub.get().landed) {
 
-		/* re-enable weather vane again after alignment */
-		pos_sp_triplet->current.disable_weather_vane = false;
-
 		/* check if the vtol_takeoff waypoint is on top of us */
 		if (do_need_move_to_takeoff()) {
 			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
 		}
 
 		set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
-		_mission_item.yaw = _navigator->get_local_position()->heading;
+		_mission_item.yaw = NAN;
 
 		// keep current setpoints (FW position controller generates wp to track during transition)
 		pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
@@ -428,9 +421,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite
 	    && !_land_detected_sub.get().landed
 	    && (num_found_items > 0u)) {
 
-		/* disable weathervane before front transition for allowing yaw to align */
-		pos_sp_triplet->current.disable_weather_vane = true;
-
 		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING;
 
 		set_align_mission_item(&_mission_item, &next_mission_items[0u]);
@@ -445,9 +435,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite
 
 		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT;
 
-		/* re-enable weather vane again after alignment */
-		pos_sp_triplet->current.disable_weather_vane = false;
-
 		pos_sp_triplet->previous = pos_sp_triplet->current;
 		// keep current setpoints (FW position controller generates wp to track during transition)
 		pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;

src/modules/navigator/mission_base.cpp

@@ -654,9 +654,6 @@ bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const p
 		(fabs(p1->lon - p2->lon) < DBL_EPSILON) &&
 		(fabsf(p1->alt - p2->alt) < FLT_EPSILON) &&
 		((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) &&
-		(p1->yaw_valid == p2->yaw_valid) &&
-		(fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) &&
-		(p1->yawspeed_valid == p2->yawspeed_valid) &&
 		(fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) &&
 		(p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) &&
 		(fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) &&
@@ -773,13 +770,11 @@ MissionBase::heading_sp_update()
 
 			_mission_item.yaw = yaw;
 			pos_sp_triplet->current.yaw = _mission_item.yaw;
-			pos_sp_triplet->current.yaw_valid = true;
 
 		} else {
-			if (!pos_sp_triplet->current.yaw_valid) {
+			if (!PX4_ISFINITE(pos_sp_triplet->current.yaw)) {
-				_mission_item.yaw = _navigator->get_local_position()->heading;
+				_mission_item.yaw = NAN;
 				pos_sp_triplet->current.yaw = _mission_item.yaw;
-				pos_sp_triplet->current.yaw_valid = true;
 			}
 		}
 

src/modules/navigator/mission_block.cpp

@@ -411,7 +411,7 @@ MissionBlock::is_mission_item_reached_or_completed()
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-		    && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))
+		    && _navigator->get_yaw_to_be_accepted(_mission_item.yaw)
 		    && _navigator->get_local_position()->heading_good_for_control) {
 
 			const float yaw_err = wrap_pi(_mission_item.yaw - _navigator->get_local_position()->heading);
@@ -423,14 +423,6 @@ MissionBlock::is_mission_item_reached_or_completed()
 				_waypoint_yaw_reached = true;
 			}
 
-			// Always accept yaw during takeoff
-			// TODO: Ideally Navigator would handle a yaw reset and adjust its yaw setpoint, making the
-			// following no longer necessary.
-			// FlightTaskAuto is currently also ignoring the yaw setpoint during takeoff and thus "handling" it.
-			if (_mission_item.nav_cmd == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
-				_waypoint_yaw_reached = true;
-			}
-
 			/* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */
 			if (!_waypoint_yaw_reached && _mission_item.force_heading &&
 			    (_navigator->get_yaw_timeout() >= FLT_EPSILON) &&
@@ -668,7 +660,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	sp->lon = item.lon;
 	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) :
 			    _navigator->get_loiter_radius();
 	sp->loiter_direction_counter_clockwise = item.loiter_radius < 0;
@@ -704,6 +695,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 
 		} else {
 			sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
+
+			// Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset.
+			// The yaw setpoint generation is handled by FlightTaskAuto.
+			sp->yaw = NAN;
 		}
 
 		break;
@@ -746,6 +741,7 @@ MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *it
 	item->altitude = pos_sp_triplet->current.alt;
 	item->loiter_radius = pos_sp_triplet->current.loiter_direction_counter_clockwise ?
 			      -pos_sp_triplet->current.loiter_radius : pos_sp_triplet->current.loiter_radius;
+	item->yaw = pos_sp_triplet->current.yaw;
 }
 
 void
@@ -765,8 +761,8 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
 	}
 
 	item->altitude = loiter_altitude_amsl;
-
 	item->loiter_radius = _navigator->get_loiter_radius();
+	item->yaw = NAN;
 }
 
 void
@@ -774,10 +770,11 @@ MissionBlock::setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s
 {
 	setLoiterItemCommonFields(item);
 
-	_navigator->calculate_breaking_stop(item->lat, item->lon, item->yaw);
+	_navigator->calculate_breaking_stop(item->lat, item->lon);
 
 	item->altitude = _navigator->get_global_position()->alt;
 	item->loiter_radius = _navigator->get_loiter_radius();
+	item->yaw = NAN;
 }
 
 void
@@ -801,7 +798,7 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude)
 	/* use current position */
 	item->lat = _navigator->get_global_position()->lat;
 	item->lon = _navigator->get_global_position()->lon;
-	item->yaw = _navigator->get_local_position()->heading;
+	item->yaw = NAN;
 
 	item->altitude = abs_altitude;
 	item->altitude_is_relative = false;
@@ -831,7 +828,7 @@ MissionBlock::set_land_item(struct mission_item_s *item)
 	// set land item to current position
 	item->lat = _navigator->get_global_position()->lat;
 	item->lon = _navigator->get_global_position()->lon;
-	item->yaw = _navigator->get_local_position()->heading;
+	item->yaw = NAN;
 
 	item->altitude = 0;
 	item->altitude_is_relative = false;
@@ -863,14 +860,7 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
 {
 	item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 	item->params[0] = (float) new_mode;
-	item->params[1] = 0.0f;
+	item->params[1] = 0.0f; // not immediate transition
-
-	// Keep yaw from previous mission item if valid, as that is containing the transition heading.
-	// If not valid use current yaw as yaw setpoint
-	if (!PX4_ISFINITE(item->yaw)) {
-		item->yaw = _navigator->get_local_position()->heading; // ideally that would be course and not heading
-	}
-
 	item->autocontinue = true;
 }
 

src/modules/navigator/navigator.h

@@ -239,14 +239,13 @@ public:
 	void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }
 
 	/**
-	 * Get the yaw acceptance given the current mission item
+	 * Get if the yaw acceptance is required at the current mission item
 	 *
 	 * @param mission_item_yaw the yaw to use in case the controller-derived radius is finite
 	 *
-	 * @return the yaw at which the next waypoint should be used or NaN if the yaw at a waypoint
+	 * @return true if the yaw acceptance is required, false if not required
-	 * should be ignored
 	 */
-	float get_yaw_acceptance(float mission_item_yaw);
+	bool get_yaw_to_be_accepted(float mission_item_yaw);
 
 	orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
 
@@ -279,7 +278,7 @@ public:
 	void release_gimbal_control();
 	void set_gimbal_neutral();
 
-	void calculate_breaking_stop(double &lat, double &lon, float &yaw);
+	void calculate_breaking_stop(double &lat, double &lon);
 
 	void stop_capturing_images();
 	void disable_camera_trigger();

src/modules/navigator/navigator_main.cpp

@@ -309,11 +309,9 @@ void Navigator::run()
 					// Go on and check which changes had been requested
 					if (PX4_ISFINITE(cmd.param4)) {
 						rep->current.yaw = cmd.param4;
-						rep->current.yaw_valid = true;
 
 					} else {
 						rep->current.yaw = NAN;
-						rep->current.yaw_valid = false;
 					}
 
 					if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
@@ -348,8 +346,7 @@ void Navigator::run()
 						if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 						    && (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
 
-							calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw);
+							calculate_breaking_stop(rep->current.lat, rep->current.lon);
-							rep->current.yaw_valid = true;
 
 						} else {
 							// For fixedwings we can use the current vehicle's position to define the loiter point
@@ -446,7 +443,6 @@ void Navigator::run()
 					rep->current.cruising_throttle = get_cruising_throttle();
 					rep->current.acceptance_radius = get_acceptance_radius();
 					rep->current.yaw = NAN;
-					rep->current.yaw_valid = false;
 
 					// Position is not changing, thus we keep the setpoint
 					rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat;
@@ -458,8 +454,7 @@ void Navigator::run()
 					if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 					    && (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
 
-						calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw);
+						calculate_breaking_stop(rep->current.lat, rep->current.lon);
-						rep->current.yaw_valid = true;
 					}
 
 					if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) {
@@ -599,19 +594,18 @@ void Navigator::run()
 				rep->current.cruising_speed = -1.f; // reset to default
 
 				if (home_global_position_valid()) {
-					// Only set yaw if we know the true heading
-					// We assume that the heading is valid when the global position is valid because true heading
-					// is required to fuse NE (e.g.: GNSS) data. // TODO: we should be more explicit here
-					rep->current.yaw = cmd.param4;
 
 					rep->previous.valid = true;
 					rep->previous.timestamp = hrt_absolute_time();
 
 				} else {
-					rep->current.yaw = get_local_position()->heading;
 					rep->previous.valid = false;
 				}
 
+				// Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset.
+				// The yaw setpoint generation is handled by FlightTaskAuto.
+				rep->current.yaw = NAN;
+
 				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
 					rep->current.lat = cmd.param5;
 					rep->current.lon = cmd.param6;
@@ -1039,8 +1033,7 @@ void Navigator::geofence_breach_check()
 				}
 
 				rep->current.timestamp = hrt_absolute_time();
-				rep->current.yaw = get_local_position()->heading;
+				rep->current.yaw = NAN;
-				rep->current.yaw_valid = true;
 				rep->current.lat = loiter_latitude;
 				rep->current.lon = loiter_longitude;
 				rep->current.alt = loiter_altitude_amsl;
@@ -1157,13 +1150,13 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp)
 	sp.timestamp = hrt_absolute_time();
 	sp.lat = static_cast<double>(NAN);
 	sp.lon = static_cast<double>(NAN);
+	sp.yaw = NAN;
 	sp.loiter_radius = get_loiter_radius();
 	sp.acceptance_radius = get_default_acceptance_radius();
 	sp.cruising_speed = get_cruising_speed();
 	sp.cruising_throttle = get_cruising_throttle();
 	sp.valid = false;
 	sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
-	sp.disable_weather_vane = false;
 	sp.loiter_direction_counter_clockwise = false;
 }
 
@@ -1193,7 +1186,7 @@ float Navigator::get_acceptance_radius()
 	return acceptance_radius;
 }
 
-float Navigator::get_yaw_acceptance(float mission_item_yaw)
+bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw)
 {
 	float yaw = mission_item_yaw;
 
@@ -1205,7 +1198,7 @@ float Navigator::get_yaw_acceptance(float mission_item_yaw)
 		yaw = pos_ctrl_status.yaw_acceptance;
 	}
 
-	return yaw;
+	return PX4_ISFINITE(yaw);
 }
 
 void Navigator::load_fence_from_file(const char *filename)
@@ -1470,21 +1463,20 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
 	return true;
 }
 
-void Navigator::calculate_breaking_stop(double &lat, double &lon, float &yaw)
+void Navigator::calculate_breaking_stop(double &lat, double &lon)
 {
 	// For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back
-	float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx);
+	const float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx);
 
 	// predict braking distance
 
 	const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
 
-	float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs,
+	const float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs,
-					    _param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto);
+			_param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto);
 
 	waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, course_over_ground,
 					   multirotor_braking_distance, &lat, &lon);
-	yaw = get_local_position()->heading;
 }
 
 void Navigator::mode_completed(uint8_t nav_state, uint8_t result)

src/modules/navigator/takeoff.cpp

@@ -130,7 +130,6 @@ Takeoff::set_takeoff_position()
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
 	pos_sp_triplet->previous.valid = false;
-	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;
 
 	if (rep->current.valid) {

src/modules/navigator/vtol_takeoff.cpp

@@ -75,7 +75,6 @@ VtolTakeoff::on_active()
 							    _mission_item.lon, _loiter_location(0), _loiter_location(1)));
 				_mission_item.force_heading = true;
 				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
-				pos_sp_triplet->current.disable_weather_vane = true;
 				pos_sp_triplet->current.cruising_speed = -1.f;
 				_navigator->set_position_setpoint_triplet_updated();
 
@@ -182,7 +181,6 @@ VtolTakeoff::set_takeoff_position()
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
 	pos_sp_triplet->previous.valid = false;
-	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_position_setpoint_triplet_updated();