From a4286e4f8121a066cf7f6962356b86a7ca74cae8 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Thu, 31 Oct 2024 10:38:11 -0300
Subject: [PATCH] wip mav sim gps

---
 libraries/SITL/SIM_GPS.cpp     |  5 +++
 libraries/SITL/SIM_GPS.h       |  3 ++
 libraries/SITL/SIM_GPS_MAV.cpp | 72 ++++++++++++++++++++++++++++++++++
 libraries/SITL/SIM_GPS_MAV.h   | 29 ++++++++++++++
 libraries/SITL/SIM_config.h    |  4 ++
 modules/COLCON_IGNORE          |  0
 6 files changed, 113 insertions(+)
 create mode 100644 libraries/SITL/SIM_GPS_MAV.cpp
 create mode 100644 libraries/SITL/SIM_GPS_MAV.h
 delete mode 100644 modules/COLCON_IGNORE

diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp
index f0b8067392fb77..979dbe98b82b30 100644
--- a/libraries/SITL/SIM_GPS.cpp
+++ b/libraries/SITL/SIM_GPS.cpp
@@ -286,6 +286,11 @@ void GPS::check_backend_allocation()
     case Type::FILE:
         backend = NEW_NOTHROW GPS_FILE(*this, instance);
         break;
+#endif
+#if AP_SIM_GPS_MAV_ENABLED
+    case Type::MAV:
+        backend = NEW_NOTHROW GPS_MAV(*this, instance);
+        break;
 #endif
     };
 
diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h
index 1bdb583d0e59d5..03f72515f86c56 100644
--- a/libraries/SITL/SIM_GPS.h
+++ b/libraries/SITL/SIM_GPS.h
@@ -127,6 +127,9 @@ class GPS : public SerialDevice {
 #if AP_SIM_GPS_TRIMBLE_ENABLED
         TRIMBLE  = 11, // matches GPS1_TYPE
 #endif
+#if AP_SIM_GPS_MAV_ENABLED
+        MAV  = 12,
+#endif
 #if AP_SIM_GPS_MSP_ENABLED
         MSP   = 19,
 #endif
diff --git a/libraries/SITL/SIM_GPS_MAV.cpp b/libraries/SITL/SIM_GPS_MAV.cpp
new file mode 100644
index 00000000000000..c3554f91f38b94
--- /dev/null
+++ b/libraries/SITL/SIM_GPS_MAV.cpp
@@ -0,0 +1,72 @@
+#include "SIM_config.h"
+
+#if AP_SIM_GPS_MAV_ENABLED
+
+#include "SIM_GPS_MAV.h"
+
+#include <SITL/SITL.h>
+#include <AP_Common/NMEA.h>
+#include <AP_HAL/AP_HAL.h>
+
+#include <time.h>
+#include <sys/time.h>
+
+extern const AP_HAL::HAL& hal;
+
+using namespace SITL;
+
+/*
+  send a new GPS NMEA packet
+ */
+void GPS_MAV::publish(const GPS_Data *d)
+{
+     mavlink_msg_gps_input_send(
+        chan,
+        AP_HAL::millis(),
+        GPS_INPUT_IGNORE_FLAG_VEL_HORIZ | GPS_INPUT_IGNORE_FLAG_VEL_VERT,
+        gps_time().ms,
+        gps_time().week,
+        5, // fix type
+        d->latitude * 1.0e7,
+        d->longitude * 1.0e7,
+        d->altitude,
+        __UINT16_MAX__,
+        __UINT16_MAX__,
+        0, // vn
+        0, // ve
+        0, // vd
+        0, // speed_accuracy
+        0, // horiz_accuracy
+        0, // vert_accuracy
+        16, // sattelites visible
+        d->yaw_deg*100) // yaw
+        ;
+
+    if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) {
+        nmea_printf("$GPHDT,%.2f,T", d->yaw_deg);
+    }
+    else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_THS) {
+        nmea_printf("$GPTHS,%.2f,%c,T", d->yaw_deg, d->have_lock ? 'A' : 'V');
+    } else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_KSXT) {
+        // Unicore support
+        // $KSXT,20211016083433.00,116.31296102,39.95817066,49.4911,223.57,-11.32,330.19,0.024,,1,3,28,27,,,,-0.012,0.021,0.020,,*2D
+        nmea_printf("$KSXT,%04u%02u%02u%02u%02u%02u.%02u,%.8f,%.8f,%.4f,%.2f,%.2f,%.2f,%.2f,%.3f,%u,%u,%u,%u,,,,%.3f,%.3f,%.3f,,",
+                    tm->tm_year+1900, tm->tm_mon+1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec, unsigned(tv.tv_usec*1.e-4),
+                    d->longitude, d->latitude,
+                    d->altitude,
+                    wrap_360(d->yaw_deg),
+                    d->pitch_deg,
+                    ground_track_deg,
+                    speed_mps,
+                    d->roll_deg,
+                    d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed,
+                    3, // fixed rtk yaw solution,
+                    d->have_lock?d->num_sats:3,
+                    d->have_lock?d->num_sats:3,
+                    d->speedE * 3.6,
+                    d->speedN * 3.6,
+                    -d->speedD * 3.6);
+    }
+}
+
+#endif  // AP_SIM_GPS_MAV_ENABLED
diff --git a/libraries/SITL/SIM_GPS_MAV.h b/libraries/SITL/SIM_GPS_MAV.h
new file mode 100644
index 00000000000000..ccbd0b61c77eaf
--- /dev/null
+++ b/libraries/SITL/SIM_GPS_MAV.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include "SIM_config.h"
+
+#if AP_SIM_GPS_MAV_ENABLED
+
+#include "SIM_GPS.h"
+
+namespace SITL {
+
+class GPS_MAV : public GPS_Backend {
+public:
+    CLASS_NO_COPY(GPS_MAV);
+
+    using GPS_Backend::GPS_Backend;
+
+    void publish(const GPS_Data *d) override;
+
+private:
+
+    // uint8_t nmea_checksum(const char *s);
+    // void nmea_printf(const char *fmt, ...);
+    // void update_nmea(const GPS_Data *d);
+
+};
+
+};
+
+#endif  // AP_SIM_GPS_MAV_ENABLED
diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h
index 91956ad90c2aef..edf35776a7621d 100644
--- a/libraries/SITL/SIM_config.h
+++ b/libraries/SITL/SIM_config.h
@@ -100,6 +100,10 @@
 #define AP_SIM_GPS_NMEA_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
 #endif
 
+#ifndef AP_SIM_GPS_MAV_ENABLED
+#define AP_SIM_GPS_MAV_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
+#endif
+
 #ifndef AP_SIM_GPS_NOVA_ENABLED
 #define AP_SIM_GPS_NOVA_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
 #endif
diff --git a/modules/COLCON_IGNORE b/modules/COLCON_IGNORE
deleted file mode 100644
index e69de29bb2d1d6..00000000000000