mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 08:13:56 -04:00
AP_Arming: use get_distance instead of location_diff
This commit is contained in:
parent
00fa07ef33
commit
6397cb0c0e
@ -434,7 +434,7 @@ bool AP_Arming::gps_checks(bool report)
|
|||||||
const Location gps_loc = gps.location();
|
const Location gps_loc = gps.location();
|
||||||
Location ahrs_loc;
|
Location ahrs_loc;
|
||||||
if (AP::ahrs().get_position(ahrs_loc)) {
|
if (AP::ahrs().get_position(ahrs_loc)) {
|
||||||
const float distance = location_diff(gps_loc, ahrs_loc).length();
|
const float distance = gps_loc.get_distance(ahrs_loc);
|
||||||
if (distance > AP_ARMING_AHRS_GPS_ERROR_MAX) {
|
if (distance > AP_ARMING_AHRS_GPS_ERROR_MAX) {
|
||||||
check_failed(ARMING_CHECK_GPS, report, "GPS and AHRS differ by %4.1fm", (double)distance);
|
check_failed(ARMING_CHECK_GPS, report, "GPS and AHRS differ by %4.1fm", (double)distance);
|
||||||
return false;
|
return false;
|
||||||
|
Loading…
Reference in New Issue
Block a user