AP_GPS: Factor out moving base offset helper

libraries/AP_GPS/AP_GPS.cpp

@@ -293,7 +293,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f),
 #endif
 
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
     // @Param: DRV_OPTIONS
     // @DisplayName: driver options
     // @Description: Additional backend specific options
@@ -322,6 +322,20 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     AP_GROUPINFO("COM_PORT2", 24, AP_GPS, _com_port[1], 1),
 #endif
 
+#if GPS_MOVING_BASELINE
+
+    // @Group: MB1_
+    // @Path: MovingBase.cpp
+    AP_SUBGROUPINFO(mb_params[0], "MB1_", 25, AP_GPS, MovingBase),
+
+#if GPS_MAX_RECEIVERS > 1
+    // @Group: MB2_
+    // @Path: MovingBase.cpp
+    AP_SUBGROUPINFO(mb_params[1], "MB2_", 26, AP_GPS, MovingBase),
+#endif // GPS_MAX_RECEIVERS > 1
+
+#endif // GPS_MOVING_BASELINE
+
     AP_GROUPEND
 };
 

libraries/AP_GPS/AP_GPS.h

@@ -43,14 +43,18 @@
 
 #define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * AP_MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS)
 
-#ifndef GPS_UBLOX_MOVING_BASELINE
-#define GPS_UBLOX_MOVING_BASELINE !HAL_MINIMIZE_FEATURES && GPS_MAX_RECEIVERS>1
+#ifndef GPS_MOVING_BASELINE
+#define GPS_MOVING_BASELINE !HAL_MINIMIZE_FEATURES && GPS_MAX_RECEIVERS>1
 #endif
 
 #ifndef HAL_MSP_GPS_ENABLED
 #define HAL_MSP_GPS_ENABLED HAL_MSP_SENSORS_ENABLED
 #endif
 
+#if GPS_MOVING_BASELINE
+#include "MovingBase.h"
+#endif // GPS_MOVING_BASELINE
+
 class AP_GPS_Backend;
 
 /// @class AP_GPS
@@ -519,6 +523,10 @@ protected:
     AP_Float _blend_tc;
     AP_Int16 _driver_options;
 
+#if GPS_MOVING_BASELINE
+    MovingBase mb_params[GPS_MAX_RECEIVERS];
+#endif // GPS_MOVING_BASELINE
+
     uint32_t _log_gps_bit = -1;
 
 private:

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -79,7 +79,7 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
     _unconfigured_messages |= CONFIG_TP5;
 #endif
 
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
     if (role == AP_GPS::GPS_ROLE_MB_BASE && !mb_use_uart2()) {
         rtcm3_parser = new RTCM3_Parser;
         if (rtcm3_parser == nullptr) {
@@ -96,14 +96,14 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
 
 AP_GPS_UBLOX::~AP_GPS_UBLOX()
 {
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
     if (rtcm3_parser) {
         delete rtcm3_parser;
     }
 #endif
 }
 
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
 /*
   config for F9 GPS in moving baseline base role
   See ZED-F9P integration manual section 3.1.5.6.1
@@ -205,7 +205,7 @@ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart2[] {
  { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
  { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
 };
-#endif // GPS_UBLOX_MOVING_BASELINE
+#endif // GPS_MOVING_BASELINE
 
 
 void
@@ -357,7 +357,7 @@ AP_GPS_UBLOX::_request_next_config(void)
         }
         break;
     case STEP_RTK_MOVBASE:
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
         if (supports_F9_config()) {
             static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart1), "done_mask too small, base1");
             static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart2), "done_mask too small, base2");
@@ -572,7 +572,7 @@ AP_GPS_UBLOX::read(void)
         // read the next byte
         data = port->read();
 
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
         if (rtcm3_parser) {
             if (rtcm3_parser->read(data)) {
                 // we've found a RTCMv3 packet. We stop parsing at
@@ -682,7 +682,7 @@ AP_GPS_UBLOX::read(void)
                 break;                                                  // bad checksum
             }
 
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
             if (rtcm3_parser) {
                 // this is a uBlox packet, discard any partial RTCMv3 state
                 rtcm3_parser->reset();
@@ -856,7 +856,7 @@ uint8_t AP_GPS_UBLOX::config_key_size(ConfigKey key) const
  */
 int8_t AP_GPS_UBLOX::find_active_config_index(ConfigKey key) const
 {
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
     if (active_config.list == nullptr) {
         return -1;
     }
@@ -1122,7 +1122,7 @@ AP_GPS_UBLOX::_parse_gps(void)
                     default:
                         break;
                 }
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
                 // see if it is in active config list
                 int8_t cfg_idx = find_active_config_index(id);
                 if (cfg_idx >= 0) {
@@ -1141,7 +1141,7 @@ AP_GPS_UBLOX::_parse_gps(void)
                         }
                     }
                 }
-#endif // GPS_UBLOX_MOVING_BASELINE
+#endif // GPS_MOVING_BASELINE
 
                 // step over the value
                 uint8_t step_size = config_key_size(id);
@@ -1326,10 +1326,10 @@ AP_GPS_UBLOX::_parse_gps(void)
         state.hdop = 130;
 #endif
         break;
+
+#if GPS_MOVING_BASELINE
     case MSG_RELPOSNED:
         {
-            const Vector3f &offset0 = gps._antenna_offset[state.instance^1].get();
-            const Vector3f &offset1 = gps._antenna_offset[state.instance].get();
             // note that we require the yaw to come from a fixed solution, not a float solution
             // yaw from a float solution would only be acceptable with a very large separation between
             // GPS modules
@@ -1342,21 +1342,6 @@ AP_GPS_UBLOX::_parse_gps(void)
                                           static_cast<uint32_t>(RELPOSNED::refObsMiss) |
                                           static_cast<uint32_t>(RELPOSNED::carrSolnFloat);
 
-            const Vector3f antenna_offset = offset0 - offset1;
-            const float offset_dist = antenna_offset.length();
-            const float rel_dist = _buffer.relposned.relPosLength * 0.01;
-            const float dist_error = offset_dist - rel_dist;
-            const float strict_length_error_allowed = 0.2; // allow for up to 20% error
-            const float min_separation = 0.05;
-            bool tilt_ok = true;
-            const float min_dist = MIN(offset_dist, rel_dist);
-#ifndef HAL_BUILD_AP_PERIPH
-            // when ahrs is available use it to constrain vertical component
-            const Vector3f antenna_tilt = AP::ahrs().get_rotation_body_to_ned() * antenna_offset;
-            const float alt_error = _buffer.relposned.relPosD*0.01 + antenna_tilt.z;
-            tilt_ok = fabsf(alt_error) < strict_length_error_allowed * min_dist;
-#endif
-
             _check_new_itow(_buffer.relposned.iTOW);
             if (_buffer.relposned.iTOW != _last_relposned_itow+200) {
                 // useful for looking at packet loss on links
@@ -1365,35 +1350,20 @@ AP_GPS_UBLOX::_parse_gps(void)
             _last_relposned_itow = _buffer.relposned.iTOW;
             _last_relposned_ms = AP_HAL::millis();
 
-            /*
-              RELPOSNED messages gives the NED distance from base to
-              rover. It comes from the rover
-             */
-            MB_Debug("RELPOSNED[%u]: od:%.2f rd:%.2f ae:%.2f flags:0x%04x t=%u",
-                     state.instance+1,
-                     offset_dist, rel_dist, alt_error,
-                     unsigned(_buffer.relposned.flags),
-                     unsigned(_buffer.relposned.iTOW));
-
             if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
                 ((_buffer.relposned.flags & invalid_mask) == 0) &&
-                rel_dist > min_separation &&
-                offset_dist > min_separation &&
-                fabsf(dist_error) < strict_length_error_allowed * min_dist &&
-                tilt_ok) {
-                float rotation_offset_rad;
-                const Vector3f diff = offset1 - offset0;
-                rotation_offset_rad = Vector2f(diff.x, diff.y).angle();
-                state.gps_yaw = wrap_360(_buffer.relposned.relPosHeading * 1e-5 - degrees(rotation_offset_rad));
-                state.have_gps_yaw = true;
+                calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5,
+                                          _buffer.relposned.relPosLength * 0.01,
+                                          _buffer.relposned.relPosD*0.01)) {
                 state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
                 state.have_gps_yaw_accuracy = true;
             } else {
-                state.have_gps_yaw = false;
                 state.have_gps_yaw_accuracy = false;
             }
         }
         break;
