FollowMe : Replace First order target position filter with Second order position and velocity filter

Follow me : tidied second order filter implementation

Added velocity filtered info to uORB follow target status message, and rebase to potaito/new-follow-me-task

FollowMe : Rebasing and missing definition fixes on target position second order filter

Follow Me : Remove Alpha filter delay compensation code, since second order filter is used for pose filtering now

Followme : Remove unused target pose estimation update function

Follow Target : Added Target orientation estimation logic

Follow Target : Replaced offset vector based setpoint to rate controlled orbital angle control

Follow Target : Bug fixes and first working version of rate based control logic, still buggy

Follow Target : Added target orientation, follow angle, orbit angle value into follow_target_status uORB message for debugging

Follow Target : Fix orbit angle step calculation typo bug

Follow Target : Few more fixes

Follow Target : Few fixes and follow angle value logging bug fix

Follow Target : Added lowpass alpha filter for yaw setpoint filtering

Follow Target : Remove unused filter delay compensation param

Follow Target : Add Yaw setpoint filter initialization logic and bufix for when unwrap had an input of NAN angle value

Follow Target : Add Yaw setpoint filtering enabler parameter

Follow Target : Change Target Velocity Deadzone to 1.0 m/s, to accomodate walking speed of 1.5 m/s

Follow Target : Add Orbit Tangential Velocity calculation for velocity setpoint and logging uORB topics

Follow target : Fix indentation in yaw setpoint filtering logic

Follow Target : Fix Follow Target Estimator timeout logic bug that was making the 2nd order pose filter reset to raw value every loop

Follow Target : Remove debug printf statement for target pose filter reset check

Follow Target : Add pose filter natural frequency parameter for filter testing

Follow Target : Make target following side param selectable and add target pose filter natural frequency param description

Follow Target : Add Terrain following altitude mode and make 2D altitude mode keep altitude relative to the home position, instead of raw local position origin

Follow Target : Log follow target estimator & status at full rate for filter characteristics test

Follow Target : Implementing RC control user input for Follow Target control

Follow Target : edit to conform to updated unwrap_pi function

Follow Target : Make follow angle, distance and height RC command input configurable

Follow Target : Make Follow Target not interruptable by moving joystick in Commander

Follow Target : reconfigure yaw, pitch and roll for better user experience in RC adjusting configurations, and add angular rate limit taking target distance into account

Follow Target : Change RC channels used for adjustments and re-order header file for clarity

Follow Target : Fix Parameters custom parent macro, since using DEFINE_PARAMETERS alone misses the parameter updates of the parent class, FlightTask

Follow Target : Fix Orbit Tangential speed actually being angular rate bug, which was causing a phenomenon of drnoe getting 'dragged' towards the target velocity direction

Follow Target : Final tidying and refactoring for master merge - step 1

Add more comments on header file

Follow Target : tidy, remove unnecessary debug uORB elements from follow_target_status message

Follow Target : Turn off Yaw filtering by default

Follow Target : Tidy maximum orbital velocity calculation

Follow Target : add yaw setpoint filter time constant parameter for testing and fix NAV_FT_HT title

Follow Target : Add RC adjustment window logic to prevent drone not catching up the change of follow target parameters

Follow Target : fixes

Follow Target: PR tidy few edits remove, and update comments

Follow Target : apply comments and reviews

Follow Target : Edit according to review comments part 2

Follow Target : Split RC adjustment code and other refactors

- Splitted the RC adjustment into follow angle, height and distance
- Added Parameter change detection to reset the follow properties
- Added comments and removed yaw setpoint filter enabler logic

Follow Target : Modify orbit angle error bufferzone bug that was causing excessive velocity setpoints when setpoint catched up with raw orbit setpoint

Follow Target : Remove buffer zone velocity ramp down logic and add acceleration and rate limited Trajectory generation library for orbit angle and velocity setpoint

Follow Target : Remove internally tracked data from local scope function's parameters to simplify code

Follow Target : Fix to track unwrapped orbit angle, with no wrapping

Follow Target : Apply user adjustment deadzone to reduce sensitivity

Follow Target : Apply suggestions from PR review round 2 by @potaito

Revert submodule update changes, fall back to potaito/new-followme-task

Follow Target : [Debug] Expose max vel and acceleration settings as parameters, instead of using Multicopter Position Controller
's settings

Follow Target : Use matrix::isEqualF() function to compare floats

Follow Target : Add Acceleration feedback enabler parameter and Velocity ramp in initial commit for overshoot phenomenon improvement

Follow Target : Implement Velocity feed forward limit and debug logging values

Follow Target : Apply Velocity Ramp-in for Acceleration as well & Apply it to total velocity setpoint and not just orbit tangential velocity component

Follow Target : Don't set Acceleration setpoint if not commanded

Follow Target : Use Jerk limited orbit angle control. Add orbit angle tracking related uORB values"

Follow Target : Add Orbit Angle Setpoint Rate Tracking filter, to take into consideration for calculating velocity setpoint for trajectory generator for orbit angle

Revert "Follow Target : Add Orbit Angle Setpoint Rate Tracking filter, to take into consideration for calculating velocity setpoint for trajectory generator for orbit angle"

This reverts commit a3f48ac7652adb70baf3a2fed3ea34d77cbd6a70.

Follow Target : Take Unfiltered target velocity into acount for target course calculation to fix overshoot orbit angle 180 deg flip problem

Follow Target : Remove Yaw Filter since it doesn't make a big difference in yaw jitterness

Follow Target : Remove velocity ramp in control & remove debug values from follow_target_status.msg

Follow Target : Tidy Follow Target Status message logging code

Follow Target : Remove jerk and acceleration settings from Follow Target orbit trajectory generation

Follow Target : Change PublicationMulti into Publication, since topics published are single instances

Follow Target : Edit comments to reflect changes in the final revision of Follow Target

Follow Target : Apply incorrectly merged conflicts during rebase & update Sticks function usage for getThrottled()

Follow Target : Apply final review comments before merge into Alessandro's PR

Apply further changes from the PR review, like units

Use RC Sticks' Expo() function for user adjustments to decrease sensitivity around the center (0 value)

Update Function styles to lowerCamelCase

And make functions const & return the params, rather than modifying them
internally via pointer / reference

Specify kFollowPerspective enum as uint8_t, so that it can't be set to negative value when converted from the parameter 'FLW_TGT_FP'

Fix bug in updateParams() to reset internally tracked params if they actually changed.

Remove unnecessary comments

Fix format of the Follow Target code

Fix Follow Perspective Param metadata

follow-me: use new trajectory_setpoint msg

Convert FollowPerspective enum into a Follow Angle float value

1. Increases flexibility in user's side, to set any arbitrary follow
angle [deg]
2. Removes the need to have a dedicated Enum, which can be a hassle to
make it match MAVSDK's side
3. A step in the direction of adding a proper Follow Mode (Perspective)
mode support, where we can support kite mode (drone behaves as if it is
hovering & getting dragged by the target with a leash) or a constant
orbit angle mode (Drone always on the East / North / etc. side, for
cinematic shots)

Continue fixing Follow Target MAVSDK code to match MAVSDK changes

- Support Follow Angle configuration instead of Follow Direction
- Change follow position tolerance logic to use the follow angle
*Still work in progress!

Update Follow Me MAVSDK Test Code to match MAVSDK v2 spec

- Add RC Adjustment Test case
- Change follow direction logic to follow angle based logic completely
- Cleanup on variable names and comment on code

follow-me: disable SITL test

Need to update MAVSDK with the following PR:
https://github.com/mavlink/MAVSDK/pull/1770

SITL is failing now because the follow-me
perspectives are no longer defined the
same way in MAVSDK and in the flight task.

update copyright year

follow-me: mark uORB topics optional

Apply review comments

more copyright years

follow-me sitl test: simpler "state machine"

flight_mode_manager: exclude AutoFollowTarget and Orbit on flash contrained boards

Remove unnecessary follow_target_status message properties

- As it eats up FLASH and consumes uLog bandwidth

msg/follow_target_status.msg

@@ -1,5 +1,12 @@
-uint64 timestamp             # time since system start (microseconds)
+uint64 timestamp                  # [microseconds] time since system start
 
-float32[3] pos_est_filtered  # Lowpass-filtered target position used for vehicle's setpoint generation
+float32 tracked_target_course     # [rad] Tracked target course in NED local frame (North is course zero)
-bool emergency_ascent
+float32 follow_angle              # [rad] Current follow angle setting
-float32 gimbal_pitch
+
+float32 orbit_angle_setpoint      # [rad] Current orbit angle setpoint from the smooth trajectory generator
+float32 angular_rate_setpoint     # [rad/s] Angular rate commanded from Jerk-limited Orbit Angle trajectory for Orbit Angle
+
+float32[3] desired_position_raw   # [m] Raw 'idealistic' desired drone position if a drone could teleport from place to places
+
+bool in_emergency_ascent          # [bool] True when doing emergency ascent (when distance to ground is below safety altitude)
+float32 gimbal_pitch              # [rad] Gimbal pitch commanded to track target in the center of the frame

src/modules/commander/Commander.cpp

@@ -3376,7 +3376,6 @@ Commander::update_control_mode()
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
@@ -3444,6 +3443,10 @@ Commander::update_control_mode()
 
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
+
+	// Follow Target supports RC adjustment, so disable auto control mode to disable
+	// the Flight Task from exiting itself when RC stick movement is detected.
 	case vehicle_status_s::NAVIGATION_STATE_ORBIT:
 		_vehicle_control_mode.flag_control_manual_enabled = false;
 		_vehicle_control_mode.flag_control_auto_enabled = false;

src/modules/flight_mode_manager/CMakeLists.txt

@@ -36,24 +36,10 @@
 # Prepare flight tasks
 ###########################################
 
-# add upstream flight tasks (they are being handled differently from the core inside the python script)
-list(APPEND flight_tasks_to_add
-	Orbit
-)
-# remove possible duplicates
-list(REMOVE_DUPLICATES flight_tasks_to_add)
-
-# remove flight tasks depending on target
-if(flight_tasks_to_remove)
-	list(REMOVE_ITEM flight_tasks_to_add
-		${flight_tasks_to_remove}
-	)
-endif()
-
 # add core flight tasks to list
+set(flight_tasks_all)
 list(APPEND flight_tasks_all
 	Auto
-	AutoFollowTarget
 	Descend
 	Failsafe
 	ManualAcceleration
@@ -62,9 +48,15 @@ list(APPEND flight_tasks_all
 	ManualPosition
 	ManualPositionSmoothVel
 	Transition
-	${flight_tasks_to_add}
 )
 
+if(NOT px4_constrained_flash_build)
+	list(APPEND flight_tasks_all
+		AutoFollowTarget
+		Orbit
+	)
+endif()
+
 # set the files to be generated
 set(files_to_generate
 	FlightTasks_generated.hpp

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -200,7 +200,11 @@ void FlightModeManager::start_flight_task()
 	// Auto-follow me
 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
 		should_disable_task = false;
-		FlightTaskError error = switchTask(FlightTaskIndex::AutoFollowTarget);
+		FlightTaskError error = FlightTaskError::InvalidTask;
+
+#if !defined(CONSTRAINED_FLASH)
+		error = switchTask(FlightTaskIndex::AutoFollowTarget);
+#endif // !CONSTRAINED_FLASH
 
 		if (error != FlightTaskError::NoError) {
 			if (prev_failure_count == 0) {

src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,277 +43,392 @@
 #include "FlightTaskAutoFollowTarget.hpp"
 #include <mathlib/mathlib.h>
 #include <lib/matrix/matrix/math.hpp>
+#include <float.h>
 
 using matrix::Vector2f;
 using matrix::Vector3f;
 using matrix::Quatf;
 using matrix::Eulerf;
 
-FlightTaskAutoFollowTarget::FlightTaskAutoFollowTarget()
+// Call the constructor of _sticks to establish Follow Target class as a parent as ModuleParams class
+// which enables chained paramter update sequence, which keeps the parameters in _sticks up to date.
+FlightTaskAutoFollowTarget::FlightTaskAutoFollowTarget() : _sticks(this)
 {
 	_target_estimator.Start();
 }
 
 FlightTaskAutoFollowTarget::~FlightTaskAutoFollowTarget()
 {
-	release_gimbal_control();
+	releaseGimbalControl();
 	_target_estimator.Stop();
 }
 
-bool FlightTaskAutoFollowTarget::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+bool FlightTaskAutoFollowTarget::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 
-	_position_setpoint = _position;
-
 	if (!PX4_ISFINITE(_position_setpoint(0)) || !PX4_ISFINITE(_position_setpoint(1))
 	    || !PX4_ISFINITE(_position_setpoint(2))) {
 		_position_setpoint = _position;
 	}
 
-	_target_position_filter.reset(Vector3f{NAN, NAN, NAN});
+	_target_position_velocity_filter.reset(Vector3f{NAN, NAN, NAN});
-	_offset_vector_filter.reset(Vector2f(0, 0));
+	_target_position_velocity_filter.setParameters(TARGET_POS_VEL_FILTER_NATURAL_FREQUENCY,
-	_follow_angle_filter.reset(0.0f);
+			TARGET_POS_VEL_FILTER_DAMPING_RATIO);
-	_velocity_ff_scale.reset(0.0f);
 
-	// Initialize to something such that the drone at least points at the target, even if it's the wrong angle for the perspective.
+	_yaw_setpoint = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
-	// The drone will move into position as soon as the target starts moving and its heading becomes known.
+	_yawspeed_setpoint = NAN;
-	Vector2f current_drone_heading_2d{cosf(_yaw), -sinf(_yaw)};
 
-	if (PX4_ISFINITE(current_drone_heading_2d(0)) && PX4_ISFINITE(current_drone_heading_2d(1))) {
+	// Update the internally tracked Follow Target characteristics
-		_offset_vector_filter.reset(current_drone_heading_2d);
+	_follow_angle_rad = matrix::wrap_pi(math::radians(_param_flw_tgt_fa.get()));
+	_follow_distance = _param_flw_tgt_dst.get();
+	_follow_height = _param_flw_tgt_ht.get();
+
+	// Reset the orbit angle trajectory generator and set maximum angular acceleration and rate
+	_orbit_angle_traj_generator.reset(0.f, 0.f, 0.f);
+
+	// Save the home position z value to enable relative altitude setpoints to arming position (home) altitude
+	if (_sub_home_position.get().valid_alt) {
+		_home_position_z = _sub_home_position.get().z;
 
 	} else {
-		_offset_vector_filter.reset(Vector2f(1.0f, 0.0f));
+		ret = false; // Don't activate Follow Target task if home position is not valid
 	}
 
-	_yawspeed_setpoint = 0.f;
-
 	return ret;
 }
 
-Vector2f FlightTaskAutoFollowTarget::calculate_offset_vector_filtered(Vector3f vel_ned_est)
+void FlightTaskAutoFollowTarget::updateParams()
 {
-	if (_param_nav_ft_fs.get() == FOLLOW_PERSPECTIVE_NONE) {
+	// Copy previous param values to check if it changes after param update
-		// NOTE: Switching between NONE any any other setting currently causes a jump in the setpoints
+	const float follow_distance_prev = _param_flw_tgt_dst.get();
-		_offset_vector_filter.reset(Vector2f{0, 0});
+	const float follow_height_prev = _param_flw_tgt_ht.get();
+	const float follow_angle_prev = _param_flw_tgt_fa.get();
 
-	} else {
+	// Call updateParams in parent class to update parameters from child up until ModuleParam base class
-		// Define and rotate offset vector based on follow-me perspective setting
+	FlightTask::updateParams();
-		const float new_follow_angle_rad = math::radians(update_follow_me_angle_setting(_param_nav_ft_fs.get()));
 
-		// Use shortest rotation to get to the new angle
+	// Compare param values and if they have changed, update the properties
-		// Example: if the current angle setting is 270, and the new angle setting is 0, it's
+	if (!matrix::isEqualF(follow_distance_prev, _param_flw_tgt_dst.get())) {
-		// faster to rotate to 360 rather than 0.
+		_follow_distance = _param_flw_tgt_dst.get();
-		// Usually the controller would automatically take the shortest path. But here some trickery
-		// is necessary because the yaw angle is run through a low-pass filter.
-		if (_follow_angle_rad - new_follow_angle_rad  > M_PI_F) {
-			_follow_angle_rad = new_follow_angle_rad + M_TWOPI_F;
-
-		} else if (_follow_angle_rad - new_follow_angle_rad  < -M_PI_F) {
-			_follow_angle_rad = new_follow_angle_rad - M_TWOPI_F;
-
-		} else {
-			_follow_angle_rad = new_follow_angle_rad;
-		}
-
-		// Lowpass the angle setting to smoothly transition to a new perspective when the user makes a change.
-		// In particular this has an effect when the setting is modified by 180 degrees, in which case the drone
-		// would pass above the target without the filter. The filtering makes it so that the drone flies around
-		// the target into the new postion.
-		_follow_angle_filter.setParameters(_deltatime, FOLLOW_ANGLE_FILTER_ALPHA);
-		_follow_angle_filter.update(_follow_angle_rad);
-
-		// Wrap around 360 degrees
-		if (_follow_angle_filter.getState() > M_TWOPI_F) {
-			_follow_angle_filter.reset(_follow_angle_filter.getState() - M_TWOPI_F);
-			_follow_angle_rad = _follow_angle_rad - M_TWOPI_F;
-
-		} else if (_follow_angle_filter.getState() < -M_TWOPI_F) {
-			_follow_angle_filter.reset(_follow_angle_filter.getState() + M_TWOPI_F);
-			_follow_angle_rad = _follow_angle_rad + M_TWOPI_F;
-		}
-
-		// Assume the target's velocity vector is its heading and use it to construct the offset vector
-		// such that drone_pos_setpoint = target_pose + offset_vector
-		if (vel_ned_est.longerThan(MINIMUM_SPEED_FOR_HEADING_CHANGE) &&
-		    vel_ned_est.longerThan(FLT_EPSILON)) {
-			// Compute offset vector relative to target position. At the same time the offset vector defines the
-			// vieweing angle of the drone
-			_target_velocity_unit_vector = Vector2f(vel_ned_est.xy()).unit_or_zero();
-		}
-
-		float offset_x = (float)cos(_follow_angle_filter.getState()) * _target_velocity_unit_vector(0) - (float)sin(
-					 _follow_angle_filter.getState()) * _target_velocity_unit_vector(1);
-		float offset_y = (float)sin(_follow_angle_filter.getState()) * _target_velocity_unit_vector(0) + (float)cos(
-					 _follow_angle_filter.getState()) * _target_velocity_unit_vector(1);
-
-		// Lowpass on the offset vector to have smooth transitions when the target turns, or when the
-		// setting for the perspective is changed by the user. This introduces only a delay in the
-		// tracking / viewing angle without disadvantages
-		_offset_vector_filter.setParameters(_deltatime, DIRECTION_FILTER_ALPHA);
-		_offset_vector_filter.update(Vector2f{offset_x, offset_y});
 	}
 
-	return _offset_vector_filter.getState();
+	if (!matrix::isEqualF(follow_height_prev, _param_flw_tgt_ht.get())) {
+		_follow_height = _param_flw_tgt_ht.get();
+	}
+
+	if (!matrix::isEqualF(follow_angle_prev, _param_flw_tgt_fa.get())) {
+		_follow_angle_rad = matrix::wrap_pi(math::radians(_param_flw_tgt_fa.get()));
+	}
 }
 
-Vector3f FlightTaskAutoFollowTarget::calculate_drone_desired_position(Vector3f target_position, Vector2f offset_vector)
+void FlightTaskAutoFollowTarget::updateTargetPositionVelocityFilter(const follow_target_estimator_s
+		&follow_target_estimator)
+{
+	const Vector3f pos_ned_est{follow_target_estimator.pos_est};
+	const Vector3f vel_ned_est{follow_target_estimator.vel_est};
+
+	const bool filtered_target_pos_is_finite = PX4_ISFINITE(_target_position_velocity_filter.getState()(0))
+			&& PX4_ISFINITE(_target_position_velocity_filter.getState()(1))
+			&& PX4_ISFINITE(_target_position_velocity_filter.getState()(2));
+	const bool filtered_target_vel_is_finite = PX4_ISFINITE(_target_position_velocity_filter.getRate()(0))
+			&& PX4_ISFINITE(_target_position_velocity_filter.getRate()(1))
+			&& PX4_ISFINITE(_target_position_velocity_filter.getRate()(2));
+
+	const bool target_estimator_timed_out = ((follow_target_estimator.timestamp - _last_valid_target_estimator_timestamp) >
+						TARGET_ESTIMATOR_TIMEOUT_US);
+	_last_valid_target_estimator_timestamp = follow_target_estimator.timestamp;
+
+	if (!filtered_target_pos_is_finite || !filtered_target_vel_is_finite || target_estimator_timed_out) {
+		_target_position_velocity_filter.reset(pos_ned_est, vel_ned_est);
+
+	} else {
+		_target_position_velocity_filter.update(_deltatime, pos_ned_est, vel_ned_est);
+	}
+}
+
+void FlightTaskAutoFollowTarget::updateRcAdjustedFollowHeight(const Sticks &sticks)
+{
+	// Only apply Follow height adjustment if height setpoint and current height are within time window
+	if (fabsf(_position_setpoint(2) - _position(2)) < FOLLOW_HEIGHT_USER_ADJUST_SPEED * USER_ADJUSTMENT_ERROR_TIME_WINDOW) {
+		// RC Throttle stick input for changing follow height
+		const float height_change_speed = FOLLOW_HEIGHT_USER_ADJUST_SPEED * sticks.getThrottleZeroCenteredExpo();
+		const float new_height = _follow_height + height_change_speed * _deltatime;
+		_follow_height = constrain(new_height, MINIMUM_SAFETY_ALTITUDE, FOLLOW_HEIGHT_MAX);
+	}
+}
+
+void FlightTaskAutoFollowTarget::updateRcAdjustedFollowDistance(const Sticks &sticks,
+		const Vector2f &drone_to_target_vector)
+{
+	// Only apply Follow distance adjustment if distance setting and current distance are within time window
+	if (fabsf(drone_to_target_vector.length() - _follow_distance) < FOLLOW_DISTANCE_USER_ADJUST_SPEED *
+	    USER_ADJUSTMENT_ERROR_TIME_WINDOW) {
+		// RC Pitch stick input for changing distance
+		const float distance_change_speed = FOLLOW_DISTANCE_USER_ADJUST_SPEED * sticks.getPitchExpo();
+		const float new_distance = _follow_distance + distance_change_speed * _deltatime;
+		_follow_distance = constrain(new_distance, MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL, FOLLOW_DISTANCE_MAX);
+	}
+}
+
+void FlightTaskAutoFollowTarget::updateRcAdjustedFollowAngle(const Sticks &sticks, const float measured_orbit_angle,
+		const float tracked_orbit_angle_setpoint)
+{
+	// Only apply Follow Angle adjustment if orbit angle setpoint and current orbit angle are within time window
+	// Wrap orbit angle difference, to get the shortest angle between them
+	if (fabsf(matrix::wrap_pi(measured_orbit_angle - tracked_orbit_angle_setpoint)) < FOLLOW_ANGLE_USER_ADJUST_SPEED *
+	    USER_ADJUSTMENT_ERROR_TIME_WINDOW) {
+		// RC Roll stick input for changing follow angle. When user commands RC stick input: +Roll (right), angle increases (clockwise)
+		// Constrain adjust speed [rad/s] so that drone can actually catch up. Otherwise, the follow angle
+		// command can be too ahead that drone's behavior would be un-responsive to RC stick inputs.
+		const float angle_adjust_speed_max = min(FOLLOW_ANGLE_USER_ADJUST_SPEED,
+						     _param_flw_tgt_max_vel.get() / _follow_distance);
+		const float angle_change_speed = angle_adjust_speed_max * sticks.getRollExpo();
+		const float new_angle = _follow_angle_rad + angle_change_speed * _deltatime;
+		_follow_angle_rad = matrix::wrap_pi(new_angle);
+	}
+}
+
+float FlightTaskAutoFollowTarget::updateTargetOrientation(const float current_target_orientation,
+		const Vector2f &target_velocity, const Vector2f &target_velocity_unfiltered) const
+{
+	// Only update the target orientation if both filtered & unfiltered target velocity are above the deadzone velocity.
+	// This prevents unintuitive follow position setpoints derived from overshooting filtered target velocity, and thus target course
+	// when the target abruptly stops.
+	if ((target_velocity.norm() >= TARGET_SPEED_DEADZONE_FOR_ORIENTATION_TRACKING)
+	    && (target_velocity_unfiltered.norm() >= TARGET_SPEED_DEADZONE_FOR_ORIENTATION_TRACKING)) {
+		return atan2f(target_velocity(1), target_velocity(0));
+	}
+
+	return current_target_orientation;
+}
+
+float FlightTaskAutoFollowTarget::updateOrbitAngleTrajectory(const float target_orientation,
+		const float previous_orbit_angle_setpoint)
+{
+	// Raw target orbit (setpoint) angle, unwrapped to be relative to the previous orbit angle setpoint
+	const float unwrapped_raw_orbit_angle = matrix::unwrap_pi(previous_orbit_angle_setpoint,
+						target_orientation + _follow_angle_rad);
+
+	// Calculate limits for orbit angular acceleration and velocity rate
+	_orbit_angle_traj_generator.setMaxJerk(ORBIT_TRAJECTORY_MAX_JERK / _follow_distance);
+	_orbit_angle_traj_generator.setMaxAccel(ORBIT_TRAJECTORY_MAX_ACCELERATION / _follow_distance);
+	_orbit_angle_traj_generator.setMaxVel(_param_flw_tgt_max_vel.get() / _follow_distance);
+
+	// Calculate the maximum angular rate setpoint based on remaining angle to raw target
+	const float remaining_angle = unwrapped_raw_orbit_angle - previous_orbit_angle_setpoint;
+	const float remaining_angle_sign = matrix::sign(remaining_angle);
+	const float max_rate = math::trajectory::computeMaxSpeedFromDistance(_orbit_angle_traj_generator.getMaxJerk(),
+			       _orbit_angle_traj_generator.getMaxAccel(), fabsf(remaining_angle), 0.f);
+
+	// Set angular rate setpoint, considering the sign direction
+	_orbit_angle_traj_generator.updateDurations(max_rate * remaining_angle_sign);
+
+	// Calculate trajectory towards the unwrapped target orbit angle
+	_orbit_angle_traj_generator.updateTraj(_deltatime);
+
+	const float unwrapped_orbit_angle_setpoint = _orbit_angle_traj_generator.getCurrentPosition();
+
+	return unwrapped_orbit_angle_setpoint;
+}
+
+Vector2f FlightTaskAutoFollowTarget::getOrbitTangentialVelocity(const float orbit_angle_setpoint) const
+{
+	const float angular_rate_setpoint = _orbit_angle_traj_generator.getCurrentVelocity();
+	return Vector2f(-sinf(orbit_angle_setpoint), cosf(orbit_angle_setpoint)) * angular_rate_setpoint * _follow_distance;
+}
+
+Vector3f FlightTaskAutoFollowTarget::calculateDesiredDronePosition(const Vector3f &target_position,
+		const float orbit_angle_setpoint) const
 {
 	Vector3f drone_desired_position{NAN, NAN, NAN};
 
-	// Correct the desired distance by the target scale determined from object detection
+	// Offset from the Target
-	const float desired_distance_to_target = _param_nav_ft_dst.get();
+	const Vector2f offset_vector = Vector2f(cosf(orbit_angle_setpoint), sinf(orbit_angle_setpoint)) * _follow_distance;
-	drone_desired_position.xy() = Vector2f(target_position.xy()) +
+
-				      offset_vector.unit_or_zero() * desired_distance_to_target;
+	drone_desired_position.xy() = Vector2f(target_position.xy()) + offset_vector;
+
+	// Calculate ground's z value in local frame for terrain tracking mode.
+	// _dist_to_ground value takes care of distance sensor value if available, otherwise
+	// it uses home z value as reference
+	const float ground_z_estimate = _position(2) + _dist_to_ground;
 
 	// Z-position based off curent and initial target altitude
-	// TODO: Parameter NAV_MIN_FT_HT has been repurposed to be used as the desired
+	switch ((kFollowAltitudeMode)_param_flw_tgt_alt_m.get()) {
-	// altitude above the target. Clarify best solution.
+	case kFollowAltitudeModeTerrain:
-	switch (_param_nav_ft_alt_m.get()) {
+		drone_desired_position(2) = ground_z_estimate - _follow_height;
-	case FOLLOW_ALTITUDE_MODE_TRACK_TARGET:
-		drone_desired_position(2) = target_position(2) - _param_nav_min_ft_ht.get();
 		break;
 
-	case FOLLOW_ALTITUDE_MODE_CONSTANT:
+	case kFollowAltitudeModeTrackTarget:
+		drone_desired_position(2) = target_position(2) - _follow_height;
+		break;
+
+	case kFollowAltitudeModeConstant:
 
 	// FALLTHROUGH
 
 	default:
-		// use the current position setpoint, unless it's closer to the ground than the minimum
+		// Calculate the desired Z position relative to the home position
-		// altitude setting
+		drone_desired_position(2) = _home_position_z - _follow_height;
-		drone_desired_position(2) = math::min(_position_setpoint(2), -_param_nav_min_ft_ht.get());
 	}
 
 	return drone_desired_position;
 }
 
-Vector3f FlightTaskAutoFollowTarget::calculate_target_position_filtered(Vector3f pos_ned_est, Vector3f vel_ned_est,
+
-		Vector3f acc_ned_est)
+float FlightTaskAutoFollowTarget::calculateGimbalHeight(const kFollowAltitudeMode altitude_mode,
+		const float target_pos_z) const
 {
-	// Reset the smoothness filter once the target position estimate is available
+	float gimbal_height{0.0f};
-	if (!PX4_ISFINITE(_target_position_filter.getState()(0)) || !PX4_ISFINITE(_target_position_filter.getState()(1))
+
-	    || !PX4_ISFINITE(_target_position_filter.getState()(2))) {
+	switch (altitude_mode) {
-		_target_position_filter.reset(pos_ned_est);
+	case kFollowAltitudeModeTerrain:
+		// Point the gimbal at the ground level in this tracking mode
+		gimbal_height = _dist_to_ground;
+		break;
+
+	case kFollowAltitudeModeTrackTarget:
+		// Point the gimbal at the target's 3D coordinates
+		gimbal_height = -(_position(2) - target_pos_z);
+		break;
+
+	case kFollowAltitudeModeConstant:
+
+	//FALLTHROUGH
+
+	default:
+		gimbal_height = _home_position_z - _position(2); // Assume target is at home position's altitude
+		break;
 	}
 
-	// Low-pass filter on target position.
+	return gimbal_height;
-	_target_position_filter.setParameters(_deltatime, POSITION_FILTER_ALPHA);
-
-	if (_param_nav_ft_delc.get() == 0) {
-		_target_position_filter.update(pos_ned_est);
-
-	} else {
-		// Use a predicted target's position to compensate the filter delay to some extent.
-		const Vector3f target_predicted_position = predict_future_pos_ned_est(POSITION_FILTER_ALPHA, pos_ned_est, vel_ned_est,
-				acc_ned_est);
-		_target_position_filter.update(target_predicted_position);
-	}
-
-	return _target_position_filter.getState();
-
 }
 
+
 bool FlightTaskAutoFollowTarget::update()
 {
-	_follow_target_estimator_sub.update(&_follow_target_estimator);
 	follow_target_status_s follow_target_status{};
+	bool in_emergency_ascent{false};
+	float gimbal_pitch{NAN};
+	float raw_orbit_angle_setpoint{NAN};
+
+	// Get the latest target estimator message for target position and velocity
+	_follow_target_estimator_sub.update(&_follow_target_estimator);
 
 	if (_follow_target_estimator.timestamp > 0 && _follow_target_estimator.valid) {
-		const Vector3f pos_ned_est{_follow_target_estimator.pos_est};
+		updateTargetPositionVelocityFilter(_follow_target_estimator);
-		const Vector3f vel_ned_est{_follow_target_estimator.vel_est};
+		const Vector3f target_position_filtered = _target_position_velocity_filter.getState();
-		const Vector3f acc_ned_est{_follow_target_estimator.acc_est};
+		const Vector3f target_velocity_filtered = _target_position_velocity_filter.getRate();
-		const Vector3f target_position_filtered = calculate_target_position_filtered(pos_ned_est, vel_ned_est, acc_ned_est);
+		const Vector2f drone_to_target_vector  = Vector2f(target_position_filtered.xy()) - Vector2f(_position.xy());
-		const Vector2f offset_vector_filtered = calculate_offset_vector_filtered(vel_ned_est);
-		const Vector3f drone_desired_position = calculate_drone_desired_position(target_position_filtered,
-							offset_vector_filtered);
 
-		// Set position and velocity setpoints
+		// Calculate heading to the target (for Yaw setpoint)
-		float desired_velocity_ff_scale = 0.0f;  // Used to ramp up velocity feedforward, avoiding harsh jumps in the setpoints
+		const float drone_to_target_heading = atan2f(drone_to_target_vector(1), drone_to_target_vector(0));
+		// Actual orbit angle measured around the target, which is pointing from target to drone, so M_PI_F difference.
+		const float measured_orbit_angle = matrix::wrap_pi(drone_to_target_heading + M_PI_F);
 
+		// Update the sticks object to fetch recent data and update follow distance, angle and height via RC commands
+		_sticks.checkAndUpdateStickInputs();
+
+		if (_sticks.isAvailable()) {
+			updateRcAdjustedFollowHeight(_sticks);
+			updateRcAdjustedFollowDistance(_sticks, drone_to_target_vector);
+			updateRcAdjustedFollowAngle(_sticks, measured_orbit_angle, _orbit_angle_setpoint_rad);
+		}
+
+		_target_course_rad = updateTargetOrientation(_target_course_rad, target_velocity_filtered.xy(),
+				     Vector3f(_follow_target_estimator.vel_est).xy());
+
+		// [Debug] Save raw idealistic orbit angle setpoint for debug message
+		raw_orbit_angle_setpoint = matrix::unwrap_pi(_orbit_angle_setpoint_rad, _target_course_rad + _follow_angle_rad);
+
+		// Update the new orbit angle (rate constrained)
+		_orbit_angle_setpoint_rad = updateOrbitAngleTrajectory(_target_course_rad, _orbit_angle_setpoint_rad);
+
+		// Orbital tangential velocity setpoint from the generated trajectory
+		const Vector2f orbit_tangential_velocity = getOrbitTangentialVelocity(_orbit_angle_setpoint_rad);
+
+		// Calculate desired position by applying orbit angle around the target
+		const Vector3f drone_desired_position = calculateDesiredDronePosition(target_position_filtered,
+							_orbit_angle_setpoint_rad);
+
+		// NOTE : Currently if Follow Target is activated when the drone is very far away from the target, since trajectory setpoint's velocity & acceleration
+		// components are directly applied in the Multicopter-Position-Controller, it can command orbiting velocity / acceleration setpoints even when the
+		// drone hasn't reached the target orbit angle setpoint yet. Which will interfere with the drone coming straight towards the orbit angle setpoint.
+		// This has to be dealed with in the Multicopter-Rate-Controller, where it should not command acceleration feed forward (setpoint) directly if there's a big pos/vel error.
+
+		// Calculate orbit acceleration
+		const Vector2f orbit_radial_accel = (orbit_tangential_velocity.norm_squared() / _follow_distance) * Vector2f(-cosf(
+				_orbit_angle_setpoint_rad), -sinf(_orbit_angle_setpoint_rad));
+		const Vector2f orbit_tangential_accel = _orbit_angle_traj_generator.getCurrentAcceleration() * _follow_distance *
+							Vector2f(-sinf(_orbit_angle_setpoint_rad), cosf(_orbit_angle_setpoint_rad));
+		const Vector2f orbit_total_accel = orbit_radial_accel + orbit_tangential_accel;
+
+		// Position + Velocity + Acceleration setpoint
 		if (PX4_ISFINITE(drone_desired_position(0)) && PX4_ISFINITE(drone_desired_position(1))
 		    && PX4_ISFINITE(drone_desired_position(2))) {
-			// Only control horizontally if drone is on target altitude to avoid accidents
 			if (fabsf(drone_desired_position(2) - _position(2)) < ALT_ACCEPTANCE_THRESHOLD) {
-
+				// Drone is close enough to the altitude target : Apply Horizontal + Velocity Control
-				// Only apply feed-forward velocity while the target is moving
-				if (vel_ned_est.longerThan(MINIMUM_SPEED_FOR_HEADING_CHANGE)) {
-					desired_velocity_ff_scale = 1.0f;
-
-				}
-
-				// Velocity setpoints is a feedforward term derived from position setpoints
-				_velocity_setpoint = (drone_desired_position - _position_setpoint) / _deltatime * _velocity_ff_scale.getState();
 				_position_setpoint = drone_desired_position;
+				_velocity_setpoint.xy() = orbit_tangential_velocity + target_velocity_filtered.xy();
+				_acceleration_setpoint.xy() = orbit_total_accel;
 
 			} else {
-				// Achieve target altitude first before controlling horizontally!
+				// Drone hasn't achieved desired altitude yet : Only apply Vertical Control
 				_position_setpoint = _position;
 				_position_setpoint(2) = drone_desired_position(2);
 			}
 
 		} else {
-			// Control setpoint: Stay in current position
+			// Desired position is not finite : Don't apply any control
 			_position_setpoint = _position;
 			_velocity_setpoint.setZero();
+			_acceleration_setpoint.setNaN();
 		}
 
-		_velocity_ff_scale.setParameters(_deltatime, VELOCITY_FF_FILTER_ALPHA);
+		// Yaw setpoint
-		_velocity_ff_scale.update(desired_velocity_ff_scale);
+		if (drone_to_target_vector.longerThan(MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL)) {
+			_yaw_setpoint = drone_to_target_heading;
+		}
+
+		// Set Gimbal pitch to track target in the center of the view
+		const float gimbal_height = calculateGimbalHeight((kFollowAltitudeMode)_param_flw_tgt_alt_m.get(),
+					    target_position_filtered(2));
+		gimbal_pitch = pointGimbalAt(drone_to_target_vector.norm(), gimbal_height);
 
 		// Emergency ascent when too close to the ground
-		_emergency_ascent = PX4_ISFINITE(_dist_to_ground) && _dist_to_ground < MINIMUM_SAFETY_ALTITUDE;
+		in_emergency_ascent = PX4_ISFINITE(_dist_to_ground) && _dist_to_ground < MINIMUM_SAFETY_ALTITUDE;
 
-		if (_emergency_ascent) {
+		if (in_emergency_ascent) {
 			_position_setpoint(0) = _position_setpoint(1) = NAN;
 			_position_setpoint(2) = _position(2);
 			_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
 			_velocity_setpoint(2) = -EMERGENCY_ASCENT_SPEED; // Slowly ascend
 		}
 
-		// Yaw setpoint: Face the target
-		const Vector2f target_to_drone_xy = Vector2f(_position.xy()) - Vector2f(
-				target_position_filtered.xy());
-
-		if (target_to_drone_xy.longerThan(MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL)) {
-			_yaw_setpoint = atan2f(-target_to_drone_xy(1), -target_to_drone_xy(0));
-		}
-
-		// Gimbal setpoint
-		float gimbal_height = 0;
-
-		switch (_param_nav_ft_gmb_m.get()) {
-		case FOLLOW_GIMBAL_MODE_2D:
-			gimbal_height = -_position(2);
-			break;
-
-		case FOLLOW_GIMBAL_MODE_3D:
-			// Point the gimbal at the target's 3D coordinates
-			gimbal_height = -(_position(2) - (target_position_filtered(2)));
-			break;
-
-		case FOLLOW_GIMBAL_MODE_2D_WITH_TERRAIN:
-			// Point the gimbal at the ground level in this tracking mode
-			gimbal_height = _dist_to_ground;
-			break;
-
-		default:
-			break;
-		}
-
-		point_gimbal_at(target_to_drone_xy.norm(), gimbal_height);
-
 	} else {
-		// Control setpoint: Stay in current position
+		// Target estimate is invalid : Stay in current position
-		_position_setpoint(0) = _position_setpoint(1) = NAN;
+		_position_setpoint.setAll(NAN);
-		_velocity_setpoint.xy() = 0;
+		_velocity_setpoint.setAll(0.0f);
 	}
 
-	// Publish status message for debugging
+	// Follow Target Status message for debugging
-	_target_position_filter.getState().copyTo(follow_target_status.pos_est_filtered);
 	follow_target_status.timestamp = hrt_absolute_time();
-	follow_target_status.emergency_ascent = _emergency_ascent;
+
-	follow_target_status.gimbal_pitch = _gimbal_pitch;
+	// Log angle values to check target course estimate & orbit angle trajectory control
+	follow_target_status.tracked_target_course = _target_course_rad;
+	follow_target_status.follow_angle = _follow_angle_rad;
+	follow_target_status.orbit_angle_setpoint = _orbit_angle_setpoint_rad;
+
+	// Log max & actual orbit angular rates to check orbit anglular rate trajectory control
+	follow_target_status.angular_rate_setpoint = _orbit_angle_traj_generator.getCurrentVelocity();
+
+	// Log desired Raw Follow position to easily check ideal vs actual follow me performance
+	const Vector3f drone_desired_position_raw = calculateDesiredDronePosition(_target_position_velocity_filter.getState(),
+			raw_orbit_angle_setpoint);
+	drone_desired_position_raw.copyTo(follow_target_status.desired_position_raw);
+
+	follow_target_status.in_emergency_ascent = in_emergency_ascent;
+	follow_target_status.gimbal_pitch = gimbal_pitch;
 	_follow_target_status_pub.publish(follow_target_status);
 
 	_constraints.want_takeoff = _checkTakeoff();
@@ -321,52 +436,7 @@ bool FlightTaskAutoFollowTarget::update()
 	return true;
 }
 
-float FlightTaskAutoFollowTarget::update_follow_me_angle_setting(int param_nav_ft_fs) const
+void FlightTaskAutoFollowTarget::releaseGimbalControl()
-{
-	switch (param_nav_ft_fs) {
-	case FOLLOW_PERSPECTIVE_BEHIND:
-		return FOLLOW_PERSPECTIVE_BEHIND_ANGLE_DEG;
-
-	case FOLLOW_PERSPECTIVE_FRONT:
-		return FOLLOW_PERSPECTIVE_FRONT_ANGLE_DEG;
-
-	case FOLLOW_PERSPECTIVE_FRONT_RIGHT:
-		return FOLLOW_PERSPECTIVE_FRONT_RIGHT_ANGLE_DEG;
-
-	case FOLLOW_PERSPECTIVE_FRONT_LEFT:
-		return FOLLOW_PERSPECTIVE_FRONT_LEFT_ANGLE_DEG;
-
-	case FOLLOW_PERSPECTIVE_MID_RIGHT:
-		return FOLLOW_PERSPECTIVE_MID_RIGHT_ANGLE_DEG;
-
-	case FOLLOW_PERSPECTIVE_MID_LEFT:
-		return FOLLOW_PERSPECTIVE_MID_LEFT_ANGLE_DEG;
-
-	case FOLLOW_PERSPECTIVE_BEHIND_RIGHT:
-		return FOLLOW_PERSPECTIVE_BEHIND_RIGHT_ANGLE_DEG;
-
-	case FOLLOW_PERSPECTIVE_BEHIND_LEFT:
-		return FOLLOW_PERSPECTIVE_BEHIND_LEFT_ANGLE_DEG;
-
-	case FOLLOW_PERSPECTIVE_MIDDLE_FOLLOW:
-		return FOLLOW_PERSPECTIVE_BEHIND_ANGLE_DEG;
-
-	default:
-		// No or invalid option
-		break;
-	}
-
-	// Default: follow from behind
-	return FOLLOW_PERSPECTIVE_BEHIND_ANGLE_DEG;
-}
-
-Vector3f FlightTaskAutoFollowTarget::predict_future_pos_ned_est(float deltatime, const Vector3f &pos_ned_est,
-		const Vector3f &vel_ned_est, const Vector3f &acc_ned_est) const
-{
-	return pos_ned_est + vel_ned_est * deltatime + 0.5f * acc_ned_est * deltatime * deltatime;
-}
-
-void FlightTaskAutoFollowTarget::release_gimbal_control()
 {
 	// NOTE: If other flight tasks start using gimbal control as well
 	// it might be worth moving this release mechanism to a common base
@@ -390,7 +460,7 @@ void FlightTaskAutoFollowTarget::release_gimbal_control()
 	_vehicle_command_pub.publish(vehicle_command);
 }
 
-void FlightTaskAutoFollowTarget::point_gimbal_at(float xy_distance, float z_distance)
+float FlightTaskAutoFollowTarget::pointGimbalAt(const float xy_distance, const float z_distance)
 {
 	gimbal_manager_set_attitude_s msg{};
 	float pitch_down_angle = 0.0f;
@@ -404,10 +474,10 @@ void FlightTaskAutoFollowTarget::point_gimbal_at(float xy_distance, float z_dist
 	}
 
 	const Quatf q_gimbal = Quatf(Eulerf(0, -pitch_down_angle, 0));
-	_gimbal_pitch = pitch_down_angle; // For logging
-
 	q_gimbal.copyTo(msg.q);
 
 	msg.timestamp = hrt_absolute_time();
 	_gimbal_manager_set_attitude_pub.publish(msg);
+
+	return pitch_down_angle;
 }

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,63 +37,93 @@
  * Flight task for autonomous, gps driven follow-me mode.
  *
  * @author Alessandro Simovic <potaito-dev@protonmail.com>
+ * @author Junwoo Hwang <junwoo091400@gmail.com>
  */
 
 #pragma once
 
 #include "FlightTaskAuto.hpp"
 #include "follow_target_estimator/TargetEstimator.hpp"
+#include "Sticks.hpp"
 
 #include <parameters/param.h>
 #include <mathlib/mathlib.h>
+
 #include <uORB/Subscription.hpp>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/follow_target_status.h>
 #include <uORB/topics/follow_target_estimator.h>
 #include <uORB/topics/gimbal_manager_set_attitude.h>
 #include <uORB/topics/vehicle_command.h>
-#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 
-// Speed above which the target heading can change.
+#include <lib/mathlib/math/filter/second_order_reference_model.hpp>
-// Used to prevent unpredictable jitter at low speeds.
+#include <lib/matrix/matrix/helper_functions.hpp>
-static constexpr float MINIMUM_SPEED_FOR_HEADING_CHANGE = 0.1f;
+#include <motion_planning/VelocitySmoothing.hpp>
 
-// Minimum distance between drone and target for the drone to do any yaw control.
+// << Follow Target Behavior related constants >>
-static constexpr float MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL = 1.0f;
 
-// Minimum safety altitude above home (or bottom distance sensor)
+// [m] Minimum safety altitude above home (or bottom distance sensor)
 // underneath which the flight task will stop moving horizontally
 static constexpr float MINIMUM_SAFETY_ALTITUDE = 1.0f;
 
+// [m/s] Vertical ascent speed when the drone detects that it is too close
+// to the ground (below MINIMUM_SAFETY_ALTITUDE).
+static constexpr float EMERGENCY_ASCENT_SPEED = 0.5f;
+
 // [m] max vertical deviation from position setpoint, above
 // which no horizontal control is done
 static constexpr float ALT_ACCEPTANCE_THRESHOLD = 3.0f;
 
-// Vertical ascent speed when the drone detects that it
+// [m] Minimum distance between drone and target for the drone to do any yaw control.
-// is too close to the ground (below MINIMUM_SAFETY_ALTITUDE)
+static constexpr float MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL = 1.0f;
-static constexpr float EMERGENCY_ASCENT_SPEED = 0.2f;
 
-// Filter on setpoints for smooth cinematic experience:
+// << Target Position Velocity Estimator related constants >>
-// Lowpass applied to the estimated position of the target
-// before using it as control input
-static constexpr float POSITION_FILTER_ALPHA =	1.5f;
 
-// Filter on setpoints for smooth cinematic experience:
+// [rad/s] Second Order reference model filter natural frequency
-// Lowpass applied to the follow-me angle setting, to ensure
+static constexpr float TARGET_POS_VEL_FILTER_NATURAL_FREQUENCY = 1.0f;
-// smooth and circular transitions between settings
-static constexpr float FOLLOW_ANGLE_FILTER_ALPHA = 3.0f;
 
-// Filter on setpoints for smooth cinematic experience:
+// [.] Second Order reference model filter damping ratio
-// Lowpass applied to the actual NED direction how the drone is facing the target
+static constexpr float TARGET_POS_VEL_FILTER_DAMPING_RATIO = 0.7071;
-// regarless of the setting. Used for dynamic tracking angles when the target makes a turn
-static constexpr float DIRECTION_FILTER_ALPHA =	3.0f;
 
-// Filter on setpoints for smooth cinematic experience:
+// [us] If target estimator isn't updated longer than this, reset the target pos/vel filter.
-// Lowpass applied for ramping up / down velocity feedforward term.
+static constexpr uint64_t TARGET_ESTIMATOR_TIMEOUT_US = 1500000UL;
-// This is to avoid aggressive jerks when the target starts moving, because
+
-// velocity feed-forward is not applied at all while the target is stationary.
+// << Target Course Angle Tracking related constants >>
-static constexpr float VELOCITY_FF_FILTER_ALPHA = 1.0f;
+
+// [m/s] Speed deadzone for which, under this velocity, the target orientation
+// tracking will freeze, since orientation can be noisy in low velocities
+static constexpr float TARGET_SPEED_DEADZONE_FOR_ORIENTATION_TRACKING = 1.0;
+
+// << Orbit Angle control related constants >>
+
+// [m/s^3] Maximum Jerk setting for generating the Follow Target Orbit trajectory
+static constexpr float ORBIT_TRAJECTORY_MAX_JERK = 4.0;
+
+// [m/s^2] Maximum acceleration setting for generating the Follow Target Orbit trajectory
+static constexpr float ORBIT_TRAJECTORY_MAX_ACCELERATION = 2.0;
+
+// << RC Adjustment related constants >>
+
+// [m/s] Speed with which the follow distance will be adjusted by when commanded with deflection via RC command
+static constexpr float FOLLOW_DISTANCE_USER_ADJUST_SPEED = 2.0;
+
+// [m] Maximum follow distance that can be set by user's RC adjustment
+static constexpr float FOLLOW_DISTANCE_MAX = 100.f;
+
+// [m/s] Speed with which the follow height will be adjusted by when commanded with deflection via RC command
+static constexpr float FOLLOW_HEIGHT_USER_ADJUST_SPEED = 1.5;
+
+// [m] Maximum follow height that can be set by user's RC adjustment
+static constexpr float FOLLOW_HEIGHT_MAX = 100.f;
+
+// [rad/s] Angular rate with which the follow distance will be adjusted by when commanded with full deflection via RC command
+static constexpr float FOLLOW_ANGLE_USER_ADJUST_SPEED = 1.5;
+
+// [s] Time window constant that gets multiplied to user adjustment speed, to calculate the
+// 'acceptable' error in orbit angle / height / distance. If the difference between the setpoint
+// and actual state of the drone is smaller than this error, RC adjustments get applied.
+// Prevents setpoints diverging from the vehicle's actual position too much
+static constexpr float USER_ADJUSTMENT_ERROR_TIME_WINDOW = 0.5f;
 
 
 class FlightTaskAutoFollowTarget : public FlightTask
@@ -102,114 +132,180 @@ public:
 	FlightTaskAutoFollowTarget();
 	virtual ~FlightTaskAutoFollowTarget();
 
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool update() override;
 
+	// Override parameter update function to check when Follow Target properties are changed
+	void updateParams() override;
+
 protected:
-	enum {
+	// Follow Altitude modes set by the parameter FLW_TGT_ALT_M
-		FOLLOW_PERSPECTIVE_NONE,
+	enum kFollowAltitudeMode {
-		FOLLOW_PERSPECTIVE_BEHIND,
+		kFollowAltitudeModeConstant,
-		FOLLOW_PERSPECTIVE_FRONT,
+		kFollowAltitudeModeTerrain,
-		FOLLOW_PERSPECTIVE_FRONT_RIGHT,
+		kFollowAltitudeModeTrackTarget
-		FOLLOW_PERSPECTIVE_FRONT_LEFT,
-		FOLLOW_PERSPECTIVE_MID_RIGHT,
-		FOLLOW_PERSPECTIVE_MID_LEFT,
-		FOLLOW_PERSPECTIVE_BEHIND_RIGHT,
-		FOLLOW_PERSPECTIVE_BEHIND_LEFT,
-		FOLLOW_PERSPECTIVE_MIDDLE_FOLLOW,
-		FOLLOW_PERSPECTIVE_INVALID  // Leave this as last!
-	};
-
-	// Angles [deg] for the different follow-me perspectives
-	enum {
-		FOLLOW_PERSPECTIVE_BEHIND_ANGLE_DEG = 180,
-		FOLLOW_PERSPECTIVE_FRONT_ANGLE_DEG = 0,
-		FOLLOW_PERSPECTIVE_FRONT_RIGHT_ANGLE_DEG = 45,
-		FOLLOW_PERSPECTIVE_FRONT_LEFT_ANGLE_DEG = 315,
-		FOLLOW_PERSPECTIVE_MID_RIGHT_ANGLE_DEG = 90,
-		FOLLOW_PERSPECTIVE_MID_LEFT_ANGLE_DEG = 270,
-		FOLLOW_PERSPECTIVE_BEHIND_RIGHT_ANGLE_DEG = 135,
-		FOLLOW_PERSPECTIVE_BEHIND_LEFT_ANGLE_DEG = 225
-	};
-
-	enum {
-		FOLLOW_ALTITUDE_MODE_CONSTANT,
-		FOLLOW_ALTITUDE_MODE_TRACK_TARGET
-	};
-
-	enum {
-		FOLLOW_GIMBAL_MODE_2D,
-		FOLLOW_GIMBAL_MODE_2D_WITH_TERRAIN,
-		FOLLOW_GIMBAL_MODE_3D
 	};
 
 	/**
-	 * Get the current follow-me perspective setting from PX4 parameters
+	 * Update the Follow height based on RC commands
 	 *
-	 * @param param_nav_ft_fs value of the parameter NAV_FT_FS
+	 * If the drone's height error to the setpoint is within the an user adjustment error time window
-	 * @return Angle [deg] from which the drone should view the target while following it, with zero degrees indicating the target's 12 o'clock
+	 * follow_height will be adjusted with a speed proportional to user RC command
+	 *
+	 * @param sticks Sticks object to get RC commanded values for adjustments
 	 */
-	float update_follow_me_angle_setting(int param_nav_ft_fs) const;
+	void updateRcAdjustedFollowHeight(const Sticks &sticks);
 
 	/**
-	 * Predict target's position through forward integration of its currently estimated position, velocity and acceleration.
+	 * Update the Follow distance based on RC commands
 	 *
-	 * @param deltatime [s] prediction horizon
+	 * If the drone's distance error to the setpoint is within the an user adjustment error time window
-	 * @return Future prediction of target position
+	 * follow_distance will be adjusted with a speed proportional to user RC command
+	 *
+	 * @param sticks Sticks object to get RC commanded values for adjustments
+	 * @param drone_to_target_vector [m] Tracked follow distance variable reference which will be updated to the new value
 	 */
-	matrix::Vector3f predict_future_pos_ned_est(float deltatime, const matrix::Vector3f &pos_ned_est,
+	void updateRcAdjustedFollowDistance(const Sticks &sticks, const Vector2f &drone_to_target_vector);
-			const matrix::Vector3f &vel_ned_est,
-			const matrix::Vector3f &acc_ned_est) const;
 
-	void release_gimbal_control();
+	/**
-	void point_gimbal_at(float xy_distance, float z_distance);
+	 * Update the Follow angle based on RC commands
-	matrix::Vector2f calculate_offset_vector_filtered(matrix::Vector3f vel_ned_est);
+	 *
-	matrix::Vector3f calculate_target_position_filtered(matrix::Vector3f pos_ned_est, matrix::Vector3f vel_ned_est,
+	 * If the drone's orbit angle in relation to target is within the an user adjustment error time window
-			matrix::Vector3f acc_ned_est);
+	 * away from the orbit angle setpoint, follow_angle will be adjusted with a speed proportional to user RC command
+	 *
+	 * @param sticks Sticks object to get RC commanded values for adjustments
+	 * @param measured_angle [rad] Measured current drone's orbit angle around the target (depends on tracked target orientation for reference)
+	 * @param tracked_orbit_angle_setpoint [rad] Rate constrained orbit angle setpoint value from last command
+	 */
+	void updateRcAdjustedFollowAngle(const Sticks &sticks, const float measured_orbit_angle,
+					 const float tracked_orbit_angle_setpoint);
 
-	// Calculate the desired position of the drone relative to the target using the offset_vector
+	/**
-	matrix::Vector3f calculate_drone_desired_position(matrix::Vector3f target_position, matrix::Vector2f offset_vector);
+	 * Update the Second Order Target Position + Velocity Filter to track kinematically feasible target position and velocity
+	 *
+	 * @param follow_target_estimator Received value from alpha-beta-gamma target estimator filter output
+	 */
+	void updateTargetPositionVelocityFilter(const follow_target_estimator_s &follow_target_estimator);
 
+	/**
+	 * Calculate the tracked target orientation
+	 *
+	 * Note : Filtered target velocity is generated via 2nd order filter can have overshooting behaviors
+	 * when target stops it's motion. This can generate a target velocity output that is opposite direction to where target was heading originally.
+	 * To check if the filtered velocity is staying true to target's actual motion, unfiltered velocity needs to be taken into account. Since during
+	 * overshoot, the unfiltered velocity stays close to 0 (indicating target already stopped), therefore not triggering a target orientation setting.
+	 *
+	 * @param current_target_orientation [rad] Tracked target orientation
+	 * @param target_velocity [m/s] Filtered Target velocity from which we will calculate target orientation
+	 * @param target_velocity_unfiltered [m/s] Unfiltered Target velocity that aids in verifying if filtered velocity is accurate
+	 *
+	 * @return [rad] Updated target orientation
+	 */
+	float updateTargetOrientation(const float current_target_orientation, const Vector2f &target_velocity,
+				      const Vector2f &target_velocity_unfiltered) const;
+
+	/**
+	 * Updates the orbit angle setpoint and a jerk limited trajectory to reach it.
+	 *
+	 * @param target_orientation [rad] Tracked target orientation
+	 * @param previous_orbit_angle_setpoint [rad] Previous orbit angle setpoint
+	 *
+	 * @return [rad] Next feasible orbit angle setpoint
+	 */
+	float updateOrbitAngleTrajectory(const float target_orientation, const float previous_orbit_angle_setpoint);
+
+	/**
+	 * Returns the orbit tangential velocity at the orbit angle setpoint, generated by the orbit angle trajectory generator
+	 *
+	 * @param orbit_angle_setpoint [rad] Orbit angle setpoint
+	 *
+	 * @return [m/s] 2D Velocity Vector of current orbit position setpoint (Local NED frame)
+	*/
+	Vector2f getOrbitTangentialVelocity(const float orbit_angle_setpoint) const;
+
+	/**
+	 * Calculates desired drone position taking into account orbit angle and the follow target altitude mode
+	 *
+	 * @param target_position [m] Target position (Local NED frame)
+	 * @param orbit_angle_setpoint [rad] Current orbit angle setpoint around the target
+	 *
+	 * @return [m] Final position setpoint for the drone (Local NED frame)
+	 */
+	Vector3f calculateDesiredDronePosition(const Vector3f &target_position, const float orbit_angle_setpoint) const;
+
+	/**
+	 * Calculate the gimbal height offset to the target
+	 *
+	 * @param altitude_mode Current Follow Target Altitude mode
+	 * @param target_pos_z [m] Target's local position z value to use for 3D tracking
+	 *
+	 * @return [m] Height Difference between the target and the drone
+	 */
+	float calculateGimbalHeight(const kFollowAltitudeMode altitude_mode, const float target_pos_z) const;
+
+	/**
+	 * Publishes gimbal control command to track the target, given xy distance and z (height) difference
+	 *
+	 * @param xy_distance [m] Horizontal distance to target
+	 * @param z_distance [m] Vertical distance to target
+	 *
+	 * @return [rad] Gimbal pitch setpoint, for logging in follow_target_status uORB message
+	 */
+	float pointGimbalAt(const float xy_distance, const float z_distance);
+
+	/**
+	 * Release Gimbal Control
+	 *
+	 * Releases Gimbal Control Authority of Follow-Target Flight Task, to allow other modules / Ground station
+	 * to control the gimbal when the task exits.
+	 * Fore more information on gimbal v2, see https://mavlink.io/en/services/gimbal_v2.html
+	 */
+	void releaseGimbalControl();
+
+	// Sticks object to read in stick commands from the user
+	Sticks _sticks;
+
+	// Alpha-Beta-Gamma estimator for initial estimation of target position and velocity
 	TargetEstimator _target_estimator;
-
+	// Struct to hold target estimator's output data
-	// Follow angle is defined with 0 degrees following from front, and then clockwise rotation
-	float _follow_angle_rad{0.0f};
-	AlphaFilter<float> _follow_angle_filter;
-
-	// Estimator for target position and velocity
 	follow_target_estimator_s _follow_target_estimator;
-	matrix::Vector2f _target_velocity_unit_vector;
 
-	// Lowpass filters for smoothingtarget position because it's used for setpoint generation
+	// Last target estimator timestamp to handle timeout filter reset
-	AlphaFilter<matrix::Vector3f> _target_position_filter;
+	uint64_t _last_valid_target_estimator_timestamp{0};
 
-	// Lowpass filter for smoothing the offset vector and have more dynamic shots when target changes direction
+	// Second Order Filter to calculate kinematically feasible target position
-	AlphaFilter<matrix::Vector2f> _offset_vector_filter;
+	SecondOrderReferenceModel<matrix::Vector3f> _target_position_velocity_filter;
 
-	// Lowpass filter assuming  values 0-1, for avoiding big steps in velocity feedforward
+	// Internally tracked Follow Target characteristics, to allow RC control input adjustments
-	AlphaFilter<float> _velocity_ff_scale;
+	float _follow_distance{8.0f}; // [m]
+	float _follow_height{10.0f}; // [m]
+	float _follow_angle_rad{0.0f}; // [rad]
 
-	// NOTE: If more of these internal state variables come into existence, it
+	// Tracked estimate of target's course (where velocity vector is pointing). Initialized as North direction
-	// would make sense to create an internal state machine with a single enum
+	float _target_course_rad{0.0f};
-	bool _emergency_ascent = false;
+	// Tracked orbit angle setpoint that gets calculated from Jerk-limited trajectory. Initialized in North direction.
+	float _orbit_angle_setpoint_rad{0.0f};
 
-	DEFINE_PARAMETERS(
+	// Angular Jerk limited orbit angle setpoint trajectory generator
+	VelocitySmoothing _orbit_angle_traj_generator;
+
+	// Variable to remember the home position (take-off point) z coordinate, where the follow height will be measured from
+	float _home_position_z;
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(
+		FlightTask,
 		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
 		(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
-		(ParamFloat<px4::params::NAV_MIN_FT_HT>) _param_nav_min_ft_ht,
+		(ParamFloat<px4::params::FLW_TGT_HT>) _param_flw_tgt_ht,
-		(ParamFloat<px4::params::NAV_FT_DST>) _param_nav_ft_dst,
+		(ParamFloat<px4::params::FLW_TGT_DST>) _param_flw_tgt_dst,
-		(ParamInt<px4::params::NAV_FT_FS>) _param_nav_ft_fs,
+		(ParamFloat<px4::params::FLW_TGT_FA>) _param_flw_tgt_fa,
-		(ParamInt<px4::params::NAV_FT_ALT_M>) _param_nav_ft_alt_m,
+		(ParamInt<px4::params::FLW_TGT_ALT_M>) _param_flw_tgt_alt_m,
-		(ParamInt<px4::params::NAV_FT_GMB_M>) _param_nav_ft_gmb_m,
+		(ParamFloat<px4::params::FLW_TGT_MAX_VEL>) _param_flw_tgt_max_vel
-		(ParamInt<px4::params::NAV_FT_DELC>) _param_nav_ft_delc
 	)
 
 	uORB::Subscription _follow_target_estimator_sub{ORB_ID(follow_target_estimator)};
 
-	uORB::PublicationMulti<follow_target_status_s> _follow_target_status_pub{ORB_ID(follow_target_status)};
+	uORB::Publication<follow_target_status_s> _follow_target_status_pub{ORB_ID(follow_target_status)};
-	uORB::PublicationMulti<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
+	uORB::Publication<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
 	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
-
-	// Debugging
-	float _gimbal_pitch{0};
 };

src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions

src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -181,7 +181,7 @@ void TargetEstimator::update()
 
 void TargetEstimator::update_filter_gains(filter_gains_s &filter_gains) const
 {
-	const float responsiveness_param = math::constrain(_param_nav_ft_rs.get(), .1F, 1.0F);
+	const float responsiveness_param = math::constrain(_param_flw_tgt_rs.get(), .1F, 1.0F);
 
 	if (fabsf(filter_gains.responsiveness - responsiveness_param) < FLT_EPSILON) {
 		// Parameter did not change since last execution. Skip calculations
@@ -193,7 +193,7 @@ void TargetEstimator::update_filter_gains(filter_gains_s &filter_gains) const
 	// The "G" gain is equivalent to "(1-responsiveness)", but beta is required for H and K gains
 	// From alpha-beta-gamma filter equations: G = 1-beta^3
 	// Therefore: beta = (1-Gp)^(1/3) = (1-(1-responsiveness))^(1/3) = (r)^(1/3)
-	const float beta_p = std::pow((filter_gains.responsiveness), 1.0f / 3.0f);
+	const float beta_p = pow((filter_gains.responsiveness), 1.0f / 3.0f);
 	const float beta_v = 0.9f * beta_p; // velocity fusion gain is slightly lower. TODO: individual parameter?
 
 	// Estimator gains for horizontal position update

src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -205,7 +205,7 @@ protected:
 	matrix::Vector3f _vel_measurement_old{};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::NAV_FT_RS>) _param_nav_ft_rs
+		(ParamFloat<px4::params::FLW_TGT_RS>) _param_flw_tgt_rs
 	)
 
 	// Subscriptions

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,22 +37,31 @@
  * Parameters for follow target mode
  *
  * @author Jimmy Johnson <catch22@fastmail.net>
- */
+ * @author Junwoo Hwang <junwoo091400@gmail.com>
-
-/*
- * Follow target parameters
  */
 
 /**
- * Minimum follow target altitude
+ * Responsiveness to target movement in Target Estimator
  *
- * The minimum height in meters relative to home for following a target
+ * lower values increase the responsiveness to changing position, but also ignore less noise
+ *
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @group Follow target
+ */
+PARAM_DEFINE_FLOAT(FLW_TGT_RS, 0.1f);
+
+/**
+ * Follow target height
+ *
+ * Following height above the target
  *
  * @unit m
  * @min 8.0
  * @group Follow target
  */
-PARAM_DEFINE_FLOAT(NAV_MIN_FT_HT, 8.0f);
+PARAM_DEFINE_FLOAT(FLW_TGT_HT, 8.0f);
 
 /**
  * Distance to follow target from
@@ -63,40 +72,23 @@ PARAM_DEFINE_FLOAT(NAV_MIN_FT_HT, 8.0f);
  * @min 1.0
  * @group Follow target
  */
-PARAM_DEFINE_FLOAT(NAV_FT_DST, 8.0f);
+PARAM_DEFINE_FLOAT(FLW_TGT_DST, 8.0f);
 
 /**
- * Side to follow target from
+ * Follow Angle setting in degrees
  *
- * The side to follow the target from
+ * Angle to follow the target from. 0.0 Equals straight in front of the target's
- * none         = 0
+ * course (direction of motion) and the angle increases in clockwise direction,
- * behind       = 1
+ * meaning Right-side would be 90.0 degrees while Left-side is -90.0 degrees
- * front        = 2
- * front right  = 3
- * front left   = 4
- * mid right    = 5
- * mid left     = 6
- * behind right = 7
- * behind left  = 8
  *
- * @min 0
+ * Note: When the user force sets the angle out of the min/max range, it will be
- * @max 8
+ * wrapped (e.g. 480 -> 120) in the range to gracefully handle the out of range.
+ *
+ * @min -180.0
+ * @max 180.0
  * @group Follow target
  */
-PARAM_DEFINE_INT32(NAV_FT_FS, 1);
+PARAM_DEFINE_FLOAT(FLW_TGT_FA, 180.0f);
-
-/**
- * Dynamic filtering algorithm responsiveness to target movement
- *
- * lower values increase the responsiveness to changing long lat
- * but also ignore less noise
- *
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @group Follow target
- */
-PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.1f);
 
 /**
  * Altitude control mode
@@ -106,39 +98,22 @@ PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.1f);
  * the target's altitude, the follow altitude NAV_MIN_FT_HT should be high enough
  * to prevent terrain collisions due to GPS inaccuracies of the target.
  *
- * TODO: Add option for 2D tracking + terrain following
+ * @value 0 2D Tracking: Maintain constant altitude relative to home and track XY position only
- *
+ * @value 1 2D + Terrain: Mantain constant altitude relative to terrain below and track XY position
- * @value 0 Maintain constant altitude and track XY position only (2D tracking)
+ * @value 2 3D Tracking: Track target's altitude (be aware that GPS altitude bias usually makes this useless)
- * @value 1 Track target's altitude (3D tracking)
  * @group Follow target
  */
-PARAM_DEFINE_INT32(NAV_FT_ALT_M, 0);
+PARAM_DEFINE_INT32(FLW_TGT_ALT_M, 0);
 
 /**
- * Gimbal tracking mode
+ * Maximum tangential velocity setting for generating the follow orbit trajectory
  *
- * @value 0 2D tracking: Point at target XY coordinates, and at ground Z coordinate
+ * This is the maximum tangential velocity the drone will circle around the target whenever
- * @value 1 2D tracking with terrain: Point at target XY coordinates, and at terrain Z coordinate
+ * an orbit angle setpoint changes. Higher value means more aggressive follow behavior.
- * @value 2 3D tracking: Point at target XYZ coordinates
+ *
+ * @min 0.0
+ * @max 20.0
+ * @decimal 1
  * @group Follow target
  */
-PARAM_DEFINE_INT32(NAV_FT_GMB_M, 0);
+PARAM_DEFINE_FLOAT(FLW_TGT_MAX_VEL, 5.0f);
-
-/**
- * Compensate filter delay
- *
- * The follow flight mode is estimating and filtering the target's position
- * and velocity in order to achieve a smoother tracking behavior. The filtering
- * introduces some delay which is noticable when the target is moving at higher
- * speeds, such as the case for bicycles and cars. For this high-speed scenario
- * the delay compensation can be enabled, which extrapolates the filtered position
- * in order to compensate for the filter delay.
- * This setting should not be used when the target is moving slowly, for example
- * when it's a pedestrian, as it will then emphasize the measurement noise instead
- * and cause unnecessary movement for the drone.
- *
- * @value 0 disabled
- * @value 1 enabled
- * @group Follow target
- */
-PARAM_DEFINE_INT32(NAV_FT_DELC, 0);

src/modules/logger/logged_topics.cpp

@@ -62,9 +62,9 @@ void LoggedTopics::add_default_topics()
 	add_topic("cpuload");
 	add_optional_topic("esc_status", 250);
 	add_topic("failure_detector_status", 100);
-	add_topic("follow_target_status", 100);
+	add_optional_topic("follow_target", 500);
-	add_topic("follow_target", 500);
+	add_optional_topic("follow_target_estimator", 200);
-	add_topic("follow_target_estimator", 100);
+	add_optional_topic("follow_target_status", 400);
 	add_topic("gimbal_manager_set_attitude", 500);
 	add_optional_topic("generator_status");
 	add_optional_topic("gps_dump");

test/mavsdk_tests/CMakeLists.txt

@@ -17,7 +17,9 @@ if(MAVSDK_FOUND)
         test_main.cpp
         autopilot_tester.cpp
         autopilot_tester_failure.cpp
-        autopilot_tester_follow_me.cpp
+        # follow-me needs a MAVSDK update:
+        # https://github.com/mavlink/MAVSDK/pull/1770
+        # autopilot_tester_follow_me.cpp
         test_multicopter_control_allocation.cpp
         test_multicopter_failure_injection.cpp
         test_multicopter_failsafe.cpp
@@ -25,7 +27,7 @@ if(MAVSDK_FOUND)
         test_multicopter_offboard.cpp
         test_multicopter_manual.cpp
         test_vtol_mission.cpp
-        test_multicopter_follow_me.cpp
+        # test_multicopter_follow_me.cpp
     )
 
     target_link_libraries(mavsdk_tests

test/mavsdk_tests/autopilot_tester_follow_me.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +36,7 @@
 // #include <mavsdk/plugins/follow_me/follow_me.h>
 
 #include "math_helpers.h"
+
 #include <iostream>
 #include <future>
 #include <thread>
@@ -163,42 +164,17 @@ void FollowTargetSimulator::check_follow_angle(FollowMe::Config config, std::arr
 		std::array<float, 3> target_pos_ned, float tolerance)
 {
 	// This check assumes that the target is travelling straight on the x-axis
-	const float x_dist_to_target = target_pos_ned[0] - drone_pos_ned[0];
+	const float target_to_drone_offset_x = drone_pos_ned[0] - target_pos_ned[0];
-	const float y_dist_to_target = target_pos_ned[1] - drone_pos_ned[1];
+	const float target_to_drone_offset_y = drone_pos_ned[1] - target_pos_ned[1];
 
-	switch (config.follow_direction) {
+	// Follow Angle is measured relative from the target's course (direction it is moving towards)
-	case FollowMe::Config::FollowDirection::None:
+	const float target_to_drone_angle_expected_rad = config.follow_angle_deg * (M_PI / 180.0f);
-		CHECK(x_dist_to_target < tolerance);
+	const float target_to_drone_offset_x_expected = config.follow_distance_m * cos(target_to_drone_angle_expected_rad);
-		CHECK(x_dist_to_target > -tolerance);
+	const float target_to_drone_offset_y_expected = config.follow_distance_m * sin(target_to_drone_angle_expected_rad);
-		CHECK(y_dist_to_target < tolerance);
-		CHECK(y_dist_to_target > -tolerance);
-		break;
 
-	case FollowMe::Config::FollowDirection::Behind:
+	// Check that drone is following at an expected position within the tolerance error
-		CHECK(drone_pos_ned[0] < target_pos_ned[0]);
+	CHECK(fabsf(target_to_drone_offset_x - target_to_drone_offset_x_expected) < tolerance);
-		CHECK(y_dist_to_target < tolerance);
+	CHECK(fabsf(target_to_drone_offset_y - target_to_drone_offset_y_expected) < tolerance);
-		CHECK(y_dist_to_target > -tolerance);
-		break;
-
-	case FollowMe::Config::FollowDirection::Front:
-		CHECK(drone_pos_ned[0] > target_pos_ned[0]);
-		CHECK(y_dist_to_target < tolerance);
-		CHECK(y_dist_to_target > -tolerance);
-		break;
-
-	case FollowMe::Config::FollowDirection::FrontRight:
-		CHECK(drone_pos_ned[0] > target_pos_ned[0]);
-		CHECK(drone_pos_ned[1] > target_pos_ned[1]);
-		break;
-
-	case FollowMe::Config::FollowDirection::FrontLeft:
-		CHECK(drone_pos_ned[0] > target_pos_ned[0]);
-		CHECK(drone_pos_ned[1] < target_pos_ned[1]);
-		break;
-
-	default:
-		break;
-	}
 }
 
 void AutopilotTesterFollowMe::connect(const std::string uri)
@@ -210,20 +186,24 @@ void AutopilotTesterFollowMe::connect(const std::string uri)
 }
 
 
-void AutopilotTesterFollowMe::straight_line_test(const float altitude_m, const bool stream_velocity)
+void AutopilotTesterFollowMe::straight_line_test(const bool stream_velocity)
 {
-	const unsigned location_update_rate = 1;
+	// CONFIGURATION for the test
-	const float position_tolerance = 6.0f;
+	const unsigned location_update_rate = 1; // [Hz] How often the GPS location update samples are generated
+	const float position_error_tolerance = 11.0f; // [m] Position error tolerance in both X and Y direction
+	const float follow_me_height_setting = 10.0f; // [m] Height above home position where the Drone will follow from
 
 	// Start with simulated target on the same plane as drone's home position
 	std::array<float, 3> start_location_ned = get_current_position_ned();
 	FollowTargetSimulator target_simulator(start_location_ned, getHome());
 
-	// Configure Min height of the drone to be "20 meters" above home & Follow direction as "Front
+	// Configure the Follow Me parameters
-	// right".
 	FollowMe::Config config;
-	config.min_height_m = altitude_m;
+	config.follow_height_m = follow_me_height_setting;
 	config.follow_distance_m = 8.0f;
+	config.follow_angle_deg = 0.0f;
+	// Ensure that tangential velocity control generates fast enough trajectory to be responsive
+	config.max_tangential_vel_m_s = 15.0f;
 
 	// Allow some time for mode switch
 	sleep_for(std::chrono::milliseconds(1000));
@@ -238,67 +218,67 @@ void AutopilotTesterFollowMe::straight_line_test(const float altitude_m, const b
 	bool target_moving = false;
 	bool perform_checks = false;
 
-	for (unsigned i = 0; i < 75 * location_update_rate; ++i) {
+	// Simulate generating 75 different target location updates
+	for (unsigned location_update_idx = 0; location_update_idx < 75 * location_update_rate; ++location_update_idx) {
 		std::array<float, 3> target_pos_ned_ground_truth = target_simulator.get_position_ground_truth_ned();
 		std::array<float, 3> position_ned = get_current_position_ned();
 		const float distance_to_target = norm(diff(target_pos_ned_ground_truth, position_ned));
 
 		// poor-man's state machine
-		if (i < 5) {
+		if (location_update_idx < 5) {
 			// Stream target location without moving
 
-		} else if (i == 5) {
+		} else if (location_update_idx == 5) {
-			// Change config
+			// Change config to Follow from 'Behind'
 			perform_checks = false;
-			config.follow_direction = FollowMe::Config::FollowDirection::Behind;
+			config.follow_angle_deg = 180.0f;
 			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
 
-		} else if (i > 5 && i < 15) {
+		} else if (location_update_idx < 15) {
-			// Move target and wait for steady state of drone
+			// Move target for 10 samples and wait for steady state of drone
 			target_moving = true;
 
-		} else if (i >= 15 && i < 20) {
+		} else if (location_update_idx < 20) {
-			// Perform positional checks in steady state
+			// Perform positional checks in steady state for 5 samples
 			perform_checks = true;
 
-		} else if (i == 20) {
+		} else if (location_update_idx == 20) {
-			// Change config
+			// Change config to follow from 'Front'
 			perform_checks = false;
-			config.follow_direction = FollowMe::Config::FollowDirection::Front;
+			config.follow_angle_deg = 0.0f;
 			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
 
-		} else if (i > 20 && i < 30) {
+		} else if (location_update_idx < 30) {
-			// Move target and wait for steady state of drone
+			// Move target for 10 samples and wait for steady state of drone
 
-		} else if (i >= 30 && i < 35) {
+		} else if (location_update_idx < 35) {
-			// Perform positional checks in steady state
+			// Perform positional checks in steady state for 5 samples
 			perform_checks = true;
 
-		} else if (i == 35) {
+		} else if (location_update_idx == 35) {
-			// Change config
+			// Change config to follow from 'Front right'
 			perform_checks = false;
-			config.follow_direction = FollowMe::Config::FollowDirection::FrontRight;
+			config.follow_angle_deg = 45.0f;
 			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
 
-		} else if (i > 35 && i < 45) {
+		} else if (location_update_idx < 45) {
-			// Move target and wait for steady state of drone
+			// Move target for 10 samples and wait for steady state of drone
 
-
+		} else if (location_update_idx < 55) {
-		} else if (i >= 45 && i < 55) {
+			// Perform positional checks in steady state for 5 samples
-			// Perform positional checks in steady state
 			perform_checks = true;
 
-		} else if (i == 55) {
+		} else if (location_update_idx == 55) {
-			// Change config
+			// Change config to follow from 'Front Left'
 			perform_checks = false;
-			config.follow_direction = FollowMe::Config::FollowDirection::FrontLeft;
+			config.follow_angle_deg = -45.0f;
 			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
 
-		} else if (i > 55 && i < 65) {
+		} else if (location_update_idx < 65) {
-			// Move target and wait for steady state of drone
+			// Move target for 10 samples and wait for steady state of drone
 
-		} else if (i >= 65 && i < 75) {
+		} else if (location_update_idx < 75) {
-			// Perform positional checks in steady state
+			// Perform positional checks in steady state for 10 samples
 			perform_checks = true;
 		}
 
@@ -307,10 +287,10 @@ void AutopilotTesterFollowMe::straight_line_test(const float altitude_m, const b
 		}
 
 		if (perform_checks) {
-			check_current_altitude(altitude_m);
+			check_current_altitude(follow_me_height_setting);
-			CHECK(distance_to_target <= config.follow_distance_m + position_tolerance);
+			CHECK(distance_to_target <= config.follow_distance_m + position_error_tolerance);
-			CHECK(distance_to_target >= config.follow_distance_m - position_tolerance);
+			CHECK(distance_to_target >= config.follow_distance_m - position_error_tolerance);
-			target_simulator.check_follow_angle(config, position_ned, target_pos_ned_ground_truth, position_tolerance);
+			target_simulator.check_follow_angle(config, position_ned, target_pos_ned_ground_truth, position_error_tolerance);
 		}
 
 		// Construct follow-me message
@@ -350,7 +330,7 @@ void AutopilotTesterFollowMe::stream_velocity_only()
 
 	// Configure follow-me
 	FollowMe::Config config;
-	config.follow_direction = FollowMe::Config::FollowDirection::Behind;
+	config.follow_angle_deg = 180.0f; // Follow from behind
 	CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
 
 	// Allow some time for mode switch
@@ -386,68 +366,112 @@ void AutopilotTesterFollowMe::stream_velocity_only()
 	CHECK(distance_travelled < position_tolerance);
 }
 
-void AutopilotTesterFollowMe::rc_override_test(const float altitude_m)
+void AutopilotTesterFollowMe::rc_adjustment_test()
 {
-	const unsigned loop_update_rate = 50;
+	// CONFIGURATION
-	const float position_tolerance = 4.0f;
+	const unsigned loop_update_rate = 50; // [Hz]
+	const float follow_height_setting = 10.0f; // [m]
+	const float follow_angle_setting = 0.0f; // [deg]
+	const float follow_distance_setting = 10.0f; // [m]
+
+	// The constants below are copied from the "FlightTaskAutoFollowTarget.hpp" to get a reference point for
+	// how much change a RC adjustment is expected to bring for each follow me parameters
+
+	// [m/s] Speed with which the follow distance will be adjusted by when commanded with deflection via RC command
+	static constexpr float FOLLOW_DISTANCE_USER_ADJUST_SPEED = 2.0;
+	// [m/s] Speed with which the follow height will be adjusted by when commanded with deflection via RC command
+	static constexpr float FOLLOW_HEIGHT_USER_ADJUST_SPEED = 1.5;
+	// [rad/s] Angular rate with which the follow distance will be adjusted by when commanded with full deflection via RC command
+	static constexpr float FOLLOW_ANGLE_USER_ADJUST_SPEED = 1.5;
 
 	// Start with simulated target on the same plane as drone's home position
 	std::array<float, 3> start_location_ned = get_current_position_ned();
 	FollowTargetSimulator target_simulator(start_location_ned, getHome());
 
-	// Configure Min height of the drone to be "20 meters" above home & Follow direction as "Front
+	// Set Follow Me parameters
-	// right".
 	FollowMe::Config config;
-	config.min_height_m = altitude_m;
+	config.follow_height_m = follow_height_setting;
-	config.follow_distance_m = 8.0f;
+	config.follow_distance_m = follow_distance_setting;
-	config.follow_direction = FollowMe::Config::FollowDirection::Behind;
+	config.follow_angle_deg = follow_angle_setting;
 	CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
 
-	// task loop
+	// [deg] Get a single sample of target's GPS coordinate
+	const std::array<double, 3> target_global_coordinate = target_simulator.get_position_global_ground_truth();
+
+	// Set TargetLocation as the sample to simulate a stationary target for a controlled RC adjustment test
+	const FollowMe::TargetLocation target_location{};
+	target_location.latitude_deg = target_global_coordinate[0];
+	target_location.longitude_deg = target_global_coordinate[1];
+	target_location.absolute_altitude_m = target_global_coordinate[2];
+
+	// [m] Save the target's location in Local NED (in meters), assuming it is where drone is at in the beginning
+	const std::array<float, 3> target_pos = get_current_position_ned();
+
+	// Start Follow-me
+	CHECK(FollowMe::Result::Success ==  _follow_me->start());
 	std::array<float, 3> drone_initial_pos;
 
-	for (unsigned i = 0; i <= 30 * loop_update_rate; ++i) {
+	// task loop
-		// Start streaming target data after x seconds to provide RC before switching to the flight task
+	for (unsigned i = 0; i <= 60 * loop_update_rate; ++i) {
-		bool stream_follow_me_data = (i > 7 * loop_update_rate);
+		_follow_me->set_target_location(target_location);
 
-		// Deflect a stick for a short time only
+		std::array<float, 3> current_drone_pos = get_current_position_ned();
-		bool deflect_rc_sticks = (i > 10 * loop_update_rate && i <= 11 * loop_update_rate);
 
-		// Switch to follow-me at this instant
+		if (i < 5 * loop_update_rate) {
-		if (i == 5 * loop_update_rate) {
+			// For 5 seconds, give time for the drone to go to it's initial following position (front)
-			// Start Follow Me
-			CHECK(FollowMe::Result::Success ==  _follow_me->start());
-		}
-
-		// After approximately 10 seconds we would expect the drone to have stopped because of the RC stick input
-		if (i == 20 * loop_update_rate) {
-			drone_initial_pos = get_current_position_ned();
-		}
-
-		if (stream_follow_me_data) {
-			target_simulator.update(1.0f / loop_update_rate);
-			std::array<double, 3> global_coordinate = target_simulator.get_position_global_noisy();
-			FollowMe::TargetLocation target_location{};
-			target_location.latitude_deg = global_coordinate[0];
-			target_location.longitude_deg = global_coordinate[1];
-			target_location.absolute_altitude_m = global_coordinate[2];
-			target_location.velocity_x_m_s = NAN;
-			target_location.velocity_y_m_s = NAN;
-			target_location.velocity_z_m_s = NAN;
-			_follow_me->set_target_location(target_location);
-		}
-
-		if (deflect_rc_sticks) {
-			CHECK(getManualControl()->set_manual_control_input(1.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
-
-		} else {
 			CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
+
+		} else if (i == 5 * loop_update_rate) {
+			// At 5 second mark, record the current drone position as initial position
+			drone_initial_pos = get_current_position_ned();
+
+		} else if (i < 8 * loop_update_rate) {
+			// FOLLOW HEIGHT ADJUSTMENT
+			// Command Throttle-up (Z = 1.0f) for 3 seconds
+			CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 1.0f, 0.f) == ManualControl::Result::Success);
+
+		} else if (i < 10 * loop_update_rate) {
+			// Center the throttle for 2 seconds
+			CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
+
+		} else if (i == 10 * loop_update_rate) {
+			// Check if altitude has increased at least half of the expected adjustment (Z is directed downwards, so flip the sign)
+			CHECK(-(current_drone_pos[2] - drone_initial_pos[2]) > FOLLOW_HEIGHT_USER_ADJUST_SPEED * 3.0f * 0.5f);
+
+		} else if (i < 13 * loop_update_rate) {
+			// FOLLOW HEIGHT ADJUSTMENT
+			// Command Pitch-down (= Forward) (X = 1.0f) for 3 seconds
+			CHECK(getManualControl()->set_manual_control_input(1.0f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
+
+		} else if (i < 15 * loop_update_rate) {
+			// Center the Pitch for 2 seconds
+			CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
+
+		} else if (i == 15 * loop_update_rate) {
+			// Check if follow distance has increased at least half of the expected adjustment
+			const float target_to_drone_x_diff = current_drone_pos[0] - target_pos[0];
+			const float target_to_drone_y_diff = current_drone_pos[1] - target_pos[1];
+			const float current_follow_distance = sqrt(sq(target_to_drone_x_diff) + sq(target_to_drone_y_diff));
+			CHECK(current_follow_distance > follow_distance_setting + FOLLOW_DISTANCE_USER_ADJUST_SPEED * 3.0f * 0.5f);
+
+		} else if (i < 18 * loop_update_rate) {
+			// FOLLOW ANGLE ADJUSTMENT
+			// Command Roll-right (=Rightwards) (Y = 1.0f) for 3 seconds
+			CHECK(getManualControl()->set_manual_control_input(0.f, 1.0f, 0.5f, 0.f) == ManualControl::Result::Success);
+
+		} else if (i < 20 * loop_update_rate) {
+			// Center the Roll for 2 seconds
+			CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
+
+		} else if (i == 20 * loop_update_rate) {
+			// Check if follow angle has increased at least half of the expected adjustment
+			// Since Roll-right corresponds to the follow angle increasing in clockwise direction
+			const float target_to_drone_x_diff = current_drone_pos[0] - target_pos[0];
+			const float target_to_drone_y_diff = current_drone_pos[1] - target_pos[1];
+			const float current_follow_angle_rad = atan2f(target_to_drone_y_diff, target_to_drone_x_diff);
+			CHECK(current_follow_angle_rad > follow_angle_setting * M_PI / 180.0f + FOLLOW_ANGLE_USER_ADJUST_SPEED * 3.0f * 0.5f);
 		}
 
 		sleep_for(std::chrono::milliseconds(1000 / loop_update_rate));
 	}
-
-	std::array<float, 3> drone_final_pos = get_current_position_ned();
-	const float distance_travelled = norm(diff(drone_initial_pos, drone_final_pos));
-	CHECK(distance_travelled < position_tolerance);
 }

test/mavsdk_tests/autopilot_tester_follow_me.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,7 +40,7 @@
 #include <mavsdk/plugins/follow_me/follow_me.h>
 
 
-// Simulated a target moving on a straight line
+// Simulate a target moving on a straight line in X+ direction
 class FollowTargetSimulator
 {
 public:
@@ -110,11 +110,29 @@ public:
 	~AutopilotTesterFollowMe() = default;
 	void connect(const std::string uri);
 
-	void straight_line_test(const float altitude_m, const bool stream_velocity);
+	/**
+	 * @brief Moves the Target in X+ direction and check the behavior
+	 *
+	 * @param stream_velocity [bool] Whether Target's FollowMe message includes velocity info or not
+	 */
+	void straight_line_test(const bool stream_velocity);
 
+	/**
+	 * @brief Sends Target Velocity data only with invalid Position Data
+	 *
+	 * This case should be considered as an 'invalid' Target movement, and the vehicle
+	 * therefore shouldn't move even when the target data is sent
+	 */
 	void stream_velocity_only();
 
-	void rc_override_test(const float altitude_m);
+	/**
+	 * @brief Test to check if RC adjustments work
+	 *
+	 * Simulates moving throttle up and down for Follow Height Control
+	 * Simulates moving roll left and right for Follow Angle Control
+	 * Simulates moving pitch up and down for Follow Distance Control
+	 */
+	void rc_adjustment_test();
 
 private:
 	std::unique_ptr<mavsdk::FollowMe> _follow_me{};

test/mavsdk_tests/test_multicopter_follow_me.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -51,7 +51,7 @@ TEST_CASE("Follow target - Position streaming", "[multicopter]")
 	tester.takeoff();
 	tester.wait_until_hovering();
 
-	tester.straight_line_test(takeoff_altitude, stream_velocity);
+	tester.straight_line_test(stream_velocity);
 
 	tester.land();
 	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
@@ -72,7 +72,7 @@ TEST_CASE("Follow target - Position and velocity streaming", "[multicopter]")
 	tester.takeoff();
 	tester.wait_until_hovering();
 
-	tester.straight_line_test(takeoff_altitude, stream_velocity);
+	tester.straight_line_test(stream_velocity);
 
 	tester.land();
 	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
@@ -103,7 +103,7 @@ TEST_CASE("Follow target - Velocity streaming only", "[multicopter]")
 	tester.check_home_within(1.0f);
 }
 
-TEST_CASE("Follow target - Manual takeover", "[multicopter]")
+TEST_CASE("Follow target - RC Adjustment", "[multicopter]")
 {
 	AutopilotTesterFollowMe tester;
 	tester.connect(connection_url);
@@ -116,14 +116,9 @@ TEST_CASE("Follow target - Manual takeover", "[multicopter]")
 	tester.takeoff();
 	tester.wait_until_hovering();
 
-	tester.rc_override_test(takeoff_altitude);
+	tester.rc_adjustment_test();
 
 	tester.land();
 	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
 	tester.wait_until_disarmed(until_disarmed_timeout);
 }
-
-TEST_CASE("Follow target - Spamming duplicate messages", "[multicopter]")
-{
-
-}