From aae4ed25533d6f4c47bc9d05d58502cfce6b1588 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 8 Mar 2018 11:44:53 +1100 Subject: [PATCH] AP_AHRS_NavEKF: accept external position estimates into AHRS --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 7 +++++++ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 3 +++ 2 files changed, 10 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index b186fc2e59..3ee3f485f8 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -1164,6 +1164,13 @@ void AP_AHRS_NavEKF::writeBodyFrameOdom(float quality, const Vector3f &delPos, EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset); } +// Write position and quaternion data from an external navigation system +void AP_AHRS_NavEKF::writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) +{ + EKF2.writeExtNavData(sensOffset, pos, quat, posErr, angErr, timeStamp_ms, resetTime_ms); +} + + // inhibit GPS usage uint8_t AP_AHRS_NavEKF::setInhibitGPS(void) { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index bad9cc4691..6a3ade1a58 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -163,6 +163,9 @@ public: // write body odometry measurements to the EKF void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset); + // Write position and quaternion data from an external navigation system + void writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) override; + // inhibit GPS usage uint8_t setInhibitGPS(void);