diff --git a/msg/follow_target_status.msg b/msg/follow_target_status.msg index 379cfbf601..713a7dcdbd 100644 --- a/msg/follow_target_status.msg +++ b/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 -bool emergency_ascent -float32 gimbal_pitch +float32 tracked_target_course # [rad] Tracked target course in NED local frame (North is course zero) +float32 follow_angle # [rad] Current follow angle setting + +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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1553faf5ab..8bddedc8e5 100644 --- a/src/modules/commander/Commander.cpp +++ b/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; diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 2a1762366b..f8b495e29f 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/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 diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index aee8b60454..10bfed9b00 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/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) { diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt index 41085c5272..9abd0c8db2 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt +++ b/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 diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp index 89a79f5154..e682638a00 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp +++ b/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 #include +#include 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}); - _offset_vector_filter.reset(Vector2f(0, 0)); - _follow_angle_filter.reset(0.0f); - _velocity_ff_scale.reset(0.0f); + _target_position_velocity_filter.reset(Vector3f{NAN, NAN, NAN}); + _target_position_velocity_filter.setParameters(TARGET_POS_VEL_FILTER_NATURAL_FREQUENCY, + TARGET_POS_VEL_FILTER_DAMPING_RATIO); - // Initialize to something such that the drone at least points at the target, even if it's the wrong angle for the perspective. - // The drone will move into position as soon as the target starts moving and its heading becomes known. - Vector2f current_drone_heading_2d{cosf(_yaw), -sinf(_yaw)}; + _yaw_setpoint = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw; + _yawspeed_setpoint = NAN; - if (PX4_ISFINITE(current_drone_heading_2d(0)) && PX4_ISFINITE(current_drone_heading_2d(1))) { - _offset_vector_filter.reset(current_drone_heading_2d); + // Update the internally tracked Follow Target characteristics + _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) { - // NOTE: Switching between NONE any any other setting currently causes a jump in the setpoints - _offset_vector_filter.reset(Vector2f{0, 0}); + // Copy previous param values to check if it changes after param update + const float follow_distance_prev = _param_flw_tgt_dst.get(); + const float follow_height_prev = _param_flw_tgt_ht.get(); + const float follow_angle_prev = _param_flw_tgt_fa.get(); - } else { - // Define and rotate offset vector based on follow-me perspective setting - const float new_follow_angle_rad = math::radians(update_follow_me_angle_setting(_param_nav_ft_fs.get())); + // Call updateParams in parent class to update parameters from child up until ModuleParam base class + FlightTask::updateParams(); - // Use shortest rotation to get to the new angle - // Example: if the current angle setting is 270, and the new angle setting is 0, it's - // faster to rotate to 360 rather than 0. - // 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}); + // Compare param values and if they have changed, update the properties + if (!matrix::isEqualF(follow_distance_prev, _param_flw_tgt_dst.get())) { + _follow_distance = _param_flw_tgt_dst.get(); } - 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 - const float desired_distance_to_target = _param_nav_ft_dst.get(); - drone_desired_position.xy() = Vector2f(target_position.xy()) + - offset_vector.unit_or_zero() * desired_distance_to_target; + // Offset from the Target + 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; + + // 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 - // altitude above the target. Clarify best solution. - switch (_param_nav_ft_alt_m.get()) { - case FOLLOW_ALTITUDE_MODE_TRACK_TARGET: - drone_desired_position(2) = target_position(2) - _param_nav_min_ft_ht.get(); + switch ((kFollowAltitudeMode)_param_flw_tgt_alt_m.get()) { + case kFollowAltitudeModeTerrain: + drone_desired_position(2) = ground_z_estimate - _follow_height; 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 - // altitude setting - drone_desired_position(2) = math::min(_position_setpoint(2), -_param_nav_min_ft_ht.get()); + // Calculate the desired Z position relative to the home position + drone_desired_position(2) = _home_position_z - _follow_height; } 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 - if (!PX4_ISFINITE(_target_position_filter.getState()(0)) || !PX4_ISFINITE(_target_position_filter.getState()(1)) - || !PX4_ISFINITE(_target_position_filter.getState()(2))) { - _target_position_filter.reset(pos_ned_est); + float gimbal_height{0.0f}; + + switch (altitude_mode) { + 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. - _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(); - + return gimbal_height; } + 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}; - const Vector3f vel_ned_est{_follow_target_estimator.vel_est}; - const Vector3f acc_ned_est{_follow_target_estimator.acc_est}; - const Vector3f target_position_filtered = calculate_target_position_filtered(pos_ned_est, vel_ned_est, acc_ned_est); - 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); + updateTargetPositionVelocityFilter(_follow_target_estimator); + const Vector3f target_position_filtered = _target_position_velocity_filter.getState(); + const Vector3f target_velocity_filtered = _target_position_velocity_filter.getRate(); + const Vector2f drone_to_target_vector = Vector2f(target_position_filtered.xy()) - Vector2f(_position.xy()); - // Set position and velocity setpoints - float desired_velocity_ff_scale = 0.0f; // Used to ramp up velocity feedforward, avoiding harsh jumps in the setpoints + // Calculate heading to the target (for Yaw setpoint) + 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) { - - // 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(); + // Drone is close enough to the altitude target : Apply Horizontal + Velocity Control _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); - _velocity_ff_scale.update(desired_velocity_ff_scale); + // Yaw setpoint + 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 - _position_setpoint(0) = _position_setpoint(1) = NAN; - _velocity_setpoint.xy() = 0; + // Target estimate is invalid : Stay in current position + _position_setpoint.setAll(NAN); + _velocity_setpoint.setAll(0.0f); } - // Publish status message for debugging - _target_position_filter.getState().copyTo(follow_target_status.pos_est_filtered); + // Follow Target Status message for debugging 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 -{ - 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() +void FlightTaskAutoFollowTarget::releaseGimbalControl() { // 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; } diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp index d7a4a23ed2..23f4fdd0b7 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp +++ b/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 + * @author Junwoo Hwang */ #pragma once #include "FlightTaskAuto.hpp" #include "follow_target_estimator/TargetEstimator.hpp" +#include "Sticks.hpp" #include #include + #include #include -#include #include #include #include #include -#include -// Speed above which the target heading can change. -// Used to prevent unpredictable jitter at low speeds. -static constexpr float MINIMUM_SPEED_FOR_HEADING_CHANGE = 0.1f; +#include +#include +#include -// Minimum distance between drone and target for the drone to do any yaw control. -static constexpr float MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL = 1.0f; +// << Follow Target Behavior related constants >> -// 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 -// is too close to the ground (below MINIMUM_SAFETY_ALTITUDE) -static constexpr float EMERGENCY_ASCENT_SPEED = 0.2f; +// [m] Minimum distance between drone and target for the drone to do any yaw control. +static constexpr float MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL = 1.0f; -// Filter on setpoints for smooth cinematic experience: -// Lowpass applied to the estimated position of the target -// before using it as control input -static constexpr float POSITION_FILTER_ALPHA = 1.5f; +// << Target Position Velocity Estimator related constants >> -// Filter on setpoints for smooth cinematic experience: -// Lowpass applied to the follow-me angle setting, to ensure -// smooth and circular transitions between settings -static constexpr float FOLLOW_ANGLE_FILTER_ALPHA = 3.0f; +// [rad/s] Second Order reference model filter natural frequency +static constexpr float TARGET_POS_VEL_FILTER_NATURAL_FREQUENCY = 1.0f; -// Filter on setpoints for smooth cinematic experience: -// Lowpass applied to the actual NED direction how the drone is facing the target -// regarless of the setting. Used for dynamic tracking angles when the target makes a turn -static constexpr float DIRECTION_FILTER_ALPHA = 3.0f; +// [.] Second Order reference model filter damping ratio +static constexpr float TARGET_POS_VEL_FILTER_DAMPING_RATIO = 0.7071; -// Filter on setpoints for smooth cinematic experience: -// Lowpass applied for ramping up / down velocity feedforward term. -// 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. -static constexpr float VELOCITY_FF_FILTER_ALPHA = 1.0f; +// [us] If target estimator isn't updated longer than this, reset the target pos/vel filter. +static constexpr uint64_t TARGET_ESTIMATOR_TIMEOUT_US = 1500000UL; + +// << Target Course Angle Tracking related constants >> + +// [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_PERSPECTIVE_NONE, - FOLLOW_PERSPECTIVE_BEHIND, - FOLLOW_PERSPECTIVE_FRONT, - FOLLOW_PERSPECTIVE_FRONT_RIGHT, - 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 + // Follow Altitude modes set by the parameter FLW_TGT_ALT_M + enum kFollowAltitudeMode { + kFollowAltitudeModeConstant, + kFollowAltitudeModeTerrain, + kFollowAltitudeModeTrackTarget }; /** - * 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 - * @return Angle [deg] from which the drone should view the target while following it, with zero degrees indicating the target's 12 o'clock + * If the drone's height error to the setpoint is within the an user adjustment error time window + * 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 - * @return Future prediction of target position + * If the drone's distance error to the setpoint is within the an user adjustment error time window + * 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, - const matrix::Vector3f &vel_ned_est, - const matrix::Vector3f &acc_ned_est) const; + void updateRcAdjustedFollowDistance(const Sticks &sticks, const Vector2f &drone_to_target_vector); - void release_gimbal_control(); - void point_gimbal_at(float xy_distance, float z_distance); - 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, - matrix::Vector3f acc_ned_est); + /** + * Update the Follow angle based on RC commands + * + * If the drone's orbit angle in relation to target is within the an user adjustment error time window + * 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; - - // Follow angle is defined with 0 degrees following from front, and then clockwise rotation - float _follow_angle_rad{0.0f}; - AlphaFilter _follow_angle_filter; - - // Estimator for target position and velocity + // Struct to hold target estimator's output data 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 - AlphaFilter _target_position_filter; + // Last target estimator timestamp to handle timeout filter reset + uint64_t _last_valid_target_estimator_timestamp{0}; - // Lowpass filter for smoothing the offset vector and have more dynamic shots when target changes direction - AlphaFilter _offset_vector_filter; + // Second Order Filter to calculate kinematically feasible target position + SecondOrderReferenceModel _target_position_velocity_filter; - // Lowpass filter assuming values 0-1, for avoiding big steps in velocity feedforward - AlphaFilter _velocity_ff_scale; + // Internally tracked Follow Target characteristics, to allow RC control input adjustments + 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 - // would make sense to create an internal state machine with a single enum - bool _emergency_ascent = false; + // Tracked estimate of target's course (where velocity vector is pointing). Initialized as North direction + float _target_course_rad{0.0f}; + // 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) _param_mav_sys_id, (ParamInt) _param_mav_comp_id, - (ParamFloat) _param_nav_min_ft_ht, - (ParamFloat) _param_nav_ft_dst, - (ParamInt) _param_nav_ft_fs, - (ParamInt) _param_nav_ft_alt_m, - (ParamInt) _param_nav_ft_gmb_m, - (ParamInt) _param_nav_ft_delc + (ParamFloat) _param_flw_tgt_ht, + (ParamFloat) _param_flw_tgt_dst, + (ParamFloat) _param_flw_tgt_fa, + (ParamInt) _param_flw_tgt_alt_m, + (ParamFloat) _param_flw_tgt_max_vel ) uORB::Subscription _follow_target_estimator_sub{ORB_ID(follow_target_estimator)}; - uORB::PublicationMulti _follow_target_status_pub{ORB_ID(follow_target_status)}; - uORB::PublicationMulti _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; + uORB::Publication _follow_target_status_pub{ORB_ID(follow_target_status)}; + uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; - - // Debugging - float _gimbal_pitch{0}; }; diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt index 5640a1c403..0312ffcbd6 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt +++ b/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 diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp index 33f5354b4a..46b670d4f8 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp +++ b/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 diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp index 2e28357125..78f43f563b 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp +++ b/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) _param_nav_ft_rs + (ParamFloat) _param_flw_tgt_rs ) // Subscriptions diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c index e566898929..2336d323e0 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c +++ b/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 - */ - -/* - * Follow target parameters + * @author Junwoo Hwang */ /** - * 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 - * none = 0 - * behind = 1 - * front = 2 - * front right = 3 - * front left = 4 - * mid right = 5 - * mid left = 6 - * behind right = 7 - * behind left = 8 + * Angle to follow the target from. 0.0 Equals straight in front of the target's + * course (direction of motion) and the angle increases in clockwise direction, + * meaning Right-side would be 90.0 degrees while Left-side is -90.0 degrees * - * @min 0 - * @max 8 + * Note: When the user force sets the angle out of the min/max range, it will be + * 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); - -/** - * 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); +PARAM_DEFINE_FLOAT(FLW_TGT_FA, 180.0f); /** * 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 Maintain constant altitude and track XY position only (2D tracking) - * @value 1 Track target's altitude (3D tracking) + * @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 2 3D Tracking: Track target's altitude (be aware that GPS altitude bias usually makes this useless) * @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 - * @value 1 2D tracking with terrain: Point at target XY coordinates, and at terrain Z coordinate - * @value 2 3D tracking: Point at target XYZ coordinates + * This is the maximum tangential velocity the drone will circle around the target whenever + * an orbit angle setpoint changes. Higher value means more aggressive follow behavior. + * + * @min 0.0 + * @max 20.0 + * @decimal 1 * @group Follow target */ -PARAM_DEFINE_INT32(NAV_FT_GMB_M, 0); - -/** - * 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); +PARAM_DEFINE_FLOAT(FLW_TGT_MAX_VEL, 5.0f); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 9da40bdc93..e827a43bbd 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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_topic("follow_target", 500); - add_topic("follow_target_estimator", 100); + add_optional_topic("follow_target", 500); + add_optional_topic("follow_target_estimator", 200); + add_optional_topic("follow_target_status", 400); add_topic("gimbal_manager_set_attitude", 500); add_optional_topic("generator_status"); add_optional_topic("gps_dump"); diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index f1eca60043..90b27f943a 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/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 diff --git a/test/mavsdk_tests/autopilot_tester_follow_me.cpp b/test/mavsdk_tests/autopilot_tester_follow_me.cpp index 98f40c872c..10fa713030 100644 --- a/test/mavsdk_tests/autopilot_tester_follow_me.cpp +++ b/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 #include "math_helpers.h" + #include #include #include @@ -163,42 +164,17 @@ void FollowTargetSimulator::check_follow_angle(FollowMe::Config config, std::arr std::array 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 y_dist_to_target = target_pos_ned[1] - drone_pos_ned[1]; + const float target_to_drone_offset_x = drone_pos_ned[0] - target_pos_ned[0]; + const float target_to_drone_offset_y = drone_pos_ned[1] - target_pos_ned[1]; - switch (config.follow_direction) { - case FollowMe::Config::FollowDirection::None: - CHECK(x_dist_to_target < tolerance); - CHECK(x_dist_to_target > -tolerance); - CHECK(y_dist_to_target < tolerance); - CHECK(y_dist_to_target > -tolerance); - break; + // Follow Angle is measured relative from the target's course (direction it is moving towards) + const float target_to_drone_angle_expected_rad = config.follow_angle_deg * (M_PI / 180.0f); + const float target_to_drone_offset_x_expected = config.follow_distance_m * cos(target_to_drone_angle_expected_rad); + const float target_to_drone_offset_y_expected = config.follow_distance_m * sin(target_to_drone_angle_expected_rad); - case FollowMe::Config::FollowDirection::Behind: - 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::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; - } + // Check that drone is following at an expected position within the tolerance error + CHECK(fabsf(target_to_drone_offset_x - target_to_drone_offset_x_expected) < tolerance); + CHECK(fabsf(target_to_drone_offset_y - target_to_drone_offset_y_expected) < tolerance); } 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; - const float position_tolerance = 6.0f; + // CONFIGURATION for the test + 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 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 - // right". + // Configure the Follow Me parameters 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 target_pos_ned_ground_truth = target_simulator.get_position_ground_truth_ned(); std::array 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) { - // Change config + } else if (location_update_idx == 5) { + // 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) { - // Move target and wait for steady state of drone + } else if (location_update_idx < 15) { + // Move target for 10 samples and wait for steady state of drone target_moving = true; - } else if (i >= 15 && i < 20) { - // Perform positional checks in steady state + } else if (location_update_idx < 20) { + // Perform positional checks in steady state for 5 samples perform_checks = true; - } else if (i == 20) { - // Change config + } else if (location_update_idx == 20) { + // 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) { - // Move target and wait for steady state of drone + } else if (location_update_idx < 30) { + // Move target for 10 samples and wait for steady state of drone - } else if (i >= 30 && i < 35) { - // Perform positional checks in steady state + } else if (location_update_idx < 35) { + // Perform positional checks in steady state for 5 samples perform_checks = true; - } else if (i == 35) { - // Change config + } else if (location_update_idx == 35) { + // 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) { - // Move target and wait for steady state of drone + } else if (location_update_idx < 45) { + // Move target for 10 samples and wait for steady state of drone - - } else if (i >= 45 && i < 55) { - // Perform positional checks in steady state + } else if (location_update_idx < 55) { + // Perform positional checks in steady state for 5 samples perform_checks = true; - } else if (i == 55) { - // Change config + } else if (location_update_idx == 55) { + // 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) { - // Move target and wait for steady state of drone + } else if (location_update_idx < 65) { + // Move target for 10 samples and wait for steady state of drone - } else if (i >= 65 && i < 75) { - // Perform positional checks in steady state + } else if (location_update_idx < 75) { + // 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(distance_to_target <= config.follow_distance_m + position_tolerance); - CHECK(distance_to_target >= config.follow_distance_m - position_tolerance); - target_simulator.check_follow_angle(config, position_ned, target_pos_ned_ground_truth, position_tolerance); + check_current_altitude(follow_me_height_setting); + CHECK(distance_to_target <= config.follow_distance_m + position_error_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_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; - const float position_tolerance = 4.0f; + // CONFIGURATION + 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 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 - // right". + // Set Follow Me parameters FollowMe::Config config; - config.min_height_m = altitude_m; - config.follow_distance_m = 8.0f; - config.follow_direction = FollowMe::Config::FollowDirection::Behind; + config.follow_height_m = follow_height_setting; + config.follow_distance_m = follow_distance_setting; + 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 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 target_pos = get_current_position_ned(); + + // Start Follow-me + CHECK(FollowMe::Result::Success == _follow_me->start()); std::array drone_initial_pos; - for (unsigned i = 0; i <= 30 * loop_update_rate; ++i) { - // Start streaming target data after x seconds to provide RC before switching to the flight task - bool stream_follow_me_data = (i > 7 * loop_update_rate); + // task loop + for (unsigned i = 0; i <= 60 * loop_update_rate; ++i) { + _follow_me->set_target_location(target_location); - // Deflect a stick for a short time only - bool deflect_rc_sticks = (i > 10 * loop_update_rate && i <= 11 * loop_update_rate); + std::array current_drone_pos = get_current_position_ned(); - // Switch to follow-me at this instant - if (i == 5 * loop_update_rate) { - // 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 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 { + if (i < 5 * loop_update_rate) { + // For 5 seconds, give time for the drone to go to it's initial following position (front) 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 drone_final_pos = get_current_position_ned(); - const float distance_travelled = norm(diff(drone_initial_pos, drone_final_pos)); - CHECK(distance_travelled < position_tolerance); } diff --git a/test/mavsdk_tests/autopilot_tester_follow_me.h b/test/mavsdk_tests/autopilot_tester_follow_me.h index 7a11e9770b..f3d0ffa1cd 100644 --- a/test/mavsdk_tests/autopilot_tester_follow_me.h +++ b/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 -// 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 _follow_me{}; diff --git a/test/mavsdk_tests/test_multicopter_follow_me.cpp b/test/mavsdk_tests/test_multicopter_follow_me.cpp index 84c2d40fa7..0ae4b0e5be 100644 --- a/test/mavsdk_tests/test_multicopter_follow_me.cpp +++ b/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]") -{ - -}