Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Arming: check ahrs and gps differ by less than 10m

Browse Source
This commit is contained in:
Randy Mackay 2017-09-18 12:14:52 +09:00
parent 075564af2c
commit 78d1c55685
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Arming/AP_Arming.cpp
View File

@ -23,6 +23,7 @@
#define AP_ARMING_BOARD_VOLTAGE_MIN 4.3f
#define AP_ARMING_BOARD_VOLTAGE_MAX 5.8f
#define AP_ARMING_ACCEL_ERROR_THRESHOLD 0.75f
#define AP_ARMING_AHRS_GPS_ERROR_MAX 10 // accept up to 10m difference between AHRS and GPS
extern const AP_HAL::HAL& hal;
@ -381,6 +382,19 @@ bool AP_Arming::gps_checks(bool report)
}
return false;
}
// check AHRS and GPS are within 10m of each other
Location gps_loc = ahrs.get_gps().location();
Location ahrs_loc;
if (ahrs.get_position(ahrs_loc)) {
float distance = location_3d_diff_NED(gps_loc, ahrs_loc).length();
if (distance > AP_ARMING_AHRS_GPS_ERROR_MAX) {
if (report) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: GPS and AHRS differ by %4.1fm", (double)distance);
}
return false;
}
}
}
return true;