generic position controller status/feedback message

msg/CMakeLists.txt

@@ -58,7 +58,6 @@ set(msg_files
 	esc_status.msg
 	estimator_status.msg
 	follow_target.msg
-	fw_pos_ctrl_status.msg
 	geofence_result.msg
 	gps_dump.msg
 	gps_inject_data.msg
@@ -81,6 +80,8 @@ set(msg_files
 	optical_flow.msg
 	parameter_update.msg
 	ping.msg
+	position_controller_landing_status.msg
+	position_controller_status.msg
 	position_setpoint.msg
 	position_setpoint_triplet.msg
 	power_button_state.msg

msg/fw_pos_ctrl_status.msg

@@ -1,14 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-float32 nav_roll
-float32 nav_pitch
-float32 nav_bearing
-
-float32 target_bearing
-float32 wp_dist
-float32 xtrack_error
-float32 turn_distance		# the optimal distance to a waypoint to switch to the next
-
-float32 landing_horizontal_slope_displacement
-float32 landing_slope_angle_rad
-float32 landing_flare_length
-bool abort_landing

msg/position_controller_landing_status.msg

@@ -0,0 +1,9 @@
+uint64 timestamp		# time since system start (microseconds)
+
+float32 horizontal_slope_displacement
+
+float32 slope_angle_rad
+
+float32 flare_length
+
+bool abort_landing				# true if landing should be aborted

msg/position_controller_status.msg

@@ -0,0 +1,14 @@
+uint64 timestamp		# time since system start (microseconds)
+
+float32 nav_roll
+float32 nav_pitch
+float32 nav_bearing
+
+float32 target_bearing
+float32 xtrack_error
+
+float32 wp_dist
+
+float32 acceptance_radius		# the optimal distance to a waypoint to switch to the next
+
+float32 yaw_acceptance			# NaN if not set

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -277,13 +277,7 @@ FixedwingPositionControl::parameters_update()
 
 	_landingslope.update(radians(land_slope_angle), land_flare_alt_relative, land_thrust_lim_alt_relative, land_H1_virt);
 
-
-	// Update and publish the navigation capabilities
-	_fw_pos_ctrl_status.landing_slope_angle_rad = _landingslope.landing_slope_angle_rad();
-	_fw_pos_ctrl_status.landing_horizontal_slope_displacement = _landingslope.horizontal_slope_displacement();
-	_fw_pos_ctrl_status.landing_flare_length = _landingslope.flare_length();
-	fw_pos_ctrl_status_publish();
-
+	landing_status_publish();
 
 	// sanity check parameters
 	if ((_parameters.airspeed_max < _parameters.airspeed_min) ||
@@ -546,18 +540,121 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const Vector2f &curr_pos
 }
 
 void
-FixedwingPositionControl::fw_pos_ctrl_status_publish()
+FixedwingPositionControl::tecs_status_publish()
+{
+	tecs_status_s t = {};
+
+	switch (_tecs.tecs_mode()) {
+	case TECS::ECL_TECS_MODE_NORMAL:
+		t.mode = tecs_status_s::TECS_MODE_NORMAL;
+		break;
+
+	case TECS::ECL_TECS_MODE_UNDERSPEED:
+		t.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
+		break;
+
+	case TECS::ECL_TECS_MODE_BAD_DESCENT:
+		t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT;
+		break;
+
+	case TECS::ECL_TECS_MODE_CLIMBOUT:
+		t.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
+		break;
+	}
+
+	t.altitude_sp = _tecs.hgt_setpoint_adj();
+	t.altitude_filtered = _tecs.vert_pos_state();
+
+	t.airspeed_sp = _tecs.TAS_setpoint_adj();
+	t.airspeed_filtered = _tecs.tas_state();
+
+	t.flight_path_angle_sp = _tecs.hgt_rate_setpoint();
+	t.flight_path_angle = _tecs.vert_vel_state();
+
+	t.airspeed_derivative_sp = _tecs.TAS_rate_setpoint();
+	t.airspeed_derivative = _tecs.speed_derivative();
+
+	t.total_energy_error = _tecs.STE_error();
+	t.total_energy_rate_error = _tecs.STE_rate_error();
+
+	t.energy_distribution_error = _tecs.SEB_error();
+	t.energy_distribution_rate_error = _tecs.SEB_rate_error();
+
+	t.throttle_integ = _tecs.throttle_integ_state();
+	t.pitch_integ = _tecs.pitch_integ_state();
+
+	t.timestamp = hrt_absolute_time();
+
+	if (_tecs_status_pub != nullptr) {
+		orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
+
+	} else {
+		_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
+	}
+}
+
+void
+FixedwingPositionControl::status_publish()
+{
+	position_controller_status_s pos_ctrl_status = {};
+
+	pos_ctrl_status.nav_roll = _att_sp.roll_body;
+	pos_ctrl_status.nav_pitch = _att_sp.pitch_body;
+	pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();
+
+	pos_ctrl_status.target_bearing = _l1_control.target_bearing();
+	pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
+
+	pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
+				  _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
+
+	pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
+
+	pos_ctrl_status.yaw_acceptance = NAN;
+
+	pos_ctrl_status.timestamp = hrt_absolute_time();
+
+	if (_pos_ctrl_status_pub != nullptr) {
+		orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status);
+
+	} else {
+		_pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status);
+	}
+}
+
+void
+FixedwingPositionControl::landing_status_publish()
 {
-	_fw_pos_ctrl_status.timestamp = hrt_absolute_time();
+	position_controller_landing_status_s pos_ctrl_landing_status = {};
 
-	if (_fw_pos_ctrl_status_pub != nullptr) {
-		orb_publish(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_pub, &_fw_pos_ctrl_status);
+	pos_ctrl_landing_status.slope_angle_rad = _landingslope.landing_slope_angle_rad();
+	pos_ctrl_landing_status.horizontal_slope_displacement = _landingslope.horizontal_slope_displacement();
+	pos_ctrl_landing_status.flare_length = _landingslope.flare_length();
+
+	pos_ctrl_landing_status.abort_landing = _land_abort;
+
+	pos_ctrl_landing_status.timestamp = hrt_absolute_time();
+
+	if (_pos_ctrl_landing_status_pub != nullptr) {
+		orb_publish(ORB_ID(position_controller_landing_status), _pos_ctrl_landing_status_pub, &pos_ctrl_landing_status);
 
 	} else {
-		_fw_pos_ctrl_status_pub = orb_advertise(ORB_ID(fw_pos_ctrl_status), &_fw_pos_ctrl_status);
+		_pos_ctrl_landing_status_pub = orb_advertise(ORB_ID(position_controller_landing_status), &pos_ctrl_landing_status);
 	}
 }
 
+void
+FixedwingPositionControl::abort_landing(bool abort)
+{
+	// only announce changes
+	if (abort && !_land_abort) {
+		mavlink_log_critical(&_mavlink_log_pub, "Landing aborted");
+	}
+
+	_land_abort = abort;
+	landing_status_publish();
+}
+
 void
 FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev,
 		position_setpoint_s &waypoint_next, bool flag_init)
@@ -677,7 +774,7 @@ bool
 FixedwingPositionControl::in_takeoff_situation()
 {
 	// in air for < 10s
-	const hrt_abstime delta_takeoff = 10000000;
+	const hrt_abstime delta_takeoff = 10_s;
 
 	return (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff)
 	       && (_global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff);
@@ -864,10 +961,10 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 				_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f));
 			}
 
-			if (_fw_pos_ctrl_status.abort_landing) {
+			if (_land_abort) {
 				if (pos_sp_curr.alt - _global_pos.alt  < _parameters.climbout_diff) {
 					// aborted landing complete, normal loiter over landing point
-					_fw_pos_ctrl_status.abort_landing = false;
+					abort_landing(false);
 
 				} else {
 					// continue straight until vehicle has sufficient altitude
@@ -1344,9 +1441,8 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 
 	// save time at which we started landing and reset abort_landing
 	if (_time_started_landing == 0) {
+		reset_landing_state();
 		_time_started_landing = hrt_absolute_time();
-
-		_fw_pos_ctrl_status.abort_landing = false;
 	}
 
 	const float bearing_airplane_currwp = get_bearing_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1),
@@ -1437,16 +1533,16 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 			// we have started landing phase but don't have valid terrain
 			// wait for some time, maybe we will soon get a valid estimate
 			// until then just use the altitude of the landing waypoint
-			if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) {
+			if (hrt_elapsed_time(&_time_started_landing) < 10_s) {
 				terrain_alt = pos_sp_curr.alt;
 
 			} else {
 				// still no valid terrain, abort landing
 				terrain_alt = pos_sp_curr.alt;
-				_fw_pos_ctrl_status.abort_landing = true;
+				abort_landing(true);
 			}
 
-		} else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000)
+		} else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT)
 			   || _land_noreturn_vertical) {
 			// use previous terrain estimate for some time and hope to recover
 			// if we are already flaring (land_noreturn_vertical) then just
@@ -1456,7 +1552,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 		} else {
 			// terrain alt was not valid for long time, abort landing
 			terrain_alt = _t_alt_prev_valid;
-			_fw_pos_ctrl_status.abort_landing = true;
+			abort_landing(true);
 		}
 	}
 
@@ -1615,8 +1711,7 @@ FixedwingPositionControl::handle_command()
 		    _pos_sp_triplet.current.valid &&
 		    _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 
-			_fw_pos_ctrl_status.abort_landing = true;
-			mavlink_log_critical(&_mavlink_log_pub, "Landing aborted");
+			abort_landing(true);
 		}
 	}
 }
@@ -1760,32 +1855,11 @@ FixedwingPositionControl::run()
 						/* advertise and publish */
 						_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
 					}
-				}
-
-				/* XXX check if radius makes sense here */
-				float turn_distance = _l1_control.switch_distance(500.0f);
-
-				/* lazily publish navigation capabilities */
-				if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) > 1000000)
-				    || (fabsf(turn_distance - _fw_pos_ctrl_status.turn_distance) > FLT_EPSILON
-					&& turn_distance > 0)) {
-
-					/* set new turn distance */
-					_fw_pos_ctrl_status.turn_distance = turn_distance;
 
-					_fw_pos_ctrl_status.nav_roll = _l1_control.get_roll_setpoint();
-					_fw_pos_ctrl_status.nav_pitch = get_tecs_pitch();
-					_fw_pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();
-
-					_fw_pos_ctrl_status.target_bearing = _l1_control.target_bearing();
-					_fw_pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
-
-					Vector2f curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon);
-
-					_fw_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1),
-								      (double)curr_wp(0), (double)curr_wp(1));
-
-					fw_pos_ctrl_status_publish();
+					// only publish status in full FW mode
+					if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
+						status_publish();
+					}
 				}
 			}
 
@@ -1826,10 +1900,9 @@ FixedwingPositionControl::reset_landing_state()
 	_land_onslope = false;
 
 	// reset abort land, unless loitering after an abort
-	if (_fw_pos_ctrl_status.abort_landing
-	    && _pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER) {
+	if (_land_abort && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) {
 
-		_fw_pos_ctrl_status.abort_landing = false;
+		abort_landing(false);
 	}
 }
 
@@ -1962,52 +2035,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 				    throttle_min, throttle_max, throttle_cruise,
 				    pitch_min_rad, pitch_max_rad);
 
-	tecs_status_s t;
-	t.timestamp = _tecs.timestamp();
-
-	switch (_tecs.tecs_mode()) {
-	case TECS::ECL_TECS_MODE_NORMAL:
-		t.mode = tecs_status_s::TECS_MODE_NORMAL;
-		break;
-
-	case TECS::ECL_TECS_MODE_UNDERSPEED:
-		t.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
-		break;
-
-	case TECS::ECL_TECS_MODE_BAD_DESCENT:
-		t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT;
-		break;
-
-	case TECS::ECL_TECS_MODE_CLIMBOUT:
-		t.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
-		break;
-	}
-
-	t.altitude_sp			= _tecs.hgt_setpoint_adj();
-	t.altitude_filtered		= _tecs.vert_pos_state();
-	t.airspeed_sp			= _tecs.TAS_setpoint_adj();
-	t.airspeed_filtered 		= _tecs.tas_state();
-
-	t.flight_path_angle_sp		= _tecs.hgt_rate_setpoint();
-	t.flight_path_angle		= _tecs.vert_vel_state();
-
-	t.airspeed_derivative_sp	= _tecs.TAS_rate_setpoint();
-	t.airspeed_derivative		= _tecs.speed_derivative();
-
-	t.total_energy_error			= _tecs.STE_error();
-	t.total_energy_rate_error		= _tecs.STE_rate_error();
-	t.energy_distribution_error		= _tecs.SEB_error();
-	t.energy_distribution_rate_error	= _tecs.SEB_rate_error();
-
-	t.throttle_integ	= _tecs.throttle_integ_state();
-	t.pitch_integ		= _tecs.pitch_integ_state();
-
-	if (_tecs_status_pub != nullptr) {
-		orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
-
-	} else {
-		_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
-	}
+	tecs_status_publish();
 }
 
 FixedwingPositionControl *FixedwingPositionControl::instantiate(int argc, char *argv[])

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -55,22 +55,23 @@
 #include <cfloat>
 
 #include <drivers/drv_hrt.h>
-#include <ecl/l1/ecl_l1_pos_controller.h>
-#include <ecl/tecs/tecs.h>
 #include <lib/ecl/geo/geo.h>
-#include <mathlib/mathlib.h>
+#include <lib/ecl/l1/ecl_l1_pos_controller.h>
+#include <lib/ecl/tecs/tecs.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/perf/perf_counter.h>
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <px4_module.h>
 #include <px4_module_params.h>
 #include <px4_posix.h>
 #include <px4_tasks.h>
-#include <perf/perf_counter.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/airspeed.h>
-#include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/position_controller_landing_status.h>
+#include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_bias.h>
@@ -86,23 +87,6 @@
 #include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>
 
-static constexpr float HDG_HOLD_DIST_NEXT =
-	3000.0f; // initial distance of waypoint in front of plane in heading hold mode
-static constexpr float HDG_HOLD_REACHED_DIST =
-	1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
-static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane
-static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;	// max yawrate at which plane locks yaw for heading hold mode
-static constexpr float HDG_HOLD_MAN_INPUT_THRESH =
-	0.01f; // max manual roll/yaw input from user which does not change the locked heading
-
-static constexpr hrt_abstime T_ALT_TIMEOUT = 1; // time after which we abort landing if terrain estimate is not valid
-
-static constexpr float THROTTLE_THRESH =
-	0.05f;	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
-static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH =
-	0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode
-static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
-
 using math::constrain;
 using math::max;
 using math::min;
@@ -118,6 +102,24 @@ using uORB::Subscription;
 
 using namespace launchdetection;
 using namespace runwaytakeoff;
