diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index f10ddbf805..32cab46b21 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -216,6 +216,10 @@ void AP_AHRS::init() // init parent class AP_AHRS_DCM::init(); + +#if HAL_NMEA_OUTPUT_ENABLED + _nmea_out = AP_NMEA_Output::probe(); +#endif } // return the smoothed gyro vector corrected for drift @@ -316,9 +320,11 @@ void AP_AHRS::update(bool skip_ins_update) _view->update(skip_ins_update); } -#if !HAL_MINIMIZE_FEATURES +#if HAL_NMEA_OUTPUT_ENABLED // update NMEA output - update_nmea_out(); + if (_nmea_out != nullptr) { + _nmea_out->update(); + } #endif EKFType active = active_EKF_type(); diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 40ab61283b..1367cd3edd 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -51,6 +51,8 @@ class AP_AHRS_View; #define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started +#include + class AP_AHRS : public AP_AHRS_DCM { friend class AP_AHRS_View; public: @@ -401,6 +403,10 @@ private: #if HAL_EXTERNAL_AHRS_ENABLED void update_external(void); #endif + +#if HAL_NMEA_OUTPUT_ENABLED + class AP_NMEA_Output* _nmea_out; +#endif }; namespace AP { diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp index 3055f7f10d..677670fae1 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Backend.cpp @@ -20,7 +20,6 @@ #include #include #include -#include extern const AP_HAL::HAL& hal; @@ -28,10 +27,6 @@ extern const AP_HAL::HAL& hal; void AP_AHRS_Backend::init() { update_orientation(); - -#if !HAL_MINIMIZE_FEATURES - _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) @@ -349,15 +344,6 @@ float AP_AHRS_Backend::get_EAS2TAS(void) const { return AP::baro().get_EAS2TAS(); } -void AP_AHRS_Backend::update_nmea_out() -{ -#if !HAL_MINIMIZE_FEATURES - if (_nmea_out != nullptr) { - _nmea_out->update(); - } -#endif -} - // return current vibration vector for primary IMU Vector3f AP_AHRS_Backend::get_vibration(void) const { diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index ea46a8cff4..8b3f127b78 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -28,7 +28,6 @@ #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 @@ -578,7 +577,6 @@ public: void Write_POS(void) const; protected: - void update_nmea_out(); // multi-thread access support HAL_Semaphore _rsem; @@ -683,8 +681,6 @@ protected: private: - AP_NMEA_Output* _nmea_out; - uint32_t takeoff_expected_start_ms; uint32_t touchdown_expected_start_ms; }; diff --git a/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp index 2ee66db749..9c1c4c2d7e 100644 --- a/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp +++ b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp @@ -19,17 +19,16 @@ #include "AP_NMEA_Output.h" -#if !HAL_MINIMIZE_FEATURES +#if HAL_NMEA_OUTPUT_ENABLED #include #include +#include #include #include #include -AP_NMEA_Output* AP_NMEA_Output::_singleton; - AP_NMEA_Output::AP_NMEA_Output() { AP_SerialManager& sm = AP::serialmanager(); @@ -46,11 +45,12 @@ AP_NMEA_Output::AP_NMEA_Output() AP_NMEA_Output* AP_NMEA_Output::probe() { - if (!_singleton && AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_NMEAOutput, 0) != nullptr) { - _singleton = new AP_NMEA_Output(); + AP_NMEA_Output *ret = new AP_NMEA_Output(); + if (ret == nullptr || ret->_num_outputs == 0) { + delete ret; + return nullptr; } - - return _singleton; + return ret; } uint8_t AP_NMEA_Output::_nmea_checksum(const char *str) @@ -187,4 +187,4 @@ void AP_NMEA_Output::update() } } -#endif // !HAL_MINIMIZE_FEATURES +#endif // HAL_NMEA_OUTPUT_ENABLED diff --git a/libraries/AP_NMEA_Output/AP_NMEA_Output.h b/libraries/AP_NMEA_Output/AP_NMEA_Output.h index 1eaf4c1bc2..34e411ee07 100644 --- a/libraries/AP_NMEA_Output/AP_NMEA_Output.h +++ b/libraries/AP_NMEA_Output/AP_NMEA_Output.h @@ -20,9 +20,12 @@ #pragma once #include -#include -#if !HAL_MINIMIZE_FEATURES +#ifndef HAL_NMEA_OUTPUT_ENABLED +#define HAL_NMEA_OUTPUT_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +#if HAL_NMEA_OUTPUT_ENABLED #include @@ -32,8 +35,7 @@ public: static AP_NMEA_Output* probe(); /* Do not allow copies */ - AP_NMEA_Output(const AP_NMEA_Output &other) = delete; - AP_NMEA_Output &operator=(const AP_NMEA_Output&) = delete; + CLASS_NO_COPY(AP_NMEA_Output); void update(); @@ -42,8 +44,6 @@ private: uint8_t _nmea_checksum(const char *str); - static AP_NMEA_Output* _singleton; - uint8_t _num_outputs; AP_HAL::UARTDriver* _uart[SERIALMANAGER_NUM_PORTS];