|
|
|
@ -217,7 +217,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
@@ -217,7 +217,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
|
|
|
|
|
float K_L1 = 4.0f * _L1_damping * _L1_damping; |
|
|
|
|
|
|
|
|
|
// Get current position and velocity
|
|
|
|
|
if (_ahrs.get_position(_current_loc) == false) { |
|
|
|
|
if (_ahrs.get_location(_current_loc) == false) { |
|
|
|
|
// if no GPS loc available, maintain last nav/target_bearing
|
|
|
|
|
_data_is_stale = true; |
|
|
|
|
return; |
|
|
|
@ -349,7 +349,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius
@@ -349,7 +349,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius
|
|
|
|
|
float K_L1 = 4.0f * _L1_damping * _L1_damping; |
|
|
|
|
|
|
|
|
|
//Get current position and velocity
|
|
|
|
|
if (_ahrs.get_position(_current_loc) == false) { |
|
|
|
|
if (_ahrs.get_location(_current_loc) == false) { |
|
|
|
|
// if no GPS loc available, maintain last nav/target_bearing
|
|
|
|
|
_data_is_stale = true; |
|
|
|
|
return; |
|
|
|
|