|
|
|
@ -141,7 +141,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
@@ -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)
|
|
|
|
|
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
|
|
|
|
|
sine_Nu1 = constrain(sine_Nu1, -0.7854f, 0.7854f); |
|
|
|
|
float Nu1 = asinf(sine_Nu1); |
|
|
|
|