+using namespace time_literals;
+
+static constexpr float HDG_HOLD_DIST_NEXT =
+	3000.0f; // initial distance of waypoint in front of plane in heading hold mode
+static constexpr float HDG_HOLD_REACHED_DIST =
+	1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
+static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane
+static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;	// max yawrate at which plane locks yaw for heading hold mode
+static constexpr float HDG_HOLD_MAN_INPUT_THRESH =
+	0.01f; // max manual roll/yaw input from user which does not change the locked heading
+
+static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort landing if terrain estimate is not valid
+
+static constexpr float THROTTLE_THRESH =
+	0.05f;	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
+static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH =
+	0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode
+static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
 
 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams
 {
@@ -161,12 +163,12 @@ private:
 	int		_sensor_baro_sub{-1};
 
 	orb_advert_t	_attitude_sp_pub{nullptr};		///< attitude setpoint */
+	orb_advert_t	_pos_ctrl_status_pub{nullptr};		///< navigation capabilities publication */
+	orb_advert_t	_pos_ctrl_landing_status_pub{nullptr};	///< landing status publication */
 	orb_advert_t	_tecs_status_pub{nullptr};		///< TECS status publication */
-	orb_advert_t	_fw_pos_ctrl_status_pub{nullptr};	///< navigation capabilities publication */
 
 	orb_id_t _attitude_setpoint_id{nullptr};
 
-	fw_pos_ctrl_status_s		_fw_pos_ctrl_status {};		///< navigation capabilities */
 	manual_control_setpoint_s	_manual {};			///< r/c channel data */
 	position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items */
 	vehicle_attitude_s	_att {};			///< vehicle attitude setpoint */
@@ -202,6 +204,7 @@ private:
 	bool _land_stayonground{false};
 	bool _land_motor_lim{false};
 	bool _land_onslope{false};
+	bool _land_abort{false};
 
 	Landingslope _landingslope;
 
@@ -367,9 +370,7 @@ private:
 	} _parameter_handles {};				///< handles for interesting parameters */
 
 
-	/**
-	 * Update our local parameter cache.
-	 */
+	// Update our local parameter cache.
 	int		parameters_update();
 
 	// Update subscriptions
@@ -383,8 +384,11 @@ private:
 	void		vehicle_land_detected_poll();
 	void		vehicle_status_poll();
 
-	// publish navigation capabilities
-	void		fw_pos_ctrl_status_publish();
+	void		status_publish();
+	void		landing_status_publish();
+	void		tecs_status_publish();
+
+	void		abort_landing(bool abort);
 
 	/**
 	 * Get a new waypoint based on heading and distance from current position

src/modules/gnd_pos_control/GroundRoverPositionControl.cpp

@@ -150,12 +150,6 @@ GroundRoverPositionControl::parameters_update()
 			   _parameters.speed_imax,
 			   _parameters.gndspeed_max);
 
-	/* Update and publish the navigation capabilities */
-	_gnd_pos_ctrl_status.landing_slope_angle_rad = 0;
-	_gnd_pos_ctrl_status.landing_horizontal_slope_displacement = 0;
-	_gnd_pos_ctrl_status.landing_flare_length = 0;
-	gnd_pos_ctrl_status_publish();
-
 	return OK;
 }
 
@@ -192,18 +186,6 @@ GroundRoverPositionControl::position_setpoint_triplet_poll()
 	}
 }
 
-void GroundRoverPositionControl::gnd_pos_ctrl_status_publish()
-{
-	_gnd_pos_ctrl_status.timestamp = hrt_absolute_time();
-
-	if (_gnd_pos_ctrl_status_pub != nullptr) {
-		orb_publish(ORB_ID(fw_pos_ctrl_status), _gnd_pos_ctrl_status_pub, &_gnd_pos_ctrl_status);
-
-	} else {
-		_gnd_pos_ctrl_status_pub = orb_advertise(ORB_ID(fw_pos_ctrl_status), &_gnd_pos_ctrl_status);
-	}
-}
-
 bool
 GroundRoverPositionControl::control_position(const matrix::Vector2f &current_position,
 		const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
@@ -440,31 +422,34 @@ GroundRoverPositionControl::task_main()
 						/* advertise and publish */
 						_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
 					}
-				}
 
 					/* XXX check if radius makes sense here */
 					float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f);
 
-				/* lazily publish navigation capabilities */
-				if ((hrt_elapsed_time(&_gnd_pos_ctrl_status.timestamp) > 1000000)
-				    || (fabsf(turn_distance - _gnd_pos_ctrl_status.turn_distance) > FLT_EPSILON
-					&& turn_distance > 0)) {
+					// publish status
+					position_controller_status_s pos_ctrl_status = {};
+
+					pos_ctrl_status.nav_roll = _gnd_control.get_roll_setpoint();
+					pos_ctrl_status.nav_pitch = 0.0f;
+					pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();
 
-					/* set new turn distance */
-					_gnd_pos_ctrl_status.turn_distance = turn_distance;
+					pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
+					pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();
 
-					_gnd_pos_ctrl_status.nav_roll = _gnd_control.get_roll_setpoint();
-					_gnd_pos_ctrl_status.nav_pitch = 0.0f;
-					_gnd_pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();
+					pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
+								  _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
 
-					_gnd_pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
-					_gnd_pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();
+					pos_ctrl_status.acceptance_radius = turn_distance;
+					pos_ctrl_status.yaw_acceptance = NAN;
 
-					matrix::Vector2f curr_wp((float)_pos_sp_triplet.current.lat, (float)_pos_sp_triplet.current.lon);
-					_gnd_pos_ctrl_status.wp_dist = get_distance_to_next_waypoint((double)current_position(0), (double)current_position(1),
-								       (double)curr_wp(0), (double)curr_wp(1));
+					pos_ctrl_status.timestamp = hrt_absolute_time();
 
-					gnd_pos_ctrl_status_publish();
+					if (_pos_ctrl_status_pub != nullptr) {
+						orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status);
+
+					} else {
+						_pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status);
+					}
 				}
 			}
 

