fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

New follow-me flight task

Browse Source 
rename follow_me_status to follow_target_status

enable follow_target_estimator on skynode

implement the responsiveness parameter:
The responsiveness parameter should behave similarly to the previous
follow-me implementation in navigator. The difference here is that
there are now two separate gains for position and velocity fusion.
The previous implemenation in navigator had no velocity fusion.

Allow follow-me to be flown without RC

SITL tests for follow-me flight task

This includes:
- Testing the setting for the follow-me angle
- Testing that streaming position only or position
  and velocity measurements both work
- Testing that RC override works

Most of these tests are done with a simulated model
of a point object that moves on a straight line. So
nothing too spectacular. But it makes the test checks
much easier.

Since the estimator for the target actually checks new
measurements and compares them to old ones, I also added
random gausian noise to the measurements with a fixed seed
for deterministic randomness. So repeated runs produce
exactly the same results over and over.

Half of the angles are still missing in MAVSDK. Need to create
an upstream PR to add center left/right and rear left/right options.
These and the corresponding SITL tests need to be implemented
later.

sitl: Increase position tolerance during follow-me

Astro seems to barely exceed the current tolerance (4.3 !< 4.0)
causing CI to fail. The point of the CI test is not to check
the accuracy of the flight behaviour, but only the fact that the
drone is doing the expected thing. So the exact value of this
tolerance is not really important.

follow-me: gimbal control in follow-me

follow-me: create sub-routines in flight task class

follow-me: use ground-dist for emergency ascent

dist_bottom is only defined when a ground facing distance sensor exist.
It's therefore better to use dist_ground instead, which has the distance
to the home altitude if no distance sensor is available.

As a consequence it will only be possible to use follow-me in a valley
when the drone has a distance sensor.

follow-me: point gimbal to the ground in 2D mode

follow-me: another fuzzy msg handling for the estimator

follow-me: bugfix in acceleration saturation limit

follow-me: parameter for filter delay compensation

mantis: dont use flow for terrain estimation

follow-me: default responsiveness 0.5 -> 0.1

0.5 is way too jerky in real and simulated tests.

flight_task: clarify comments for bottom distance

follow-me: minor comment improvement

follow-me: [debug] log emergency_ascent

follow-me: [debug] log gimbal pitch

follow-me: [debug] status values for follow-me estimator

follow-me: setting for gimbal tracking mode

follow-me: release gimbal control at destruction

mavsdk: cosmetics 💄
This commit is contained in:
Alessandro Simovic 2021-07-20 09:11:17 +02:00 committed by Daniel Agar
parent 285556e463
commit de1fa11e96
32 changed files with 2121 additions and 713 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
View File

@ -47,6 +47,7 @@ param set-default COM_RC_LOSS_T 3
# ekf2
param set-default EKF2_AID_MASK 33
param set-default EKF2_TERR_MASK 1
param set-default EKF2_BARO_DELAY 0
param set-default EKF2_BARO_NOISE 2.0

2
msg/CMakeLists.txt
View File

@ -81,7 +81,9 @@ set(msg_files
estimator_aid_source_2d.msg
estimator_aid_source_3d.msg
event.msg
follow_target_status.msg
follow_target.msg
follow_target_estimator.msg
failure_detector_status.msg
generator_status.msg
geofence_result.msg

19
msg/follow_target.msg
View File

@ -1,8 +1,11 @@
uint64 timestamp # time since system start (microseconds)
float64 lat # target position (deg * 1e7)
float64 lon # target position (deg * 1e7)
float32 alt # target position
float32 vy # target vel in y
float32 vx # target vel in x
float32 vz # target vel in z
uint8 est_cap # target reporting capabilities
uint64 timestamp # time since system start (microseconds)
float64 lat # target position (deg * 1e7)
float64 lon # target position (deg * 1e7)
float32 alt # target position
float32 vy # target vel in y
float32 vx # target vel in x
float32 vz # target vel in z
uint8 est_cap # target reporting capabilities

16
msg/follow_target_estimator.msg Normal file
View File

@ -0,0 +1,16 @@
uint64 timestamp # time since system start (microseconds)
uint64 last_filter_reset_timestamp # time of last filter reset (microseconds)
bool valid # True if estimator states are okay to be used
bool stale # True if estimator stopped receiving follow_target messages for some time. The estimate can still be valid, though it might be inaccurate.
float64 lat_est # Estimated target latitude
float64 lon_est # Estimated target longitude
float32 alt_est # Estimated target altitude
float32[3] pos_est # Estimated target NED position (m)
float32[3] vel_est # Estimated target NED velocity (m/s)
float32[3] acc_est # Estimated target NED acceleration (m^2/s)
uint64 prediction_count
uint64 fusion_count

5
msg/follow_target_status.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
float32[3] pos_est_filtered # Lowpass-filtered target position used for vehicle's setpoint generation
bool emergency_ascent
float32 gimbal_pitch

15
src/modules/commander/state_machine_helper.cpp
View File

@ -484,7 +484,22 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
// failsafe: datalink is lost
// Trigger RTL
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);
} else if (status.rc_signal_lost && status_flags.rc_signal_found_once && !data_link_loss_act_configured && is_armed) {
// Trigger failsafe on RC loss only if RC was present once before
// Otherwise fly without RC, as follow-target only depends on the datalink
enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
} else {
// no failsafe, RC is not mandatory for follow_target
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
}

2
src/modules/flight_mode_manager/CMakeLists.txt
View File

