|
|
|
@ -224,7 +224,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
@@ -224,7 +224,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
|
|
|
|
|
Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); |
|
|
|
|
|
|
|
|
|
// update _target_bearing_cd
|
|
|
|
|
_target_bearing_cd = get_bearing_cd(_current_loc, next_WP); |
|
|
|
|
_target_bearing_cd = _current_loc.get_bearing_to(next_WP); |
|
|
|
|
|
|
|
|
|
//Calculate groundspeed
|
|
|
|
|
float groundSpeed = _groundspeed_vector.length(); |
|
|
|
@ -360,7 +360,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius
@@ -360,7 +360,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// update _target_bearing_cd
|
|
|
|
|
_target_bearing_cd = get_bearing_cd(_current_loc, center_WP); |
|
|
|
|
_target_bearing_cd = _current_loc.get_bearing_to(center_WP); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Calculate time varying control parameters
|
|
|
|
|