addressing PR review

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

@@ -61,7 +61,7 @@ bool FlightTaskAutoFollowTarget::activate(const vehicle_local_position_setpoint_
 
 	// Initialize to something such that the drone at least points at the target, even if it's the wrong angle for the perspective.
 	// The drone will move into position as soon as the target starts moving and its heading becomes known.
-	matrix::Vector2f current_drone_heading_2d{cos(_yaw), -sin(_yaw)};
+	matrix::Vector2f current_drone_heading_2d{cosf(_yaw), -sinf(_yaw)};
 
 	if (PX4_ISFINITE(current_drone_heading_2d(0)) && PX4_ISFINITE(current_drone_heading_2d(1))) {
 		_offset_vector_filtered.reset(current_drone_heading_2d);
@@ -154,7 +154,7 @@ bool FlightTaskAutoFollowTarget::update()
 				_target_velocity_unit_vector = matrix::Vector2f(v_ned_est.xy()).unit_or_zero();
 			}
 
-			const double follow_angle_rad = (double)_follow_angle_filtered.getState() * M_DEG_TO_RAD;
+			const float follow_angle_rad = math::radians(_follow_angle_filtered.getState());
 			float offset_x = (float)cos(follow_angle_rad) * _target_velocity_unit_vector(0) - (float)sin(
 						 follow_angle_rad) * _target_velocity_unit_vector(1);
 			float offset_y = (float)sin(follow_angle_rad) * _target_velocity_unit_vector(0) + (float)cos(
@@ -218,7 +218,7 @@ bool FlightTaskAutoFollowTarget::update()
 		} else {
 			// Control setpoint: Stay in current position
 			_position_setpoint = _position;
-			_velocity_setpoint = matrix::Vector3f{0, 0, 0};
+			_velocity_setpoint.setAll(0.0f);
 		}
 
 		_velocity_ff_scale.setParameters(_deltatime, VELOCITY_FF_FILTER_ALPHA);

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

@@ -51,28 +51,28 @@
 #include <uORB/topics/follow_target_estimator.h>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
 
-#define MINIMUM_SPEED_FOR_HEADING_CHANGE 0.1f 	// Speed above which the target heading can change. Used to prevent unpredictable jitter at low speeds.
-#define MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL 1.0f  // Minimum distance between drone and target for the drone to do any yaw control.
-#define MINIMUM_SAFETY_ALTITUDE 1.0f  // Minimum safety altitude above home (or bottom distance sensor) underneath which the flight task will stop moving horizontally
-#define ALT_ACCEPTANCE_THRESHOLD 3.0f // [m] max vertical deviation from position setpoint, above which no horizontal control is done
-#define EMERGENCY_ASCENT_SPEED 0.2f
+static constexpr float MINIMUM_SPEED_FOR_HEADING_CHANGE = 0.1f; 		// Speed above which the target heading can change. Used to prevent unpredictable jitter at low speeds.
+static constexpr float MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL = 1.0f;  	// Minimum distance between drone and target for the drone to do any yaw control.
+static constexpr float MINIMUM_SAFETY_ALTITUDE = 1.0f;  			// Minimum safety altitude above home (or bottom distance sensor) underneath which the flight task will stop moving horizontally
+static constexpr float ALT_ACCEPTANCE_THRESHOLD = 3.0f; 			// [m] max vertical deviation from position setpoint, above which no horizontal control is done
+static constexpr float EMERGENCY_ASCENT_SPEED = 0.2f;
 
 // Filter gains for the different kinds of septoints
 // that should ensure a cinematic experience
-#define POSITION_FILTER_ALPHA 1.5f		// Lowpass applied to the estimated position of the target before using it as control input
-#define FOLLOW_ANGLE_FILTER_ALPHA 3.0f		// Lowpass applied to the follow-me angle setting, to ensure smooth and circular transitions between settings
-#define DIRECTION_FILTER_ALPHA 3.0f		// Lowpass applied to the actual NED direction how the drone is facing the target regarless of the setting. Used for dynamic tracking angles when the target makes a turn
-#define VELOCITY_FF_FILTER_ALPHA 1.0f		// Lowpass applied for ramping up / down velocity feedforward term
+static constexpr float POSITION_FILTER_ALPHA = 1.5f;		// Lowpass applied to the estimated position of the target before using it as control input
+static constexpr float FOLLOW_ANGLE_FILTER_ALPHA = 3.0f;	// Lowpass applied to the follow-me angle setting, to ensure smooth and circular transitions between settings
+static constexpr float DIRECTION_FILTER_ALPHA = 3.0f;		// Lowpass applied to the actual NED direction how the drone is facing the target regarless of the setting. Used for dynamic tracking angles when the target makes a turn
+static constexpr float VELOCITY_FF_FILTER_ALPHA = 1.0f;	// Lowpass applied for ramping up / down velocity feedforward term
 
 // Angles [deg] for the different follow-me perspectives
-#define FOLLOW_PERSPECTIVE_BEHIND_ANGLE_DEG 180.0
-#define FOLLOW_PERSPECTIVE_FRONT_ANGLE_DEG 0.0
-#define FOLLOW_PERSPECTIVE_FRONT_RIGHT_ANGLE_DEG 45.0
-#define FOLLOW_PERSPECTIVE_FRONT_LEFT_ANGLE_DEG 315.0
-#define FOLLOW_PERSPECTIVE_MID_RIGHT_ANGLE_DEG 90.0
-#define FOLLOW_PERSPECTIVE_MID_LEFT_ANGLE_DEG 270.0
-#define FOLLOW_PERSPECTIVE_BEHIND_RIGHT_ANGLE_DEG 135.0
-#define FOLLOW_PERSPECTIVE_BEHIND_LEFT_ANGLE_DEG 225.0
+static constexpr float FOLLOW_PERSPECTIVE_BEHIND_ANGLE_DEG = 180.0;
+static constexpr float FOLLOW_PERSPECTIVE_FRONT_ANGLE_DEG = 0.0;
+static constexpr float FOLLOW_PERSPECTIVE_FRONT_RIGHT_ANGLE_DEG = 45.0;
+static constexpr float FOLLOW_PERSPECTIVE_FRONT_LEFT_ANGLE_DEG = 315.0;
+static constexpr float FOLLOW_PERSPECTIVE_MID_RIGHT_ANGLE_DEG = 90.0;
+static constexpr float FOLLOW_PERSPECTIVE_MID_LEFT_ANGLE_DEG = 270.0;
+static constexpr float FOLLOW_PERSPECTIVE_BEHIND_RIGHT_ANGLE_DEG = 135.0;
+static constexpr float FOLLOW_PERSPECTIVE_BEHIND_LEFT_ANGLE_DEG = 225.0;
 
 
 class FlightTaskAutoFollowTarget : public FlightTask

src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c

@@ -87,12 +87,8 @@ PARAM_DEFINE_INT32(NAV_FT_FS, 1);
 
 /**
  * Dynamic filtering algorithm responsiveness to target movement
-<<<<<<< HEAD:src/modules/navigator/follow_target_params.c
  *
- * lower numbers increase the responsiveness to changing long lat
-=======
  * lower values increase the responsiveness to changing long lat
->>>>>>> e95e66c534... New follow-me flight task:src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c
  * but also ignore less noise
  *
  * @min 0.0

src/modules/follow_target_estimator/TargetEstimator.cpp

@@ -97,8 +97,8 @@ void TargetEstimator::update()
 	// Keep position estimate as the last known position
 	// but stop moving the estimate
 	if (is_stale(GPS_MESSAGE_STALE_TIMEOUT_MS)) {
-		_filter_states.v_ned_est = matrix::Vector3f{0.0f, 0.0f, 0.0f};
-		_filter_states.a_ned_est = matrix::Vector3f{0.0f, 0.0f, 0.0f};
+		_filter_states.v_ned_est.setAll(0.0f);
+		_filter_states.a_ned_est.setAll(0.0f);
 	}
 
 	const bool states_are_finite = _filter_states.is_finite();
@@ -185,7 +185,7 @@ void TargetEstimator::measurement_update(follow_target_s follow_target)
 	if (_last_follow_target_timestamp == 0) {
 		_filter_states.x_ned_est = x_measured;
 		_filter_states.v_ned_est = v_measured;
-		_filter_states.a_ned_est = {0.0f, 0.0f, 0.0f};
+		_filter_states.a_ned_est.setAll(0.0f);
 	}
 
 	_last_follow_target_timestamp = follow_target.timestamp;
@@ -218,7 +218,7 @@ void TargetEstimator::measurement_update(follow_target_s follow_target)
 
 	if (gps_vel_stale) {
 		// Reset XY-acceleration while GPS is stale to avoid filter runoffs
-		_filter_states.a_ned_est = matrix::Vector3f{0.0f, 0.0f, 0.0f};
+		_filter_states.a_ned_est.setAll(0.0f);
 	}
 
 	if (vel_fusion_old_enough && ((target_moving && !gps_vel_stale) || !target_moving)) {
@@ -282,10 +282,9 @@ void TargetEstimator::prediction_update(float deltatime)
 	}
 }
 
-
 matrix::Vector3<double> TargetEstimator::get_lat_lon_alt_est() const
 {
-	matrix::Vector3<double> lat_lon_alt{NAN, NAN, NAN};
+	matrix::Vector3<double> lat_lon_alt{(double)NAN, (double)NAN, (double)NAN};
 
 	if (PX4_ISFINITE(_filter_states.x_ned_est(0)) && PX4_ISFINITE(_filter_states.x_ned_est(0))) {
 		map_projection_reproject(&_reference_position, _filter_states.x_ned_est(0), _filter_states.x_ned_est(1),
@@ -303,15 +302,14 @@ bool TargetEstimator::is_stale(const float timeout_duration_ms) const
 	return measurements_stale;
 }
 
-
 void TargetEstimator::reset()
 {
 	_last_filter_reset_timestamp = hrt_absolute_time();  // debug only
 	_last_position_fusion_timestamp = _last_velocity_fusion_timestamp = 0;
 	_last_follow_target_timestamp = 0;
-	_filter_states.x_ned_est = {NAN, NAN, NAN};
-	_filter_states.v_ned_est = {NAN, NAN, NAN};
-	_filter_states.a_ned_est = {NAN, NAN, NAN};
-	_x_measurement_old = {NAN, NAN, NAN};
-	_v_measurement_old = {NAN, NAN, NAN};
+	_filter_states.x_ned_est.setAll(NAN);
+	_filter_states.v_ned_est.setAll(NAN);
+	_filter_states.a_ned_est.setAll(NAN);
+	_x_measurement_old.setAll(NAN);
+	_v_measurement_old.setAll(NAN);
 }

src/modules/follow_target_estimator/TargetEstimator.hpp

@@ -53,20 +53,23 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 
-#define GPS_MESSAGE_STALE_TIMEOUT_MS 3000  	// Duration after which the connection to the target is considered lost
-#define MINIMUM_TIME_BETWEEN_POS_FUSIONS_MS 500
-#define MINIMUM_TIME_BETWEEN_VEL_FUSIONS_MS 100
-#define ACCELERATION_SATURATION 20.0f 		// 2*g
-#define MINIMUM_SPEED_FOR_TARGET_MOVING 0.1f 	// speed threshold above which the target is considered to be moving
+static constexpr float GPS_MESSAGE_STALE_TIMEOUT_MS = 3000.0f;  	// Duration after which the connection to the target is considered lost
+static constexpr float MINIMUM_TIME_BETWEEN_POS_FUSIONS_MS = 500.0f;
+static constexpr float MINIMUM_TIME_BETWEEN_VEL_FUSIONS_MS = 100.0f;
+static constexpr float ACCELERATION_SATURATION = 20.0f; 		// 2*g
+static constexpr float MINIMUM_SPEED_FOR_TARGET_MOVING = 0.1f; 	// speed threshold above which the target is considered to be moving
 
 using namespace time_literals;
 
 struct filter_gains_s {
 	// Position fusion gains
-	float G_p, H_p, K_p;
+	float G_p;
+	float H_p;
+	float K_p;
 
 	// Velocity fusion gains
-	float G_v, H_v;
+	float G_v;
+	float H_v;
 
 	// Estimator position / velocity update gains
 	float responsiveness;