From f12a7dd04bad600f99cd1a1433067e908d43d327 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 24 Jun 2021 06:43:50 +1000 Subject: [PATCH] AP_Mount: fixed longitude subtraction --- libraries/AP_Mount/AP_Mount_Backend.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 483abf6337..d941f849c1 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -185,7 +185,7 @@ bool AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vec if (!AP::ahrs().get_position(current_loc)) { return false; } - const float GPS_vector_x = (target.lng-current_loc.lng)*cosf(ToRad((current_loc.lat+target.lat)*0.00000005f))*0.01113195f; + const float GPS_vector_x = Location::diff_longitude(target.lng,current_loc.lng)*cosf(ToRad((current_loc.lat+target.lat)*0.00000005f))*0.01113195f; const float GPS_vector_y = (target.lat-current_loc.lat)*0.01113195f; int32_t target_alt_cm = 0; if (!target.get_alt_cm(Location::AltFrame::ABOVE_HOME, target_alt_cm)) { @@ -217,4 +217,4 @@ bool AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vec return true; } -#endif // HAL_MOUNT_ENABLED \ No newline at end of file +#endif // HAL_MOUNT_ENABLED