aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

WPNav: divide by zero checks

Browse Source
This commit is contained in:
Randy Mackay 2013-05-15 11:51:26 +09:00
parent 610edd8f9a
commit cb795ea536
2 changed files with 71 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -79,6 +79,8 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM
_target_vel(0,0,0), _target_vel(0,0,0),
_vel_last(0,0,0), _vel_last(0,0,0),
_lean_angle_max(MAX_LEAN_ANGLE), _lean_angle_max(MAX_LEAN_ANGLE),
_loiter_leash(WPNAV_MIN_LEASH_LENGTH),
_wp_leash_xy(WPNAV_MIN_LEASH_LENGTH),
dist_error(0,0), dist_error(0,0),
desired_vel(0,0), desired_vel(0,0),
desired_accel(0,0) desired_accel(0,0)
@ -100,24 +102,25 @@ void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velo
float linear_velocity; // the velocity we swap between linear and sqrt. float linear_velocity; // the velocity we swap between linear and sqrt.
float vel_total; float vel_total;
float target_dist; float target_dist;
float kP = _pid_pos_lat->kP();
// avoid divide by zero // calculate current velocity
if( _pid_pos_lat->kP() <= 0.1 ) { vel_total = safe_sqrt(velocity.x*velocity.x + velocity.y*velocity.y);
// avoid divide by zero by using current position if the velocity is below 10cm/s or kP is very low
if (vel_total < 10.0f || kP <= 0.0f) {
target = position; target = position;
return; return;
} }
// calculate point at which velocity switches from linear to sqrt // calculate point at which velocity switches from linear to sqrt
linear_velocity = MAX_LOITER_POS_ACCEL/_pid_pos_lat->kP(); linear_velocity = MAX_LOITER_POS_ACCEL/kP;
// calculate total current velocity
vel_total = safe_sqrt(velocity.x*velocity.x + velocity.y*velocity.y);
// calculate distance within which we can stop // calculate distance within which we can stop
if (vel_total < linear_velocity) { if (vel_total < linear_velocity) {
target_dist = vel_total/_pid_pos_lat->kP(); target_dist = vel_total/kP;
} else { } else {
linear_distance = MAX_LOITER_POS_ACCEL/(2*_pid_pos_lat->kP()*_pid_pos_lat->kP()); linear_distance = MAX_LOITER_POS_ACCEL/(2*kP*kP);
target_dist = linear_distance + (vel_total*vel_total)/(2*MAX_LOITER_POS_ACCEL); target_dist = linear_distance + (vel_total*vel_total)/(2*MAX_LOITER_POS_ACCEL);
} }
target_dist = constrain_float(target_dist, 0, _loiter_leash); target_dist = constrain_float(target_dist, 0, _loiter_leash);
@ -169,7 +172,7 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
// constrain the velocity vector and scale if necessary // constrain the velocity vector and scale if necessary
vel_delta_total = safe_sqrt(target_vel_adj.x*target_vel_adj.x + target_vel_adj.y*target_vel_adj.y); vel_delta_total = safe_sqrt(target_vel_adj.x*target_vel_adj.x + target_vel_adj.y*target_vel_adj.y);
vel_max = 2.0*MAX_LOITER_POS_ACCEL*nav_dt; vel_max = 2.0*MAX_LOITER_POS_ACCEL*nav_dt;
if( vel_delta_total > vel_max) { if (vel_delta_total > vel_max && vel_delta_total > 0.0f) {
target_vel_adj.x = vel_max * target_vel_adj.x/vel_delta_total; target_vel_adj.x = vel_max * target_vel_adj.x/vel_delta_total;
target_vel_adj.y = vel_max * target_vel_adj.y/vel_delta_total; target_vel_adj.y = vel_max * target_vel_adj.y/vel_delta_total;
} }
@ -180,7 +183,7 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
// constrain the velocity vector and scale if necessary // constrain the velocity vector and scale if necessary
vel_total = safe_sqrt(_target_vel.x*_target_vel.x + _target_vel.y*_target_vel.y); vel_total = safe_sqrt(_target_vel.x*_target_vel.x + _target_vel.y*_target_vel.y);
if( vel_total > _loiter_speed_cms ) { if (vel_total > _loiter_speed_cms && vel_total > 0.0f) {
_target_vel.x = _loiter_speed_cms * _target_vel.x/vel_total; _target_vel.x = _loiter_speed_cms * _target_vel.x/vel_total;
_target_vel.y = _loiter_speed_cms * _target_vel.y/vel_total; _target_vel.y = _loiter_speed_cms * _target_vel.y/vel_total;
} }
@ -193,7 +196,7 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
Vector3f curr_pos = _inav->get_position(); Vector3f curr_pos = _inav->get_position();
Vector3f distance_err = _target - curr_pos; Vector3f distance_err = _target - curr_pos;
float distance = safe_sqrt(distance_err.x*distance_err.x + distance_err.y*distance_err.y); float distance = safe_sqrt(distance_err.x*distance_err.x + distance_err.y*distance_err.y);
if( distance > _loiter_leash ) { if (distance > _loiter_leash && distance > 0.0f) {
_target.x = curr_pos.x + _loiter_leash * distance_err.x/distance; _target.x = curr_pos.x + _loiter_leash * distance_err.x/distance;
_target.y = curr_pos.y + _loiter_leash * distance_err.y/distance; _target.y = curr_pos.y + _loiter_leash * distance_err.y/distance;
} }
@ -237,6 +240,12 @@ void AC_WPNav::calculate_loiter_leash_length()
// get loiter position P // get loiter position P
float kP = _pid_pos_lat->kP(); float kP = _pid_pos_lat->kP();
// avoid divide by zero
if (kP <= 0.0f) {
_loiter_leash = WPNAV_MIN_LEASH_LENGTH;
return;
}
// calculate horiztonal leash length // calculate horiztonal leash length
if(_loiter_speed_cms <= MAX_LOITER_POS_ACCEL / kP) { if(_loiter_speed_cms <= MAX_LOITER_POS_ACCEL / kP) {
// linear leash length based on speed close in // linear leash length based on speed close in
@ -247,8 +256,8 @@ void AC_WPNav::calculate_loiter_leash_length()
} }
// ensure leash is at least 1m long // ensure leash is at least 1m long
if( _loiter_leash < 100 ) { if( _loiter_leash < WPNAV_MIN_LEASH_LENGTH ) {
_loiter_leash = 100; _loiter_leash = WPNAV_MIN_LEASH_LENGTH;
} }
} }
@ -286,7 +295,16 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
// scale up z-axis position delta (i.e. distance) to make later leash length calculations simpler // scale up z-axis position delta (i.e. distance) to make later leash length calculations simpler
pos_delta.z = pos_delta.z * _vert_track_scale; pos_delta.z = pos_delta.z * _vert_track_scale;
_track_length = pos_delta.length(); _track_length = pos_delta.length();
_pos_delta_unit = pos_delta/_track_length;
// calculate each axis' percentage of the total distance to the destination
if (_track_length == 0.0f) {
// avoid possible divide by zero
_pos_delta_unit.x = 0;
_pos_delta_unit.y = 0;
_pos_delta_unit.z = 0;
}else{
_pos_delta_unit = pos_delta/_track_length;
}
// initialise intermediate point to the origin // initialise intermediate point to the origin
_track_desired = 0; _track_desired = 0;
@ -419,29 +437,36 @@ void AC_WPNav::get_loiter_position_to_velocity(float dt, float max_speed_cms)
float vel_total; float vel_total;
float linear_distance; // the distace we swap between linear and sqrt. float linear_distance; // the distace we swap between linear and sqrt.
float kP = _pid_pos_lat->kP();
// calculate distance error // avoid divide by zero
dist_error.x = _target.x - curr.x; if (kP <= 0.0f) {
dist_error.y = _target.y - curr.y; desired_vel.x = 0.0;
desired_vel.y = 0.0;
linear_distance = MAX_LOITER_POS_ACCEL/(2*_pid_pos_lat->kP()*_pid_pos_lat->kP());
_distance_to_target = linear_distance; // for reporting purposes
dist_error_total = safe_sqrt(dist_error.x*dist_error.x + dist_error.y*dist_error.y);
if( dist_error_total > 2*linear_distance ) {
vel_sqrt = safe_sqrt(2*MAX_LOITER_POS_ACCEL*(dist_error_total-linear_distance));
desired_vel.x = vel_sqrt * dist_error.x/dist_error_total;
desired_vel.y = vel_sqrt * dist_error.y/dist_error_total;
}else{ }else{
desired_vel.x = _pid_pos_lat->get_p(dist_error.x); // calculate distance error
desired_vel.y = _pid_pos_lon->get_p(dist_error.y); dist_error.x = _target.x - curr.x;
} dist_error.y = _target.y - curr.y;
// ensure velocity stays within limits linear_distance = MAX_LOITER_POS_ACCEL/(2*kP*kP);
vel_total = safe_sqrt(desired_vel.x*desired_vel.x + desired_vel.y*desired_vel.y); _distance_to_target = linear_distance; // for reporting purposes
if( vel_total > max_speed_cms ) {
desired_vel.x = max_speed_cms * desired_vel.x/vel_total; dist_error_total = safe_sqrt(dist_error.x*dist_error.x + dist_error.y*dist_error.y);
desired_vel.y = max_speed_cms * desired_vel.y/vel_total; if( dist_error_total > 2*linear_distance ) {
vel_sqrt = safe_sqrt(2*MAX_LOITER_POS_ACCEL*(dist_error_total-linear_distance));
desired_vel.x = vel_sqrt * dist_error.x/dist_error_total;
desired_vel.y = vel_sqrt * dist_error.y/dist_error_total;
}else{
desired_vel.x = _pid_pos_lat->get_p(dist_error.x);
desired_vel.y = _pid_pos_lon->get_p(dist_error.y);
}
// ensure velocity stays within limits
vel_total = safe_sqrt(desired_vel.x*desired_vel.x + desired_vel.y*desired_vel.y);
if( vel_total > max_speed_cms ) {
desired_vel.x = max_speed_cms * desired_vel.x/vel_total;
desired_vel.y = max_speed_cms * desired_vel.y/vel_total;
}
} }
// call velocity to acceleration controller // call velocity to acceleration controller
@ -545,6 +570,12 @@ void AC_WPNav::calculate_wp_leash_length(bool climb)
// get loiter position P // get loiter position P
float kP = _pid_pos_lat->kP(); float kP = _pid_pos_lat->kP();
// avoid divide by zero
if (kP <= 0.0f) {
_wp_leash_xy = WPNAV_MIN_LEASH_LENGTH;
_vert_track_scale = 1.0f;
return;
}
// calculate horiztonal leash length // calculate horiztonal leash length
if(_wp_speed_cms <= MAX_LOITER_POS_ACCEL / kP) { if(_wp_speed_cms <= MAX_LOITER_POS_ACCEL / kP) {
// linear leash length based on speed close in // linear leash length based on speed close in
@ -555,8 +586,8 @@ void AC_WPNav::calculate_wp_leash_length(bool climb)
} }
// ensure leash is at least 1m long // ensure leash is at least 1m long
if( _wp_leash_xy < 100 ) { if( _wp_leash_xy < WPNAV_MIN_LEASH_LENGTH ) {
_wp_leash_xy = 100; _wp_leash_xy = WPNAV_MIN_LEASH_LENGTH;
} }
// calculate vertical leash length // calculate vertical leash length
@ -575,8 +606,8 @@ void AC_WPNav::calculate_wp_leash_length(bool climb)
} }
// ensure leash is at least 1m long // ensure leash is at least 1m long
if( leash_z < 100 ) { if( leash_z < WPNAV_MIN_LEASH_LENGTH ) {
leash_z = 100; leash_z = WPNAV_MIN_LEASH_LENGTH;
} }
// calculate vertical track scale used to give altitude equal weighting to horizontal position // calculate vertical track scale used to give altitude equal weighting to horizontal position

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -30,6 +30,8 @@
#define WPNAV_WP_ACCELERATION 500.0f // acceleration in cm/s/s used to increase the speed of the intermediate point up to it's maximum speed held in _speed_xy_cms #define WPNAV_WP_ACCELERATION 500.0f // acceleration in cm/s/s used to increase the speed of the intermediate point up to it's maximum speed held in _speed_xy_cms
#define WPNAV_MIN_LEASH_LENGTH 100.0f // minimum leash lengths in cm
class AC_WPNav class AC_WPNav
{ {
public: public: