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

fixed position tracking overshoot

Browse Source
This commit is contained in:
Jimmy Johnson 2016-03-15 11:19:18 -07:00 committed by Lorenz Meier
parent 142884bcd8
commit 3aaad68c76
5 changed files with 61 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1647,8 +1647,6 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(follow_target), _follow_target_pub, &follow_target_topic);
}
warnx("new msg recieved");
}
void

61
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1023,7 +1023,10 @@ void MulticopterPositionControl::control_auto(float dt)
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) && previous_setpoint_valid) {
if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) &&
previous_setpoint_valid) {
/* follow "previous - current" line */
if ((curr_sp - prev_sp).length() > MIN_DIST) {
@ -1372,30 +1375,52 @@ MulticopterPositionControl::task_main()
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
// if there is a velocity sp available, use it
// do not go slower than the follow target velocity when position tracking is active
if(_pos_sp_triplet.current.velocity_valid) {
_vel_sp(0) += _pos_sp_triplet.current.vx;
_vel_sp(1) += _pos_sp_triplet.current.vy;
if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
_pos_sp_triplet.current.velocity_valid) {
math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
float cos_ratio = (ft_vel*_vel_sp)/(ft_vel.length()*_vel_sp.length());
// only override velocity set points when uav is traveling in same direction as target and vector component
// is greater than calculated position set point velocity component
if(cos_ratio > 0) {
ft_vel *= (cos_ratio);
// min speed a little faster than target vel
ft_vel += ft_vel.normalized()*1.5f;
} else {
ft_vel.zero();
}
_vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
_vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);
}
// Ignore position control calculated vel set points
if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_VELOCITY &&
_pos_sp_triplet.current.velocity_valid) {
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;
}
/* 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));
if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
}
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));
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
_pos_sp_triplet.current.velocity_valid) {
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;
} else if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
}
/* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));

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

@ -101,8 +101,7 @@ void FollowTarget::pause()
{
math::Vector<3> vel(0, 0, 0);
_current_vel(0) = 0;
_current_vel(1) = 0;
_current_vel.zero();
set_loiter_item(&_mission_item, _param_min_alt.get());
@ -172,14 +171,11 @@ void FollowTarget::on_active()
void FollowTarget::track_target_position()
{
set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
// keep the current velocity updated
_current_vel(0) = _navigator->get_global_position()->vel_n;
_current_vel(1) = _navigator->get_global_position()->vel_e;
_current_vel = _target_vel;
}
void FollowTarget::track_target_velocity()
@ -249,8 +245,7 @@ void FollowTarget::update_target_velocity()
// 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)));
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
@ -279,6 +274,18 @@ void FollowTarget::update_position_sp(math::Vector<3> & vel)
pos_sp_triplet->previous.valid = true;
pos_sp_triplet->previous = pos_sp_triplet->current;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
switch(_follow_target_state) {
case TRACK_POSITION:
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
break;
case TRACK_VELOCITY:
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_VELOCITY;
break;
default:
break;
}
pos_sp_triplet->current.vx = vel(0);
pos_sp_triplet->current.vy = vel(1);
pos_sp_triplet->current.velocity_valid = true;

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

@ -60,7 +60,7 @@ public:
private:
static constexpr int TARGET_TIMEOUT_S = 10;
static constexpr int TARGET_TIMEOUT_S = 5;
static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
static constexpr int INTERPOLATION_PNTS = 20;
static constexpr float FF_K = .15f;

4
src/modules/navigator/mission_block.cpp
View File

@ -374,9 +374,9 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){
sp->disable_mc_yaw_control = true;
}
break;
break;// get rid of this
case NAV_CMD_FOLLOW_TARGET:
sp->type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
sp->type = position_setpoint_s::SETPOINT_TYPE_VELOCITY;
break;
default: