FW Positon Controller: set references to 0 if not provided by local_p…

…osition (PX4#22101) * FW Positon Controller: set altitude_ref to 0 if not provided by GPS Signed-off-by: Silvan Fuhrer <silvan@auterion.com> * FW Positon Controller: set lat/lon reference to 0 if not provided in local_position Signed-off-by: Silvan Fuhrer <silvan@auterion.com> --------- Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
MaEtUgR · Sep 20, 2023 · ec7db4b · ec7db4b
1 parent 7ff00db
commit ec7db4b
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 5 deletions.
diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
@@ -2144,6 +2144,13 @@ FixedwingPositionControl::Run()
 			_current_longitude = gpos.lon;
 		}
 
+		if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) {
+			_reference_altitude = _local_pos.ref_alt;
+
+		} else {
+			_reference_altitude = 0.f;
+		}
+
 		_current_altitude = -_local_pos.z + _local_pos.ref_alt; // Altitude AMSL in meters
 
 		// handle estimator reset events. we only adjust setpoins for manual modes
@@ -2171,9 +2178,16 @@ FixedwingPositionControl::Run()
 		    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
 		    || (_local_pos.vxy_reset_counter != _pos_reset_counter)) {
 
-			_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
+			double reference_latitude = 0.;
+			double reference_longitude = 0.;
+
+			if (_local_pos.xy_global && PX4_ISFINITE(_local_pos.ref_lat) && PX4_ISFINITE(_local_pos.ref_lon)) {
+				reference_latitude = _local_pos.ref_lat;
+				reference_longitude = _local_pos.ref_lon;
+			}
+
+			_global_local_proj_ref.initReference(reference_latitude, reference_longitude,
 							     _local_pos.ref_timestamp);
-			_global_local_alt0 = _local_pos.ref_alt;
 		}
 
 		if (_control_mode.flag_control_offboard_enabled) {
@@ -2202,7 +2216,7 @@ FixedwingPositionControl::Run()
 						_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.position[2];
+						_pos_sp_triplet.current.alt = _reference_altitude - trajectory_setpoint.position[2];
 					}
 
 				}
@@ -2782,7 +2796,7 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo
 
 	local_position_setpoint.x = current_setpoint(0);
 	local_position_setpoint.y = current_setpoint(1);
-	local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt;
+	local_position_setpoint.z = _reference_altitude - current_waypoint.alt;
 	local_position_setpoint.yaw = NAN;
 	local_position_setpoint.yawspeed = NAN;
 	local_position_setpoint.vx = NAN;

diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp
@@ -266,7 +266,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	float _body_velocity_x{0.f};
 
 	MapProjection _global_local_proj_ref{};
-	float _global_local_alt0{NAN};
+
+	float _reference_altitude{NAN}; // [m AMSL] altitude of the local projection reference point
 
 	bool _landed{true};