AP_AHRS: create AP_AHRS_NavEKF class

subclass of AP_AHRS_DCM, just a wrapper for now, will add EKF calls
next

libraries/AP_AHRS/AP_AHRS.h

@@ -322,6 +322,7 @@ protected:
 };
 
 #include <AP_AHRS_DCM.h>
+#include <AP_AHRS_NavEKF.h>
 #include <AP_AHRS_HIL.h>
 
 #endif // __AP_AHRS_H__

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -0,0 +1,94 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *  NavEKF based AHRS (Attitude Heading Reference System) interface for
+ *  ArduPilot
+ *
+ */
+#include <AP_HAL.h>
+#include <AP_AHRS.h>
+
+#if AP_AHRS_NAVEKF_AVAILABLE
+
+// return the smoothed gyro vector corrected for drift
+const Vector3f AP_AHRS_NavEKF::get_gyro(void) const
+{
+    return AP_AHRS_DCM::get_gyro();
+}
+
+const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const
+{
+    return AP_AHRS_DCM::get_dcm_matrix();
+}
+
+const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const
+{
+    return AP_AHRS_DCM::get_gyro_drift();
+}
+
+void AP_AHRS_NavEKF::update(void)
+{
+    AP_AHRS_DCM::update();
+}
+
+void AP_AHRS_NavEKF::reset(bool recover_eulers)
+{
+    AP_AHRS_DCM::reset(recover_eulers);
+}
+
+// reset the current attitude, used on new IMU calibration
+void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, const float &_yaw)
+{
+    AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw);
+}
+
+// dead-reckoning support
+bool AP_AHRS_NavEKF::get_position(struct Location &loc)
+{
+    return AP_AHRS_DCM::get_position(loc);
+}
+
+// status reporting of estimated error
+float AP_AHRS_NavEKF::get_error_rp(void)
+{
+    return AP_AHRS_DCM::get_error_rp();
+}
+
+float AP_AHRS_NavEKF::get_error_yaw(void)
+{
+    return AP_AHRS_DCM::get_error_yaw();
+}
+
+// return a wind estimation vector, in m/s
+Vector3f AP_AHRS_NavEKF::wind_estimate(void)
+{
+    return AP_AHRS_DCM::wind_estimate();
+}
+
+// return an airspeed estimate if available. return true
+// if we have an estimate
+bool AP_AHRS_NavEKF::airspeed_estimate(float *airspeed_ret)
+{
+    return AP_AHRS_DCM::airspeed_estimate(airspeed_ret);
+}
+
+// true if compass is being used
+bool AP_AHRS_NavEKF::use_compass(void)
+{
+    return AP_AHRS_DCM::use_compass();
+}
+
+#endif // AP_AHRS_NAVEKF_AVAILABLE

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -0,0 +1,80 @@
+#ifndef __AP_AHRS_NAVEKF_H__
+#define __AP_AHRS_NAVEKF_H__
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *  NavEKF based AHRS (Attitude Heading Reference System) interface for
+ *  ArduPilot
+ *
+ */
+
+#include <AP_AHRS.h>
+
+#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
+#include <AP_NavEKF.h>
+
+#define AP_AHRS_NAVEKF_AVAILABLE 1
+
+class AP_AHRS_NavEKF : public AP_AHRS_DCM
+{
+public:
+    // Constructor
+    AP_AHRS_NavEKF(AP_InertialSensor &ins, GPS *&gps, AP_Baro &baro) :
+        AP_AHRS_DCM(ins, gps),
+        EKF(this, baro),
+        _baro(baro)
+        {
+        }
+
+    // return the smoothed gyro vector corrected for drift
+    const Vector3f get_gyro(void) const;
+    const Matrix3f &get_dcm_matrix(void) const;
+
+    // return the current drift correction integrator value
+    const Vector3f &get_gyro_drift(void) const;
+
+    void            update(void);
+    void            reset(bool recover_eulers = false);
+
+    // reset the current attitude, used on new IMU calibration
+    void reset_attitude(const float &roll, const float &pitch, const float &yaw);
+
+    // dead-reckoning support
+    bool get_position(struct Location &loc);
+
+    // status reporting of estimated error
+    float           get_error_rp(void);
+    float           get_error_yaw(void);
+
+    // return a wind estimation vector, in m/s
+    Vector3f wind_estimate(void);
+
+    // return an airspeed estimate if available. return true
+    // if we have an estimate
+    bool airspeed_estimate(float *airspeed_ret);
+
+    // true if compass is being used
+    bool use_compass(void);
+
+    const NavEKF &get_NavEKF(void) const { return EKF; }
+
+private:
+    NavEKF EKF;
+    AP_Baro &_baro;
+};
+#endif
+
+#endif // __AP_AHRS_NAVEKF_H__