mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-01 02:30:35 +08:00
making code more robust
This commit is contained in:
Jimmy Johnson
committed by
Lorenz Meier
Lorenz Meier
parent
6ff1b2d896
commit
f2777cb6ee
@@ -60,12 +60,10 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
|
||||
MissionBlock(navigator, name),
|
||||
_navigator(navigator),
|
||||
_param_min_alt(this, "MIS_TAKEOFF_ALT", false),
|
||||
_follow_target_state(ASCEND),
|
||||
_follow_target_state(WAIT_FOR_TARGET_POSITION),
|
||||
_follow_target_sub(-1),
|
||||
_step_time_in_ms(0.0f),
|
||||
_previous_target_gps_pos_valid(false),
|
||||
_radius_entered(false),
|
||||
_radius_exited(false),
|
||||
_target_updates(0),
|
||||
_last_update_time(0),
|
||||
_current_target_motion({}),
|
||||
_previous_target_motion({})
|
||||
@@ -83,8 +81,7 @@ FollowTarget::~FollowTarget()
|
||||
|
||||
void FollowTarget::on_inactive()
|
||||
{
|
||||
_previous_target_gps_pos_valid = false;
|
||||
_follow_target_state = ASCEND;
|
||||
reset_target_validity();
|
||||
}
|
||||
|
||||
void FollowTarget::on_activation()
|
||||
@@ -92,207 +89,183 @@ void FollowTarget::on_activation()
|
||||
if (_follow_target_sub < 0) {
|
||||
_follow_target_sub = orb_subscribe(ORB_ID(follow_target));
|
||||
}
|
||||
|
||||
update_target_motion();
|
||||
reset_mission_item_reached();
|
||||
}
|
||||
|
||||
void FollowTarget::pause()
|
||||
{
|
||||
math::Vector<3> vel(0, 0, 0);
|
||||
|
||||
_current_vel.zero();
|
||||
|
||||
set_loiter_item(&_mission_item, _param_min_alt.get());
|
||||
|
||||
update_position_sp(vel);
|
||||
|
||||
_current_target_motion.lat = _navigator->get_global_position()->lat;
|
||||
_current_target_motion.lon = _navigator->get_global_position()->lon;
|
||||
}
|
||||
|
||||
void FollowTarget::on_active()
|
||||
{
|
||||
void FollowTarget::on_active() {
|
||||
struct map_projection_reference_s target_ref;
|
||||
|
||||
update_target_motion();
|
||||
|
||||
// get distance to target
|
||||
|
||||
map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1));
|
||||
|
||||
// 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);
|
||||
|
||||
switch (_follow_target_state) {
|
||||
case TRACK_POSITION: {
|
||||
if (_radius_entered == true) {
|
||||
_follow_target_state = TRACK_VELOCITY;
|
||||
} else {
|
||||
track_target_position();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case TRACK_VELOCITY: {
|
||||
if (_radius_exited == true) {
|
||||
_follow_target_state = TRACK_POSITION;
|
||||
} else {
|
||||
track_target_velocity();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case ASCEND: {
|
||||
// ascend to the minimum altitude
|
||||
|
||||
pause();
|
||||
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
|
||||
if (is_mission_item_reached()) {
|
||||
_follow_target_state = TRACK_POSITION;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case TARGET_TIMEOUT: {
|
||||
// Loiter until signal is regained
|
||||
|
||||
pause();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
update_position_sp(_current_vel);
|
||||
}
|
||||
|
||||
void FollowTarget::track_target_position()
|
||||
{
|
||||
set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
|
||||
|
||||
// keep the current velocity updated
|
||||
|
||||
_current_vel = _target_vel;
|
||||
}
|
||||
|
||||
void FollowTarget::track_target_velocity()
|
||||
{
|
||||
|
||||
math::Vector<3> target_position(0, 0, 0);
|
||||
uint64_t current_time = hrt_absolute_time();
|
||||
|
||||
if (_previous_target_gps_pos_valid == false) {
|
||||
return;
|
||||
}
|
||||
|
||||
set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
|
||||
|
||||
if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) {
|
||||
_current_vel += _step_vel;
|
||||
_last_update_time = current_time;
|
||||
}
|
||||
}
|
||||
|
||||
void FollowTarget::update_target_motion()
|
||||
{
|
||||
bool updated;
|
||||
bool _radius_entered = false;
|
||||
bool _radius_exited = false;
|
||||
bool updated = false;
|
||||
float dt_ms = 0;
|
||||
|
||||
orb_check(_follow_target_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
|
||||
_target_updates++;
|
||||
|
||||
// save last known motion topic
|
||||
|
||||
_previous_target_motion = _current_target_motion;
|
||||
|
||||
orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion);
|
||||
|
||||
update_target_velocity();
|
||||
} else if (((current_time - _previous_target_motion.timestamp) / 1000 / 1000) > TARGET_TIMEOUT_S && target_velocity_valid()) {
|
||||
reset_target_validity();
|
||||
}
|
||||
|
||||
if (_previous_target_gps_pos_valid == false) {
|
||||
_previous_target_motion = _current_target_motion;
|
||||
_previous_target_gps_pos_valid = true;
|
||||
// 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()) {
|
||||
|
||||
// get distance to target
|
||||
|
||||
map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1));
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
// update state machine
|
||||
|
||||
switch (_follow_target_state) {
|
||||
|
||||
case TRACK_POSITION: {
|
||||
|
||||
if (_radius_entered == true) {
|
||||
_follow_target_state = TRACK_VELOCITY;
|
||||
} else if (target_velocity_valid()) {
|
||||
set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
|
||||
// keep the current velocity updated with the target velocity for when it's needed
|
||||
_current_vel = _target_vel;
|
||||
update_position_sp(true, true);
|
||||
}
|
||||
|
||||
warnx(" lat %f (%f) lon %f (%f), dist = %f mode = %d",
|
||||
_current_target_motion.lat,
|
||||
(double)_navigator->get_global_position()->lat,
|
||||
_current_target_motion.lon,
|
||||
(double)_navigator->get_global_position()->lon,
|
||||
(double) _target_distance.length(), _follow_target_state);
|
||||
break;
|
||||
}
|
||||
|
||||
if ((float) ((hrt_absolute_time() - _previous_target_motion.timestamp) / 1000 / 1000) > TARGET_TIMEOUT_S) {
|
||||
_follow_target_state = TARGET_TIMEOUT;
|
||||
} else if (_follow_target_state == TARGET_TIMEOUT) {
|
||||
_follow_target_state = TRACK_POSITION;
|
||||
case TRACK_VELOCITY: {
|
||||
|
||||
if (_radius_exited == true) {
|
||||
_follow_target_state = TRACK_POSITION;
|
||||
} else if (target_velocity_valid()) {
|
||||
set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
|
||||
if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) {
|
||||
_current_vel += _step_vel;
|
||||
_last_update_time = current_time;
|
||||
}
|
||||
update_position_sp(true, false);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case 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_min_alt.get(), target, NAN);
|
||||
|
||||
update_position_sp(false, false);
|
||||
|
||||
if (is_mission_item_reached() && target_velocity_valid()) {
|
||||
_follow_target_state = TRACK_POSITION;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void FollowTarget::update_target_velocity()
|
||||
void FollowTarget::update_position_sp(bool use_velocity, bool use_position)
|
||||
{
|
||||
float dt_ms;
|
||||
math::Vector<3> target_position(0, 0, 0);
|
||||
struct map_projection_reference_s target_ref;
|
||||
// convert mission item to current setpoint
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void FollowTarget::update_position_sp(math::Vector<3> & vel)
|
||||
{
|
||||
|
||||
/* 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
|
||||
// activate line following in pos control if position is valid
|
||||
|
||||
pos_sp_triplet->previous.valid = true;
|
||||
pos_sp_triplet->previous.valid = use_position;
|
||||
pos_sp_triplet->previous = pos_sp_triplet->current;
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
|
||||
|
||||
switch(_follow_target_state) {
|
||||
case TRACK_POSITION:
|
||||
pos_sp_triplet->current.position_valid = true;
|
||||
pos_sp_triplet->current.velocity_valid = true;
|
||||
break;
|
||||
case TRACK_VELOCITY:
|
||||
pos_sp_triplet->current.position_valid = false;
|
||||
pos_sp_triplet->current.velocity_valid = true;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
pos_sp_triplet->current.vx = vel(0);
|
||||
pos_sp_triplet->current.vy = vel(1);
|
||||
pos_sp_triplet->current.position_valid = use_position;
|
||||
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;
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
void FollowTarget::reset_target_validity()
|
||||
{
|
||||
_previous_target_motion = {};
|
||||
_current_target_motion = {};
|
||||
_target_updates = 0;
|
||||
_current_vel.zero();
|
||||
_step_vel.zero();
|
||||
_target_vel.zero();
|
||||
_target_distance.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);
|
||||
}
|
||||
|
||||
bool FollowTarget::target_position_valid()
|
||||
{
|
||||
// need at least 1 data point for position estimate
|
||||
return (_target_updates >= 1);
|
||||
}
|
||||
|
||||
@@ -66,10 +66,9 @@ private:
|
||||
static constexpr float FF_K = .15f;
|
||||
|
||||
enum FollowTargetState {
|
||||
ASCEND,
|
||||
TRACK_POSITION,
|
||||
TRACK_VELOCITY,
|
||||
TARGET_TIMEOUT
|
||||
WAIT_FOR_TARGET_POSITION
|
||||
};
|
||||
|
||||
Navigator *_navigator;
|
||||
@@ -78,9 +77,7 @@ private:
|
||||
int _follow_target_sub;
|
||||
float _step_time_in_ms;
|
||||
|
||||
bool _previous_target_gps_pos_valid;
|
||||
bool _radius_entered;
|
||||
bool _radius_exited;
|
||||
uint64_t _target_updates;
|
||||
|
||||
uint64_t _last_update_time;
|
||||
|
||||
@@ -94,8 +91,10 @@ private:
|
||||
|
||||
void track_target_position();
|
||||
void track_target_velocity();
|
||||
void pause();
|
||||
void update_position_sp(math::Vector<3> & vel);
|
||||
bool target_velocity_valid();
|
||||
bool target_position_valid();
|
||||
void reset_target_validity();
|
||||
void update_position_sp(bool velocity_valid, bool position_valid);
|
||||
void update_target_motion();
|
||||
void update_target_velocity();
|
||||
};
|
||||
|
||||
@@ -447,7 +447,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
|
||||
|
||||
item->lat = target.lat;
|
||||
item->lon = target.lon;
|
||||
item->altitude = target.alt;
|
||||
item->altitude = target.alt + _navigator->get_home_position()->alt;
|
||||
|
||||
if (((min_clearance > 0.0f) && (item->altitude < _navigator->get_home_position()->alt + min_clearance)) || PX4_ISFINITE(target.alt)) {
|
||||
item->altitude = _navigator->get_home_position()->alt + min_clearance;
|
||||
|
||||
Reference in New Issue
Block a user