forked from Archive/PX4-Autopilot
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
This commit is contained in:
Junwoo Hwang
Daniel Agar
parent
de1fa11e96
commit
8bae4e5c0e
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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});
|
||||
_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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
// Used to prevent unpredictable jitter at low speeds.
|
||||
static constexpr float MINIMUM_SPEED_FOR_HEADING_CHANGE = 0.1f;
|
||||
#include <lib/mathlib/math/filter/second_order_reference_model.hpp>
|
||||
#include <lib/matrix/matrix/helper_functions.hpp>
|
||||
#include <motion_planning/VelocitySmoothing.hpp>
|
||||
|
||||
// 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<float> _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<matrix::Vector3f> _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<matrix::Vector2f> _offset_vector_filter;
|
||||
// Second Order Filter to calculate kinematically feasible target position
|
||||
SecondOrderReferenceModel<matrix::Vector3f> _target_position_velocity_filter;
|
||||
|
||||
// Lowpass filter assuming values 0-1, for avoiding big steps in velocity feedforward
|
||||
AlphaFilter<float> _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<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::NAV_FT_DST>) _param_nav_ft_dst,
|
||||
(ParamInt<px4::params::NAV_FT_FS>) _param_nav_ft_fs,
|
||||
(ParamInt<px4::params::NAV_FT_ALT_M>) _param_nav_ft_alt_m,
|
||||
(ParamInt<px4::params::NAV_FT_GMB_M>) _param_nav_ft_gmb_m,
|
||||
(ParamInt<px4::params::NAV_FT_DELC>) _param_nav_ft_delc
|
||||
(ParamFloat<px4::params::FLW_TGT_HT>) _param_flw_tgt_ht,
|
||||
(ParamFloat<px4::params::FLW_TGT_DST>) _param_flw_tgt_dst,
|
||||
(ParamFloat<px4::params::FLW_TGT_FA>) _param_flw_tgt_fa,
|
||||
(ParamInt<px4::params::FLW_TGT_ALT_M>) _param_flw_tgt_alt_m,
|
||||
(ParamFloat<px4::params::FLW_TGT_MAX_VEL>) _param_flw_tgt_max_vel
|
||||
)
|
||||
|
||||
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::PublicationMulti<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
|
||||
uORB::Publication<follow_target_status_s> _follow_target_status_pub{ORB_ID(follow_target_status)};
|
||||
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};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
*/
|
||||
|
||||
/*
|
||||
* Follow target parameters
|
||||
* @author Junwoo Hwang <junwoo091400@gmail.com>
|
||||
*/
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 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<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
|
||||
// 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<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) {
|
||||
// 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<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
|
||||
// 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<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) {
|
||||
// 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<float, 3> 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<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 {
|
||||
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<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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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{};
|
||||
|
|
|
|||
|
|
@ -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]")
|
||||
{
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue