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

follow target safety updates

Browse Source
This commit is contained in:
Jimmy Johnson 2016-04-11 21:30:05 -07:00
parent f3ee22b22c
commit 94f3c50f83
3 changed files with 130 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -45,7 +45,9 @@
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>
#include <float.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
@ -58,20 +60,24 @@
FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_navigator(navigator),
_param_min_alt(this, "MIS_TAKEOFF_ALT", false),
_param_min_alt(this, "NAV_MIN_FT_HT", false),
_follow_target_state(WAIT_FOR_TARGET_POSITION),
_follow_target_position(FOLLOW_FROM_BEHIND),
_follow_target_sub(-1),
_step_time_in_ms(0.0f),
_target_updates(0),
_last_update_time(0),
_current_target_motion( {}),
_previous_target_motion({})
_previous_target_motion({})
{
updateParams();
_current_vel.zero();
_step_vel.zero();
_target_vel.zero();
_target_distance.zero();
_target_position_offset.zero();
_rot_matrix = (_follow_position_matricies[_follow_target_position]);
}
FollowTarget::~FollowTarget()
@ -94,11 +100,13 @@ void FollowTarget::on_active()
{
struct map_projection_reference_s target_ref;
math::Vector<3> target_position(0, 0, 0);
follow_target_s target_motion = {};
uint64_t current_time = hrt_absolute_time();
bool _radius_entered = false;
bool _radius_exited = false;
bool updated = false;
float dt_ms = 0;
float yaw_angle = NAN;
orb_check(_follow_target_sub, &updated);
@ -112,43 +120,11 @@ void FollowTarget::on_active()
orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion);
} else if (((current_time - _previous_target_motion.timestamp) / 1000 / 1000) > TARGET_TIMEOUT_S
} else if (((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS
&& target_velocity_valid()) {
reset_target_validity();
}
// update target velocity
if (target_velocity_valid() && updated) {
dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);
// get last gps known reference for target
map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
// calculate distance the target has moved
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)),
&(target_position(1)));
// update the average velocity of the target based on the position
_target_vel = target_position / (dt_ms / 1000.0f);
// 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 to the target
_step_vel = (_target_vel - _current_vel) + _target_distance * FF_K;
_step_vel /= (dt_ms / 1000.0f * (float) INTERPOLATION_PNTS);
_step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS;
}
// update distance to target
if (target_position_valid()) {
@ -159,12 +135,84 @@ void FollowTarget::on_active()
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
&_target_distance(1));
target_motion = _current_target_motion;
// use target offset
map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon);
map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion.lat, &target_motion.lon);
// 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_distance.length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
_radius_entered = (_target_distance.length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
_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);
}
// update target velocity
if (target_velocity_valid() && updated) {
dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);
// protect from to small of a dt
if (dt_ms > 10) {
// get last gps known reference for target
map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
// calculate distance the target has moved
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)), &(target_position(1)));
// only calculate new velocity if target has moved
if(target_position.length() > 0.0F) {
// track to the left, right, behind, or front
_target_position_offset = _rot_matrix * (target_position.normalized() * OFFSET_M);
// update the average velocity of the target based on the position
_target_vel = target_position / (dt_ms / 1000.0f);
// 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 to the target
_step_vel = (_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;
}
}
}
// always point towards target
if (target_position_valid() && updated) {
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);
// warnx(" lat %f (%f) lon %f (%f), dist x %f y %f (%f) yaw = %f mode = %d",
// target_motion.lat,
// (double )_navigator->get_global_position()->lat,
// target_motion.lon,
// (double ) _navigator->get_global_position()->lon,
// (double ) _target_distance(0),
// (double ) _target_distance(1),
// (double ) _target_distance.length(),
// (double) yaw_angle,
// _follow_target_state);
}
// update state machine
@ -177,10 +225,12 @@ void FollowTarget::on_active()
_follow_target_state = TRACK_VELOCITY;
} else if (target_velocity_valid()) {
set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
// keep the current velocity updated with the target velocity for when it's needed
_current_vel = _target_vel;
update_position_sp(true, true);
} else {
_follow_target_state = WAIT_FOR_TARGET_POSITION;
}
break;
@ -192,7 +242,7 @@ void FollowTarget::on_active()
_follow_target_state = TRACK_POSITION;
} else if (target_velocity_valid()) {
set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle);
if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) {
_current_vel += _step_vel;
@ -200,6 +250,8 @@ void FollowTarget::on_active()
}
update_position_sp(true, false);
} else {
_follow_target_state = WAIT_FOR_TARGET_POSITION;
}
break;
@ -248,7 +300,6 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position)
pos_sp_triplet->current.vx = _current_vel(0);
pos_sp_triplet->current.vy = _current_vel(1);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
}
@ -261,18 +312,19 @@ void FollowTarget::reset_target_validity()
_step_vel.zero();
_target_vel.zero();
_target_distance.zero();
_target_position_offset.zero();
reset_mission_item_reached();
_follow_target_state = WAIT_FOR_TARGET_POSITION;
}
bool FollowTarget::target_velocity_valid()
{
// need at least 2 data points for velocity estimate
return (_target_updates >= 2);
// need at least 5 continuous data points for velocity estimate
return (_target_updates >= 5);
}
bool FollowTarget::target_position_valid()
{
// need at least 1 data point for position estimate
return (_target_updates >= 1);
// need at least 2 continuous data points for position estimate
return (_target_updates >= 2);
}

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

@ -43,7 +43,7 @@
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <lib/mathlib/math/Vector.hpp>
#include <lib/mathlib/math/Matrix.hpp>
#include "navigator_mode.h"
#include "mission_block.h"
@ -60,10 +60,11 @@ public:
private:
static constexpr int TARGET_TIMEOUT_S = 5;
static constexpr int TARGET_TIMEOUT_MS = 1500;
static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
static constexpr int INTERPOLATION_PNTS = 20;
static constexpr float FF_K = .15f;
static constexpr float FF_K = .40F;
static constexpr float OFFSET_M = 8;
enum FollowTargetState {
TRACK_POSITION,
@ -71,9 +72,36 @@ private:
WAIT_FOR_TARGET_POSITION
};
enum FollowTargetPosition {
FOLLOW_FROM_RIGHT,
FOLLOW_FROM_BEHIND,
FOLLOW_FROM_FRONT,
FOLLOW_FROM_LEFT
};
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
Navigator *_navigator;
control::BlockParamFloat _param_min_alt;
FollowTargetState _follow_target_state;
FollowTargetPosition _follow_target_position;
int _follow_target_sub;
float _step_time_in_ms;
@ -85,10 +113,11 @@ private:
math::Vector<3> _step_vel;
math::Vector<3> _target_vel;
math::Vector<3> _target_distance;
math::Vector<3> _target_position_offset;
follow_target_s _current_target_motion;
follow_target_s _previous_target_motion;
math::Matrix<3,3> _rot_matrix;
void track_target_position();
void track_target_velocity();
bool target_velocity_valid();

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

@ -61,14 +61,13 @@ enum NAV_CMD {
NAV_CMD_TAKEOFF = 22,
NAV_CMD_ROI = 80,
NAV_CMD_PATHPLANNING = 81,
NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder
NAV_CMD_GOTO_TAREGT = 195,
NAV_CMD_VTOL_TAKEOFF = 84,
NAV_CMD_VTOL_LAND = 85,
NAV_CMD_DO_JUMP = 177,
NAV_CMD_DO_CHANGE_SPEED = 178,
NAV_CMD_DO_SET_SERVO=183,
NAV_CMD_DO_REPEAT_SERVO=184,
NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder
NAV_CMD_DO_DIGICAM_CONTROL=203,
NAV_CMD_DO_SET_CAM_TRIGG_DIST=206,
NAV_CMD_DO_VTOL_TRANSITION=3000,