src/modules/gnd_pos_control/GroundRoverPositionControl.hpp

@@ -41,23 +41,22 @@
  * @author Marco Zorzi <mzorzi@student.ethz.ch>
  */
 
-#include <px4_config.h>
-#include <px4_defines.h>
-#include <px4_posix.h>
-#include <px4_tasks.h>
-
 #include <cfloat>
 
 #include <drivers/drv_hrt.h>
-#include <ecl/l1/ecl_l1_pos_controller.h>
 #include <lib/ecl/geo/geo.h>
-#include <mathlib/mathlib.h>
-#include <perf/perf_counter.h>
-#include <pid/pid.h>
+#include <lib/ecl/l1/ecl_l1_pos_controller.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/pid/pid.h>
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <px4_tasks.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -94,7 +93,7 @@ public:
 
 private:
 	orb_advert_t	_attitude_sp_pub{nullptr};		/**< attitude setpoint */
-	orb_advert_t	_gnd_pos_ctrl_status_pub{nullptr};		/**< navigation capabilities publication */
+	orb_advert_t	_pos_ctrl_status_pub{nullptr};		/**< navigation capabilities publication */
 
 	bool		_task_should_exit{false};		/**< if true, sensor task should exit */
 	bool		_task_running{false};			/**< if true, task is running in its mainloop */
@@ -105,7 +104,6 @@ private:
 	int		_params_sub{-1};			/**< notification of parameter updates */
 	int		_pos_sp_triplet_sub{-1};
 
-	fw_pos_ctrl_status_s			_gnd_pos_ctrl_status{};		/**< navigation capabilities */
 	manual_control_setpoint_s		_manual{};			/**< r/c channel data */
 	position_setpoint_triplet_s		_pos_sp_triplet{};		/**< triplet of mission items */
 	vehicle_attitude_setpoint_s		_att_sp{};			/**< vehicle attitude setpoint */
@@ -188,11 +186,6 @@ private:
 	void		position_setpoint_triplet_poll();
 	void		vehicle_control_mode_poll();
 
-	/**
-	 * Publish navigation capabilities
-	 */
-	void		gnd_pos_ctrl_status_publish();
-
 	/**
 	 * Control position.
 	 */

src/modules/logger/logger.cpp

@@ -655,10 +655,11 @@ void Logger::add_default_topics()
 	add_topic("actuator_controls_virtual_fw");
 	add_topic("actuator_controls_virtual_mc");
 	add_topic("commander_state");
-	add_topic("fw_pos_ctrl_status");
 	add_topic("fw_virtual_attitude_setpoint");
 	add_topic("mc_virtual_attitude_setpoint");
 	add_topic("multirotor_motor_limits");
+	add_topic("position_controller_status");
+	add_topic("position_controller_landingstatus");
 	add_topic("offboard_control_mode");
 	add_topic("time_offset");
 	add_topic("vehicle_attitude_groundtruth", 10);

src/modules/mavlink/mavlink_high_latency2.cpp

@@ -39,17 +39,16 @@
 
 #include "mavlink_high_latency2.h"
 
-#include <mathlib/mathlib.h>
-#include <matrix/math.hpp>
-#include <lib/ecl/geo/geo.h>
 #include <commander/px4_custom_mode.h>
-
+#include <lib/ecl/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/estimator_status.h>
-#include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/geofence_result.h>
+#include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/tecs_status.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -75,8 +74,8 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink
 	_battery_time(0),
 	_estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
 	_estimator_status_time(0),
-	_fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))),
-	_fw_pos_ctrl_status_time(0),
+	_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))),
+	_pos_ctrl_status_time(0),
 	_geofence_sub(_mavlink->add_orb_subscription(ORB_ID(geofence_result))),
 	_geofence_time(0),
 	_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
@@ -283,13 +282,13 @@ bool MavlinkStreamHighLatency2::write_estimator_status(mavlink_high_latency2_t *
 
 bool MavlinkStreamHighLatency2::write_fw_ctrl_status(mavlink_high_latency2_t *msg)
 {
-	struct fw_pos_ctrl_status_s fw_pos_ctrl_status;
+	position_controller_status_s pos_ctrl_status = {};
 
-	const bool updated = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_time, &fw_pos_ctrl_status);
+	const bool updated = _pos_ctrl_status_sub->update(&_pos_ctrl_status_time, &pos_ctrl_status);
 
-	if (_fw_pos_ctrl_status_time > 0) {
+	if (_pos_ctrl_status_time > 0) {
 		uint16_t target_distance;
-		convert_limit_safe(fw_pos_ctrl_status.wp_dist * 0.1f, target_distance);
+		convert_limit_safe(pos_ctrl_status.wp_dist * 0.1f, target_distance);
 		msg->target_distance = target_distance;
 	}
 

src/modules/mavlink/mavlink_high_latency2.h

@@ -101,8 +101,8 @@ private:
 	MavlinkOrbSubscription *_estimator_status_sub;
 	uint64_t _estimator_status_time;
 
-	MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
-	uint64_t _fw_pos_ctrl_status_time;
+	MavlinkOrbSubscription *_pos_ctrl_status_sub;
+	uint64_t _pos_ctrl_status_time;
 
 	MavlinkOrbSubscription *_geofence_sub;
 	uint64_t _geofence_time;

src/modules/mavlink/mavlink_messages.cpp

@@ -49,8 +49,8 @@
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_rc_input.h>
 #include <lib/ecl/geo/geo.h>
-#include <mathlib/mathlib.h>
-#include <matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
 #include <px4_time.h>
 #include <systemlib/mavlink_log.h>
 
@@ -69,13 +69,13 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/estimator_status.h>
-#include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include <uORB/topics/optical_flow.h>
+#include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_bias.h>
@@ -3743,15 +3743,15 @@ public:
 
 	unsigned get_size()
 	{
-		return (_fw_pos_ctrl_status_sub->is_published()) ?
+		return (_pos_ctrl_status_sub->is_published()) ?
 		       MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
-	MavlinkOrbSubscription *_fw_pos_ctrl_status_sub;
+	MavlinkOrbSubscription *_pos_ctrl_status_sub;
 	MavlinkOrbSubscription *_tecs_status_sub;
 
-	uint64_t _fw_pos_ctrl_status_timestamp{0};
+	uint64_t _pos_ctrl_status_timestamp{0};
 	uint64_t _tecs_status_timestamp{0};
 
 	/* do not allow top copying this class */
@@ -3760,30 +3760,30 @@ private:
 
 protected:
 	explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))),
+		_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))),
 		_tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status)))
 	{}
 
 	bool send(const hrt_abstime t)
 	{
-		fw_pos_ctrl_status_s _fw_pos_ctrl_status = {};
-		tecs_status_s _tecs_status = {};
+		position_controller_status_s pos_ctrl_status = {};
+		tecs_status_s tecs_status = {};
 
 		bool updated = false;
-		updated |= _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_timestamp, &_fw_pos_ctrl_status);
-		updated |= _tecs_status_sub->update(&_tecs_status_timestamp, &_tecs_status);
+		updated |= _pos_ctrl_status_sub->update(&_pos_ctrl_status_timestamp, &pos_ctrl_status);
+		updated |= _tecs_status_sub->update(&_tecs_status_timestamp, &tecs_status);
 
 		if (updated) {
 			mavlink_nav_controller_output_t msg = {};
 
-			msg.nav_roll = math::degrees(_fw_pos_ctrl_status.nav_roll);
-			msg.nav_pitch = math::degrees(_fw_pos_ctrl_status.nav_pitch);
-			msg.nav_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.nav_bearing);
-			msg.target_bearing = (int16_t)math::degrees(_fw_pos_ctrl_status.target_bearing);
-			msg.wp_dist = (uint16_t)_fw_pos_ctrl_status.wp_dist;
-			msg.xtrack_error = _fw_pos_ctrl_status.xtrack_error;
-			msg.alt_error = _tecs_status.altitude_filtered - _tecs_status.altitude_sp;
-			msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeed_sp;
+			msg.nav_roll = math::degrees(pos_ctrl_status.nav_roll);
+			msg.nav_pitch = math::degrees(pos_ctrl_status.nav_pitch);
+			msg.nav_bearing = (int16_t)math::degrees(pos_ctrl_status.nav_bearing);
+			msg.target_bearing = (int16_t)math::degrees(pos_ctrl_status.target_bearing);
+			msg.wp_dist = (uint16_t)pos_ctrl_status.wp_dist;
+			msg.xtrack_error = pos_ctrl_status.xtrack_error;
+			msg.alt_error = tecs_status.altitude_filtered - tecs_status.altitude_sp;
+			msg.aspd_error = tecs_status.airspeed_filtered - tecs_status.airspeed_sp;
 
 			mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);
 

src/modules/navigator/mission_feasibility_checker.cpp

@@ -47,8 +47,10 @@
 #include <drivers/drv_pwm_output.h>
 #include <fw_pos_control_l1/Landingslope.hpp>
 #include <lib/ecl/geo/geo.h>
-#include <mathlib/mathlib.h>
+#include <lib/mathlib/mathlib.h>
 #include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/position_controller_landing_status.h>
 
 bool
 MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
@@ -395,19 +397,22 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 
 				if (MissionBlock::item_contains_position(missionitem_previous)) {
 
-					const bool fw_status_valid = (_navigator->get_fw_pos_ctrl_status()->timestamp > 0);
+					uORB::Subscription<position_controller_landing_status_s> landing_status{ORB_ID(position_controller_landing_status)};
+					landing_status.forcedUpdate();
+
+					const bool landing_status_valid = (landing_status.get().timestamp > 0);
 					const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
 								  missionitem.lat, missionitem.lon);
 
