diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 43ca5c184d..53e8ce200c 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -18,6 +18,7 @@ #include "AP_AHRS_View.h" #include #include +#include extern const AP_HAL::HAL& hal; @@ -159,6 +160,16 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { AP_GROUPEND }; +// init sets up INS board orientation +void AP_AHRS::init() +{ + update_orientation(); + +#if !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE + _nmea_out = AP_NMEA_Output::probe(); +#endif +} + // return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet) Vector3f AP_AHRS::get_gyro_latest(void) const { @@ -492,6 +503,15 @@ void AP_AHRS::Log_Write_Home_And_Origin() } +void AP_AHRS::update_nmea_out() +{ +#if !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE + if (_nmea_out != nullptr) { + _nmea_out->update(); + } +#endif +} + // singleton instance AP_AHRS *AP_AHRS::_singleton; diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 2413d60599..f01f5363b9 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -31,6 +31,7 @@ #include #include +class AP_NMEA_Output; class OpticalFlow; #define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees #define AP_AHRS_RP_P_MIN 0.05f // minimum value for AHRS_RP_P parameter @@ -86,9 +87,7 @@ public: } // init sets up INS board orientation - virtual void init() { - update_orientation(); - }; + virtual void init(); // Accessors void set_fly_forward(bool b) { @@ -590,6 +589,7 @@ public: } protected: + void update_nmea_out(); // multi-thread access support HAL_Semaphore_Recursive _rsem; @@ -698,6 +698,7 @@ protected: private: static AP_AHRS *_singleton; + AP_NMEA_Output* _nmea_out; }; #include "AP_AHRS_DCM.h" diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index d204a58d3b..4f379632b7 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -130,6 +130,11 @@ void AP_AHRS_NavEKF::update(bool skip_ins_update) // update optional alternative attitude view _view->update(skip_ins_update); } + +#if !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE + // update NMEA output + update_nmea_out(); +#endif } void AP_AHRS_NavEKF::update_DCM(bool skip_ins_update)