From 94f3c50f830a69dc5a97c6877bdfffd0ab596d06 Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Mon, 11 Apr 2016 21:30:05 -0700 Subject: [PATCH] follow target safety updates --- src/modules/navigator/follow_target.cpp | 140 ++++++++++++++++-------- src/modules/navigator/follow_target.h | 37 ++++++- src/modules/navigator/navigation.h | 3 +- 3 files changed, 130 insertions(+), 50 deletions(-) diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index ff23802efd..0e259a683c 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -45,7 +45,9 @@ #include #include #include +#include +#include #include #include @@ -58,20 +60,24 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _navigator(navigator), - _param_min_alt(this, "MIS_TAKEOFF_ALT", false), + _param_min_alt(this, "NAV_MIN_FT_HT", false), _follow_target_state(WAIT_FOR_TARGET_POSITION), + _follow_target_position(FOLLOW_FROM_BEHIND), _follow_target_sub(-1), _step_time_in_ms(0.0f), _target_updates(0), _last_update_time(0), _current_target_motion( {}), - _previous_target_motion({}) + _previous_target_motion({}) { updateParams(); _current_vel.zero(); _step_vel.zero(); _target_vel.zero(); _target_distance.zero(); + _target_position_offset.zero(); + + _rot_matrix = (_follow_position_matricies[_follow_target_position]); } FollowTarget::~FollowTarget() @@ -94,11 +100,13 @@ void FollowTarget::on_active() { struct map_projection_reference_s target_ref; math::Vector<3> target_position(0, 0, 0); + follow_target_s target_motion = {}; uint64_t current_time = hrt_absolute_time(); bool _radius_entered = false; bool _radius_exited = false; bool updated = false; float dt_ms = 0; + float yaw_angle = NAN; orb_check(_follow_target_sub, &updated); @@ -112,43 +120,11 @@ void FollowTarget::on_active() orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion); - } else if (((current_time - _previous_target_motion.timestamp) / 1000 / 1000) > TARGET_TIMEOUT_S + } else if (((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000 ) > TARGET_TIMEOUT_MS && target_velocity_valid()) { reset_target_validity(); } - // 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()) { @@ -159,12 +135,84 @@ void FollowTarget::on_active() map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1)); + target_motion = _current_target_motion; + + // use target offset + + map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon); + map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion.lat, &target_motion.lon); + // 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); + _radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); + _radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M); + + } + + // update target velocity + + if (target_velocity_valid() && updated) { + + dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000); + + // protect from to small of a dt + + if (dt_ms > 10) { + + // 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))); + + // only calculate new velocity if target has moved + + if(target_position.length() > 0.0F) { + + // track to the left, right, behind, or front + + _target_position_offset = _rot_matrix * (target_position.normalized() * OFFSET_M); + + // 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_position_offset + _target_distance) * FF_K; + _step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS); + _step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS; + } + } + } + + // always point towards target + + if (target_position_valid() && updated) { + + yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _current_target_motion.lat, _current_target_motion.lon); + +// warnx(" lat %f (%f) lon %f (%f), dist x %f y %f (%f) yaw = %f mode = %d", +// target_motion.lat, +// (double )_navigator->get_global_position()->lat, +// target_motion.lon, +// (double ) _navigator->get_global_position()->lon, +// (double ) _target_distance(0), +// (double ) _target_distance(1), +// (double ) _target_distance.length(), +// (double) yaw_angle, +// _follow_target_state); } // update state machine @@ -177,10 +225,12 @@ void FollowTarget::on_active() _follow_target_state = TRACK_VELOCITY; } else if (target_velocity_valid()) { - set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN); + set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle); // keep the current velocity updated with the target velocity for when it's needed _current_vel = _target_vel; update_position_sp(true, true); + } else { + _follow_target_state = WAIT_FOR_TARGET_POSITION; } break; @@ -192,7 +242,7 @@ void FollowTarget::on_active() _follow_target_state = TRACK_POSITION; } else if (target_velocity_valid()) { - set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN); + set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion, yaw_angle); if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) { _current_vel += _step_vel; @@ -200,6 +250,8 @@ void FollowTarget::on_active() } update_position_sp(true, false); + } else { + _follow_target_state = WAIT_FOR_TARGET_POSITION; } break; @@ -248,7 +300,6 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position) 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(); } @@ -261,18 +312,19 @@ void FollowTarget::reset_target_validity() _step_vel.zero(); _target_vel.zero(); _target_distance.zero(); + _target_position_offset.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); + // need at least 5 continuous data points for velocity estimate + return (_target_updates >= 5); } bool FollowTarget::target_position_valid() { - // need at least 1 data point for position estimate - return (_target_updates >= 1); + // need at least 2 continuous data points for position estimate + return (_target_updates >= 2); } diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index b018bc58ee..019c93c293 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -43,7 +43,7 @@ #include #include #include - +#include #include "navigator_mode.h" #include "mission_block.h" @@ -60,10 +60,11 @@ public: private: - static constexpr int TARGET_TIMEOUT_S = 5; + static constexpr int TARGET_TIMEOUT_MS = 1500; static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5; static constexpr int INTERPOLATION_PNTS = 20; - static constexpr float FF_K = .15f; + static constexpr float FF_K = .40F; + static constexpr float OFFSET_M = 8; enum FollowTargetState { TRACK_POSITION, @@ -71,9 +72,36 @@ private: WAIT_FOR_TARGET_POSITION }; + enum FollowTargetPosition { + FOLLOW_FROM_RIGHT, + FOLLOW_FROM_BEHIND, + FOLLOW_FROM_FRONT, + FOLLOW_FROM_LEFT + }; + + float _follow_position_matricies[4][9] = { + {1.0F, -1.0F, 0.0F, + 1.0F, 1.0F, 0.0F, + 0.0F, 0.0F, 1.0F}, // follow right + + {-1.0F, 0.0F, 0.0F, + 0.0F, -1.0F, 0.0F, + 0.0F, 0.0F, 1.0F}, // follow behind + + {1.0F, 0.0F, 0.0F, + 0.0F, 1.0F, 0.0F, + 0.0F, 0.0F, 1.0F}, // follow front + + {1.0F, 1.0F, 0.0F, + -1.0F, 1.0F, 0.0F, + 0.0F, 0.0F, 1.0F}}; // follow left side + + Navigator *_navigator; control::BlockParamFloat _param_min_alt; FollowTargetState _follow_target_state; + FollowTargetPosition _follow_target_position; + int _follow_target_sub; float _step_time_in_ms; @@ -85,10 +113,11 @@ private: math::Vector<3> _step_vel; math::Vector<3> _target_vel; math::Vector<3> _target_distance; + math::Vector<3> _target_position_offset; follow_target_s _current_target_motion; follow_target_s _previous_target_motion; - + math::Matrix<3,3> _rot_matrix; void track_target_position(); void track_target_velocity(); bool target_velocity_valid(); diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index a7da600330..7fadabbd2c 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -61,14 +61,13 @@ enum NAV_CMD { NAV_CMD_TAKEOFF = 22, NAV_CMD_ROI = 80, NAV_CMD_PATHPLANNING = 81, - NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder - NAV_CMD_GOTO_TAREGT = 195, NAV_CMD_VTOL_TAKEOFF = 84, NAV_CMD_VTOL_LAND = 85, NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_CHANGE_SPEED = 178, NAV_CMD_DO_SET_SERVO=183, NAV_CMD_DO_REPEAT_SERVO=184, + NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder NAV_CMD_DO_DIGICAM_CONTROL=203, NAV_CMD_DO_SET_CAM_TRIGG_DIST=206, NAV_CMD_DO_VTOL_TRANSITION=3000,