mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 16:48:52 +08:00
code clean up complete
This commit is contained in:
Jimmy Johnson
committed by
Lorenz Meier
Lorenz Meier
parent
26cb645ee0
commit
8829db8844
@@ -1645,9 +1645,6 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
|
||||
if (_follow_me_pub == nullptr) {
|
||||
_follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic);
|
||||
} else {
|
||||
warnx("publishing follow lat = %f lon = %f alt = %f", (double) follow_target_topic.lat,
|
||||
(double) follow_target_topic.lon,
|
||||
(double) follow_target_topic.alt);
|
||||
orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1369,20 +1369,23 @@ MulticopterPositionControl::task_main()
|
||||
} else {
|
||||
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
|
||||
if (_run_pos_control) {
|
||||
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0) + _pos_sp_triplet.current.vx;
|
||||
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1) + _pos_sp_triplet.current.vy;
|
||||
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
|
||||
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
|
||||
}
|
||||
|
||||
// if there is a velocity sp available, use it
|
||||
|
||||
if(_pos_sp_triplet.current.velocity_valid) {
|
||||
_vel_sp(0) += _pos_sp_triplet.current.vx;
|
||||
_vel_sp(1) += _pos_sp_triplet.current.vy;
|
||||
}
|
||||
|
||||
if (_run_alt_control) {
|
||||
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
|
||||
}
|
||||
|
||||
/* make sure velocity setpoint is saturated in xy*/
|
||||
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
|
||||
_vel_sp(1) * _vel_sp(1));
|
||||
|
||||
// float sp_vel = sqrtf(_pos_sp_triplet.current.vx*_pos_sp_triplet.current.vx +
|
||||
// _pos_sp_triplet.current.vy*_pos_sp_triplet.current.vy);
|
||||
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
|
||||
|
||||
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
|
||||
_vel_sp(0) = _pos_sp_triplet.current.vx;
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -45,7 +45,8 @@
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
#include <lib/matrix/matrix/Vector2.hpp>
|
||||
#include <lib/mathlib/math/Vector.hpp>
|
||||
#include <lib/matrix/matrix/Matrix.hpp>
|
||||
|
||||
class FollowTarget : public MissionBlock
|
||||
{
|
||||
@@ -61,31 +62,41 @@ public:
|
||||
|
||||
virtual void on_active();
|
||||
|
||||
private:
|
||||
Navigator *_navigator;
|
||||
int _tracker_motion_position_sub; /**< tracker motion subscription */
|
||||
control::BlockParamFloat _param_min_alt;
|
||||
matrix::Vector2f pos_pair[2];
|
||||
matrix::Vector2f gps_pair;
|
||||
bool gps_valid;
|
||||
uint64_t _last_message_time;
|
||||
uint64_t _last_publish_time;
|
||||
float _steps;
|
||||
bool follow_target_reached;
|
||||
int _index;
|
||||
|
||||
struct pos_history_s{
|
||||
struct position_setpoint_triplet_s pos_history[6];
|
||||
uint64_t wp_time;
|
||||
enum {
|
||||
ACSEND,
|
||||
TRACK_POSITION,
|
||||
TRACK_VELOCITY,
|
||||
TARGET_TIMEOUT
|
||||
};
|
||||
|
||||
pos_history_s wp_history[6];
|
||||
int wp_cnt;
|
||||
matrix::Vector2f _current_vel;
|
||||
matrix::Vector2f target_vel;
|
||||
void update_position_sp(matrix::Vector2f & vel);
|
||||
private:
|
||||
static constexpr int TARGET_TIMEOUT_INT_S = 10;
|
||||
static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 10;
|
||||
|
||||
Navigator *_navigator;
|
||||
control::BlockParamFloat _param_min_alt;
|
||||
int _current_target_state;
|
||||
int _target_motion_position_sub;
|
||||
|
||||
bool _previous_target_gps_pos_valid;
|
||||
bool _radius_entered;
|
||||
bool _radius_exited;
|
||||
|
||||
uint64_t _last_publish_time;
|
||||
|
||||
math::Vector<3> _current_vel;
|
||||
math::Vector<3> _step_vel;
|
||||
math::Vector<3> _target_vel;
|
||||
math::Vector<3> _target_distance;
|
||||
|
||||
follow_target_s _current_target_motion;
|
||||
follow_target_s _previous_target_motion;
|
||||
float _dt;
|
||||
|
||||
void track_position();
|
||||
void track_velocity();
|
||||
void loiter();
|
||||
float target_dist_x;
|
||||
float target_dist_y;
|
||||
follow_target_s target;
|
||||
void update_position_sp(math::Vector<3> & vel);
|
||||
void update_target_motion();
|
||||
void update_target_velocity();
|
||||
};
|
||||
|
||||
@@ -79,7 +79,7 @@ Loiter::on_activation()
|
||||
|
||||
/* convert mission item to current setpoint */
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
pos_sp_triplet->current.vx = pos_sp_triplet->current.vy = pos_sp_triplet->current.vz = 0;
|
||||
pos_sp_triplet->current.velocity_valid = false;
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
pos_sp_triplet->next.valid = false;
|
||||
|
||||
@@ -438,7 +438,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
|
||||
}
|
||||
|
||||
void
|
||||
MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target)
|
||||
MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target, float yaw)
|
||||
{
|
||||
if (_navigator->get_vstatus()->condition_landed) {
|
||||
/* landed, don't takeoff, but switch to IDLE mode */
|
||||
@@ -459,7 +459,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
|
||||
}
|
||||
|
||||
item->altitude_is_relative = false;
|
||||
item->yaw = NAN;
|
||||
item->yaw = yaw;
|
||||
item->loiter_radius = _navigator->get_loiter_radius();
|
||||
item->loiter_direction = 1;
|
||||
item->acceptance_radius = _navigator->get_acceptance_radius();
|
||||
|
||||
@@ -125,7 +125,7 @@ protected:
|
||||
/**
|
||||
* set follow_target item
|
||||
*/
|
||||
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target);
|
||||
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target, float yaw);
|
||||
|
||||
void issue_command(const struct mission_item_s *item);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user