@ -53,7 +53,7 @@ endif()
# add core flight tasks to list
list(APPEND flight_tasks_all
Auto
AutoFollowMe
AutoFollowTarget
Descend
Failsafe
ManualAcceleration

2
src/modules/flight_mode_manager/FlightModeManager.cpp
View File

@ -200,7 +200,7 @@ 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::AutoFollowMe);
FlightTaskError error = switchTask(FlightTaskIndex::AutoFollowTarget);
if (error != FlightTaskError::NoError) {
if (prev_failure_count == 0) {

49
src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp
View File

@ -1,49 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskAutoFollowMe.cpp
*/
#include "FlightTaskAutoFollowMe.hpp"
#include <mathlib/mathlib.h>
using namespace matrix;
bool FlightTaskAutoFollowMe::update()
{
_position_setpoint = _target;
matrix::Vector2f vel_sp = _getTargetVelocityXY();
_velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f);
return true;
}

50
src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp
View File

@ -1,50 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskAutoFollowMe.hpp
*
* Flight task for autonomous, gps driven follow-me mode.
*/
#pragma once
#include "FlightTaskAuto.hpp"
class FlightTaskAutoFollowMe : public FlightTaskAuto
{
public:
FlightTaskAutoFollowMe() = default;
virtual ~FlightTaskAutoFollowMe() = default;
bool update() override;
};

41
src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt Normal file
View File

@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 2021 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
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
add_subdirectory(follow_target_estimator)
px4_add_library(FlightTaskAutoFollowTarget
FlightTaskAutoFollowTarget.cpp
)
target_link_libraries(FlightTaskAutoFollowTarget PUBLIC FlightTaskAuto follow_target_estimator)
target_include_directories(FlightTaskAutoFollowTarget PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

413
src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp Normal file
View File

@ -0,0 +1,413 @@
/****************************************************************************
*
* Copyright (c) 2021 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskAutoFollowTarget.cpp
*
* Flight Task for follow-me flight mode. It consumes follow_target_estimator messages from
* TargetEstimator. The drone then tracks this target's GPS coordinates from a specified
* angle and distance.
*
*/
#include "FlightTaskAutoFollowTarget.hpp"
#include <mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
using matrix::Vector2f;
using matrix::Vector3f;
using matrix::Quatf;
using matrix::Eulerf;
FlightTaskAutoFollowTarget::FlightTaskAutoFollowTarget()
{
_target_estimator.Start();
}
FlightTaskAutoFollowTarget::~FlightTaskAutoFollowTarget()
{
release_gimbal_control();
_target_estimator.Stop();
}
bool FlightTaskAutoFollowTarget::activate(const vehicle_local_position_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);
// 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)};
if (PX4_ISFINITE(current_drone_heading_2d(0)) && PX4_ISFINITE(current_drone_heading_2d(1))) {
_offset_vector_filter.reset(current_drone_heading_2d);
} else {
_offset_vector_filter.reset(Vector2f(1.0f, 0.0f));
}
_yawspeed_setpoint = 0.f;
return ret;
}
Vector2f FlightTaskAutoFollowTarget::calculate_offset_vector_filtered(Vector3f vel_ned_est)
{
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});
} 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()));
// 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});
}
return _offset_vector_filter.getState();
}
Vector3f FlightTaskAutoFollowTarget::calculate_drone_desired_position(Vector3f target_position, Vector2f offset_vector)
{
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;
// 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();
break;
case FOLLOW_ALTITUDE_MODE_CONSTANT:
// 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());
}
return drone_desired_position;
}
Vector3f FlightTaskAutoFollowTarget::calculate_target_position_filtered(Vector3f pos_ned_est, Vector3f vel_ned_est,
Vector3f acc_ned_est)
{
// 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);
}
// 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();
}
bool FlightTaskAutoFollowTarget::update()
{
_follow_target_estimator_sub.update(&_follow_target_estimator);
follow_target_status_s follow_target_status{};
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);
// Set position and velocity setpoints
float desired_velocity_ff_scale = 0.0f; // Used to ramp up velocity feedforward, avoiding harsh jumps in the setpoints
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();
_position_setpoint = drone_desired_position;
} else {
// Achieve target altitude first before controlling horizontally!
_position_setpoint = _position;
_position_setpoint(2) = drone_desired_position(2);
}
} else {
// Control setpoint: Stay in current position
_position_setpoint = _position;
_velocity_setpoint.setZero();
}
_velocity_ff_scale.setParameters(_deltatime, VELOCITY_FF_FILTER_ALPHA);
_velocity_ff_scale.update(desired_velocity_ff_scale);
// Emergency ascent when too close to the ground
_emergency_ascent = PX4_ISFINITE(_dist_to_ground) && _dist_to_ground < MINIMUM_SAFETY_ALTITUDE;
if (_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;
}
// Publish status message for debugging
_target_position_filter.getState().copyTo(follow_target_status.pos_est_filtered);
follow_target_status.timestamp = hrt_absolute_time();
follow_target_status.emergency_ascent = _emergency_ascent;
follow_target_status.gimbal_pitch = _gimbal_pitch;
_follow_target_status_pub.publish(follow_target_status);
_constraints.want_takeoff = _checkTakeoff();
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()
{
// NOTE: If other flight tasks start using gimbal control as well
// it might be worth moving this release mechanism to a common base
// class for gimbal-control flight tasks
vehicle_command_s vehicle_command = {};
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
vehicle_command.param1 = -3.0f; // Remove control if it had it.
vehicle_command.param2 = -3.0f; // Remove control if it had it.
vehicle_command.param3 = -1.0f; // Leave unchanged.
vehicle_command.param4 = -1.0f; // Leave unchanged.
vehicle_command.timestamp = hrt_absolute_time();
vehicle_command.source_system = _param_mav_sys_id.get();
vehicle_command.source_component = _param_mav_comp_id.get();
vehicle_command.target_system = _param_mav_sys_id.get();
vehicle_command.target_component = _param_mav_comp_id.get();
vehicle_command.confirmation = false;
vehicle_command.from_external = false;
_vehicle_command_pub.publish(vehicle_command);
}
void FlightTaskAutoFollowTarget::point_gimbal_at(float xy_distance, float z_distance)
{
gimbal_manager_set_attitude_s msg{};
float pitch_down_angle = 0.0f;
if (PX4_ISFINITE(z_distance)) {
pitch_down_angle = atan2f(z_distance, xy_distance);
}
if (!PX4_ISFINITE(pitch_down_angle)) {
pitch_down_angle = 0.0;
}
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);
}

215
src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp Normal file
View File

@ -0,0 +1,215 @@
/****************************************************************************
*
* Copyright (c) 2021 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskAutoFollowTarget.hpp
*
* Flight task for autonomous, gps driven follow-me mode.
*
* @author Alessandro Simovic <potaito-dev@protonmail.com>
*/
#pragma once
#include "FlightTaskAuto.hpp"
#include "follow_target_estimator/TargetEstimator.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;
// 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;
// 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] 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;
// 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;
// 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;
// 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;
// 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;
class FlightTaskAutoFollowTarget : public FlightTask
{
public:
FlightTaskAutoFollowTarget();
virtual ~FlightTaskAutoFollowTarget();
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool update() 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
};
/**
* Get the current follow-me perspective setting from PX4 parameters
*
* @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
*/
float update_follow_me_angle_setting(int param_nav_ft_fs) const;
/**
* Predict target's position through forward integration of its currently estimated position, velocity and acceleration.
*
* @param deltatime [s] prediction horizon
* @return Future prediction of target position
*/
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 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);
// 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);
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
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;
// Lowpass filter for smoothing the offset vector and have more dynamic shots when target changes direction
AlphaFilter<matrix::Vector2f> _offset_vector_filter;
// Lowpass filter assuming values 0-1, for avoiding big steps in velocity feedforward
AlphaFilter<float> _velocity_ff_scale;
// 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;
DEFINE_PARAMETERS(
(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
)
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<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
// Debugging
float _gimbal_pitch{0};
};

13
src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt → src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt
View File

