From 0797c7fc216ab1f400a548b94d92e8d9ebb5a48c Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Wed, 24 Feb 2016 10:03:46 -0800 Subject: [PATCH] velocity smoothing --- msg/position_setpoint.msg | 1 + .../mc_pos_control/mc_pos_control_main.cpp | 19 ++- src/modules/navigator/follow_target.cpp | 110 +++++++++++++++++- src/modules/navigator/follow_target.h | 25 +++- src/modules/navigator/mission_block.cpp | 7 +- src/modules/navigator/navigation.h | 1 + 6 files changed, 146 insertions(+), 17 deletions(-) diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index 6c06f9138c..394885933b 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -7,6 +7,7 @@ uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) uint8 SETPOINT_TYPE_OFFBOARD=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard +uint8 SETPOINT_TYPE_FOLLOW_TARGET=7 # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target bool valid # true if setpoint is valid uint8 type # setpoint type to adjust behavior of position controller 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 39dbb75438..fc516818a5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -720,6 +720,7 @@ MulticopterPositionControl::reset_pos_sp() // we have logic in the main function which chooses the velocity setpoint such that the attitude setpoint is // continuous when switching into velocity controlled mode, therefore, we don't need to bother about resetting // position in a special way. In position control mode the position will be reset anyway until the vehicle has reduced speed. + _pos_sp(0) = _pos(0); _pos_sp(1) = _pos(1); } @@ -1023,7 +1024,7 @@ 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 && 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) { @@ -1380,11 +1381,17 @@ MulticopterPositionControl::task_main() 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; - } +// float sp_vel = sqrtf(_pos_sp_triplet.current.vx*_pos_sp_triplet.current.vx + +// _pos_sp_triplet.current.vy*_pos_sp_triplet.current.vy); + + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) { + _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)); diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index c3238c89bb..b2d0058f11 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -52,16 +52,27 @@ #include #include #include +#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) + _param_min_alt(this, "MIS_TAKEOFF_ALT", false), + gps_valid(false), + _last_message_time(0), + _index(0) { /* load initial params */ updateParams(); + follow_target_reached = false; + pos_pair[0].setZero(); + pos_pair[1].setZero(); + _current_vel.setZero(); + _steps = 0.0f; } FollowTarget::~FollowTarget() @@ -71,11 +82,15 @@ FollowTarget::~FollowTarget() void FollowTarget::on_inactive() { + gps_valid = false; } void FollowTarget::on_activation() { + Vector2f vel; + vel.setZero(); + if(_tracker_motion_position_sub < 0) { _tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target)); } @@ -84,35 +99,118 @@ FollowTarget::on_activation() set_loiter_item(&_mission_item, _param_min_alt.get()); - convert_mission_item_to_sp(); + convert_mission_item_to_sp(vel); } void FollowTarget::on_active() { follow_target_s target; bool updated; + struct map_projection_reference_s target_ref; + float target_dist_x,target_dist_y; + uint64_t current_time = hrt_absolute_time(); + + orb_check(_tracker_motion_position_sub, &updated); if (updated) { if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) { + + if ((gps_valid == false) ) { + gps_pair(0) = target.lat; + gps_pair(1) = target.lon; + gps_valid = true; + + return; + } + + // 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); + + follow_target_reached = (sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y) < 5); + + if(follow_target_reached == false && gps_valid == true) { + Vector2f go_to_vel; + + 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)); + + set_follow_target_item(&_mission_item, _param_min_alt.get(), target); + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + convert_mission_item_to_sp(go_to_vel); + _navigator->get_position_setpoint_triplet()->previous.valid = false; + _navigator->set_position_setpoint_triplet_updated(); + //target_vel + return; + } + + // get last gps reference + + 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))); + + target_vel = pos_pair[1]/((double)(current_time - _last_message_time) * 1e-6); + + + 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)); + set_follow_target_item(&_mission_item, _param_min_alt.get(), target); - convert_mission_item_to_sp(); + + gps_pair(0) = target.lat; + gps_pair(1) = target.lon; + + _last_message_time = current_time; + + _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)10.0f; + //pos_pair[0] = pos_pair[1]; } } + if ((((double) (current_time - _last_publish_time) * 1e-3) > 100) && (follow_target_reached == true)) { + + if(_current_vel(0) <= target_vel(0)) { + _current_vel(0) += _steps; + } + + if(_current_vel(0) >= target_vel(0)) { + _current_vel(0) -= _steps; + } + + if(_current_vel(1) <= target_vel(1)) { + _current_vel(1) += _steps; + } + + if(_current_vel(1) >= target_vel(1)) { + _current_vel(1) -= _steps; + } + convert_mission_item_to_sp(_current_vel); + _navigator->set_position_setpoint_triplet_updated(); + warnx("updating x %f y %f %f", (double)_current_vel(0), (double)_current_vel(1), (double) _steps); + _last_publish_time = current_time; + } + } void -FollowTarget::convert_mission_item_to_sp() { +FollowTarget::convert_mission_item_to_sp(Vector2f & 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 - - pos_sp_triplet->previous.valid = true; + pos_sp_triplet->previous.valid = false; pos_sp_triplet->previous = pos_sp_triplet->current; 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->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 fe343c2e90..42d4bb1ca4 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -45,11 +45,10 @@ #include "navigator_mode.h" #include "mission_block.h" +#include class FollowTarget : public MissionBlock { - Navigator *_navigator; - int _tracker_motion_position_sub; /**< tracker motion subscription */ public: FollowTarget(Navigator *navigator, const char *name); @@ -63,6 +62,26 @@ public: virtual void on_active(); private: + Navigator *_navigator; + int _tracker_motion_position_sub; /**< tracker motion subscription */ control::BlockParamFloat _param_min_alt; - void convert_mission_item_to_sp(); + matrix::Vector2f pos_pair[2]; + matrix::Vector2f gps_pair; + bool gps_valid; + uint64_t _last_message_time; + 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; + }; + + pos_history_s wp_history[6]; + int wp_cnt; + matrix::Vector2f _current_vel; + matrix::Vector2f target_vel; + void convert_mission_item_to_sp(matrix::Vector2f & vel); }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index a949cc84c2..3390fbd515 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -228,7 +228,7 @@ MissionBlock::is_mission_item_reached() /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { - + return true; if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; @@ -239,7 +239,7 @@ MissionBlock::is_mission_item_reached() } /* check if the MAV was long enough inside the waypoint orbit */ - if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { + if ((now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f)) { return true; } } @@ -376,6 +376,9 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->disable_mc_yaw_control = true; } break; + case NAV_CMD_FOLLOW_TARGET: + sp->type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET; + break; default: sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index f7f7bb4158..82d87d09d3 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -62,6 +62,7 @@ enum NAV_CMD { NAV_CMD_ROI = 80, NAV_CMD_PATHPLANNING = 81, NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder + NAV_CMD_GOTO_TAREGT = 195, NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_CHANGE_SPEED = 178, NAV_CMD_DO_SET_SERVO=183,