From f2777cb6eedc4d365cfb2b2023fd42f465b18031 Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Tue, 22 Mar 2016 12:33:38 -0700 Subject: [PATCH] making code more robust --- src/modules/navigator/follow_target.cpp | 323 +++++++++++------------- src/modules/navigator/follow_target.h | 13 +- src/modules/navigator/mission_block.cpp | 2 +- 3 files changed, 155 insertions(+), 183 deletions(-) diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index aeb88a78fc..df81c5a06b 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -60,12 +60,10 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _navigator(navigator), _param_min_alt(this, "MIS_TAKEOFF_ALT", false), - _follow_target_state(ASCEND), + _follow_target_state(WAIT_FOR_TARGET_POSITION), _follow_target_sub(-1), _step_time_in_ms(0.0f), - _previous_target_gps_pos_valid(false), - _radius_entered(false), - _radius_exited(false), + _target_updates(0), _last_update_time(0), _current_target_motion({}), _previous_target_motion({}) @@ -83,8 +81,7 @@ FollowTarget::~FollowTarget() void FollowTarget::on_inactive() { - _previous_target_gps_pos_valid = false; - _follow_target_state = ASCEND; + reset_target_validity(); } void FollowTarget::on_activation() @@ -92,207 +89,183 @@ void FollowTarget::on_activation() if (_follow_target_sub < 0) { _follow_target_sub = orb_subscribe(ORB_ID(follow_target)); } - - update_target_motion(); - reset_mission_item_reached(); } -void FollowTarget::pause() -{ - math::Vector<3> vel(0, 0, 0); - - _current_vel.zero(); - - set_loiter_item(&_mission_item, _param_min_alt.get()); - - update_position_sp(vel); - - _current_target_motion.lat = _navigator->get_global_position()->lat; - _current_target_motion.lon = _navigator->get_global_position()->lon; -} - -void FollowTarget::on_active() -{ +void FollowTarget::on_active() { struct map_projection_reference_s target_ref; - - 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, _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 velocity controller - // a chance to catch up - - _radius_exited = (_target_distance.length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); - _radius_entered = (_target_distance.length() < (float) TARGET_ACCEPTANCE_RADIUS_M); - - switch (_follow_target_state) { - case TRACK_POSITION: { - if (_radius_entered == true) { - _follow_target_state = TRACK_VELOCITY; - } else { - track_target_position(); - } - break; - } - case TRACK_VELOCITY: { - if (_radius_exited == true) { - _follow_target_state = TRACK_POSITION; - } else { - track_target_velocity(); - } - break; - } - case ASCEND: { - // ascend to the minimum altitude - - pause(); - - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - - if (is_mission_item_reached()) { - _follow_target_state = TRACK_POSITION; - } - break; - } - case TARGET_TIMEOUT: { - // Loiter until signal is regained - - pause(); - break; - } - } - - update_position_sp(_current_vel); -} - -void FollowTarget::track_target_position() -{ - set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN); - - // keep the current velocity updated - - _current_vel = _target_vel; -} - -void FollowTarget::track_target_velocity() -{ - + math::Vector<3> target_position(0, 0, 0); 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); - - if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) { - _current_vel += _step_vel; - _last_update_time = current_time; - } -} - -void FollowTarget::update_target_motion() -{ - bool updated; + bool _radius_entered = false; + bool _radius_exited = false; + bool updated = false; + float dt_ms = 0; orb_check(_follow_target_sub, &updated); if (updated) { + _target_updates++; + // save last known motion topic _previous_target_motion = _current_target_motion; orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion); - update_target_velocity(); + } else if (((current_time - _previous_target_motion.timestamp) / 1000 / 1000) > TARGET_TIMEOUT_S && target_velocity_valid()) { + reset_target_validity(); + } - if (_previous_target_gps_pos_valid == false) { - _previous_target_motion = _current_target_motion; - _previous_target_gps_pos_valid = true; + // update target velocity + + if (target_velocity_valid() && updated) { + + dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000); + + // get last gps known 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 based on the position + + _target_vel = target_position / (dt_ms / 1000.0f); + + // to keep the velocity increase/decrease smooth + // calculate how many velocity increments/decrements + // it will take to reach the targets velocity + // with the given amount of steps also add a feed forward input that adjusts the + // velocity as the position gap increases since + // just traveling at the exact velocity of the target will not + // get any closer to the target + + _step_vel = (_target_vel - _current_vel) + _target_distance * FF_K; + _step_vel /= (dt_ms / 1000.0f * (float) INTERPOLATION_PNTS); + _step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS; + } + + // update distance to target + + if (target_position_valid()) { + + // get distance to target + + map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + 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 velocity controller + // a chance to catch up + + _radius_exited = (_target_distance.length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); + _radius_entered = (_target_distance.length() < (float) TARGET_ACCEPTANCE_RADIUS_M); + } + + // update state machine + + switch (_follow_target_state) { + + case TRACK_POSITION: { + + if (_radius_entered == true) { + _follow_target_state = TRACK_VELOCITY; + } else if (target_velocity_valid()) { + set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN); + // keep the current velocity updated with the target velocity for when it's needed + _current_vel = _target_vel; + update_position_sp(true, true); } - warnx(" lat %f (%f) lon %f (%f), dist = %f mode = %d", - _current_target_motion.lat, - (double)_navigator->get_global_position()->lat, - _current_target_motion.lon, - (double)_navigator->get_global_position()->lon, - (double) _target_distance.length(), _follow_target_state); + break; } - if ((float) ((hrt_absolute_time() - _previous_target_motion.timestamp) / 1000 / 1000) > TARGET_TIMEOUT_S) { - _follow_target_state = TARGET_TIMEOUT; - } else if (_follow_target_state == TARGET_TIMEOUT) { - _follow_target_state = TRACK_POSITION; + case TRACK_VELOCITY: { + + if (_radius_exited == true) { + _follow_target_state = TRACK_POSITION; + } else if (target_velocity_valid()) { + set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN); + if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) { + _current_vel += _step_vel; + _last_update_time = current_time; + } + update_position_sp(true, false); + } + + break; + } + + case WAIT_FOR_TARGET_POSITION: { + + // Climb to the minimum altitude + // and wait until a position is received + + follow_target_s target = { }; + + // for now set the target at the minimum height above the uav + + target.lat = _navigator->get_global_position()->lat; + target.lon = _navigator->get_global_position()->lon; + target.alt = 0.0F; + + set_follow_target_item(&_mission_item, _param_min_alt.get(), target, NAN); + + update_position_sp(false, false); + + if (is_mission_item_reached() && target_velocity_valid()) { + _follow_target_state = TRACK_POSITION; + } + + break; + } } } -void FollowTarget::update_target_velocity() +void FollowTarget::update_position_sp(bool use_velocity, bool use_position) { - float dt_ms; - math::Vector<3> target_position(0, 0, 0); - struct map_projection_reference_s target_ref; + // convert mission item to current setpoint - dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000); - - // get last gps known 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 based on the position - - _target_vel = target_position / (dt_ms / 1000.0f); - - // to keep the velocity increase/decrease smooth - // calculate how many velocity increments/decrements - // it will take to reach the targets velocity - // with the given amount of steps also add a feed forward input that adjusts the - // velocity as the position gap increases since - // just traveling at the exact velocity of the target will not - // get any closer to the target - - _step_vel = (_target_vel - _current_vel) + _target_distance * FF_K; - _step_vel /= (dt_ms / 1000.0f * (float) INTERPOLATION_PNTS); - _step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS; -} - -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(); - // activate line following in pos control + // activate line following in pos control if position is valid - pos_sp_triplet->previous.valid = true; + pos_sp_triplet->previous.valid = use_position; pos_sp_triplet->previous = pos_sp_triplet->current; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET; - - switch(_follow_target_state) { - case TRACK_POSITION: - pos_sp_triplet->current.position_valid = true; - pos_sp_triplet->current.velocity_valid = true; - break; - case TRACK_VELOCITY: - pos_sp_triplet->current.position_valid = false; - pos_sp_triplet->current.velocity_valid = true; - break; - default: - break; - } - - pos_sp_triplet->current.vx = vel(0); - pos_sp_triplet->current.vy = vel(1); + pos_sp_triplet->current.position_valid = use_position; + pos_sp_triplet->current.velocity_valid = use_velocity; + pos_sp_triplet->current.vx = _current_vel(0); + pos_sp_triplet->current.vy = _current_vel(1); pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated(); } + +void FollowTarget::reset_target_validity() +{ + _previous_target_motion = {}; + _current_target_motion = {}; + _target_updates = 0; + _current_vel.zero(); + _step_vel.zero(); + _target_vel.zero(); + _target_distance.zero(); + reset_mission_item_reached(); + _follow_target_state = WAIT_FOR_TARGET_POSITION; +} + +bool FollowTarget::target_velocity_valid() +{ + // need at least 2 data points for velocity estimate + return (_target_updates >= 2); +} + +bool FollowTarget::target_position_valid() +{ + // need at least 1 data point for position estimate + return (_target_updates >= 1); +} diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index 8285542f59..a653563937 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -66,10 +66,9 @@ private: static constexpr float FF_K = .15f; enum FollowTargetState { - ASCEND, TRACK_POSITION, TRACK_VELOCITY, - TARGET_TIMEOUT + WAIT_FOR_TARGET_POSITION }; Navigator *_navigator; @@ -78,9 +77,7 @@ private: int _follow_target_sub; float _step_time_in_ms; - bool _previous_target_gps_pos_valid; - bool _radius_entered; - bool _radius_exited; + uint64_t _target_updates; uint64_t _last_update_time; @@ -94,8 +91,10 @@ private: void track_target_position(); void track_target_velocity(); - void pause(); - void update_position_sp(math::Vector<3> & vel); + bool target_velocity_valid(); + bool target_position_valid(); + void reset_target_validity(); + void update_position_sp(bool velocity_valid, bool position_valid); void update_target_motion(); void update_target_velocity(); }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index eef6a303d3..c0d5351f55 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -447,7 +447,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea item->lat = target.lat; item->lon = target.lon; - item->altitude = target.alt; + item->altitude = target.alt + _navigator->get_home_position()->alt; if (((min_clearance > 0.0f) && (item->altitude < _navigator->get_home_position()->alt + min_clearance)) || PX4_ISFINITE(target.alt)) { item->altitude = _navigator->get_home_position()->alt + min_clearance;