FlightTask: use centralized _dist_to_ground computed in FlightTask instead of

recomputing it in AutoMapper, AutoMapper2 and AutoLine

src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp

@@ -214,7 +214,7 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
 
 		// limit vertical downwards speed (positive z) close to ground
 		// for now we use the altitude above home and assume that we want to land at same height as we took off
-		float vel_limit = math::gradual(_alt_above_ground,
+		float vel_limit = math::gradual(_dist_to_ground,
 						_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
 						_param_mpc_land_speed.get(), _constraints.speed_down);
 

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -52,8 +52,6 @@ bool FlightTaskAutoMapper::update()
 	// always reset constraints because they might change depending on the type
 	_setDefaultConstraints();
 
-	_updateAltitudeAboveGround();
-
 	bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position;
 	bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position;
 
@@ -137,7 +135,7 @@ void FlightTaskAutoMapper::_generateTakeoffSetpoints()
 	_velocity_setpoint = Vector3f(NAN, NAN, NAN);
 
 	// limit vertical speed during takeoff
-	_constraints.speed_up = math::gradual(_alt_above_ground, _param_mpc_land_alt2.get(),
+	_constraints.speed_up = math::gradual(_dist_to_ground, _param_mpc_land_alt2.get(),
 					      _param_mpc_land_alt1.get(), _param_mpc_tko_speed.get(), _constraints.speed_up);
 
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
@@ -152,21 +150,6 @@ void FlightTaskAutoMapper::_generateVelocitySetpoints()
 	_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
 }
 
-void FlightTaskAutoMapper::_updateAltitudeAboveGround()
-{
-	// Altitude above ground is by default just the negation of the current local position in D-direction.
-	_alt_above_ground = -_position(2);
-
-	if (PX4_ISFINITE(_dist_to_bottom)) {
-		// We have a valid distance to ground measurement
-		_alt_above_ground = _dist_to_bottom;
-
-	} else if (_sub_home_position.get().valid_alt) {
-		// if home position is set, then altitude above ground is relative to the home position
-		_alt_above_ground = -_position(2) + _sub_home_position.get().z;
-	}
-}
-
 void FlightTaskAutoMapper::updateParams()
 {
 	FlightTaskAuto::updateParams();
@@ -178,5 +161,5 @@ void FlightTaskAutoMapper::updateParams()
 bool FlightTaskAutoMapper::_highEnoughForLandingGear()
 {
 	// return true if altitude is above two meters
-	return _alt_above_ground > 2.0f;
+	return _dist_to_ground > 2.0f;
 }

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -59,11 +59,8 @@ protected:
 	void _generateVelocitySetpoints();
 	void _generateTakeoffSetpoints();
 
-	void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */
 	void updateParams() override; /**< See ModuleParam class */
 
-	float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
-
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
 					(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,

src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -52,8 +52,6 @@ bool FlightTaskAutoMapper2::update()
 	// always reset constraints because they might change depending on the type
 	_setDefaultConstraints();
 
-	_updateAltitudeAboveGround();
-
 	// The only time a thrust set-point is sent out is during
 	// idle. Hence, reset thrust set-point to NAN in case the
 	// vehicle exits idle.
@@ -144,7 +142,7 @@ void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
 {
 	// Takeoff is completely defined by target position
 	_position_setpoint = _target;
-	const float speed_tko = (_alt_above_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up :
+	const float speed_tko = (_dist_to_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up :
 				_param_mpc_tko_speed.get();
 	_velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed
 
@@ -167,21 +165,6 @@ void FlightTaskAutoMapper2::_preparePositionSetpoints()
 	_velocity_setpoint = Vector3f(NAN, NAN, NAN); // No special velocity limitations
 }
 
-void FlightTaskAutoMapper2::_updateAltitudeAboveGround()
-{
-	// Altitude above ground is by default just the negation of the current local position in D-direction.
-	_alt_above_ground = -_position(2);
-
-	if (PX4_ISFINITE(_dist_to_bottom)) {
-		// We have a valid distance to ground measurement
-		_alt_above_ground = _dist_to_bottom;
-
-	} else if (_sub_home_position.get().valid_alt) {
-		// if home position is set, then altitude above ground is relative to the home position
-		_alt_above_ground = -_position(2) + _sub_home_position.get().z;
-	}
-}
-
 void FlightTaskAutoMapper2::updateParams()
 {
 	FlightTaskAuto::updateParams();
@@ -193,7 +176,7 @@ void FlightTaskAutoMapper2::updateParams()
 bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
 {
 	// return true if altitude is above two meters
-	return _alt_above_ground > 2.0f;
+	return _dist_to_ground > 2.0f;
 }
 
 float FlightTaskAutoMapper2::_getLandSpeed()
@@ -209,7 +192,7 @@ float FlightTaskAutoMapper2::_getLandSpeed()
 
 	float speed = 0;
 
-	if (_alt_above_ground > _param_mpc_land_alt1.get()) {
+	if (_dist_to_ground > _param_mpc_land_alt1.get()) {
 		speed = _constraints.speed_down;
 
 	} else {

src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp

@@ -60,11 +60,8 @@ protected:
 	void _prepareTakeoffSetpoints();
 	void _preparePositionSetpoints();
 
-	void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */
 	void updateParams() override; /**< See ModuleParam class */
 
-	float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
-
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
 					(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

@@ -164,9 +164,9 @@ void FlightTask::_evaluateVehicleLocalPosition()
 
 void FlightTask::_evaluateDistanceToGround()
 {
-	_dist_to_ground = NAN;
+	// Altitude above ground is by default just the negation of the current local position in D-direction.
+	_dist_to_ground = -_position(2);
 
-	// if there is a valid distance to bottom or vertical distance to home
 	if (PX4_ISFINITE(_dist_to_bottom)) {
 		_dist_to_ground = _dist_to_bottom;