@ -31,9 +31,14 @@
#
############################################################################
px4_add_library(FlightTaskAutoFollowMe
FlightTaskAutoFollowMe.cpp
px4_add_library(follow_target_estimator
TargetEstimator.cpp
)
target_link_libraries(FlightTaskAutoFollowMe PUBLIC FlightTaskAuto)
target_include_directories(FlightTaskAutoFollowMe PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_compile_options(follow_target_estimator PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_link_libraries(follow_target_estimator
PRIVATE
mathlib
px4_work_queue
)

358
src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp Normal file
View File

@ -0,0 +1,358 @@
/****************************************************************************
*
* Copyright (c) 2021 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file TargetEstimator.cpp
*
*/
#include "TargetEstimator.hpp"
#include <mathlib/mathlib.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
using matrix::Vector2f;
using matrix::Vector3f;
using matrix::Vector3;
TargetEstimator::TargetEstimator() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
}
TargetEstimator::~TargetEstimator()
{
Stop();
}
bool TargetEstimator::Start()
{
// Initialize this filter
reset();
// force initialize parameters update
parameters_update(true);
// follow_target messages needed for estimation
if (!_follow_target_sub.registerCallback()) {
PX4_ERR("target_estimator callback registration failed");
}
if (!_vehicle_local_position_sub.registerCallback()) {
PX4_ERR("target_estimator callback registration failed");
}
return true;
}
void TargetEstimator::Stop()
{
// clear all registered callbacks
_follow_target_sub.unregisterCallback();
_vehicle_local_position_sub.unregisterCallback();
Deinit();
}
void TargetEstimator::Run()
{
ScheduleDelayed(10_ms);
update();
}
void TargetEstimator::parameters_update(bool force)
{
// check for parameter updates
if (_parameter_update_sub.updated() || force) {
// clear update
parameter_update_s update;
_parameter_update_sub.copy(&update);
// update parameters from storage
updateParams();
}
}
void TargetEstimator::update()
{
const hrt_abstime now = hrt_absolute_time();
// compute deltatime between update() calls
if (_last_iteration_timestamp == 0) {
_last_iteration_timestamp = now;
return;
}
const float deltatime = math::constrain((now - _last_iteration_timestamp) * 1e-6f, 1e-3f, 2.0f);
_last_iteration_timestamp = now;
parameters_update();
update_filter_gains(_filter_gains);
// Get GPS reference location for NED frame, needed for projection
_vehicle_local_position_sub.update(&_vehicle_local_position);
// Perform sensor fusion update if there's a new GPS message from the follow-target
prediction_update(deltatime);
follow_target_s follow_target;
if (_follow_target_sub.update(&follow_target)) {
// Don't perform measurement update if two follow_target messages with identical timestamps are used
// This can happen when using the MAVSDK and more than one outgoing follow_target message is queued.
const bool duplicate_measurement_received = follow_target.timestamp == _last_follow_target_timestamp;
// Skip measurements that lie in the past
const bool measurement_in_the_past = _last_position_fusion_timestamp >= follow_target.timestamp;
// Need at least one vehicle_local_position before estimator can work
const bool vehicle_local_position_invalid = _vehicle_local_position.timestamp == 0;
if (!duplicate_measurement_received && !measurement_in_the_past && !vehicle_local_position_invalid) {
measurement_update(follow_target);
}
}
// Keep position estimate as the last known position
// but stop moving the estimate
if (is_stale(GPS_MESSAGE_STALE_TIMEOUT_MS)) {
_filter_states.vel_ned_est.setZero();
_filter_states.acc_ned_est.setZero();
}
const bool states_are_finite = _filter_states.is_finite();
if (!states_are_finite) {
reset();
}
// Publish estimator message
follow_target_estimator_s follow_target_estimator{};
follow_target_estimator.timestamp = hrt_absolute_time();
follow_target_estimator.valid = states_are_finite;
follow_target_estimator.stale = is_stale(GPS_MESSAGE_STALE_TIMEOUT_MS);
follow_target_estimator.last_filter_reset_timestamp = _last_filter_reset_timestamp;
follow_target_estimator.lat_est = get_lat_lon_alt_est()(0);
follow_target_estimator.lon_est = get_lat_lon_alt_est()(1);
follow_target_estimator.alt_est = get_lat_lon_alt_est()(2);
_filter_states.pos_ned_est.copyTo(follow_target_estimator.pos_est);
_filter_states.vel_ned_est.copyTo(follow_target_estimator.vel_est);
_filter_states.acc_ned_est.copyTo(follow_target_estimator.acc_est);
follow_target_estimator.prediction_count = _prediction_count;
follow_target_estimator.fusion_count = _fusion_count;
_follow_target_estimator_pub.publish(follow_target_estimator);
}
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);
if (fabsf(filter_gains.responsiveness - responsiveness_param) < FLT_EPSILON) {
// Parameter did not change since last execution. Skip calculations
return;
}
filter_gains.responsiveness = responsiveness_param;
// 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_v = 0.9f * beta_p; // velocity fusion gain is slightly lower. TODO: individual parameter?
// Estimator gains for horizontal position update
filter_gains.G_p = 1.0f - beta_p * beta_p * beta_p;
filter_gains.H_p = 1.5f * (1.0f - beta_p) * (1.0f - beta_p) * (1.0f + beta_p);
filter_gains.K_p = 0.5f * (1.0f - beta_p) * (1.0f - beta_p) * (1.0f - beta_p);
// Estimator gains for velocity update
filter_gains.G_v = 1.0f - beta_v * beta_v ;
filter_gains.H_v = (1.0f - beta_v) * (1.0f - beta_v);
}
bool TargetEstimator::measurement_can_be_fused(const Vector3f &current_measurement,
const Vector3f &previous_measurement,
uint64_t last_fusion_timestamp, float min_delta_t) const
{
const bool measurement_valid = PX4_ISFINITE(current_measurement(0)) && PX4_ISFINITE(current_measurement(1))
&& PX4_ISFINITE(current_measurement(2));
const bool sensor_data_changed = Vector3f(current_measurement - previous_measurement).longerThan(2.0f * FLT_EPSILON)
|| !PX4_ISFINITE(previous_measurement(0)) || !PX4_ISFINITE(previous_measurement(1))
|| !PX4_ISFINITE(previous_measurement(2));
// This is required as a throttle
const bool fusion_old_enough = hrt_absolute_time() - last_fusion_timestamp >
min_delta_t * 1000;
// TODO: Remove this workaround
const bool fusion_too_old = hrt_absolute_time() - last_fusion_timestamp >
2 * min_delta_t * 1000;
return measurement_valid && fusion_old_enough && (sensor_data_changed || fusion_too_old);
// return measurement_valid;
}
void TargetEstimator::measurement_update(follow_target_s follow_target)
{
_fusion_count++;
// Decompose follow_target message into the individual measurements for position and velocity
const Vector3f vel_measured{follow_target.vx, follow_target.vy, follow_target.vz};
Vector3f pos_measured{NAN, NAN, -(follow_target.alt - _vehicle_local_position.ref_alt)};
_reference_position.initReference(_vehicle_local_position.ref_lat, _vehicle_local_position.ref_lon);
_reference_position.project(follow_target.lat, follow_target.lon, pos_measured(0), pos_measured(1));
// Initialize filter if necessary
if (_last_follow_target_timestamp == 0) {
_filter_states.pos_ned_est = pos_measured;
if (PX4_ISFINITE(vel_measured(0)) && PX4_ISFINITE(vel_measured(1)) && PX4_ISFINITE(vel_measured(2))) {
_filter_states.vel_ned_est = vel_measured;
} else {
_filter_states.vel_ned_est.setAll(0.0f);
}
_pos_measurement_old = pos_measured;
_vel_measurement_old = vel_measured;
_filter_states.acc_ned_est.setZero();
}
_last_follow_target_timestamp = follow_target.timestamp;
// Fuse position measurement
//
// Filter duplicate GPS POS and VEL messages
// QGC sends the same GPS coordinates multiple times per second, even though the phone's GPS
// typically only updates at 1 Hz
if (measurement_can_be_fused(pos_measured, _pos_measurement_old, _last_position_fusion_timestamp,
MINIMUM_TIME_BETWEEN_POS_FUSIONS_MS)) {
// Update with only position measurement
const float dt_update_pos = math::constrain((follow_target.timestamp - _last_position_fusion_timestamp) * 1e-6f, 1e-3f,
20.0f); // seconds
_last_position_fusion_timestamp = follow_target.timestamp;
const Vector3f pos_innovation = pos_measured - _filter_states.pos_ned_est;
// Position, velocity and acceleration update
_filter_states.pos_ned_est += _filter_gains.G_p * pos_innovation;
_filter_states.vel_ned_est += _filter_gains.H_p / (dt_update_pos) * pos_innovation;
_filter_states.acc_ned_est += 2.0f * _filter_gains.K_p /
(dt_update_pos * dt_update_pos) * pos_innovation;
_pos_measurement_old = pos_measured;
}
// Fuse velocity measurement
//
// Use target's velocity data for update only if
// - the last velocity fusion is a while ago to prevent repeated measurements to cause a quick convergence
// - the target is considered to be moving. Otherwise it's enough to only update the position
// - the GPS velocity measurement from the target is not stale
// Additionally also wait with first velocity fusion until at least one position fusion has been done (states become finite)
if (measurement_can_be_fused(vel_measured, _vel_measurement_old, _last_velocity_fusion_timestamp,
MINIMUM_TIME_BETWEEN_VEL_FUSIONS_MS)) {
// Update with only velocity measurement
const float dt_update_vel = math::constrain((follow_target.timestamp - _last_velocity_fusion_timestamp) * 1e-6f, 1e-3f,
20.0f); // seconds
_last_velocity_fusion_timestamp = follow_target.timestamp;
const Vector3f vel_innovation = vel_measured - _filter_states.vel_ned_est;
// Velocity and acceleration update
_filter_states.vel_ned_est += _filter_gains.G_v * vel_innovation;
_filter_states.acc_ned_est += _filter_gains.H_v / (dt_update_vel) * vel_innovation;
_vel_measurement_old = vel_measured;
}
_filter_states.saturate_acceleration(ACCELERATION_SATURATION);
}
void TargetEstimator::prediction_update(float deltatime)
{
_prediction_count++;
// Temporary copy to not mix old and new values during the update calculations
const Vector3f vel_ned_est_prev = _filter_states.vel_ned_est;
const Vector3f acc_ned_est_prev = _filter_states.acc_ned_est;
if (PX4_ISFINITE(vel_ned_est_prev(0)) && PX4_ISFINITE(vel_ned_est_prev(1)) && PX4_ISFINITE(vel_ned_est_prev(2))) {
_filter_states.pos_ned_est += deltatime * vel_ned_est_prev + 0.5f * acc_ned_est_prev * deltatime * deltatime;
}
if (PX4_ISFINITE(acc_ned_est_prev(0)) && PX4_ISFINITE(acc_ned_est_prev(1)) && PX4_ISFINITE(acc_ned_est_prev(2))) {
_filter_states.vel_ned_est += deltatime * acc_ned_est_prev;
}
}
Vector3<double> TargetEstimator::get_lat_lon_alt_est() const
{
Vector3<double> lat_lon_alt{(double)NAN, (double)NAN, (double)NAN};
if (PX4_ISFINITE(_filter_states.pos_ned_est(0)) && PX4_ISFINITE(_filter_states.pos_ned_est(0))) {
_reference_position.reproject(_filter_states.pos_ned_est(0), _filter_states.pos_ned_est(1), lat_lon_alt(0),
lat_lon_alt(1));
lat_lon_alt(2) = -(double)_filter_states.pos_ned_est(2) + (double)_vehicle_local_position.ref_alt;
}
return lat_lon_alt;
}
bool TargetEstimator::is_stale(const float timeout_duration_ms) const
{
const bool measurements_stale = (hrt_absolute_time() - _last_follow_target_timestamp) / 1000.0f >=
timeout_duration_ms;
return measurements_stale;
}
void TargetEstimator::reset()
{
_last_filter_reset_timestamp = hrt_absolute_time(); // debug only
_last_position_fusion_timestamp = _last_velocity_fusion_timestamp = 0;
_last_follow_target_timestamp = 0;
_filter_states.pos_ned_est.setAll(NAN);
_filter_states.vel_ned_est.setAll(NAN);
_filter_states.acc_ned_est.setAll(NAN);
_pos_measurement_old.setAll(NAN);
_vel_measurement_old.setAll(NAN);
}