+#endif // GPS_MOVING_BASELINE
+
     case MSG_PVT:
         Debug("MSG_PVT");
 
@@ -1707,7 +1677,7 @@ AP_GPS_UBLOX::_configure_valget(ConfigKey key)
 bool
 AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit)
 {
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
     active_config.list = list;
     active_config.count = count;
     active_config.done_mask = 0;
@@ -1887,7 +1857,7 @@ bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
         // F9 lag not verified yet from flight log, but likely to be at least
         // as good as M8
         lag_sec = 0.12f;
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
         if (role == AP_GPS::GPS_ROLE_MB_BASE ||
             role == AP_GPS::GPS_ROLE_MB_ROVER) {
             // the moving baseline rover will lag about 40ms from the
@@ -1924,7 +1894,7 @@ void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
 // support for retrieving RTCMv3 data from a moving baseline base
 bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
 {
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
     if (rtcm3_parser) {
         len = rtcm3_parser->get_len(bytes);
         return len > 0;
@@ -1936,7 +1906,7 @@ bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
 // clear previous RTCM3 packet
 void AP_GPS_UBLOX::clear_RTCMV3(void)
 {
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
     if (rtcm3_parser) {
         rtcm3_parser->clear_packet();
     }
@@ -1952,7 +1922,7 @@ bool AP_GPS_UBLOX::is_healthy(void) const
         return true;
     }
 #endif
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
     if ((role == AP_GPS::GPS_ROLE_MB_BASE ||
         role == AP_GPS::GPS_ROLE_MB_ROVER) &&
         !supports_F9_config()) {

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -742,7 +742,7 @@ private:
         return (uint8_t)(ubx_msg + (state.instance * UBX_MSG_TYPES));
     }
 
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
     // see if we should use uart2 for moving baseline config
     bool mb_use_uart2(void) const {
         return (driver_options() & unsigned(DRV_OPTIONS::MB_USE_UART2))?true:false;
@@ -770,7 +770,7 @@ private:
     // return true if GPS is capable of F9 config
     bool supports_F9_config(void) const;
 
-#if GPS_UBLOX_MOVING_BASELINE
+#if GPS_MOVING_BASELINE
     // config for moving baseline base
     static const config_list config_MB_Base_uart1[];
     static const config_list config_MB_Base_uart2[];
@@ -789,5 +789,5 @@ private:
 
     // RTCM3 parser for when in moving baseline base mode
     RTCM3_Parser *rtcm3_parser;
-#endif // GPS_UBLOX_MOVING_BASELINE
+#endif // GPS_MOVING_BASELINE
 };

libraries/AP_GPS/GPS_Backend.cpp

@@ -18,6 +18,7 @@
 #include <AP_Logger/AP_Logger.h>
 #include <time.h>
 #include <AP_RTC/AP_RTC.h>
+#include <AP_InternalError/AP_InternalError.h>
 
 #define GPS_BACKEND_DEBUGGING 0
 
@@ -301,3 +302,80 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
         }
     }
 }
+
+#if GPS_MOVING_BASELINE
+bool AP_GPS_Backend::calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D)
+{
+    constexpr float minimum_antenna_seperation = 0.05; // meters
+    constexpr float permitted_error_length_pct = 0.2;  // percentage
+
+    bool selectedOffset = false;
+    Vector3f offset;
+    switch (MovingBase::Type(gps.mb_params[state.instance].type.get())) {
+        case MovingBase::Type::RelativeToAlternateInstance:
+            offset = gps._antenna_offset[state.instance^1].get() - gps._antenna_offset[state.instance].get();
+            selectedOffset = true;
+            break;
+        case MovingBase::Type::RelativeToCustomBase:
+            offset = gps.mb_params[state.instance].base_offset.get();
+            selectedOffset = true;
+            break;
+    }
+
+    if (!selectedOffset) {
+        // invalid type, let's throw up a flag
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        goto bad_yaw;
+    }
+
+    {
+        const float offset_dist = offset.length();
+        const float min_dist = MIN(offset_dist, reported_distance);
+
+        if (offset_dist < minimum_antenna_seperation) {
+            // offsets have to be sufficently large to get a meaningful angle off of them
+            Debug("Insufficent antenna offset (%f, %f, %f)", (double)offset.x, (double)offset.y, (double)offset.z);
+            goto bad_yaw;
+        }
+
+        if (reported_distance < minimum_antenna_seperation) {
+            // if the reported distance is less then the minimum seperation it's not sufficently robust
+            Debug("Reported baseline distance (%f) was less then the minimum antenna seperation (%f)",
+                  (double)reported_distance, (double)minimum_antenna_seperation);
+            goto bad_yaw;
+        }
+
+
+        if ((offset_dist - reported_distance) > (min_dist * permitted_error_length_pct)) {
+            // the magnitude of the vector is much further then we were expecting
+            Debug("Exceeded the permitted error margin %f > %f",
+                  (double)(offset_dist - reported_distance), (double)(min_dist * permitted_error_length_pct));
+            goto bad_yaw;
+        }
+
+#ifndef HAL_BUILD_AP_PERIPH
+        {
+            const Vector3f antenna_tilt = AP::ahrs().get_rotation_body_to_ned() * offset;
+            const float alt_error = reported_D + antenna_tilt.z;
+            if (fabsf(alt_error) > permitted_error_length_pct * min_dist) {
+                // the vertical component is out of range, reject it
+                goto bad_yaw;
+            }
+        }
+#endif // HAL_BUILD_AP_PERIPH
+
+        {
+            // at this point the offsets are looking okay, go ahead and actually calculate a useful heading
+            const float rotation_offset_rad = Vector2f(-offset.x, -offset.y).angle();
+            state.gps_yaw = wrap_360(reported_heading_deg - degrees(rotation_offset_rad));
+            state.have_gps_yaw = true;
+        }
+    }
+
+    return true;
+
+bad_yaw:
+    state.have_gps_yaw = false;
+    return false;
+}
+#endif // GPS_MOVING_BASELINE

