fix(mavlink): restore fixed-wing navigation telemetry

msg/FixedWingLateralGuidanceStatus.msg

@@ -10,5 +10,6 @@ float32 bearing_feas_on_track   # [@range 0,1] on-track bearing feasibility
 float32 signed_track_error      # [m] signed track error
 float32 track_error_bound       # [m] track error bound
 float32 switch_distance         # [m] [@range 0, INF] [@invalid NaN] distance from the current waypoint at which the navigator should advance to the next one (turn anticipation). If below NAV_ACC_RAD, the parameter value is used instead.
+float32 wp_dist                 # [m] [@range 0, INF] distance to the active waypoint
 float32 adapted_period          # [s] adapted period (if auto-tuning enabled)
 uint8 wind_est_valid            # [boolean] true = wind estimate is valid and/or being used by controller (also indicates if wind estimate usage is disabled despite being valid)

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -2807,6 +2807,11 @@ void FixedWingModeManager::publish_lateral_guidance_status(const hrt_abstime now
 	fixed_wing_lateral_guidance_status.adapted_period = _directional_guidance.getAdaptedPeriod();
 	fixed_wing_lateral_guidance_status.wind_est_valid = _wind_valid;
 
+	if (_position_setpoint_current_valid) {
+		fixed_wing_lateral_guidance_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude,
+				_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
+	}
+
 	_fixed_wing_lateral_guidance_status_pub.publish(fixed_wing_lateral_guidance_status);
 }
 

src/modules/mavlink/streams/HIGH_LATENCY2.hpp

@@ -43,6 +43,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/fixed_wing_lateral_guidance_status.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/position_controller_status.h>
@@ -340,6 +341,15 @@ class MavlinkStreamHighLatency2 : public MavlinkStream
 			return true;
 		}
 
+		fixed_wing_lateral_guidance_status_s fw_lateral_guidance_status;
+
+		if (_fw_lateral_guidance_status_sub.update(&fw_lateral_guidance_status)) {
+			uint16_t target_distance;
+			convert_limit_safe(fw_lateral_guidance_status.wp_dist * 0.1f, target_distance);
+			msg->target_distance = target_distance;
+			return true;
+		}
+
 		return false;
 	}
 
@@ -672,6 +682,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream
 	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
 	uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)};
+	uORB::Subscription _fw_lateral_guidance_status_sub{ORB_ID(fixed_wing_lateral_guidance_status)};
 	uORB::Subscription _geofence_sub{ORB_ID(geofence_result)};
 	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};

src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp

@@ -34,6 +34,7 @@
 #ifndef NAV_CONTROLLER_OUTPUT_HPP
 #define NAV_CONTROLLER_OUTPUT_HPP
 
+#include <uORB/topics/fixed_wing_lateral_guidance_status.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/tecs_status.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -51,14 +52,16 @@ class MavlinkStreamNavControllerOutput : public MavlinkStream
 
 	unsigned get_size() override
 	{
-		return _position_controller_status_sub.advertised() ? MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN +
+		return (_position_controller_status_sub.advertised() || _fw_lateral_guidance_status_sub.advertised()) ?
+		       MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN +
 		       MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
 	explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
 	uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)};
+	uORB::Subscription _fw_lateral_guidance_status_sub{ORB_ID(fixed_wing_lateral_guidance_status)};
 	uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 
@@ -90,6 +93,28 @@ class MavlinkStreamNavControllerOutput : public MavlinkStream
 			return true;
 		}
 
+		fixed_wing_lateral_guidance_status_s fw_lateral_guidance_status;
+
+		if (_fw_lateral_guidance_status_sub.update(&fw_lateral_guidance_status)) {
+			tecs_status_s tecs_status{};
+			_tecs_status_sub.copy(&tecs_status);
+
+			vehicle_global_position_s vehicle_global_position{};
+			_vehicle_global_position_sub.copy(&vehicle_global_position);
+
+			mavlink_nav_controller_output_t msg{};
+
+			msg.nav_bearing = roundf(math::degrees(fw_lateral_guidance_status.course_setpoint));
+			msg.wp_dist = math::constrain(roundf(fw_lateral_guidance_status.wp_dist), 0.f, (float)UINT16_MAX);
+			msg.xtrack_error = fw_lateral_guidance_status.signed_track_error;
+			msg.alt_error = tecs_status.altitude_sp - vehicle_global_position.alt;
+			msg.aspd_error = tecs_status.true_airspeed_filtered - tecs_status.true_airspeed_sp;
+
+			mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
 		return false;
 	}
 };