From 3aaad68c76bf119e0ff1bd944605c434b671ca71 Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Tue, 15 Mar 2016 11:19:18 -0700 Subject: [PATCH] fixed position tracking overshoot --- src/modules/mavlink/mavlink_receiver.cpp | 2 - .../mc_pos_control/mc_pos_control_main.cpp | 53 ++++++++++++++----- src/modules/navigator/follow_target.cpp | 23 +++++--- src/modules/navigator/follow_target.h | 2 +- src/modules/navigator/mission_block.cpp | 4 +- 5 files changed, 57 insertions(+), 27 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4c8a9e21a4..28c973be13 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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 diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index b4ed4103d3..c09678a9f4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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) { - if (_run_alt_control) { - _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2); - } + 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); + } - /* 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)); + // Ignore position control calculated vel set points - if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && - _pos_sp_triplet.current.velocity_valid) { + 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; - } else if (vel_norm_xy > _params.vel_max(0)) { + } + + /* 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 z*/ float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2)); diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 5a2af86c57..1c061b933e 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -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; diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index 5db8cb8241..8285542f59 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -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; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index d0d307e21b..15d1eb343a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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: