From e9963ef6b8c6d6fbce98a64a8e4e8d9ccb6f5e2e Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 22 Nov 2023 16:52:08 +0900
Subject: [PATCH] AP_GPS: SBF reports antenna pos

---
 libraries/AP_GPS/AP_GPS_SBF.cpp | 20 +++++++++++++++++++-
 libraries/AP_GPS/AP_GPS_SBF.h   | 24 ++++++++++++++++++++++++
 2 files changed, 43 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp
index 73675c0839c58..0f729c988a0f0 100644
--- a/libraries/AP_GPS/AP_GPS_SBF.cpp
+++ b/libraries/AP_GPS/AP_GPS_SBF.cpp
@@ -122,7 +122,7 @@ AP_GPS_SBF::read(void)
                                 const char *extra_config;
                                 switch (get_type()) {
                                     case AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA:
-                                        extra_config = "+AttEuler+AttCovEuler";
+                                        extra_config = "+AttEuler+AttCovEuler+AuxAntPositions";
                                         break;
                                     case AP_GPS::GPS_Type::GPS_TYPE_SBF:
                                     default:
@@ -583,6 +583,24 @@ AP_GPS_SBF::process_message(void)
         }
         break;
     }
+    case AuxAntPositions:
+    {
+        //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: got antenna positions");
+        if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) {
+            const msg5942 &temp = sbf_msg.data.msg5942u;
+            check_new_itow(temp.TOW, sbf_msg.length);
+
+            //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: %d sub blocks", (int)temp.N);
+            if (temp.N > 0) {
+                if (temp.ant1.Error == 0) {
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: E:%4.2f N:%4.2f U:%4.2f", (double)temp.ant1.DeltaEast, (double)temp.ant1.DeltaNorth, (double)temp.ant1.DeltaUp);
+                } else {
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SBF: Ant pos invalid Err:%d", (int)temp.ant1.Error);
+                }
+            }
+        }
+        break;
+    }
     case BaseVectorGeod:
     {
 #pragma GCC diagnostic push
diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h
index 1e45bdcdf1555..5fd37770f8199 100644
--- a/libraries/AP_GPS/AP_GPS_SBF.h
+++ b/libraries/AP_GPS/AP_GPS_SBF.h
@@ -115,6 +115,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
         VelCovGeodetic = 5908,
         AttEuler = 5938,
         AttEulerCov = 5939,
+        AuxAntPositions = 5942,
     };
 
     struct PACKED msg4007 // PVTGeodetic
@@ -252,6 +253,28 @@ class AP_GPS_SBF : public AP_GPS_Backend
         float Cov_PitchRoll;
     };
 
+    struct PACKED AuxAntPositionSubBlock {
+        uint8_t NrSV;           // total number of satellites tracked by the antenna
+        uint8_t Error;          // aux antenna position error code
+        uint8_t AmbiguityType;  // aux antenna positions obtained with 0: fixed ambiguities, 1: float ambiguities
+        uint8_t AuxAntID;       // aux antenna ID: 1 for the first auxiliary antenna, 2 for the second, etc.
+        double DeltaEast;       // position in East direction (relative to main antenna)
+        double DeltaNorth;      // position in North direction (relative to main antenna)
+        double DeltaUp;         // position in Up direction (relative to main antenna)
+        double EastVel;         // velocity in East direction (relative to main antenna)
+        double NorthVel;        // velocity in North direction (relative to main antenna)
+        double UpVel;           // velocity in Up direction (relative to main antenna)
+    };
+
+    struct PACKED msg5942 // AuxAntPositions
+    {
+        uint32_t TOW;
+        uint16_t WNc;
+        uint8_t N;          // number of AuxAntPosition sub-blocks in this AuxAntPositions block
+        uint8_t SBLength;   // length of one sub-block in bytes
+        AuxAntPositionSubBlock ant1;    // first aux antennas position
+    };
+
     union PACKED msgbuffer {
         msg4007 msg4007u;
         msg4001 msg4001u;
@@ -260,6 +283,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
         msg5908 msg5908u;
         msg5938 msg5938u;
         msg5939 msg5939u;
+        msg5942 msg5942u;
         uint8_t bytes[256];
     };