220
src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp Normal file
View File

@ -0,0 +1,220 @@
/****************************************************************************
*
* Copyright (c) 2021 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file TargetEstimator.hpp
*
* @author Alessandro Simovic <potaito-dev@protonmail.com>
*/
#pragma once
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/follow_target.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/follow_target_estimator.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/PublicationMulti.hpp>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <lib/geo/geo.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
static constexpr float GPS_MESSAGE_STALE_TIMEOUT_MS =
3000.0f; // Duration after which the connection to the target is considered lost
static constexpr float MINIMUM_TIME_BETWEEN_POS_FUSIONS_MS = 500.0f;
static constexpr float MINIMUM_TIME_BETWEEN_VEL_FUSIONS_MS = 100.0f;
static constexpr float ACCELERATION_SATURATION = 20.0f; // 2*g
static constexpr float MINIMUM_SPEED_FOR_TARGET_MOVING =
0.1f; // speed threshold above which the target is considered to be moving
using namespace time_literals;
struct filter_gains_s {
// Position fusion gains
float G_p;
float H_p;
float K_p;
// Velocity fusion gains
float G_v;
float H_v;
// Estimator position / velocity update gains
float responsiveness;
};
struct filter_states_s {
matrix::Vector3f pos_ned_est{}; // target's position in NED frame
matrix::Vector3f vel_ned_est{}; // target's velocity in NED frame
matrix::Vector3f acc_ned_est{}; // target's acceleration in NED frame
/**
* Check if all state are finite
*
* @return true if all state are finite, or false as soon as any state is NAN
*/
bool is_finite()
{
return PX4_ISFINITE(pos_ned_est(0)) && PX4_ISFINITE(pos_ned_est(1)) && PX4_ISFINITE(pos_ned_est(2)) &&
PX4_ISFINITE(vel_ned_est(0)) && PX4_ISFINITE(vel_ned_est(1)) && PX4_ISFINITE(vel_ned_est(2)) &&
PX4_ISFINITE(acc_ned_est(0)) && PX4_ISFINITE(acc_ned_est(1)) && PX4_ISFINITE(acc_ned_est(2));
}
/**
* Limits the acceleration state to some sane value to prevent unrealistic
* spikes in the acceleration, which could cause severely unexpected behaviour in the drone
* that is tracking the target
*
* @param saturation [m/s^2] limit to use for acceleration saturation
*/
void saturate_acceleration(float saturation)
{
if (acc_ned_est.norm_squared() > saturation * saturation) {
acc_ned_est = acc_ned_est.unit_or_zero() * saturation;
}
}
};
class TargetEstimator : public ModuleParams, public px4::ScheduledWorkItem
{
public:
TargetEstimator();
~TargetEstimator() override;
bool Start();
void Stop();
protected:
void Run() override;
/**
* Process new measurement data and iterate filter to current time
*/
void update();
/**
* Check for parameter changes and update them if needed.
*
* @param force for a parameter update
*/
void parameters_update(bool force = false);
/**
* Recompute filter gains
*
* @param filter_gains filter gains that will be updated based on current PX4 parameters
*/
void update_filter_gains(filter_gains_s &filter_gains) const;
/**
* Checks whether a sensor measurement should be fused or not
*
*
* @param current_measurement the most recent measurement that has not yet been fused
* @param previous_measurement the previous measurement that was accepted and therefore fused
* @param last_fusion_timestamp last timestamp of when this sensor data was fused
* @param min_delta_t minimum amount of time that needs to pass between two sensor fusions
*/
bool measurement_can_be_fused(const matrix::Vector3f &current_measurement, const matrix::Vector3f &previous_measurement,
uint64_t last_fusion_timestamp, float min_delta_t) const;
/**
* Perform filter update with new follow_target data
*
* @param follow_target GPS data last received from target
*/
void measurement_update(follow_target_s follow_target);
/**
* Perform prediction step based on simple position-velocity-acceleration model of a point mass
* Can be called at a much higher frequency than measurement data is being received.
*
* @param deltatime time [s] since the last prediction or measurement update
*/
void prediction_update(float deltatime);
/**
* Get current LAT/LON/ALT estimate of target
*
* @return Current position estimate of target as latitude / longitude / altitude vector
*/
matrix::Vector3<double> get_lat_lon_alt_est() const;
/**
* Check if last received data from target is too old
*
* @param timeout_duration_ms timeout in [ms] to use for this check
*/
bool is_stale(const float timeout_duration_ms) const;
/**
* Reset all filter states causing it to completely forget the old filter state
*/
void reset();
filter_gains_s _filter_gains;
filter_states_s _filter_states;
MapProjection _reference_position{};
vehicle_local_position_s _vehicle_local_position{};
hrt_abstime _last_iteration_timestamp{0};
hrt_abstime _last_filter_reset_timestamp{0};
hrt_abstime _last_position_fusion_timestamp{0};
hrt_abstime _last_velocity_fusion_timestamp{0};
hrt_abstime _last_follow_target_timestamp{0};
// Pos/vel from previous measurement update. Required for filtering duplicate messages
matrix::Vector3f _pos_measurement_old{};
matrix::Vector3f _vel_measurement_old{};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_FT_RS>) _param_nav_ft_rs
)
// Subscriptions
uORB::SubscriptionCallbackWorkItem _follow_target_sub{this, ORB_ID(follow_target)};
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
uORB::PublicationMulti<follow_target_estimator_s> _follow_target_estimator_pub{ORB_ID(follow_target_estimator)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
// Debugging
uint64_t _prediction_count{0};
uint64_t _fusion_count{0};
};

61
src/modules/navigator/follow_target_params.c → src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c
View File

@ -68,10 +68,19 @@ PARAM_DEFINE_FLOAT(NAV_FT_DST, 8.0f);
/**
* Side to follow target from
*
* The side to follow the target from (front right = 0, behind = 1, front = 2, front left = 3)
* 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
*
* @min 0
* @max 3
* @max 8
* @group Follow target
*/
PARAM_DEFINE_INT32(NAV_FT_FS, 1);
@ -79,7 +88,7 @@ PARAM_DEFINE_INT32(NAV_FT_FS, 1);
/**
* Dynamic filtering algorithm responsiveness to target movement
*
* lower numbers increase the responsiveness to changing long lat
* lower values increase the responsiveness to changing long lat
* but also ignore less noise
*
* @min 0.0
@ -87,5 +96,49 @@ PARAM_DEFINE_INT32(NAV_FT_FS, 1);
* @decimal 2
* @group Follow target
*/
PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.5f);
PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.1f);
/**
* Altitude control mode
*
* Maintain altitude or track target's altitude. When maintaining the altitude,
* the drone can crash into terrain when the target moves uphill. When tracking
* 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)
* @group Follow target
*/
PARAM_DEFINE_INT32(NAV_FT_ALT_M, 0);
/**
* Gimbal tracking mode
*
* @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
* @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);

4
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
View File

@ -226,8 +226,8 @@ protected:
float _yaw{}; /**< current vehicle yaw heading */
bool _is_yaw_good_for_control{}; /**< true if the yaw estimate can be used for yaw control */
float _dist_to_bottom{}; /**< current height above ground level */
float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */
float _dist_to_bottom{}; /**< current height above ground level if dist_bottom is valid */
float _dist_to_ground{}; /**< equals _dist_to_bottom if available, height above home otherwise */
/**
* Setpoints which the position controller has to execute.

5
src/modules/logger/logged_topics.cpp
View File

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

1
src/modules/navigator/CMakeLists.txt
View File

@ -48,7 +48,6 @@ px4_add_module(
precland.cpp
mission_feasibility_checker.cpp
geofence.cpp
follow_target.cpp
vtol_takeoff.cpp
DEPENDS
geo

407
src/modules/navigator/follow_target.cpp
View File

@ -1,407 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2016 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file followme.cpp
*
* Helper class to track and follow a given position
*
* @author Jimmy Johnson <catch22@fastmail.net>
*/
#include "follow_target.h"
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/follow_target.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/math/Limits.hpp>
#include "navigator.h"
using namespace matrix;
constexpr float FollowTarget::_follow_position_matricies[4][9];
FollowTarget::FollowTarget(Navigator *navigator) :
MissionBlock(navigator),
ModuleParams(navigator)
{
_current_vel.zero();
_step_vel.zero();
_est_target_vel.zero();
_target_distance.zero();
_target_position_offset.zero();
_target_position_delta.zero();
}
void FollowTarget::on_inactive()
{
reset_target_validity();
}
void FollowTarget::on_activation()
{
_follow_offset = _param_nav_ft_dst.get() < 1.0F ? 1.0F : _param_nav_ft_dst.get();
_responsiveness = math::constrain((float) _param_nav_ft_rs.get(), .1F, 1.0F);
_follow_target_position = _param_nav_ft_fs.get();
if ((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) {
_follow_target_position = FOLLOW_FROM_BEHIND;
}
_rot_matrix = Dcmf(_follow_position_matricies[_follow_target_position]);
}
void FollowTarget::on_active()
{
MapProjection target_ref;
follow_target_s target_motion_with_offset = {};
uint64_t current_time = hrt_absolute_time();
bool radius_entered = false;
bool radius_exited = false;
bool updated = false;
float dt_ms = 0;
if (_follow_target_sub.updated()) {
updated = true;
follow_target_s target_motion;
_target_updates++;
// save last known motion topic for interpolation later
_previous_target_motion = _current_target_motion;
_follow_target_sub.copy(&target_motion);
if (_current_target_motion.timestamp == 0) {
_current_target_motion = target_motion;
}
_current_target_motion = target_motion;
_current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)(
1 - _responsiveness);
_current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)(
1 - _responsiveness);
} else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) {
reset_target_validity();
}
// update distance to target
if (target_position_valid()) {
// get distance to target
target_ref.initReference(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
_target_distance(0), _target_distance(1));
}
// update target velocity
if (target_velocity_valid() && updated) {
dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);
// ignore a small dt
if (dt_ms > 10.0F) {
// get last gps known reference for target
target_ref.initReference(_previous_target_motion.lat, _previous_target_motion.lon);
// calculate distance the target has moved
target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
(_target_position_delta(0)), (_target_position_delta(1)));
// update the average velocity of the target based on the position
if (PX4_ISFINITE(_current_target_motion.vx) && PX4_ISFINITE(_current_target_motion.vy)) {
// No need to estimate target velocity if we can take it from the target
_est_target_vel(0) = _current_target_motion.vx;
_est_target_vel(1) = _current_target_motion.vy;
_est_target_vel(2) = 0.0f;
} else {
_est_target_vel = _target_position_delta / (dt_ms / 1000.0f);
}
// if the target is moving add an offset and rotation
if (_est_target_vel.length() > .5F) {
_target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset;
}
// are we within the target acceptance radius?
// give a buffer to exit/enter the radius to give the velocity controller
// a chance to catch up
radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
// to keep the velocity increase/decrease smooth
// calculate how many velocity increments/decrements
// it will take to reach the targets velocity
// with the given amount of steps also add a feed forward input that adjusts the
// velocity as the position gap increases since
// just traveling at the exact velocity of the target will not
// get any closer or farther from the target
_step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
_step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS);
// if we are less than 1 meter from the target don't worry about trying to yaw
// lock the yaw until we are at a distance that makes sense
if ((_target_distance).length() > 1.0F) {
// yaw rate smoothing
// this really needs to control the yaw rate directly in the attitude pid controller
// but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode
_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_current_target_motion.lat,
_current_target_motion.lon);
_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f));
} else {
_yaw_angle = _yaw_rate = NAN;
}
}
// warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d yaw rate = %3.6f",
// (double) _step_vel(0),
// (double) _step_vel(1),
// (double) _current_vel(0),
// (double) _current_vel(1),
// (double) _est_target_vel(0),
// (double) _est_target_vel(1),
// (double) (_target_distance).length(),
// (double) (_target_position_offset + _target_distance).length(),
// _follow_target_state,
// (double) _yaw_rate);
}
if (target_position_valid()) {
// get the target position using the calculated offset
target_ref.initReference(_current_target_motion.lat, _current_target_motion.lon);
target_ref.reproject(_target_position_offset(0), _target_position_offset(1),
target_motion_with_offset.lat, target_motion_with_offset.lon);
}
// clamp yaw rate smoothing if we are with in
// 3 degrees of facing target
if (PX4_ISFINITE(_yaw_rate)) {
if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->heading)) < math::radians(3.0F)) {
_yaw_rate = NAN;
}
}
// update state machine
switch (_follow_target_state) {
case TRACK_POSITION: {
if (radius_entered) {
_follow_target_state = TRACK_VELOCITY;
} else if (target_velocity_valid()) {
set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle);
// keep the current velocity updated with the target velocity for when it's needed
_current_vel = _est_target_vel;
update_position_sp(true, true, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
break;
}
case TRACK_VELOCITY: {
if (radius_exited) {
_follow_target_state = TRACK_POSITION;
} else if (target_velocity_valid()) {
if ((float)(current_time - _last_update_time) / 1000.0f >= _step_time_in_ms) {
_current_vel += _step_vel;
_last_update_time = current_time;
}
set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle);
update_position_sp(true, false, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
break;
}
case SET_WAIT_FOR_TARGET_POSITION: {
// Climb to the minimum altitude
// and wait until a position is received
follow_target_s target = {};
// for now set the target at the minimum height above the uav
target.lat = _navigator->get_global_position()->lat;
target.lon = _navigator->get_global_position()->lon;
target.alt = 0.0F;
set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target, _yaw_angle);
update_position_sp(false, false, _yaw_rate);
_follow_target_state = WAIT_FOR_TARGET_POSITION;
}
/* FALLTHROUGH */
case WAIT_FOR_TARGET_POSITION: {
if (is_mission_item_reached() && target_velocity_valid()) {
_target_position_offset(0) = _follow_offset;
_follow_target_state = TRACK_POSITION;
}
break;
}
}
}
void FollowTarget::update_position_sp(bool use_velocity, bool use_position, float yaw_rate)
{
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// activate line following in pos control if position is valid
pos_sp_triplet->previous.valid = use_position;
pos_sp_triplet->previous = pos_sp_triplet->current;
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
pos_sp_triplet->current.velocity_valid = use_velocity;
pos_sp_triplet->current.vx = _current_vel(0);
pos_sp_triplet->current.vy = _current_vel(1);
pos_sp_triplet->next.valid = false;
pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate);
pos_sp_triplet->current.yawspeed = yaw_rate;
_navigator->set_position_setpoint_triplet_updated();
}
void FollowTarget::reset_target_validity()
{
_yaw_rate = NAN;
_previous_target_motion = {};
_current_target_motion = {};
_target_updates = 0;
_current_vel.zero();
_step_vel.zero();
_est_target_vel.zero();
_target_distance.zero();
_target_position_offset.zero();
reset_mission_item_reached();
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
bool FollowTarget::target_velocity_valid()
{
// need at least 2 continuous data points for velocity estimate
return (_target_updates >= 2);
}
bool FollowTarget::target_position_valid()
{
// need at least 1 continuous data points for position estimate
return (_target_updates >= 1);
}
void
FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target,
float yaw)
{
if (_navigator->get_land_detected()->landed) {
/* landed, don't takeoff, but switch to IDLE mode */
item->nav_cmd = NAV_CMD_IDLE;
} else {
item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
/* use current target position */
item->lat = target.lat;
item->lon = target.lon;
item->altitude = _navigator->get_home_position()->alt;
if (min_clearance > 8.0f) {
item->altitude += min_clearance;
} else {
item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)
}
}
item->altitude_is_relative = false;
item->yaw = yaw;
item->loiter_radius = _navigator->get_loiter_radius();
item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}

