Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Randy Mackay 2013-04-09 11:50:12 +09:00
parent 35001619f0
commit 9d7d174995
2 changed files with 42 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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

29
libraries/AC_WPNav/AC_WPNav.h
View File

@ -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;