libraries/AP_GPS/GPS_Backend.h

@@ -118,6 +118,10 @@ protected:
         return uint16_t(gps._driver_options.get());
     }
 
+#if GPS_MOVING_BASELINE
+    bool calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D);
+#endif //GPS_MOVING_BASELINE
+
 private:
     // itow from previous message
     uint32_t _last_itow;

libraries/AP_GPS/MovingBase.cpp

@@ -0,0 +1,43 @@
+#include "MovingBase.h"
+
+const AP_Param::GroupInfo MovingBase::var_info[] = {
+    // @Param: TYPE
+    // @DisplayName: Moving base type
+    // @Description: Controls the type of moving base used if using moving base.
+    // @Values: 0:Relative to alternate GPS instance,1:RelativeToCustomBase
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO_FLAGS("TYPE", 1, MovingBase, type, int8_t(Type::RelativeToAlternateInstance), AP_PARAM_FLAG_ENABLE),
+
+    // @Param: OFS_X
+    // @DisplayName: Base antenna X position offset
+    // @Description: X position of the base GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
+    // @Units: m
+    // @Range: -5 5
+    // @Increment: 0.01
+    // @User: Advanced
+    
+    // @Param: OFS_Y
+    // @DisplayName: Base antenna Y position offset    
+    // @Description: Y position of the base GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
+    // @Units: m
+    // @Range: -5 5
+    // @Increment: 0.01
+    // @User: Advanced
+    
+    // @Param: OFS_Z
+    // @DisplayName: Base antenna Z position offset
+    // @Description: Z position of the base GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
+    // @Units: m
+    // @Range: -5 5
+    // @Increment: 0.01
+    // @User: Advanced
+    AP_GROUPINFO("OFS", 2, MovingBase, base_offset, 0.0f),
+
+    AP_GROUPEND
+
+};
+
+MovingBase::MovingBase(void) {
+    AP_Param::setup_object_defaults(this, var_info);
+}

libraries/AP_GPS/MovingBase.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <AP_Param/AP_Param.h>
+#include <AP_Math/AP_Math.h>
+
+class MovingBase {
+public:
+    static const struct AP_Param::GroupInfo var_info[];
+
+    MovingBase(void);
+
+    /* Do not allow copies */
+    MovingBase(const MovingBase &other) = delete;
+    MovingBase &operator=(const MovingBase&) = delete;
+
+    enum class Type : int8_t {
+        RelativeToAlternateInstance = 0,
+        RelativeToCustomBase        = 1,
+    };
+
+    AP_Int8 type;            // an option from MovingBaseType
+    AP_Vector3f base_offset; // base position offset from the selected GPS reciever
+
+};