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

New follow-me flight task

Browse Source 
rename follow_me_status to follow_target_status

enable follow_target_estimator on skynode

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

Allow follow-me to be flown without RC

SITL tests for follow-me flight task

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

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

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

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

sitl: Increase position tolerance during follow-me

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

follow-me: gimbal control in follow-me

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

follow-me: use ground-dist for emergency ascent

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

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

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

follow-me: another fuzzy msg handling for the estimator

follow-me: bugfix in acceleration saturation limit

follow-me: parameter for filter delay compensation

mantis: dont use flow for terrain estimation

follow-me: default responsiveness 0.5 -> 0.1

0.5 is way too jerky in real and simulated tests.

flight_task: clarify comments for bottom distance

follow-me: minor comment improvement

follow-me: [debug] log emergency_ascent

follow-me: [debug] log gimbal pitch

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

follow-me: setting for gimbal tracking mode

follow-me: release gimbal control at destruction

mavsdk: cosmetics 💄
This commit is contained in:
Alessandro Simovic
2022-06-16 16:14:57 -04:00
committed by Daniel Agar
parent 285556e463
commit de1fa11e96
32 changed files with 2121 additions and 713 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/CMakeLists.txt
-1
View File
@@ -48,7 +48,6 @@ px4_add_module(
precland.cpp
mission_feasibility_checker.cpp
geofence.cpp
follow_target.cpp
vtol_takeoff.cpp
DEPENDS
geo
src/modules/navigator/follow_target.cpp
-407
View File
@@ -1,407 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file followme.cpp
*
* Helper class to track and follow a given position
*
* @author Jimmy Johnson <catch22@fastmail.net>
*/
#include "follow_target.h"
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/follow_target.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/math/Limits.hpp>
#include "navigator.h"
using namespace matrix;
constexpr float FollowTarget::_follow_position_matricies[4][9];
FollowTarget::FollowTarget(Navigator *navigator) :
MissionBlock(navigator),
ModuleParams(navigator)
{
_current_vel.zero();
_step_vel.zero();
_est_target_vel.zero();
_target_distance.zero();
_target_position_offset.zero();
_target_position_delta.zero();
}
void FollowTarget::on_inactive()
{
reset_target_validity();
}
void FollowTarget::on_activation()
{
_follow_offset = _param_nav_ft_dst.get() < 1.0F ? 1.0F : _param_nav_ft_dst.get();
_responsiveness = math::constrain((float) _param_nav_ft_rs.get(), .1F, 1.0F);
_follow_target_position = _param_nav_ft_fs.get();
if ((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) {
_follow_target_position = FOLLOW_FROM_BEHIND;
}
_rot_matrix = Dcmf(_follow_position_matricies[_follow_target_position]);
}
void FollowTarget::on_active()
{
MapProjection target_ref;
follow_target_s target_motion_with_offset = {};
uint64_t current_time = hrt_absolute_time();
bool radius_entered = false;
bool radius_exited = false;
bool updated = false;
float dt_ms = 0;
if (_follow_target_sub.updated()) {
updated = true;
follow_target_s target_motion;
_target_updates++;
// save last known motion topic for interpolation later
_previous_target_motion = _current_target_motion;
_follow_target_sub.copy(&target_motion);
if (_current_target_motion.timestamp == 0) {
_current_target_motion = target_motion;
}
_current_target_motion = target_motion;
_current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)(
1 - _responsiveness);
_current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)(
1 - _responsiveness);
} else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) {
reset_target_validity();
}
// update distance to target
if (target_position_valid()) {
// get distance to target
target_ref.initReference(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
_target_distance(0), _target_distance(1));
}
// update target velocity
if (target_velocity_valid() && updated) {
dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);
// ignore a small dt
if (dt_ms > 10.0F) {
// get last gps known reference for target
target_ref.initReference(_previous_target_motion.lat, _previous_target_motion.lon);
// calculate distance the target has moved
target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
(_target_position_delta(0)), (_target_position_delta(1)));
// update the average velocity of the target based on the position
if (PX4_ISFINITE(_current_target_motion.vx) && PX4_ISFINITE(_current_target_motion.vy)) {
// No need to estimate target velocity if we can take it from the target
_est_target_vel(0) = _current_target_motion.vx;
_est_target_vel(1) = _current_target_motion.vy;
_est_target_vel(2) = 0.0f;
} else {
_est_target_vel = _target_position_delta / (dt_ms / 1000.0f);
}
// if the target is moving add an offset and rotation
if (_est_target_vel.length() > .5F) {
_target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset;
}
// are we within the target acceptance radius?
// give a buffer to exit/enter the radius to give the velocity controller
// a chance to catch up
radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
// to keep the velocity increase/decrease smooth
// calculate how many velocity increments/decrements
// it will take to reach the targets velocity
// with the given amount of steps also add a feed forward input that adjusts the
// velocity as the position gap increases since
// just traveling at the exact velocity of the target will not
// get any closer or farther from the target
_step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
_step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS);
// if we are less than 1 meter from the target don't worry about trying to yaw
// lock the yaw until we are at a distance that makes sense
if ((_target_distance).length() > 1.0F) {
// yaw rate smoothing
// this really needs to control the yaw rate directly in the attitude pid controller
// but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode
_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_current_target_motion.lat,
_current_target_motion.lon);
_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f));
} else {
_yaw_angle = _yaw_rate = NAN;
}
}
// warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d yaw rate = %3.6f",
// (double) _step_vel(0),
// (double) _step_vel(1),
// (double) _current_vel(0),
// (double) _current_vel(1),
// (double) _est_target_vel(0),
// (double) _est_target_vel(1),
// (double) (_target_distance).length(),
// (double) (_target_position_offset + _target_distance).length(),
// _follow_target_state,
// (double) _yaw_rate);
}
if (target_position_valid()) {
// get the target position using the calculated offset
target_ref.initReference(_current_target_motion.lat, _current_target_motion.lon);
target_ref.reproject(_target_position_offset(0), _target_position_offset(1),
target_motion_with_offset.lat, target_motion_with_offset.lon);
}
// clamp yaw rate smoothing if we are with in
// 3 degrees of facing target
if (PX4_ISFINITE(_yaw_rate)) {
if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->heading)) < math::radians(3.0F)) {
_yaw_rate = NAN;
}
}
// update state machine
switch (_follow_target_state) {
case TRACK_POSITION: {
if (radius_entered) {
_follow_target_state = TRACK_VELOCITY;
} else if (target_velocity_valid()) {
set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle);
// keep the current velocity updated with the target velocity for when it's needed
_current_vel = _est_target_vel;
update_position_sp(true, true, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
break;
}
case TRACK_VELOCITY: {
if (radius_exited) {
_follow_target_state = TRACK_POSITION;
} else if (target_velocity_valid()) {
if ((float)(current_time - _last_update_time) / 1000.0f >= _step_time_in_ms) {
_current_vel += _step_vel;
_last_update_time = current_time;
}
set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle);
update_position_sp(true, false, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
break;
}
case SET_WAIT_FOR_TARGET_POSITION: {
// Climb to the minimum altitude
// and wait until a position is received
follow_target_s target = {};
// for now set the target at the minimum height above the uav
target.lat = _navigator->get_global_position()->lat;
target.lon = _navigator->get_global_position()->lon;
target.alt = 0.0F;
set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target, _yaw_angle);
update_position_sp(false, false, _yaw_rate);
_follow_target_state = WAIT_FOR_TARGET_POSITION;
}
/* FALLTHROUGH */
case WAIT_FOR_TARGET_POSITION: {
if (is_mission_item_reached() && target_velocity_valid()) {
_target_position_offset(0) = _follow_offset;
_follow_target_state = TRACK_POSITION;
}
break;
}
}
}
void FollowTarget::update_position_sp(bool use_velocity, bool use_position, float yaw_rate)
{
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// activate line following in pos control if position is valid
pos_sp_triplet->previous.valid = use_position;
pos_sp_triplet->previous = pos_sp_triplet->current;
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
pos_sp_triplet->current.velocity_valid = use_velocity;
pos_sp_triplet->current.vx = _current_vel(0);
pos_sp_triplet->current.vy = _current_vel(1);
pos_sp_triplet->next.valid = false;
pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate);
pos_sp_triplet->current.yawspeed = yaw_rate;
_navigator->set_position_setpoint_triplet_updated();
}
void FollowTarget::reset_target_validity()
{
_yaw_rate = NAN;
_previous_target_motion = {};
_current_target_motion = {};
_target_updates = 0;
_current_vel.zero();
_step_vel.zero();
_est_target_vel.zero();
_target_distance.zero();
_target_position_offset.zero();
reset_mission_item_reached();
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
bool FollowTarget::target_velocity_valid()
{
// need at least 2 continuous data points for velocity estimate
return (_target_updates >= 2);
}
bool FollowTarget::target_position_valid()
{
// need at least 1 continuous data points for position estimate
return (_target_updates >= 1);
}
void
FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target,
float yaw)
{
if (_navigator->get_land_detected()->landed) {
/* landed, don't takeoff, but switch to IDLE mode */
item->nav_cmd = NAV_CMD_IDLE;
} else {
item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
/* use current target position */
item->lat = target.lat;
item->lon = target.lon;
item->altitude = _navigator->get_home_position()->alt;
if (min_clearance > 8.0f) {
item->altitude += min_clearance;
} else {
item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)
}
}
item->altitude_is_relative = false;
item->yaw = yaw;
item->loiter_radius = _navigator->get_loiter_radius();
item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}
src/modules/navigator/follow_target.h
-148
View File
@@ -1,148 +0,0 @@
/***************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file followme.cpp
*
* Helper class to track and follow a given position
*
* @author Jimmy Johnson <catch22@fastmail.net>
*/
#pragma once
#include "navigator_mode.h"
#include "mission_block.h"
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/follow_target.h>
class FollowTarget : public MissionBlock, public ModuleParams
{
public:
FollowTarget(Navigator *navigator);
~FollowTarget() = default;
void on_inactive() override;
void on_activation() override;
void on_active() override;
private:
static constexpr int TARGET_TIMEOUT_MS = 2500;
static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
static constexpr int INTERPOLATION_PNTS = 20;
static constexpr float FF_K = .25F;
static constexpr float OFFSET_M = 8;
enum FollowTargetState {
TRACK_POSITION,
TRACK_VELOCITY,
SET_WAIT_FOR_TARGET_POSITION,
WAIT_FOR_TARGET_POSITION
};
enum {
FOLLOW_FROM_RIGHT,
FOLLOW_FROM_BEHIND,
FOLLOW_FROM_FRONT,
FOLLOW_FROM_LEFT
};
static constexpr float _follow_position_matricies[4][9] = {
{ 1.0F, -1.0F, 0.0F, 1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow right
{-1.0F, 0.0F, 0.0F, 0.0F, -1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow behind
{ 1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow front
{ 1.0F, 1.0F, 0.0F, -1.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F} // follow left side
};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_MIN_FT_HT>) _param_nav_min_ft_ht,
(ParamFloat<px4::params::NAV_FT_DST>) _param_nav_ft_dst,
(ParamInt<px4::params::NAV_FT_FS>) _param_nav_ft_fs,
(ParamFloat<px4::params::NAV_FT_RS>) _param_nav_ft_rs
)
FollowTargetState _follow_target_state{SET_WAIT_FOR_TARGET_POSITION};
int _follow_target_position{FOLLOW_FROM_BEHIND};
uORB::Subscription _follow_target_sub{ORB_ID(follow_target)};
float _step_time_in_ms{0.0f};
float _follow_offset{OFFSET_M};
uint64_t _target_updates{0};
uint64_t _last_update_time{0};
matrix::Vector3f _current_vel;
matrix::Vector3f _step_vel;
matrix::Vector3f _est_target_vel;
matrix::Vector3f _target_distance;
matrix::Vector3f _target_position_offset;
matrix::Vector3f _target_position_delta;
matrix::Vector3f _filtered_target_position_delta;
follow_target_s _current_target_motion{};
follow_target_s _previous_target_motion{};
float _yaw_rate{0.0f};
float _responsiveness{0.0f};
float _yaw_angle{0.0f};
// Mavlink defined motion reporting capabilities
enum {
POS = 0,
VEL = 1,
ACCEL = 2,
ATT_RATES = 3
};
matrix::Dcmf _rot_matrix;
void track_target_position();
void track_target_velocity();
bool target_velocity_valid();
bool target_position_valid();
void reset_target_validity();
void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate);
void update_target_motion();
void update_target_velocity();
/**
* Set follow_target item
*/
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
};
src/modules/navigator/follow_target_params.c
-91
View File
@@ -1,91 +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 follow_target_params.c
*
* Parameters for follow target mode
*
* @author Jimmy Johnson <catch22@fastmail.net>
*/
/*
* Follow target parameters
*/
/**
* Minimum follow target altitude
*
* The minimum height in meters relative to home for following a target
*
* @unit m
* @min 8.0
* @group Follow target
*/
PARAM_DEFINE_FLOAT(NAV_MIN_FT_HT, 8.0f);
/**
* Distance to follow target from
*
* The distance in meters to follow the target at
*
* @unit m
* @min 1.0
* @group Follow target
*/
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)
*
* @min 0
* @max 3
* @group Follow target
*/
PARAM_DEFINE_INT32(NAV_FT_FS, 1);
/**
* Dynamic filtering algorithm responsiveness to target movement
*
* lower numbers increase the responsiveness to changing long lat
* but also ignore less noise
*
* @min 0.0
* @max 1.0
* @decimal 2
* @group Follow target
*/
PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.5f);
src/modules/navigator/mission_block.cpp
+1 -2
View File
@@ -577,8 +577,7 @@ MissionBlock::item_contains_position(const mission_item_s &item)
item.nav_cmd == NAV_CMD_TAKEOFF ||
item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
item.nav_cmd == NAV_CMD_VTOL_LAND ||
item.nav_cmd == NAV_CMD_DO_FOLLOW_REPOSITION;
item.nav_cmd == NAV_CMD_VTOL_LAND;
}
bool
src/modules/navigator/navigation.h
-1
View File
@@ -66,7 +66,6 @@ enum NAV_CMD {
NAV_CMD_LAND = 21,
NAV_CMD_TAKEOFF = 22,
NAV_CMD_LOITER_TO_ALT = 31,
NAV_CMD_DO_FOLLOW_REPOSITION = 33,
NAV_CMD_VTOL_TAKEOFF = 84,
NAV_CMD_VTOL_LAND = 85,
NAV_CMD_DELAY = 93,
src/modules/navigator/navigator.h
-2
View File
@@ -41,7 +41,6 @@
#pragma once
#include "follow_target.h"
#include "geofence.h"
#include "land.h"
#include "precland.h"
@@ -392,7 +391,6 @@ private:
Land _land; /**< class for handling land commands */
PrecLand _precland; /**< class for handling precision land commands */
RTL _rtl; /**< class that handles RTL */
FollowTarget _follow_target;
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE] {}; /**< array of navigation modes */
src/modules/navigator/navigator_main.cpp
+2 -8
View File
@@ -77,8 +77,7 @@ Navigator::Navigator() :
_vtol_takeoff(this),
_land(this),
_precland(this),
_rtl(this),
_follow_target(this)
_rtl(this)
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
@@ -88,7 +87,6 @@ Navigator::Navigator() :
_navigation_mode_array[4] = &_land;
_navigation_mode_array[5] = &_precland;
_navigation_mode_array[6] = &_vtol_takeoff;
_navigation_mode_array[7] = &_follow_target;
_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
_handle_reverse_delay = param_find("VT_B_REV_DEL");
@@ -428,6 +426,7 @@ void Navigator::run()
// If one of them is non-finite set the current global position as target
rep->current.lat = get_global_position()->lat;
rep->current.lon = get_global_position()->lon;
}
rep->current.alt = cmd.param7;
@@ -698,11 +697,6 @@ void Navigator::run()
_precland.set_mode(PrecLandMode::Required);
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_follow_target;
break;
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL: