AP_GPS: Send RTK baseline data from SBF

Tools/ardupilotwaf/ap_library.py

@@ -205,12 +205,34 @@ def __str__(self):
     def keyword(self):
         return 'Checking included headers'
 
+def double_precision_check(tasks):
+    '''check for tasks marked as double precision'''
+
+    for t in tasks:
+        if len(t.inputs) == 1:
+            # get a list of tasks we need to change to be double precision
+            double_tasks = []
+            for library in t.env.DOUBLE_PRECISION_SOURCES.keys():
+                for s in t.env.DOUBLE_PRECISION_SOURCES[library]:
+                    double_tasks.append([library, s])
+
+            src = str(t.inputs[0]).split('/')[-2:]
+            if src in double_tasks:
+                single_precision_option='-fsingle-precision-constant'
+                t.env.CXXFLAGS = t.env.CXXFLAGS[:]
+                if single_precision_option in t.env.CXXFLAGS:
+                    t.env.CXXFLAGS.remove(single_precision_option)
+                t.env.CXXFLAGS.append("-DALLOW_DOUBLE_MATH_FUNCTIONS")
+
+
 @feature('ap_library_object')
 @after_method('process_source')
 def ap_library_register_for_check(self):
     if not hasattr(self, 'compiled_tasks'):
         return
 
+    double_precision_check(self.compiled_tasks)
+
     if not self.env.ENABLE_HEADER_CHECKS:
         return
 
@@ -221,3 +243,4 @@ def ap_library_register_for_check(self):
 def configure(cfg):
     cfg.env.AP_LIBRARIES_OBJECTS_KW = dict()
     cfg.env.AP_LIB_EXTRA_SOURCES = dict()
+    cfg.env.DOUBLE_PRECISION_SOURCES = dict()

Tools/ardupilotwaf/ardupilotwaf.py

@@ -245,7 +245,6 @@ def ap_program(bld,
     if use_legacy_defines:
         kw['defines'].extend(get_legacy_defines(bld.path.name))
 
-    kw['cxxflags'] = kw.get('cxxflags', []) + ['-include', 'ap_config.h']
     kw['features'] = kw.get('features', []) + bld.env.AP_PROGRAM_FEATURES
 
     program_groups = Utils.to_list(program_groups)

libraries/AP_GPS/AP_GPS_ERB.h

@@ -35,7 +35,7 @@ class AP_GPS_ERB : public AP_GPS_Backend
 
     AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }
 
-    bool supports_mavlink_gps_rtk_message() override { return true; }
+    bool supports_mavlink_gps_rtk_message() const override { return true; }
 
     static bool _detect(struct ERB_detect_state &state, uint8_t data);
 

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -18,8 +18,6 @@
 //  Code by Michael Oborne
 //
 
-#define ALLOW_DOUBLE_MATH_FUNCTIONS
-
 #include "AP_GPS.h"
 #include "AP_GPS_SBF.h"
 #include <GCS_MAVLink/GCS.h>
@@ -62,6 +60,9 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
 
     port->write((const uint8_t*)_port_enable, strlen(_port_enable));
     _config_last_ack_time = AP_HAL::millis();
+
+    // if we ever parse RTK observations it will always be of type NED, so set it once
+    state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED;
 }
 
 AP_GPS_SBF::~AP_GPS_SBF (void) {
@@ -411,6 +412,37 @@ AP_GPS_SBF::process_message(void)
         }
         break;
     }
+    case BaseVectorGeod:
+    {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as it's false positive when testing for DNU values
+        const msg4028 &temp = sbf_msg.data.msg4028u;
+
+        // just breakout any consts we need for Do Not Use (DNU) reasons
+        constexpr double doubleDNU = -2e-10;
+
+        check_new_itow(temp.TOW, sbf_msg.length);
+
+        if (temp.N == 0) { // no sub blocks so just bail, we can't do anything useful here
+            state.rtk_num_sats = 0;
+            state.rtk_age_ms = 0;
+            state.rtk_baseline_y_mm = 0;
+            state.rtk_baseline_x_mm = 0;
+            state.rtk_baseline_z_mm = 0;
+            break;
+        }
+
+        state.rtk_num_sats = temp.info.NrSV;
+
+        state.rtk_age_ms = (temp.info.CorrAge != 65535) ? ((uint32_t)temp.info.CorrAge) * 10 : 0;
+
+        // copy the position as long as the data isn't DNU
+        state.rtk_baseline_y_mm = (temp.info.DeltaEast != doubleDNU) ?  temp.info.DeltaEast * 1e3 : 0;
+        state.rtk_baseline_x_mm = (temp.info.DeltaNorth != doubleDNU) ? temp.info.DeltaNorth * 1e3 : 0;
+        state.rtk_baseline_z_mm = (temp.info.DeltaUp != doubleDNU) ? temp.info.DeltaUp * -1e3 : 0;
+#pragma GCC diagnostic pop
+        break;
+    }
     }
 
     return false;

libraries/AP_GPS/AP_GPS_SBF.h

