From b6568b1c7e59a0d16f4a1484c6243e5fca10215c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 2 May 2017 19:36:15 +1000 Subject: [PATCH] AP_AHRS: support set_origin and get_origin for EKF 10 --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 8729ce359a..0a6d3ff76e 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -587,6 +587,14 @@ bool AP_AHRS_NavEKF::set_origin(const Location &loc) case EKF_TYPE3: return ret3; +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKF_TYPE_SITL: { + struct SITL::sitl_fdm &fdm = _sitl->state; + fdm.home = loc; + return true; + } +#endif + default: return false; } @@ -1411,8 +1419,9 @@ bool AP_AHRS_NavEKF::get_origin(Location &ret) const #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKF_TYPE_SITL: - ret = get_home(); - return ret.lat != 0 || ret.lng != 0; + const struct SITL::sitl_fdm &fdm = _sitl->state; + ret = fdm.home; + return true; #endif } }