From 8829db88440e59ce4f4e5faf3b8661c71d5755e6 Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Fri, 11 Mar 2016 22:25:56 -0800 Subject: [PATCH] code clean up complete --- src/modules/mavlink/mavlink_receiver.cpp | 3 - .../mc_pos_control/mc_pos_control_main.cpp | 17 +- src/modules/navigator/follow_target.cpp | 240 +++++++++++------- src/modules/navigator/follow_target.h | 53 ++-- src/modules/navigator/loiter.cpp | 2 +- src/modules/navigator/mission_block.cpp | 4 +- src/modules/navigator/mission_block.h | 2 +- 7 files changed, 198 insertions(+), 123 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 567b08fc54..f0be47aba4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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); } } 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 ecd7a9ffc5..e3eba9c45e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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; diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 16ad9da02a..fc1791fd7c 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -55,26 +55,25 @@ #include #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)); + + // are we within the target acceptance radius? + // give a buffer to exit/enter the radius to give the controller + // a chance to catch up + + _radius_entered = (_target_distance.length() < (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); + _radius_exited = (_target_distance.length() > (float) TARGET_ACCEPTANCE_RADIUS_M); + + switch (_current_target_state) { + case TRACK_POSITION: + { + if (_radius_entered == true) { + _current_target_state = TRACK_VELOCITY; + } else { + track_position(); + } + break; + } + case TRACK_VELOCITY: + { + if (_radius_exited == true) { + _current_target_state = TRACK_POSITION; + } else { + track_velocity(); + } + break; + } + case ACSEND: + { + // ascend to the minimum altitude - follow_target_reached = (sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y) < 5); + loiter(); - if(follow_target_reached == false) { - _current_vel(0) = _navigator->get_global_position()->vel_n; - _current_vel(1) = _navigator->get_global_position()->vel_e; + _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; + } } - orb_check(_tracker_motion_position_sub, &updated); + update_position_sp(_current_vel); +} - if (updated) { - if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) { +void +FollowTarget::track_position() { - float dt = ((double)(current_time - _last_message_time) * 1e-6); + set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN); + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - if ((gps_valid == false) ) { - gps_pair(0) = target.lat; - gps_pair(1) = target.lon; - gps_valid = true; + // keep the current velocity updated - return; - } + _current_vel(0) = _navigator->get_global_position()->vel_n; + _current_vel(1) = _navigator->get_global_position()->vel_e; +} - // get last gps reference +void +FollowTarget::track_velocity() { - 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))); + uint64_t current_time = hrt_absolute_time(); - target_vel = pos_pair[1]/(dt); + if (_previous_target_gps_pos_valid == false) { + return; + } - target_vel(0) += target_dist_x*.1f; - target_vel(1) += target_dist_y*.1f; + set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN); - 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)); + // publish a new velocity every 100 ms - set_follow_target_item(&_mission_item, _param_min_alt.get(), target); + if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100)) { + _current_vel += _step_vel; + _last_publish_time = current_time; + } +} - gps_pair(0) = target.lat; - gps_pair(1) = target.lon; +void +FollowTarget::update_target_motion() { + bool updated; - _last_message_time = current_time; + orb_check(_target_motion_position_sub, &updated); - 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; - } + if (updated) { + + // save last known motion topic + + _previous_target_motion = _current_target_motion; - _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); + 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; } - } else if (((double)(current_time - _last_message_time) * 1e-6) > 10) { - on_activation(); + + 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) (current_time - _last_publish_time) * 1e-3) >= 100) && (follow_target_reached == true)) { + 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; + } +} - if (_current_vel(0) <= target_vel(0)) { - _current_vel(0) += (_steps); - } +void +FollowTarget::update_target_velocity() { + struct map_projection_reference_s target_ref; + math::Vector<3> target_position(0,0,0); - if (_current_vel(0) >= target_vel(0)) { - _current_vel(0) -= (_steps); - } + _dt = ((double) (_current_target_motion.timestamp - _previous_target_motion.timestamp) * 1e-6); - if (_current_vel(1) <= target_vel(1)) { - _current_vel(1) += (_steps); - } + // get last gps reference for target - if (_current_vel(1) >= target_vel(1)) { - _current_vel(1) -= (_steps); - } + map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon); - update_position_sp(_current_vel); + // calculate distance the target has moved - 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)); + map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)), &(target_position(1))); - _last_publish_time = current_time; - } + // 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(Vector2f & vel) { +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(); diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index d5604d43c9..3b2c6654f4 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -45,7 +45,8 @@ #include "navigator_mode.h" #include "mission_block.h" -#include +#include +#include class FollowTarget : public MissionBlock { @@ -61,31 +62,41 @@ public: virtual void on_active(); + enum { + ACSEND, + TRACK_POSITION, + TRACK_VELOCITY, + TARGET_TIMEOUT + }; + private: + static constexpr int TARGET_TIMEOUT_INT_S = 10; + static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 10; + 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; + 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; - float _steps; - bool follow_target_reached; - int _index; - struct pos_history_s{ - struct position_setpoint_triplet_s pos_history[6]; - uint64_t wp_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; - pos_history_s wp_history[6]; - int wp_cnt; - matrix::Vector2f _current_vel; - matrix::Vector2f target_vel; - void update_position_sp(matrix::Vector2f & vel); + 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(); }; diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 98eeb86abd..f59c024529 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -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; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index a261c2b6ee..aac9439563 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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(); diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 0fea354ec7..700e0b91c1 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -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);