From 29d4ad1848725d05d7b2edf9e3aaeaaaff982187 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 16 Mar 2021 00:58:04 -0400
Subject: [PATCH] fw_pos_control_l1: OFFBOARD fully populate
 position_setpoint_triplet

---
 .../FixedwingPositionControl.cpp              | 67 ++++++++++++-------
 1 file changed, 42 insertions(+), 25 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 7efea241ec..d75aeb2b2e 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -1687,9 +1687,48 @@ FixedwingPositionControl::Run()
 		_alt_reset_counter = _local_pos.vz_reset_counter;
 		_pos_reset_counter = _local_pos.vxy_reset_counter;
 
-		if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
-			// reset the altitude foh (first order hold) logic
-			_min_current_sp_distance_xy = FLT_MAX;
+
+		if (_control_mode.flag_control_offboard_enabled) {
+			// Convert Local setpoints to global setpoints
+			if (!map_projection_initialized(&_global_local_proj_ref)
+			    || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
+
+				map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
+								_local_pos.ref_timestamp);
+				_global_local_alt0 = _local_pos.ref_alt;
+			}
+
+			vehicle_local_position_setpoint_s trajectory_setpoint;
+
+			if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
+				if (PX4_ISFINITE(trajectory_setpoint.x) && PX4_ISFINITE(trajectory_setpoint.y) && PX4_ISFINITE(trajectory_setpoint.z)) {
+					double lat;
+					double lon;
+
+					if (map_projection_reproject(&_global_local_proj_ref, trajectory_setpoint.x, trajectory_setpoint.y, &lat, &lon) == 0) {
+						_pos_sp_triplet = {}; // clear any existing
+
+						_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
+						_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
+						_pos_sp_triplet.current.valid = true;
+						_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+						_pos_sp_triplet.current.lat = lat;
+						_pos_sp_triplet.current.lon = lon;
+						_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
+						_pos_sp_triplet.current.cruising_speed = NAN; // ignored
+						_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
+					}
+
+				} else {
+					mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint");
+				}
+			}
+
+		} else {
+			if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
+				// reset the altitude foh (first order hold) logic
+				_min_current_sp_distance_xy = FLT_MAX;
+			}
 		}
 
 		airspeed_poll();
@@ -1711,28 +1750,6 @@ FixedwingPositionControl::Run()
 		Vector2d curr_pos(_current_latitude, _current_longitude);
 		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
 
-		// Convert Local setpoints to global setpoints
-		if (_control_mode.flag_control_offboard_enabled) {
-			if (!map_projection_initialized(&_global_local_proj_ref)
-			    || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
-
-				map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
-								_local_pos.ref_timestamp);
-				_global_local_alt0 = _local_pos.ref_alt;
-			}
-
-			vehicle_local_position_setpoint_s trajectory_setpoint;
-
-			if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
-				map_projection_reproject(&_global_local_proj_ref,
-							 trajectory_setpoint.x, trajectory_setpoint.y,
-							 &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon);
-
-				_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
-				_pos_sp_triplet.current.valid = true;
-			}
-		}
-
 		/*
 		 * Attempt to control position, on success (= sensors present and not in manual mode),
 		 * publish setpoint.