Browse Source

AP_L1_Control : Fixed potential divide by zero in Nu1 angle calculation

master
priseborough 12 years ago committed by Andrew Tridgell
parent
commit
9ff0888a17
  1. 2
      libraries/AP_L1_Control/AP_L1_Control.cpp

2
libraries/AP_L1_Control/AP_L1_Control.cpp

@ -141,7 +141,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
//Calculate Nu1 angle (Angle to L1 reference point) //Calculate Nu1 angle (Angle to L1 reference point)
float xtrackErr = _cross2D(A_air, AB_unit); float xtrackErr = _cross2D(A_air, AB_unit);
float sine_Nu1 = xtrackErr/_L1_dist; float sine_Nu1 = xtrackErr/_maxf(_L1_dist , 0.1f);
//Limit sine of Nu1 to provide a controlled track capture angle of 45 deg //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
sine_Nu1 = constrain(sine_Nu1, -0.7854f, 0.7854f); sine_Nu1 = constrain(sine_Nu1, -0.7854f, 0.7854f);
float Nu1 = asinf(sine_Nu1); float Nu1 = asinf(sine_Nu1);

Loading…
Cancel
Save