Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Landing: use get_distance_NE instead of location_diff

Browse Source
This commit is contained in:
Pierre Kancir 2019-04-08 15:16:19 +02:00 committed by Tom Pittenger
parent 25507d4d7b
commit 49be270377
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Landing/AP_Landing_Deepstall.cpp
View File

@ -576,7 +576,7 @@ float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const f
bool AP_Landing_Deepstall::verify_breakout(const Location &current_loc, const Location &target_loc, bool AP_Landing_Deepstall::verify_breakout(const Location &current_loc, const Location &target_loc,
const float height_error) const const float height_error) const
{ {
Vector2f location_delta = location_diff(current_loc, target_loc); const Vector2f location_delta = current_loc.get_distance_NE(target_loc);
const float heading_error = degrees(landing.ahrs.groundspeed_vector().angle(location_delta)); const float heading_error = degrees(landing.ahrs.groundspeed_vector().angle(location_delta));
// Check to see if the the plane is heading toward the land waypoint. We use 20 degrees (+/-10 deg) // Check to see if the the plane is heading toward the land waypoint. We use 20 degrees (+/-10 deg)
@ -607,9 +607,9 @@ float AP_Landing_Deepstall::update_steering()
float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) * 1e-3; float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) * 1e-3;
last_time = time; last_time = time;
Vector2f ab = location_diff(arc_exit, extended_approach); Vector2f ab = arc_exit.get_distance_NE(extended_approach);
ab.normalize(); ab.normalize();
Vector2f a_air = location_diff(arc_exit, current_loc); const Vector2f a_air = arc_exit.get_distance_NE(current_loc);
crosstrack_error = a_air % ab; crosstrack_error = a_air % ab;
float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f); float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);