-					if (fw_status_valid && (wp_distance > _navigator->get_fw_pos_ctrl_status()->landing_flare_length)) {
+					if (landing_status_valid && (wp_distance > landing_status.get().flare_length)) {
 						/* Last wp is before flare region */
 
 						const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
 
 						if (delta_altitude < 0) {
 
-							const float horizontal_slope_displacement = _navigator->get_fw_pos_ctrl_status()->landing_horizontal_slope_displacement;
-							const float slope_angle_rad = _navigator->get_fw_pos_ctrl_status()->landing_slope_angle_rad;
+							const float horizontal_slope_displacement = landing_status.get().horizontal_slope_displacement;
+							const float slope_angle_rad = landing_status.get().slope_angle_rad;
 							const float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
 										    horizontal_slope_displacement, slope_angle_rad);
 

src/modules/navigator/mission_feasibility_checker.h

@@ -43,7 +43,6 @@
 
 #include <dataman/dataman.h>
 #include <uORB/topics/mission.h>
-#include <uORB/topics/fw_pos_ctrl_status.h>
 
 class Geofence;
 class Navigator;

src/modules/navigator/navigator.h

@@ -55,15 +55,17 @@
 #include "rtl.h"
 #include "takeoff.h"
 
-#include <navigator/navigation.h>
-#include <px4_module_params.h>
+#include "navigation.h"
+
+#include <lib/perf/perf_counter.h>
 #include <px4_module.h>
-#include <perf/perf_counter.h>
-#include <uORB/topics/fw_pos_ctrl_status.h>
+#include <px4_module_params.h>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
@@ -145,7 +147,6 @@ public:
 	/**
 	 * Getters
 	 */
-	struct fw_pos_ctrl_status_s *get_fw_pos_ctrl_status() { return &_fw_pos_ctrl_status; }
 	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; }
@@ -278,7 +279,6 @@ public:
 	bool		force_vtol();
 
 private:
-	int		_fw_pos_ctrl_status_sub{-1};	/**< notification of vehicle capabilities updates */
 	int		_global_pos_sub{-1};		/**< global position subscription */
 	int		_gps_pos_sub{-1};		/**< gps position subscription */
 	int		_home_pos_sub{-1};		/**< home position subscription */
@@ -286,6 +286,7 @@ private:
 	int		_local_pos_sub{-1};		/**< local position subscription */
 	int		_offboard_mission_sub{-1};	/**< offboard mission subscription */
 	int		_param_update_sub{-1};		/**< param update subscription */
+	int		_pos_ctrl_landing_status_sub{-1};	/**< position controller landing status subscription */
 	int		_traffic_sub{-1};		/**< traffic subscription */
 	int		_vehicle_command_sub{-1};	/**< vehicle commands (onboard and offboard) */
 	int		_vstatus_sub{-1};		/**< vehicle status subscription */
@@ -299,7 +300,6 @@ private:
 	orb_advert_t	_vehicle_roi_pub{nullptr};
 
 	// Subscriptions
-	fw_pos_ctrl_status_s				_fw_pos_ctrl_status{};	/**< fixed wing navigation capabilities */
 	home_position_s					_home_pos{};		/**< home position for RTL */
 	mission_result_s				_mission_result{};
 	vehicle_global_position_s			_global_pos{};		/**< global vehicle position */
@@ -307,7 +307,11 @@ private:
 	vehicle_land_detected_s				_land_detected{};	/**< vehicle land_detected */
 	vehicle_local_position_s			_local_pos{};		/**< local vehicle position */
 	vehicle_status_s				_vstatus{};		/**< vehicle status */
+
+	uORB::Subscription<position_controller_status_s>	_position_controller_status_sub{ORB_ID(position_controller_status)};
+
 	uint8_t						_previous_nav_state{}; /**< nav_state of the previous iteration*/
+
 	// Publications
 	geofence_result_s				_geofence_result{};
 	position_setpoint_triplet_s			_pos_sp_triplet{};	/**< triplet of position setpoints */
@@ -369,7 +373,6 @@ private:
 	float _mission_throttle{-1.0f};
 
 	// update subscriptions
-	void		fw_pos_ctrl_status_update(bool force = false);
 	void		global_position_update();
 	void		gps_position_update();
 	void		home_position_update(bool force = false);

src/modules/navigator/navigator_main.cpp

@@ -46,26 +46,24 @@
 #include "navigator.h"
 
 #include <cfloat>
-#include <sys/ioctl.h>
 #include <sys/stat.h>
-#include <sys/types.h>
 
 #include <dataman/dataman.h>
 #include <drivers/drv_hrt.h>
 #include <lib/ecl/geo/geo.h>
-#include <mathlib/mathlib.h>
+#include <lib/mathlib/mathlib.h>
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <px4_posix.h>
 #include <px4_tasks.h>
 #include <systemlib/mavlink_log.h>
-#include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
+#include <uORB/topics/position_controller_landing_status.h>
+#include <uORB/topics/transponder_report.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/transponder_report.h>
 #include <uORB/uORB.h>
 
 /**
@@ -77,6 +75,8 @@ extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
 #define GEOFENCE_CHECK_INTERVAL 200000
 
+using namespace time_literals;
+
 namespace navigator
 {
 Navigator	*g_navigator;
@@ -141,17 +141,6 @@ Navigator::home_position_update(bool force)
 	}
 }
 
-void
-Navigator::fw_pos_ctrl_status_update(bool force)
-{
-	bool updated = false;
-	orb_check(_fw_pos_ctrl_status_sub, &updated);
-
-	if (updated || force) {
-		orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
-	}
-}
-
 void
 Navigator::vehicle_status_update()
 {
@@ -193,7 +182,7 @@ Navigator::run()
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
+	_pos_ctrl_landing_status_sub = orb_subscribe(ORB_ID(position_controller_landing_status));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
@@ -209,7 +198,6 @@ Navigator::run()
 	local_position_update();
 	gps_position_update();
 	home_position_update(true);
-	fw_pos_ctrl_status_update(true);
 	params_update();
 
 	/* wakeup source(s) */
@@ -293,11 +281,7 @@ Navigator::run()
 		}
 
 		/* navigation capabilities updated */
-		orb_check(_fw_pos_ctrl_status_sub, &updated);
-
-		if (updated) {
-			fw_pos_ctrl_status_update();
-		}
+		_position_controller_status_sub.update();
 
 		/* home position updated */
 		orb_check(_home_pos_sub, &updated);
@@ -762,14 +746,11 @@ Navigator::run()
 		}
 
 		if (_pos_sp_triplet_updated) {
-			_pos_sp_triplet.timestamp = hrt_absolute_time();
 			publish_position_setpoint_triplet();
-			_pos_sp_triplet_updated = false;
 		}
 
 		if (_mission_result_updated) {
 			publish_mission_result();
-			_mission_result_updated = false;
 		}
 
 		perf_end(_loop_perf);
@@ -778,7 +759,7 @@ Navigator::run()
 	orb_unsubscribe(_global_pos_sub);
 	orb_unsubscribe(_local_pos_sub);
 	orb_unsubscribe(_gps_pos_sub);
-	orb_unsubscribe(_fw_pos_ctrl_status_sub);
+	orb_unsubscribe(_pos_ctrl_landing_status_sub);
 	orb_unsubscribe(_vstatus_sub);
 	orb_unsubscribe(_land_detected_sub);
 	orb_unsubscribe(_home_pos_sub);
@@ -827,11 +808,13 @@ Navigator::print_status()
 void
 Navigator::publish_position_setpoint_triplet()
 {
-	/* do not publish an empty triplet */
+	// do not publish an invalid setpoint
 	if (!_pos_sp_triplet.current.valid) {
 		return;
 	}
 
+	_pos_sp_triplet.timestamp = hrt_absolute_time();
+
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub != nullptr) {
 		orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
@@ -839,6 +822,8 @@ Navigator::publish_position_setpoint_triplet()
 	} else {
 		_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
 	}
+
+	_pos_sp_triplet_updated = false;
 }
 
 float
@@ -949,10 +934,11 @@ Navigator::get_acceptance_radius(float mission_item_radius)
 	// when in fixed wing mode
 	// this might need locking against a commanded transition
 	// so that a stale _vstatus doesn't trigger an accepted mission item.
-	if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode) {
-		if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 5000000) && (_fw_pos_ctrl_status.turn_distance > radius)) {
-			radius = _fw_pos_ctrl_status.turn_distance;
-		}
+
+	const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
+
+	if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && pos_ctrl_status.acceptance_radius > radius) {
+		radius = pos_ctrl_status.acceptance_radius;
 	}
 
 	return radius;
@@ -1114,15 +1100,24 @@ void Navigator::check_traffic()
 bool
 Navigator::abort_landing()
 {
+	// only abort if currently landing and position controller status updated
 	bool should_abort = false;
 
-	if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode) {
-		if (hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 1000000) {
+	if (_pos_sp_triplet.current.valid
+	    && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 
-			if (get_position_setpoint_triplet()->current.valid
-			    && get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+		bool updated = false;
+
+		orb_check(_pos_ctrl_landing_status_sub, &updated);
+
+		if (updated) {
+			position_controller_landing_status_s landing_status = {};
 
-				should_abort = _fw_pos_ctrl_status.abort_landing;
+			// landing status from position controller must be newer than navigator's last position setpoint
+			if (orb_copy(ORB_ID(position_controller_landing_status), _pos_ctrl_landing_status_sub, &landing_status) == PX4_OK) {
+				if (landing_status.timestamp > _pos_sp_triplet.timestamp) {
+					should_abort = landing_status.abort_landing;
+				}
 			}
 		}
 	}
@@ -1214,6 +1209,8 @@ Navigator::publish_mission_result()
 	_mission_result.item_do_jump_changed = false;
 	_mission_result.item_changed_index = 0;
 	_mission_result.item_do_jump_remaining = 0;
+
+	_mission_result_updated = false;
 }
 
 void