@@ -43,6 +43,8 @@ class AP_GPS_SBF : public AP_GPS_Backend
 
     void broadcast_configuration_failure_reason(void) const override;
 
+    bool supports_mavlink_gps_rtk_message(void) const override { return true; };
+
     // get the velocity lag, returns true if the driver is confident in the returned value
     bool get_lag(float &lag_sec) const override { lag_sec = 0.08f; return true; } ;
 
@@ -65,7 +67,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
     uint8_t _init_blob_index;
     uint32_t _init_blob_time;
     char *_initial_sso;
-    const char* _sso_normal = ", PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic, msec100\n";
+    const char* _sso_normal = ", PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod, msec100\n";
     const char* _initialisation_blob[4] = {
     "srd, Moderate, UAV\n",
     "sem, PVT, 5\n",
@@ -87,6 +89,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
         DOP = 4001,
         PVTGeodetic = 4007,
         ReceiverStatus = 4014,
+        BaseVectorGeod = 4028,
         VelCovGeodetic = 5908
     };
 
@@ -123,7 +126,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
          uint16_t VAccuracy;
          uint8_t Misc;
     };
-  
+
     struct PACKED msg4001 // DOP
     {
          uint32_t TOW;
@@ -150,6 +153,33 @@ class AP_GPS_SBF : public AP_GPS_Backend
          // remaining data is AGCData, which we don't have a use for, don't extract the data
     };
 
+    struct PACKED VectorInfoGeod {
+        uint8_t NrSV;
+        uint8_t Error;
+        uint8_t Mode;
+        uint8_t Misc;
+        double DeltaEast;
+        double DeltaNorth;
+        double DeltaUp;
+        float DeltaVe;
+        float DeltaVn;
+        float DeltaVu;
+        uint16_t Azimuth;
+        int16_t Elevation;
+        uint8_t ReferenceID;
+        uint16_t CorrAge;
+        uint32_t SignalInfo;
+    };
+
+    struct PACKED msg4028 // BaseVectorGeod
+    {
+        uint32_t TOW;
+        uint16_t WNc;
+        uint8_t N; // number of baselines
+        uint8_t SBLength;
+        VectorInfoGeod info; // there can be multiple baselines here, but we will only consume the first one, so don't worry about anything after
+    };
+
     struct PACKED msg5908 // VelCovGeodetic
     {
         uint32_t TOW;
@@ -172,6 +202,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
         msg4007 msg4007u;
         msg4001 msg4001u;
         msg4014 msg4014u;
+        msg4028 msg4028u;
         msg5908 msg5908u;
         uint8_t bytes[256];
     };

libraries/AP_GPS/AP_GPS_SBP.h

@@ -31,7 +31,7 @@ class AP_GPS_SBP : public AP_GPS_Backend
 
     AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }
 
-    bool supports_mavlink_gps_rtk_message() override { return true; }
+    bool supports_mavlink_gps_rtk_message() const override { return true; }
 
     // Methods
     bool read() override;

libraries/AP_GPS/GPS_Backend.h

@@ -45,7 +45,7 @@ class AP_GPS_Backend
     virtual void inject_data(const uint8_t *data, uint16_t len);
 
     //MAVLink methods
-    virtual bool supports_mavlink_gps_rtk_message() { return false; }
+    virtual bool supports_mavlink_gps_rtk_message() const { return false; }
     virtual void send_mavlink_gps_rtk(mavlink_channel_t chan);
 
     virtual void broadcast_configuration_failure_reason(void) const { return ; }

libraries/AP_GPS/wscript

@@ -0,0 +1,4 @@
+
+# SBF driver needs double precision
+def configure(cfg):
+    cfg.env.DOUBLE_PRECISION_SOURCES['AP_GPS'] = ['AP_GPS_SBF.cpp']

libraries/AP_HAL/AP_HAL_Macros.h

@@ -10,7 +10,9 @@
 /*
   allow double maths on Linux and SITL to avoid problems with system headers
  */
-#define ALLOW_DOUBLE_MATH_FUNCTIONS
+  #if !defined(ALLOW_DOUBLE_MATH_FUNCTIONS)
+    #define ALLOW_DOUBLE_MATH_FUNCTIONS
+  #endif
 #endif
 
 // we need to include math.h here for newer compilers (eg. g++ 7.3.1 for stm32)

wscript

@@ -320,6 +320,8 @@ def configure(cfg):
         cfg.end_msg('enabled')
         cfg.recurse('libraries/AP_Scripting')
 
+    cfg.recurse('libraries/AP_GPS')
+
     cfg.start_msg('Scripting runtime checks')
     if cfg.options.scripting_checks:
         cfg.end_msg('enabled')
@@ -354,6 +356,9 @@ def configure(cfg):
 
     cfg.write_config_header(os.path.join(cfg.variant, 'ap_config.h'))
 
+    # add in generated flags
+    cfg.env.CXXFLAGS += ['-include', 'ap_config.h']
+
     _collect_autoconfig_files(cfg)
 
 def collect_dirs_to_recurse(bld, globs, **kw):