From 9d7d174995f4e1ff1cbc9326f5bf1cb7ec1376c4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Apr 2013 11:50:12 +0900 Subject: [PATCH] AC_WPNAV: check distance to waypoint within library bug fix for loiter using lat/lon position instead of NED position when calculating desired velocity towards target --- libraries/AC_WPNav/AC_WPNav.cpp | 53 +++++++++++++++++---------------- libraries/AC_WPNav/AC_WPNav.h | 29 +++++++++--------- 2 files changed, 42 insertions(+), 40 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 71b7dae590..fb61fc964c 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -12,7 +12,16 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = { // @Description: The desired horizontal speed in cm/s while travelling between waypoints // @Range: 0 1000 // @Increment: 50 - AP_GROUPINFO("SPEED", 0, AC_WPNav, _speed_cms, WP_SPEED), + AP_GROUPINFO("SPEED", 0, AC_WPNav, _speed_cms, WPNAV_WP_SPEED), + + // @Param: RADIUS + // @DisplayName: Waypoint Radius + // @Description: Defines the distance from a waypoint, that when crossed indicates the wp has been hit. + // @Units: Centimeters + // @Range: 100 1000 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("RADIUS", 1, AC_WPNav, _wp_radius_cm, WPNAV_WP_RADIUS), AP_GROUPEND }; @@ -126,9 +135,6 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt) _target.x = curr_pos.x + MAX_LOITER_OVERSHOOT * distance_err.x/distance; _target.y = curr_pos.y + MAX_LOITER_OVERSHOOT * distance_err.y/distance; } - - // debug -- remove me! - //hal.console->printf_P(PSTR("\nLTarX:%4.2f Y:%4.2f Z:%4.2f\n"),_target.x, _target.y, _target.z); } /// get_distance_to_target - get horizontal distance to loiter target in cm @@ -172,6 +178,7 @@ void AC_WPNav::update_loiter() /// set_destination - set destination using cm from home void AC_WPNav::set_destination(const Vector3f& destination) { + // To-Do: use projection of current position & velocity to set origin set_origin_and_destination(_inav->get_position(), destination); } @@ -188,40 +195,29 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f _pos_delta_unit = pos_delta/_track_length; _track_desired = 0; - - // debug -- remove me! - //hal.console->printf_P(PSTR("\nSOAD: Ox:%4.2f y:%4.2f z:%4.2f Dx:%4.2f y:%4.2f z:%4.2f\n"),_origin.x, _origin.y, _origin.z, _destination.x, _destination.y, _destination.z); - //hal.console->printf_P(PSTR("\nVTS:%4.2f TL:%4.2f\n"),_vert_track_scale, _track_length); - //hal.console->printf_P(PSTR("\nPDP: x:%4.2f y:%4.2f z:%4.2f\n"),_pos_delta_unit.x, _pos_delta_unit.y, _pos_delta_unit.z); + _reached_destination = false; } /// advance_target_along_track - move target location along track from origin to destination void AC_WPNav::advance_target_along_track(float velocity_cms, float dt) { - //float cross_track_dist; float track_covered; float track_error; float track_desired_max; float track_desired_temp = _track_desired; float track_extra_max; float curr_delta_length; - //float alt_error; // get current location - Vector3f curr_delta = _inav->get_position() - _origin; + Vector3f curr_pos = _inav->get_position(); + Vector3f curr_delta = curr_pos - _origin; curr_delta.z = curr_delta.z * _vert_track_scale; curr_delta_length = curr_delta.length(); track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z; track_error = safe_sqrt(curr_delta_length*curr_delta_length - track_covered*track_covered); - // we don't need the following but we may want to log them to see how we are going. - // cross_track_dist = -(curr.x-_origin.x) * _pos_delta_unit.y + (curr.y-_origin.y) * _pos_delta_unit.x; - // alt_error = fabsf(_origin.z + _pos_delta_unit.z * track_covered - curr.z); - track_extra_max = safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - track_error*track_error); - // debug -- remove me! - //hal.console->printf_P(PSTR("\nvel_cms:%4.2f Tcov:%4.2f\n"),velocity_cms, track_covered); // we could save a sqrt by doing the following and not assigning track_error // track_extra_max = safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - (curr_delta_length*curr_delta_length - track_covered*track_covered)); @@ -231,9 +227,6 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt) // advance the current target track_desired_temp += velocity_cms * dt; - // debug -- remove me! - //hal.console->printf_P(PSTR("\nTdes:%4.2f TdesMx:%4.2f\n"),track_desired_temp, track_desired_max); - // constrain the target from moving too far if( track_desired_temp > track_desired_max ) { track_desired_temp = track_desired_max; @@ -246,8 +239,17 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt) _target.x = _origin.x + _pos_delta_unit.x * _track_desired; _target.y = _origin.y + _pos_delta_unit.y * _track_desired; _target.z = _origin.z + (_pos_delta_unit.z * _track_desired)/_vert_track_scale; - // we could save a divide by having a variable for 1/_vert_track_scale - //hal.console->printf_P(PSTR("\nTarX:%4.2f Y:%4.2f Z:%4.2f\n"),_target.x, _target.y, _target.z); + + // check if we've reached the waypoint + if( !_reached_destination ) { + if( _track_desired >= _track_length ) { + Vector3f dist_to_dest = curr_pos - _destination; + dist_to_dest.z *=_vert_track_scale; + if( dist_to_dest.length() <= _wp_radius_cm ) { + _reached_destination = true; + } + } + } } /// get_distance_to_destination - get horizontal distance to destination in cm @@ -294,6 +296,7 @@ void AC_WPNav::get_loiter_pos_lat_lon(float target_lat_from_home, float target_l { Vector2f dist_error; Vector2f desired_vel; + Vector3f curr = _inav->get_position(); float dist_error_total; float vel_sqrt; @@ -302,8 +305,8 @@ void AC_WPNav::get_loiter_pos_lat_lon(float target_lat_from_home, float target_l float linear_distance; // the distace we swap between linear and sqrt. // calculate distance error - dist_error.x = target_lat_from_home - _inav->get_latitude_diff(); - dist_error.y = target_lon_from_home - _inav->get_longitude_diff(); + dist_error.x = target_lat_from_home - curr.x; + dist_error.y = target_lon_from_home - curr.y; linear_distance = MAX_LOITER_POS_ACCEL/(2*_pid_pos_lat->kP()*_pid_pos_lat->kP()); _distance_to_target = linear_distance; // for reporting purposes diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 7ea7c4f4a0..2239558c27 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -29,7 +29,8 @@ // MAX_LOITER_OVERSHOOT = min(200, MAX_LOITER_POS_VELOCITY/Pid_P); // to stop it being over sensitive to error // end -#define WP_SPEED 500 // default horizontal speed betwen waypoints in cm/s +#define WPNAV_WP_SPEED 500 // default horizontal speed betwen waypoints in cm/s +#define WPNAV_WP_RADIUS 200 // default waypoint radius in cm #define WPINAV_MAX_POS_ERROR 531.25f // maximum distance (in cm) that the desired track can stray from our current location. // D0 = MAX_LOITER_POS_ACCEL/(2*Pid_P^2); // if WP_SPEED > 2*D0*Pid_P @@ -61,9 +62,6 @@ public: /// get_loiter_target - get loiter target as position vector (from home in cm) Vector3f get_loiter_target() { return _target; } - /// get_target_alt - get loiter's target altitude - float get_target_alt() { return _target.z; } - /// set_loiter_target in cm from home void set_loiter_target(const Vector3f& position) { _target = position; } @@ -92,7 +90,7 @@ public: int32_t get_angle_limit() { return _lean_angle_max; } /// - /// waypoint navigation + /// waypoint controller /// /// get_destination waypoint using position vector (distance from home in cm) @@ -107,18 +105,15 @@ public: /// advance_target_along_track - move target location along track from origin to destination void advance_target_along_track(float velocity_cms, float dt); - /// get_destination_alt - get target altitude above home in cm - float get_destination_alt() { return _destination.z; } - - /// set_destination_alt - set target altitude above home in cm - void set_destination_alt(float altitude_in_cm) { _destination.z = altitude_in_cm; } - /// get_distance_to_destination - get horizontal distance to destination in cm float get_distance_to_destination(); /// get_bearing_to_destination - get bearing to next waypoint in centi-degrees int32_t get_bearing_to_destination(); + /// reached_destination - true when we have come within RADIUS cm of the waypoint + bool reached_destination() { return _reached_destination; } + /// update_wp - update waypoint controller void update_wpnav(); @@ -126,12 +121,15 @@ public: /// shared methods /// - /// get desired roll, pitch and altitude which should be fed into stabilize controllers + /// get desired roll, pitch which should be fed into stabilize controllers int32_t get_desired_roll() { return _desired_roll; }; int32_t get_desired_pitch() { return _desired_pitch; }; - /// desired altitude (in cm) that should be fed into altitude hold controller. only valid when navigating between waypoints - int32_t get_desired_altitude() { return _desired_altitude; }; + /// get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller + float get_desired_alt() { return _target.z; } + + /// set_desired_alt - set desired altitude (in cm above home) + float set_desired_alt(float desired_alt) { _target.z = desired_alt; } /// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame void set_cos_sin_yaw(float cos_yaw, float sin_yaw, float cos_roll) { @@ -180,6 +178,7 @@ protected: // parameters AP_Float _speed_cms; // default horizontal speed in cm/s float _speedz_cms; // max vertical climb rate in cm/s. To-Do: rename or pull this from main code + AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached uint32_t _last_update; // time of last update call float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame float _sin_yaw; @@ -188,7 +187,6 @@ protected: // output from controller int32_t _desired_roll; // fed to stabilize controllers at 50hz int32_t _desired_pitch; // fed to stabilize controllers at 50hz - int32_t _desired_altitude; // fed to alt hold controller at 50hz int32_t _lean_angle_max; // maximum lean angle. can we set from main code so that throttle controller can stop leans that cause copter to lose altitude @@ -203,6 +201,7 @@ protected: float _track_desired; // our desired distance along the track in cm float _distance_to_target; // distance to loiter target float _vert_track_scale; // vertical scaling to give altitude equal weighting to position + bool _reached_destination; // true if we have reached the destination // pilot inputs for loiter int16_t _pilot_vel_forward_cms;