diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index 8924514369..1fd3bd40be 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -79,6 +79,13 @@ public: */ virtual nav_filter_status get_filter_status() const; + /** + * get_origin - returns the inertial navigation origin in lat/lon/alt + * + * @return origin Location + */ + virtual struct Location get_origin() const { return _ahrs.get_home(); } + // // XY Axis specific methods // diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp index b51ffc28bb..2e3c46873e 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp @@ -51,6 +51,15 @@ nav_filter_status AP_InertialNav_NavEKF::get_filter_status() const return AP_InertialNav::get_filter_status(); } +struct Location AP_InertialNav_NavEKF::get_origin() const { + if (_ahrs.have_inertial_nav()) { + struct Location ret; + _ahrs_ekf.get_NavEKF().getOriginLLH(ret); + return ret; + } + return AP_InertialNav::get_origin(); +} + /** * get_position - returns the current position relative to the home location in cm. * diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h index b77d29412b..b3e14a9571 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h @@ -38,6 +38,13 @@ public: */ nav_filter_status get_filter_status() const; + /** + * get_origin - returns the inertial navigation origin in lat/lon/alt + * + * @return origin Location + */ + struct Location get_origin() const; + /** * get_position - returns the current position relative to the home location in cm. *