AP_AHRS: support NMEA output

ArduPilot · May 20, 2019 · 869a369 · 869a369
1 parent e06556a
commit 869a369
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 3 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -18,6 +18,7 @@
 #include "AP_AHRS_View.h"
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AP_NMEA_Output/AP_NMEA_Output.h>
 
 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
@@ -31,6 +31,7 @@
 #include <AP_Param/AP_Param.h>
 #include <AP_Common/Location.h>
 
+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 @@ class AP_AHRS
     }
 
     // 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 @@ class AP_AHRS
     }
 
 protected:
+    void update_nmea_out();
 
     // multi-thread access support
     HAL_Semaphore_Recursive _rsem;
@@ -698,6 +698,7 @@ class AP_AHRS
 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
@@ -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)