|
|
|
@ -97,7 +97,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
@@ -97,7 +97,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
|
|
|
|
|
_ahrs->get_position(_current_loc); |
|
|
|
|
_ahrs->get_projected_position(_current_loc); |
|
|
|
|
|
|
|
|
|
// update _target_bearing_cd
|
|
|
|
|
_target_bearing_cd = get_bearing_cd(_current_loc, next_WP); |
|
|
|
@ -197,7 +197,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius
@@ -197,7 +197,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
|
|
|
|
|
_ahrs->get_position(_current_loc); |
|
|
|
|
_ahrs->get_projected_position(_current_loc); |
|
|
|
|
|
|
|
|
|
// update _target_bearing_cd
|
|
|
|
|
_target_bearing_cd = get_bearing_cd(_current_loc, center_WP); |
|
|
|
|