From de1fa11e968ce96a939020ae9194fd2ffe850411 Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Tue, 20 Jul 2021 09:11:17 +0200 Subject: [PATCH] New follow-me flight task 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 :lipstick: --- .../init.d/airframes/4061_atl_mantis_edu | 1 + msg/CMakeLists.txt | 2 + msg/follow_target.msg | 19 +- msg/follow_target_estimator.msg | 16 + msg/follow_target_status.msg | 5 + .../commander/state_machine_helper.cpp | 15 + .../flight_mode_manager/CMakeLists.txt | 2 +- .../flight_mode_manager/FlightModeManager.cpp | 2 +- .../AutoFollowMe/FlightTaskAutoFollowMe.cpp | 49 -- .../AutoFollowMe/FlightTaskAutoFollowMe.hpp | 50 -- .../tasks/AutoFollowTarget/CMakeLists.txt | 41 ++ .../FlightTaskAutoFollowTarget.cpp | 413 ++++++++++++++++ .../FlightTaskAutoFollowTarget.hpp | 215 +++++++++ .../follow_target_estimator}/CMakeLists.txt | 13 +- .../TargetEstimator.cpp | 358 ++++++++++++++ .../TargetEstimator.hpp | 220 +++++++++ .../AutoFollowTarget}/follow_target_params.c | 61 ++- .../tasks/FlightTask/FlightTask.hpp | 4 +- src/modules/logger/logged_topics.cpp | 5 +- src/modules/navigator/CMakeLists.txt | 1 - src/modules/navigator/follow_target.cpp | 407 ---------------- src/modules/navigator/follow_target.h | 148 ------ src/modules/navigator/mission_block.cpp | 3 +- src/modules/navigator/navigation.h | 1 - src/modules/navigator/navigator.h | 2 - src/modules/navigator/navigator_main.cpp | 10 +- test/mavsdk_tests/CMakeLists.txt | 2 + test/mavsdk_tests/autopilot_tester.cpp | 8 +- test/mavsdk_tests/autopilot_tester.h | 58 ++- .../autopilot_tester_follow_me.cpp | 453 ++++++++++++++++++ .../mavsdk_tests/autopilot_tester_follow_me.h | 121 +++++ .../test_multicopter_follow_me.cpp | 129 +++++ 32 files changed, 2121 insertions(+), 713 deletions(-) create mode 100644 msg/follow_target_estimator.msg create mode 100644 msg/follow_target_status.msg delete mode 100644 src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp delete mode 100644 src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp create mode 100644 src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt create mode 100644 src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp create mode 100644 src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp rename src/modules/flight_mode_manager/tasks/{AutoFollowMe => AutoFollowTarget/follow_target_estimator}/CMakeLists.txt (88%) create mode 100644 src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp create mode 100644 src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp rename src/modules/{navigator => flight_mode_manager/tasks/AutoFollowTarget}/follow_target_params.c (55%) delete mode 100644 src/modules/navigator/follow_target.cpp delete mode 100644 src/modules/navigator/follow_target.h create mode 100644 test/mavsdk_tests/autopilot_tester_follow_me.cpp create mode 100644 test/mavsdk_tests/autopilot_tester_follow_me.h create mode 100644 test/mavsdk_tests/test_multicopter_follow_me.cpp diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index de54602d02..56812f15d0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -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 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2064d91ba2..2ae9a92335 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -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 diff --git a/msg/follow_target.msg b/msg/follow_target.msg index f0afe3f4b8..e88c2460c8 100644 --- a/msg/follow_target.msg +++ b/msg/follow_target.msg @@ -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 diff --git a/msg/follow_target_estimator.msg b/msg/follow_target_estimator.msg new file mode 100644 index 0000000000..9d3df9f6f8 --- /dev/null +++ b/msg/follow_target_estimator.msg @@ -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 diff --git a/msg/follow_target_status.msg b/msg/follow_target_status.msg new file mode 100644 index 0000000000..379cfbf601 --- /dev/null +++ b/msg/follow_target_status.msg @@ -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 diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 002e71d6db..345b732310 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -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; } diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 1d8e82bf98..2a1762366b 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -53,7 +53,7 @@ endif() # add core flight tasks to list list(APPEND flight_tasks_all Auto - AutoFollowMe + AutoFollowTarget Descend Failsafe ManualAcceleration diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index a2c33b0997..aee8b60454 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -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) { diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp deleted file mode 100644 index 195472465a..0000000000 --- a/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp +++ /dev/null @@ -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 - -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; -} diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp deleted file mode 100644 index 733c8d3754..0000000000 --- a/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp +++ /dev/null @@ -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; -}; diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt new file mode 100644 index 0000000000..41085c5272 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt @@ -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}) diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp new file mode 100644 index 0000000000..89a79f5154 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp @@ -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 +#include + +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); +} diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp new file mode 100644 index 0000000000..d7a4a23ed2 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp @@ -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 + */ + +#pragma once + +#include "FlightTaskAuto.hpp" +#include "follow_target_estimator/TargetEstimator.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Speed above which the target heading can change. +// Used to prevent unpredictable jitter at low speeds. +static constexpr float MINIMUM_SPEED_FOR_HEADING_CHANGE = 0.1f; + +// 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 _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 _target_position_filter; + + // Lowpass filter for smoothing the offset vector and have more dynamic shots when target changes direction + AlphaFilter _offset_vector_filter; + + // Lowpass filter assuming values 0-1, for avoiding big steps in velocity feedforward + AlphaFilter _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) _param_mav_sys_id, + (ParamInt) _param_mav_comp_id, + (ParamFloat) _param_nav_min_ft_ht, + (ParamFloat) _param_nav_ft_dst, + (ParamInt) _param_nav_ft_fs, + (ParamInt) _param_nav_ft_alt_m, + (ParamInt) _param_nav_ft_gmb_m, + (ParamInt) _param_nav_ft_delc + ) + + uORB::Subscription _follow_target_estimator_sub{ORB_ID(follow_target_estimator)}; + + uORB::PublicationMulti _follow_target_status_pub{ORB_ID(follow_target_status)}; + uORB::PublicationMulti _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + + // Debugging + float _gimbal_pitch{0}; +}; diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt similarity index 88% rename from src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt index 2e757dc29f..5640a1c403 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt @@ -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 +) diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp new file mode 100644 index 0000000000..33f5354b4a --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp @@ -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 +#include +#include + +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 ¤t_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 TargetEstimator::get_lat_lon_alt_est() const +{ + Vector3 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); +} diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp new file mode 100644 index 0000000000..2e28357125 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp @@ -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 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +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 ¤t_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 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) _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_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}; +}; diff --git a/src/modules/navigator/follow_target_params.c b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c similarity index 55% rename from src/modules/navigator/follow_target_params.c rename to src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c index 4dc50bf6e5..e566898929 100644 --- a/src/modules/navigator/follow_target_params.c +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c @@ -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); diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 486c41651b..9591b70261 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -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. diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 1a0840006d..9da40bdc93 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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"); diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 5da49df860..1c658d09d8 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -48,7 +48,6 @@ px4_add_module( precland.cpp mission_feasibility_checker.cpp geofence.cpp - follow_target.cpp vtol_takeoff.cpp DEPENDS geo diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp deleted file mode 100644 index 56e9a35ec8..0000000000 --- a/src/modules/navigator/follow_target.cpp +++ /dev/null @@ -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 - */ - -#include "follow_target.h" - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#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; -} diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h deleted file mode 100644 index 25086dca8e..0000000000 --- a/src/modules/navigator/follow_target.h +++ /dev/null @@ -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 - */ - -#pragma once - -#include "navigator_mode.h" -#include "mission_block.h" - -#include -#include - -#include -#include -#include - -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) _param_nav_min_ft_ht, - (ParamFloat) _param_nav_ft_dst, - (ParamInt) _param_nav_ft_fs, - (ParamFloat) _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); -}; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index ddaa4501a6..6e864eb7a8 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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 diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 964ee05405..8da0df2c40 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -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, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index aeffa50fda..d4bb945890 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -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 */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ee52ed4a90..87ee640e47 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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: diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 01bdf58727..f1eca60043 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -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 diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index bf82c0a4b2..d7831dc79d 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -37,6 +37,7 @@ #include #include #include +#include std::string connection_url {"udp://"}; std::optional 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 AutopilotTester::get_current_position_ned() { mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned(); diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 941e09cbab..a36a70f7bf 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -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 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 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 + void sleep_for(std::chrono::duration 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 - void sleep_for(std::chrono::duration 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 _action{}; diff --git a/test/mavsdk_tests/autopilot_tester_follow_me.cpp b/test/mavsdk_tests/autopilot_tester_follow_me.cpp new file mode 100644 index 0000000000..98f40c872c --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_follow_me.cpp @@ -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 + +#include "math_helpers.h" +#include +#include +#include +#include +#include +#include + + +FollowTargetSimulator::FollowTargetSimulator(std::array 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 FollowTargetSimulator::get_position_global(bool add_noise) +{ + std::array pos_ned = _position_ned; + + if (add_noise) { + unsigned seed = _udpate_count; + std::default_random_engine generator(seed); + std::normal_distribution 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 global_pos{global_coordinate.latitude_deg, global_coordinate.longitude_deg, pos_ned[2] + _home.absolute_altitude_m}; + return global_pos; +} + +std::array FollowTargetSimulator::get_position_ned(bool add_noise) +{ + + std::array pos_ned = _position_ned; + + if (add_noise) { + unsigned seed = _udpate_count; + std::default_random_engine generator(seed); + std::normal_distribution 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 FollowTargetSimulator::get_velocity_ned_noisy() +{ + return get_velocity_ned(true); +} + +std::array FollowTargetSimulator::get_velocity_ned_ground_truth() +{ + return get_velocity_ned(false); +} + +std::array FollowTargetSimulator::get_velocity_ned(bool add_noise) +{ + std::array vel_ned = _velocity_ned; + + if (add_noise) { + unsigned seed = _udpate_count; + std::default_random_engine generator(seed); + std::normal_distribution 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 FollowTargetSimulator::get_position_ned_noisy() +{ + return get_position_ned(true); +} + +std::array FollowTargetSimulator::get_position_ground_truth_ned() +{ + return get_position_ned(false); +} + +std::array FollowTargetSimulator::get_position_global_noisy() +{ + return get_position_global(true); +} + +std::array FollowTargetSimulator::get_position_global_ground_truth() +{ + return get_position_global(false); +} + +void FollowTargetSimulator::check_follow_angle(FollowMe::Config config, std::array drone_pos_ned, + std::array target_pos_ned, float tolerance) +{ + // This check assumes that the target is travelling straight on the x-axis + const float x_dist_to_target = target_pos_ned[0] - drone_pos_ned[0]; + const float y_dist_to_target = target_pos_ned[1] - drone_pos_ned[1]; + + 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 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 target_pos_ned_ground_truth = target_simulator.get_position_ground_truth_ned(); + std::array position_ned = get_current_position_ned(); + const float distance_to_target = norm(diff(target_pos_ned_ground_truth, position_ned)); + + // poor-man's state machine + if (i < 5) { + // 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 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 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 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 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 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 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 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 drone_final_pos = get_current_position_ned(); + const float distance_travelled = norm(diff(drone_initial_pos, drone_final_pos)); + CHECK(distance_travelled < position_tolerance); +} diff --git a/test/mavsdk_tests/autopilot_tester_follow_me.h b/test/mavsdk_tests/autopilot_tester_follow_me.h new file mode 100644 index 0000000000..7a11e9770b --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_follow_me.h @@ -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 +#include +#include + + +// Simulated a target moving on a straight line +class FollowTargetSimulator +{ +public: + FollowTargetSimulator(std::array 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 get_position_ned_noisy(); + + // Retrieve ground truth of position state in NED coordinate frame + std::array 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 get_velocity_ned_noisy(); + + // Retrieve ground truth of velocity state in NED coordinate frame + std::array 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 get_position_global_noisy(); + + // Retrieve ground truth of position state in global coordinate frame (lat, lon, alt) + std::array 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 drone_pos_ned, + std::array 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 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 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 get_velocity_ned(bool add_noise); + + std::array _position_ned; + std::array _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 _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 _follow_me{}; +}; diff --git a/test/mavsdk_tests/test_multicopter_follow_me.cpp b/test/mavsdk_tests/test_multicopter_follow_me.cpp new file mode 100644 index 0000000000..84c2d40fa7 --- /dev/null +++ b/test/mavsdk_tests/test_multicopter_follow_me.cpp @@ -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 + +#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]") +{ + +}