mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 21:23:35 +08:00
code clean up complete
This commit is contained in:
Jimmy Johnson
committed by
Lorenz Meier
Lorenz Meier
parent
26cb645ee0
commit
8829db8844
@@ -55,26 +55,25 @@
|
||||
#include <lib/geo/geo.h>
|
||||
#include "navigator.h"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
|
||||
MissionBlock(navigator, name),
|
||||
_navigator(navigator),
|
||||
_tracker_motion_position_sub(-1),
|
||||
_param_min_alt(this, "MIS_TAKEOFF_ALT", false),
|
||||
gps_valid(false),
|
||||
_last_message_time(0),
|
||||
_index(0)
|
||||
_current_target_state(ACSEND),
|
||||
_target_motion_position_sub(-1),
|
||||
_previous_target_gps_pos_valid(false),
|
||||
_radius_entered(false),
|
||||
_radius_exited(false),
|
||||
_last_publish_time(0),
|
||||
_current_target_motion({0}),
|
||||
_previous_target_motion({0}),
|
||||
_dt(0)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
follow_target_reached = false;
|
||||
pos_pair[0].setZero();
|
||||
pos_pair[1].setZero();
|
||||
_current_vel.setZero();
|
||||
_steps = 0.0f;
|
||||
target_dist_x = target_dist_y = 0.0f;
|
||||
target = {};
|
||||
_current_vel.zero();
|
||||
_step_vel.zero();
|
||||
_target_vel.zero();
|
||||
_target_distance.zero();
|
||||
}
|
||||
|
||||
FollowTarget::~FollowTarget()
|
||||
@@ -84,133 +83,197 @@ FollowTarget::~FollowTarget()
|
||||
void
|
||||
FollowTarget::on_inactive()
|
||||
{
|
||||
gps_valid = false;
|
||||
_previous_target_gps_pos_valid = false;
|
||||
_current_target_state = ACSEND;
|
||||
}
|
||||
|
||||
void
|
||||
FollowTarget::on_activation()
|
||||
{
|
||||
Vector2f vel;
|
||||
vel.setZero();
|
||||
|
||||
if(_tracker_motion_position_sub < 0) {
|
||||
_tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target));
|
||||
if(_target_motion_position_sub < 0) {
|
||||
_target_motion_position_sub = orb_subscribe(ORB_ID(follow_target));
|
||||
}
|
||||
|
||||
// inital set point is same as loiter sp
|
||||
update_target_motion();
|
||||
|
||||
reset_mission_item_reached();
|
||||
}
|
||||
|
||||
void
|
||||
FollowTarget::loiter() {
|
||||
math::Vector<3> vel;
|
||||
|
||||
_current_vel(0) = 0;
|
||||
_current_vel(1) = 0;
|
||||
|
||||
set_loiter_item(&_mission_item, _param_min_alt.get());
|
||||
|
||||
update_position_sp(vel);
|
||||
|
||||
target.lat = _navigator->get_global_position()->lat;
|
||||
target.lon = _navigator->get_global_position()->lon;
|
||||
_current_target_motion.lat = _navigator->get_global_position()->lat;
|
||||
_current_target_motion.lon = _navigator->get_global_position()->lon;
|
||||
}
|
||||
|
||||
void
|
||||
FollowTarget::on_active() {
|
||||
|
||||
bool updated;
|
||||
struct map_projection_reference_s target_ref;
|
||||
uint64_t current_time = hrt_absolute_time();
|
||||
|
||||
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, target.lat, target.lon, &target_dist_x, &target_dist_y);
|
||||
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1));
|
||||
|
||||
follow_target_reached = (sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y) < 5);
|
||||
// are we within the target acceptance radius?
|
||||
// give a buffer to exit/enter the radius to give the controller
|
||||
// a chance to catch up
|
||||
|
||||
if(follow_target_reached == false) {
|
||||
_current_vel(0) = _navigator->get_global_position()->vel_n;
|
||||
_current_vel(1) = _navigator->get_global_position()->vel_e;
|
||||
}
|
||||
_radius_entered = (_target_distance.length() < (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
|
||||
_radius_exited = (_target_distance.length() > (float) TARGET_ACCEPTANCE_RADIUS_M);
|
||||
|
||||
orb_check(_tracker_motion_position_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) {
|
||||
|
||||
float dt = ((double)(current_time - _last_message_time) * 1e-6);
|
||||
|
||||
if ((gps_valid == false) ) {
|
||||
gps_pair(0) = target.lat;
|
||||
gps_pair(1) = target.lon;
|
||||
gps_valid = true;
|
||||
|
||||
return;
|
||||
switch (_current_target_state) {
|
||||
case TRACK_POSITION:
|
||||
{
|
||||
if (_radius_entered == true) {
|
||||
_current_target_state = TRACK_VELOCITY;
|
||||
} else {
|
||||
track_position();
|
||||
}
|
||||
|
||||
// get last gps reference
|
||||
|
||||
map_projection_init(&target_ref, gps_pair(0), gps_pair(1));
|
||||
map_projection_project(&target_ref, target.lat, target.lon, &(pos_pair[1](0)), &(pos_pair[1](1)));
|
||||
|
||||
target_vel = pos_pair[1]/(dt);
|
||||
|
||||
target_vel(0) += target_dist_x*.1f;
|
||||
target_vel(1) += target_dist_y*.1f;
|
||||
|
||||
warnx("tracker x %f y %f m, x %f m/s, y %f m/s gs = %f m/s dis = %f m",
|
||||
(double) pos_pair[1](0),
|
||||
(double) pos_pair[1](1),
|
||||
(double) target_vel(0),
|
||||
(double) target_vel(1),
|
||||
(double) sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1)),
|
||||
(double) sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y));
|
||||
|
||||
set_follow_target_item(&_mission_item, _param_min_alt.get(), target);
|
||||
|
||||
gps_pair(0) = target.lat;
|
||||
gps_pair(1) = target.lon;
|
||||
|
||||
_last_message_time = current_time;
|
||||
|
||||
if(follow_target_reached == false) {
|
||||
warnx("WP not reached lat %f (%f) lon %f (%f), %f", target.lat, (double)_navigator->get_global_position()->lat, (double)_navigator->get_global_position()->lon, target.lon, (double)sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y));
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
update_position_sp(target_vel);
|
||||
return;
|
||||
}
|
||||
|
||||
_steps = fabs((double)((sqrtf(_current_vel(0)*_current_vel(0) + _current_vel(1)*_current_vel(1)) -
|
||||
sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1))))) / (double) (dt*10);
|
||||
|
||||
break;
|
||||
}
|
||||
case TRACK_VELOCITY:
|
||||
{
|
||||
if (_radius_exited == true) {
|
||||
_current_target_state = TRACK_POSITION;
|
||||
} else {
|
||||
track_velocity();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case ACSEND:
|
||||
{
|
||||
// ascend to the minimum altitude
|
||||
|
||||
loiter();
|
||||
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
|
||||
if(is_mission_item_reached()) {
|
||||
_current_target_state = TRACK_POSITION;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case TARGET_TIMEOUT:
|
||||
{
|
||||
// Loiter until signal is regained
|
||||
|
||||
loiter();
|
||||
break;
|
||||
}
|
||||
} else if (((double)(current_time - _last_message_time) * 1e-6) > 10) {
|
||||
on_activation();
|
||||
}
|
||||
|
||||
if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100) && (follow_target_reached == true)) {
|
||||
update_position_sp(_current_vel);
|
||||
}
|
||||
|
||||
if (_current_vel(0) <= target_vel(0)) {
|
||||
_current_vel(0) += (_steps);
|
||||
}
|
||||
void
|
||||
FollowTarget::track_position() {
|
||||
|
||||
if (_current_vel(0) >= target_vel(0)) {
|
||||
_current_vel(0) -= (_steps);
|
||||
}
|
||||
set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
|
||||
if (_current_vel(1) <= target_vel(1)) {
|
||||
_current_vel(1) += (_steps);
|
||||
}
|
||||
// keep the current velocity updated
|
||||
|
||||
if (_current_vel(1) >= target_vel(1)) {
|
||||
_current_vel(1) -= (_steps);
|
||||
}
|
||||
_current_vel(0) = _navigator->get_global_position()->vel_n;
|
||||
_current_vel(1) = _navigator->get_global_position()->vel_e;
|
||||
}
|
||||
|
||||
update_position_sp(_current_vel);
|
||||
void
|
||||
FollowTarget::track_velocity() {
|
||||
|
||||
warnx("updating x %f m y %f m x %f m/s y %f m/s %f ( uav gs %f)", (double )target_dist_x, (double )target_dist_y,
|
||||
(double )_current_vel(0), (double )_current_vel(1), (double ) _steps,
|
||||
(double ) sqrtf(_navigator->get_global_position()->vel_e * _navigator->get_global_position()->vel_e + _navigator->get_global_position()->vel_n * _navigator->get_global_position()->vel_n));
|
||||
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);
|
||||
|
||||
// publish a new velocity every 100 ms
|
||||
|
||||
if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100)) {
|
||||
_current_vel += _step_vel;
|
||||
_last_publish_time = current_time;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FollowTarget::update_position_sp(Vector2f & vel) {
|
||||
FollowTarget::update_target_motion() {
|
||||
bool updated;
|
||||
|
||||
orb_check(_target_motion_position_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
|
||||
// save last known motion topic
|
||||
|
||||
_previous_target_motion = _current_target_motion;
|
||||
|
||||
orb_copy(ORB_ID(follow_target), _target_motion_position_sub, &_current_target_motion);
|
||||
|
||||
update_target_velocity();
|
||||
|
||||
if (_previous_target_gps_pos_valid == false) {
|
||||
_previous_target_motion = _current_target_motion;
|
||||
_previous_target_gps_pos_valid = true;
|
||||
}
|
||||
|
||||
warnx(" lat %f (%f) lon %f (%f), dist = %f",
|
||||
_current_target_motion.lat,
|
||||
(double)_navigator->get_global_position()->lat,
|
||||
_current_target_motion.lon,
|
||||
(double)_navigator->get_global_position()->lon,
|
||||
(double) _target_distance.length());
|
||||
}
|
||||
|
||||
if ((((double)(hrt_absolute_time() - _previous_target_motion.timestamp) * 1e-6) > 10)) {
|
||||
_current_target_state = TARGET_TIMEOUT;
|
||||
} else if(_current_target_state == TARGET_TIMEOUT) {
|
||||
_current_target_state = TRACK_POSITION;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FollowTarget::update_target_velocity() {
|
||||
struct map_projection_reference_s target_ref;
|
||||
math::Vector<3> target_position(0,0,0);
|
||||
|
||||
_dt = ((double) (_current_target_motion.timestamp - _previous_target_motion.timestamp) * 1e-6);
|
||||
|
||||
// get last gps 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
|
||||
|
||||
_target_vel = target_position / _dt;
|
||||
|
||||
// to keep the velocity increase/decrease smooth
|
||||
// calculate how many velocity increments/decrements
|
||||
// it will take to reach the targets velocity
|
||||
// in 10 steps also add a feed forward input that increases
|
||||
// velocity as the position gap increases
|
||||
|
||||
_step_vel = (_target_vel - _current_vel) + _target_distance * .15f;
|
||||
_step_vel /= (_dt * 10);
|
||||
}
|
||||
|
||||
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();
|
||||
@@ -221,6 +284,7 @@ FollowTarget::update_position_sp(Vector2f & vel) {
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
pos_sp_triplet->current.vx = vel(0);
|
||||
pos_sp_triplet->current.vy = vel(1);
|
||||
pos_sp_triplet->current.velocity_valid = true;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
|
||||
Reference in New Issue
Block a user