148
src/modules/navigator/follow_target.h
View File

@ -1,148 +0,0 @@
/***************************************************************************
*
* Copyright (c) 2016 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file followme.cpp
*
* Helper class to track and follow a given position
*
* @author Jimmy Johnson <catch22@fastmail.net>
*/
#pragma once
#include "navigator_mode.h"
#include "mission_block.h"
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/follow_target.h>
class FollowTarget : public MissionBlock, public ModuleParams
{
public:
FollowTarget(Navigator *navigator);
~FollowTarget() = default;
void on_inactive() override;
void on_activation() override;
void on_active() override;
private:
static constexpr int TARGET_TIMEOUT_MS = 2500;
static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
static constexpr int INTERPOLATION_PNTS = 20;
static constexpr float FF_K = .25F;
static constexpr float OFFSET_M = 8;
enum FollowTargetState {
TRACK_POSITION,
TRACK_VELOCITY,
SET_WAIT_FOR_TARGET_POSITION,
WAIT_FOR_TARGET_POSITION
};
enum {
FOLLOW_FROM_RIGHT,
FOLLOW_FROM_BEHIND,
FOLLOW_FROM_FRONT,
FOLLOW_FROM_LEFT
};
static constexpr float _follow_position_matricies[4][9] = {
{ 1.0F, -1.0F, 0.0F, 1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow right
{-1.0F, 0.0F, 0.0F, 0.0F, -1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow behind
{ 1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow front
{ 1.0F, 1.0F, 0.0F, -1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F} // follow left side
};
DEFINE_PARAMETERS(
(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,
(ParamFloat<px4::params::NAV_FT_RS>) _param_nav_ft_rs
)
FollowTargetState _follow_target_state{SET_WAIT_FOR_TARGET_POSITION};
int _follow_target_position{FOLLOW_FROM_BEHIND};
uORB::Subscription _follow_target_sub{ORB_ID(follow_target)};
float _step_time_in_ms{0.0f};
float _follow_offset{OFFSET_M};
uint64_t _target_updates{0};
uint64_t _last_update_time{0};
matrix::Vector3f _current_vel;
matrix::Vector3f _step_vel;
matrix::Vector3f _est_target_vel;
matrix::Vector3f _target_distance;
matrix::Vector3f _target_position_offset;
matrix::Vector3f _target_position_delta;
matrix::Vector3f _filtered_target_position_delta;
follow_target_s _current_target_motion{};
follow_target_s _previous_target_motion{};
float _yaw_rate{0.0f};
float _responsiveness{0.0f};
float _yaw_angle{0.0f};
// Mavlink defined motion reporting capabilities
enum {
POS = 0,
VEL = 1,
ACCEL = 2,
ATT_RATES = 3
};
matrix::Dcmf _rot_matrix;
void track_target_position();
void track_target_velocity();
bool target_velocity_valid();
bool target_position_valid();
void reset_target_validity();
void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate);
void update_target_motion();
void update_target_velocity();
/**
* Set follow_target item
*/
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
};

3
src/modules/navigator/mission_block.cpp
View File

@ -577,8 +577,7 @@ MissionBlock::item_contains_position(const mission_item_s &item)
item.nav_cmd == NAV_CMD_TAKEOFF ||
item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
item.nav_cmd == NAV_CMD_VTOL_LAND ||
item.nav_cmd == NAV_CMD_DO_FOLLOW_REPOSITION;
item.nav_cmd == NAV_CMD_VTOL_LAND;
}
bool

1
src/modules/navigator/navigation.h
View File

@ -66,7 +66,6 @@ enum NAV_CMD {
NAV_CMD_LAND = 21,
NAV_CMD_TAKEOFF = 22,
NAV_CMD_LOITER_TO_ALT = 31,
NAV_CMD_DO_FOLLOW_REPOSITION = 33,
NAV_CMD_VTOL_TAKEOFF = 84,
NAV_CMD_VTOL_LAND = 85,
NAV_CMD_DELAY = 93,

2
src/modules/navigator/navigator.h
View File

@ -41,7 +41,6 @@
#pragma once
#include "follow_target.h"
#include "geofence.h"
#include "land.h"
#include "precland.h"
@ -392,7 +391,6 @@ private:
Land _land; /**< class for handling land commands */
PrecLand _precland; /**< class for handling precision land commands */
RTL _rtl; /**< class that handles RTL */
FollowTarget _follow_target;
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE] {}; /**< array of navigation modes */

10
src/modules/navigator/navigator_main.cpp
View File

@ -77,8 +77,7 @@ Navigator::Navigator() :
_vtol_takeoff(this),
_land(this),
_precland(this),
_rtl(this),
_follow_target(this)
_rtl(this)
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
@ -88,7 +87,6 @@ Navigator::Navigator() :
_navigation_mode_array[4] = &_land;
_navigation_mode_array[5] = &_precland;
_navigation_mode_array[6] = &_vtol_takeoff;
_navigation_mode_array[7] = &_follow_target;
_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
_handle_reverse_delay = param_find("VT_B_REV_DEL");
@ -428,6 +426,7 @@ void Navigator::run()
// If one of them is non-finite set the current global position as target
rep->current.lat = get_global_position()->lat;
rep->current.lon = get_global_position()->lon;
}
rep->current.alt = cmd.param7;
@ -698,11 +697,6 @@ void Navigator::run()
_precland.set_mode(PrecLandMode::Required);
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_follow_target;
break;
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:

2
test/mavsdk_tests/CMakeLists.txt
View File

@ -17,6 +17,7 @@ if(MAVSDK_FOUND)
test_main.cpp
autopilot_tester.cpp
autopilot_tester_failure.cpp
autopilot_tester_follow_me.cpp
test_multicopter_control_allocation.cpp
test_multicopter_failure_injection.cpp
test_multicopter_failsafe.cpp
@ -24,6 +25,7 @@ if(MAVSDK_FOUND)
test_multicopter_offboard.cpp
test_multicopter_manual.cpp
test_vtol_mission.cpp
test_multicopter_follow_me.cpp
)
target_link_libraries(mavsdk_tests

8
test/mavsdk_tests/autopilot_tester.cpp
View File

@ -37,6 +37,7 @@
#include <future>
#include <thread>
#include <unistd.h>
#include <cmath>
std::string connection_url {"udp://"};
std::optional<float> speed_factor {std::nullopt};
@ -64,7 +65,7 @@ void AutopilotTester::connect(const std::string uri)
REQUIRE(poll_condition_with_timeout(
[this]() { return _mavsdk.systems().size() > 0; }, std::chrono::seconds(25)));
auto system = _mavsdk.systems().at(0);
auto system = get_system();
_action.reset(new Action(system));
_failure.reset(new Failure(system));
@ -534,6 +535,11 @@ void AutopilotTester::check_tracks_mission(float corridor_radius_m)
});
}
void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float max_distance_m)
{
CHECK(std::abs(_telemetry->position().relative_altitude_m - target_rel_altitude_m) <= max_distance_m);
}
std::array<float, 3> AutopilotTester::get_current_position_ned()
{
mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();

58
test/mavsdk_tests/autopilot_tester.h
View File

@ -135,6 +135,7 @@ public:
void check_tracks_mission(float corridor_radius_m = 1.5f);
void start_checking_altitude(const float max_deviation_m);
void stop_checking_altitude();
void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f);
// Blocking call to get the drone's current position in NED frame
std::array<float, 3> get_current_position_ned();
@ -147,7 +148,41 @@ public:
protected:
mavsdk::Param *getParams() const { return _param.get();}
mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();}
mavsdk::ManualControl *getManualControl() const { return _manual_control.get();}
std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0);}
Telemetry::GroundTruth getHome()
{
// Check if home was stored before it is accessed
CHECK(_home.absolute_altitude_m != NAN);
CHECK(_home.latitude_deg != NAN);
CHECK(_home.longitude_deg != NAN);
return _home;
}
template<typename Rep, typename Period>
void sleep_for(std::chrono::duration<Rep, Period> duration)
{
const std::chrono::microseconds duration_us(duration);
if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) {
const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us;
while (true) {
// Hopefully this is often enough not to have PX4 time out on us.
std::this_thread::sleep_for(std::chrono::milliseconds(1));
const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
if (elapsed_time_us > duration_us.count()) {
return;
}
}
} else {
std::this_thread::sleep_for(duration);
}
}
private:
mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();
@ -218,30 +253,7 @@ private:
return true;
}
template<typename Rep, typename Period>
void sleep_for(std::chrono::duration<Rep, Period> duration)
{
const std::chrono::microseconds duration_us(duration);
if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) {
const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us;
while (true) {
// Hopefully this is often enough not to have PX4 time out on us.
std::this_thread::sleep_for(std::chrono::milliseconds(1));
const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
if (elapsed_time_us > duration_us.count()) {
return;
}
}
} else {
std::this_thread::sleep_for(duration);
}
}
mavsdk::Mavsdk _mavsdk{};
std::unique_ptr<mavsdk::Action> _action{};

