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

AP_L1_Control : Fix potential divide by zero error

Browse Source
This commit is contained in:
priseborough 2014-08-07 18:42:02 +10:00 committed by Andrew Tridgell
parent 2b89d728f3
commit bc3c85be20
1 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -262,7 +262,19 @@ void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius
Vector2f A_air = location_diff(center_WP, _current_loc);
// Calculate the unit vector from WP A to aircraft
Vector2f A_air_unit = A_air.normalized();
// protect against being on the waypoint and having zero velocity
// if too close to the waypoint, use the velocity vector
// if the velocity vector is too small, use the heading vector
Vector2f A_air_unit;
if (A_air.length() > 0.1) {
A_air_unit = A_air.normalized();
} else {
if (_groundspeed_vector.length() < 0.1f) {
A_air_unit = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw));
} else {
A_air_unit = _groundspeed_vector.normalized();
}
}
//Calculate Nu to capture center_WP
float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP