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]") +{ + +}