453
test/mavsdk_tests/autopilot_tester_follow_me.cpp Normal file
View File

@ -0,0 +1,453 @@
/****************************************************************************
*
* Copyright (c) 2021 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "autopilot_tester_follow_me.h"
// #include <mavsdk/plugins/follow_me/follow_me.h>
#include "math_helpers.h"
#include <iostream>
#include <future>
#include <thread>
#include <unistd.h>
#include <cmath>
#include <random>
FollowTargetSimulator::FollowTargetSimulator(std::array<float, 3> initial_position_ned,
mavsdk::Telemetry::GroundTruth home) :
_position_ned(initial_position_ned), _home(home)
{
_velocity_ned[0] = 0.0f;
_velocity_ned[1] = 0.0f;
_velocity_ned[2] = 0.0f;
}
FollowTargetSimulator::~FollowTargetSimulator()
{
}
void FollowTargetSimulator::update(float delta_t_s)
{
const float velocity_m_s = 2.0;
_velocity_ned[0] = velocity_m_s;
_velocity_ned[1] = 0.0;
_position_ned[0] += _velocity_ned[0] * delta_t_s;
_position_ned[1] += _velocity_ned[1] * delta_t_s;
_position_ned[2] += _velocity_ned[2] * delta_t_s;
_udpate_count++;
}
std::array<double, 3> FollowTargetSimulator::get_position_global(bool add_noise)
{
std::array<float, 3> pos_ned = _position_ned;
if (add_noise) {
unsigned seed = _udpate_count;
std::default_random_engine generator(seed);
std::normal_distribution<double> distribution(0.0, 1.0);
pos_ned[0] += distribution(generator);
pos_ned[1] += distribution(generator);
pos_ned[2] += distribution(generator);
}
CHECK(std::isfinite(_home.latitude_deg));
CHECK(std::isfinite(_home.longitude_deg));
const auto ct = CoordinateTransformation({_home.latitude_deg, _home.longitude_deg});
mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate{pos_ned[0], pos_ned[1]};
mavsdk::geometry::CoordinateTransformation::GlobalCoordinate global_coordinate = ct.global_from_local(local_coordinate);
std::array<double, 3> global_pos{global_coordinate.latitude_deg, global_coordinate.longitude_deg, pos_ned[2] + _home.absolute_altitude_m};
return global_pos;
}
std::array<float, 3> FollowTargetSimulator::get_position_ned(bool add_noise)
{
std::array<float, 3> pos_ned = _position_ned;
if (add_noise) {
unsigned seed = _udpate_count;
std::default_random_engine generator(seed);
std::normal_distribution<double> distribution(0.0, pos_noise_std);
pos_ned[0] += distribution(generator);
pos_ned[1] += distribution(generator);
pos_ned[2] += distribution(generator);
}
return pos_ned;
}
std::array<float, 3> FollowTargetSimulator::get_velocity_ned_noisy()
{
return get_velocity_ned(true);
}
std::array<float, 3> FollowTargetSimulator::get_velocity_ned_ground_truth()
{
return get_velocity_ned(false);
}
std::array<float, 3> FollowTargetSimulator::get_velocity_ned(bool add_noise)
{
std::array<float, 3> vel_ned = _velocity_ned;
if (add_noise) {
unsigned seed = _udpate_count;
std::default_random_engine generator(seed);
std::normal_distribution<double> distribution(0.0, vel_noise_std);
vel_ned[0] += distribution(generator);
vel_ned[1] += distribution(generator);
vel_ned[2] += distribution(generator);
}
return vel_ned;
}
std::array<float, 3> FollowTargetSimulator::get_position_ned_noisy()
{
return get_position_ned(true);
}
std::array<float, 3> FollowTargetSimulator::get_position_ground_truth_ned()
{
return get_position_ned(false);
}
std::array<double, 3> FollowTargetSimulator::get_position_global_noisy()
{
return get_position_global(true);
}
std::array<double, 3> FollowTargetSimulator::get_position_global_ground_truth()
{
return get_position_global(false);
}
void FollowTargetSimulator::check_follow_angle(FollowMe::Config config, std::array<float, 3> drone_pos_ned,
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];
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;
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;
}
}
void AutopilotTesterFollowMe::connect(const std::string uri)
{
AutopilotTester::connect(uri);
auto system = get_system();
_follow_me.reset(new FollowMe(system));
}
void AutopilotTesterFollowMe::straight_line_test(const float altitude_m, const bool stream_velocity)
{
const unsigned location_update_rate = 1;
const float position_tolerance = 6.0f;
// 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".
FollowMe::Config config;
config.min_height_m = altitude_m;
config.follow_distance_m = 8.0f;
// Allow some time for mode switch
sleep_for(std::chrono::milliseconds(1000));
// Start Follow Me
CHECK(FollowMe::Result::Success == _follow_me->start());
// Allow some time for mode switch
sleep_for(std::chrono::milliseconds(1000));
// task loop
bool target_moving = false;
bool perform_checks = false;
for (unsigned i = 0; i < 75 * location_update_rate; ++i) {
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) {
// Stream target location without moving
} else if (i == 5) {
// Change config
perform_checks = false;
config.follow_direction = FollowMe::Config::FollowDirection::Behind;
CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
} else if (i > 5 && i < 15) {
// Move target and wait for steady state of drone
target_moving = true;
} else if (i >= 15 && i < 20) {
// Perform positional checks in steady state
perform_checks = true;
} else if (i == 20) {
// Change config
perform_checks = false;
config.follow_direction = FollowMe::Config::FollowDirection::Front;
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 (i >= 30 && i < 35) {
// Perform positional checks in steady state
perform_checks = true;
} else if (i == 35) {
// Change config
perform_checks = false;
config.follow_direction = FollowMe::Config::FollowDirection::FrontRight;
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 (i >= 45 && i < 55) {
// Perform positional checks in steady state
perform_checks = true;
} else if (i == 55) {
// Change config
perform_checks = false;
config.follow_direction = FollowMe::Config::FollowDirection::FrontLeft;
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 (i >= 65 && i < 75) {
// Perform positional checks in steady state
perform_checks = true;
}
if (target_moving) {
target_simulator.update(1.0f / location_update_rate);
}
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);
}
// Construct follow-me message
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];
if (stream_velocity) {
std::array<float, 3> target_vel_ned = target_simulator.get_velocity_ned_noisy();
target_location.velocity_x_m_s = target_vel_ned[0];
target_location.velocity_y_m_s = target_vel_ned[1];
target_location.velocity_z_m_s = target_vel_ned[2];
} else {
target_location.velocity_x_m_s = NAN;
target_location.velocity_y_m_s = NAN;
target_location.velocity_z_m_s = NAN;
}
// Send message and check result
CHECK(FollowMe::Result::Success == _follow_me->set_target_location(target_location));
sleep_for(std::chrono::milliseconds(1000 / location_update_rate));
}
CHECK(FollowMe::Result::Success == _follow_me->stop());
}
void AutopilotTesterFollowMe::stream_velocity_only()
{
const unsigned loop_update_rate = 1;
const float position_tolerance = 4.0f;
// Configure follow-me
FollowMe::Config config;
config.follow_direction = FollowMe::Config::FollowDirection::Behind;
CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
// Allow some time for mode switch
sleep_for(std::chrono::milliseconds(1000));
// Start Follow Me
CHECK(FollowMe::Result::Success == _follow_me->start());
// Allow some time for mode switch
sleep_for(std::chrono::milliseconds(1000));
std::array<float, 3> drone_initial_pos = get_current_position_ned();
// Start streaming velocity only. The drone should not move.
for (unsigned i = 0; i < 15 * loop_update_rate; i++) {
FollowMe::TargetLocation target_location{};
target_location.latitude_deg = NAN;
target_location.longitude_deg = NAN;
target_location.absolute_altitude_m = NAN;
target_location.velocity_x_m_s = 2.0f;
target_location.velocity_y_m_s = 1.0f;
target_location.velocity_z_m_s = 0.5f;
// Send message and check result
CHECK(FollowMe::Result::Success == _follow_me->set_target_location(target_location));
sleep_for(std::chrono::milliseconds(1000 / loop_update_rate));
}
// Check that drone is still close to initial position and has not moved much
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);
}
void AutopilotTesterFollowMe::rc_override_test(const float altitude_m)
{
const unsigned loop_update_rate = 50;
const float position_tolerance = 4.0f;
// 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".
FollowMe::Config config;
config.min_height_m = altitude_m;
config.follow_distance_m = 8.0f;
config.follow_direction = FollowMe::Config::FollowDirection::Behind;
CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
// task loop
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);
// Deflect a stick for a short time only
bool deflect_rc_sticks = (i > 10 * loop_update_rate && i <= 11 * loop_update_rate);
// Switch to follow-me at this instant
if (i == 5 * loop_update_rate) {
// Start Follow Me
CHECK(FollowMe::Result::Success == _follow_me->start());
}
// After approximately 10 seconds we would expect the drone to have stopped because of the RC stick input
if (i == 20 * loop_update_rate) {
drone_initial_pos = get_current_position_ned();
}
if (stream_follow_me_data) {
target_simulator.update(1.0f / loop_update_rate);
std::array<double, 3> global_coordinate = target_simulator.get_position_global_noisy();
FollowMe::TargetLocation target_location{};
target_location.latitude_deg = global_coordinate[0];
target_location.longitude_deg = global_coordinate[1];
target_location.absolute_altitude_m = global_coordinate[2];
target_location.velocity_x_m_s = NAN;
target_location.velocity_y_m_s = NAN;
target_location.velocity_z_m_s = NAN;
_follow_me->set_target_location(target_location);
}
if (deflect_rc_sticks) {
CHECK(getManualControl()->set_manual_control_input(1.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
} else {
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
}
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);
}

121
test/mavsdk_tests/autopilot_tester_follow_me.h Normal file
View File

@ -0,0 +1,121 @@
/****************************************************************************
*
* Copyright (c) 2021 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "autopilot_tester.h"
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/follow_me/follow_me.h>
// Simulated a target moving on a straight line
class FollowTargetSimulator
{
public:
FollowTargetSimulator(std::array<float, 3> initial_position_ned, mavsdk::Telemetry::GroundTruth home);
~FollowTargetSimulator();
// Integrate simulator by one time step
// This moves the target on a line
void update(float delta_t_s);
// Retrieve noisy version of position state in NED coordinate frame
// The noise is deterministic and changes whenever the update() function is called
std::array<float, 3> get_position_ned_noisy();
// Retrieve ground truth of position state in NED coordinate frame
std::array<float, 3> get_position_ground_truth_ned();
// Retrieve noisy version of velocity state in NED coordinate frame
// The noise is deterministic and changes whenever the update() function is called
std::array<float, 3> get_velocity_ned_noisy();
// Retrieve ground truth of velocity state in NED coordinate frame
std::array<float, 3> get_velocity_ned_ground_truth();
// Retrieve noisy version of position state in global coordinate frame (lat, lon, alt)
// The noise is deterministic and changes whenever the update() function is called
std::array<double, 3> get_position_global_noisy();
// Retrieve ground truth of position state in global coordinate frame (lat, lon, alt)
std::array<double, 3> get_position_global_ground_truth();
// Run checks whether the drone has the correct angle towards the target, specified by the follow-me configuration
void check_follow_angle(FollowMe::Config config, std::array<float, 3> drone_pos_ned,
std::array<float, 3> target_pos_ned, float tolerance);
private:
// Retrieve estimate with the option to add deterministic gaussian noise
//
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
std::array<double, 3> get_position_global(bool add_noise);
// Retrieve estimate with the option to add deterministic gaussian noise
//
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
std::array<float, 3> get_position_ned(bool add_noise);
// Retrieve estimate with the option to add deterministic gaussian noise
//
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
std::array<float, 3> get_velocity_ned(bool add_noise);
std::array<float, 3> _position_ned;
std::array<float, 3> _velocity_ned;
mavsdk::Telemetry::GroundTruth _home;
size_t _udpate_count = 0;
const double pos_noise_std = 0.3;
const double vel_noise_std = 0.1;
std::unique_ptr<mavsdk::Telemetry> _telemetry{};
};
class AutopilotTesterFollowMe : public AutopilotTester
{
public:
AutopilotTesterFollowMe() = default;
~AutopilotTesterFollowMe() = default;
void connect(const std::string uri);
void straight_line_test(const float altitude_m, const bool stream_velocity);
void stream_velocity_only();
void rc_override_test(const float altitude_m);
private:
std::unique_ptr<mavsdk::FollowMe> _follow_me{};
};

129
test/mavsdk_tests/test_multicopter_follow_me.cpp Normal file
View File

@ -0,0 +1,129 @@
/****************************************************************************
*
* Copyright (c) 2021 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <chrono>
#include "autopilot_tester_follow_me.h"
TEST_CASE("Follow target - Position streaming", "[multicopter]")
{
const bool stream_velocity = false;
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
tester.arm();
const float takeoff_altitude = 10.0;
tester.set_takeoff_altitude(takeoff_altitude);
tester.takeoff();
tester.wait_until_hovering();
tester.straight_line_test(takeoff_altitude, stream_velocity);
tester.land();
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Follow target - Position and velocity streaming", "[multicopter]")
{
const bool stream_velocity = true;
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
tester.arm();
const float takeoff_altitude = 10.0;
tester.set_takeoff_altitude(takeoff_altitude);
tester.takeoff();
tester.wait_until_hovering();
tester.straight_line_test(takeoff_altitude, stream_velocity);
tester.land();
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Follow target - Velocity streaming only", "[multicopter]")
{
// Streaming only velocity should not work, the drone should not move.
// There needs to be at least one position message for follow-me
// to be able to integrate velocity.
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
tester.arm();
const float takeoff_altitude = 10.0;
tester.set_takeoff_altitude(takeoff_altitude);
tester.takeoff();
tester.wait_until_hovering();
tester.stream_velocity_only();
tester.land();
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
tester.wait_until_disarmed(until_disarmed_timeout);
tester.check_home_within(1.0f);
}
TEST_CASE("Follow target - Manual takeover", "[multicopter]")
{
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
tester.arm();
const float takeoff_altitude = 10.0;
tester.set_takeoff_altitude(takeoff_altitude);
tester.takeoff();
tester.wait_until_hovering();
tester.rc_override_test(takeoff_altitude);
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]")
{
}