From 78a7c86bbd392943abbb2d5ea37b85df7c38d214 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Wed, 26 Jun 2024 23:55:23 -0300
Subject: [PATCH] Sub: create backup origin for gps-less operation with WMM

Co-authored-by: Clyde McQueen <clyde@mcqueen.net>
---
 ArduSub/AP_Arming_Sub.cpp |  5 +++++
 ArduSub/ArduSub.cpp       | 47 +++++++++++++++++++++++++++++++++++++++
 ArduSub/Parameters.cpp    | 21 ++++++++++++++++-
 ArduSub/Parameters.h      |  3 +++
 ArduSub/Sub.h             |  2 ++
 5 files changed, 77 insertions(+), 1 deletion(-)

diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp
index c3749c3f0c..bcf5c0c8b8 100644
--- a/ArduSub/AP_Arming_Sub.cpp
+++ b/ArduSub/AP_Arming_Sub.cpp
@@ -149,6 +149,11 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
     // flag exiting this function
     in_arm_motors = false;
 
+    // if we do not have an ekf origin then we can't use the WMM tables
+    if (!sub.ensure_ekf_origin()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Compass performance degraded");
+    }
+
     // return success
     return true;
 }
diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index 8d63ccda5a..cf1c6f9abc 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -429,4 +429,51 @@ float Sub::get_alt_msl() const
     return -posD;
 }
 
+bool Sub::ensure_ekf_origin()
+{
+    Location ekf_origin;
+    if (ahrs.get_origin(ekf_origin)) {
+        // ekf origin is set
+        return true;
+    }
+
+    if (gps.num_sensors() > 0) {
+        // wait for the gps sensor to set the origin
+        // alert the pilot to poor compass performance
+        return false;
+    }
+
+    auto backup_origin = Location(static_cast<int32_t>(sub.g2.backup_origin_lat * 1e7),
+                                  static_cast<int32_t>(sub.g2.backup_origin_lon * 1e7),
+                                  static_cast<int32_t>(sub.g2.backup_origin_alt * 100),
+                                  Location::AltFrame::ABSOLUTE);
+
+    if (backup_origin.lat == 0 || backup_origin.lng == 0) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Backup location parameters are missing or zero");
+        return false;
+    }
+
+    if (!check_latlng(backup_origin.lat, backup_origin.lng)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Backup location parameters are not valid");
+        return false;
+    }
+
+    if (!ahrs.set_origin(backup_origin)) {
+        // a possible problem is that ek3_srcn_posxy is set to 3 (gps)
+        gcs().send_text(MAV_SEVERITY_WARNING, "Failed to set origin, check EK3_SRC parameters");
+        return false;
+    }
+
+    gcs().send_text(MAV_SEVERITY_INFO, "Using backup location");
+
+#if HAL_LOGGING_ENABLED
+    ahrs.Log_Write_Home_And_Origin();
+#endif
+
+    // send ekf origin to GCS
+    gcs().send_message(MSG_ORIGIN);
+
+    return true;
+}
+
 AP_HAL_MAIN_CALLBACKS(&sub);
diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp
index ee41bcd9ee..158d58e390 100644
--- a/ArduSub/Parameters.cpp
+++ b/ArduSub/Parameters.cpp
@@ -706,7 +706,26 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
 
     // 18 was scripting
 
-    // 19 was airspeed
+    // @Param: ORIGIN_LAT
+    // @DisplayName: Backup latitude for EKF origin
+    // @Description:  Backup EKF origin latitude used when not using a positioning system.
+    // @Units: degrees
+    // @User: Standard
+    AP_GROUPINFO("ORIGIN_LAT", 19, ParametersG2, backup_origin_lat, 0),
+
+    // @Param: ORIGIN_LON
+    // @DisplayName: Backup longitude for EKF origin
+    // @Description:  Backup EKF origin longitude used when not using a positioning system.
+    // @Units: degrees
+    // @User: Standard
+    AP_GROUPINFO("ORIGIN_LON", 20, ParametersG2, backup_origin_lon, 0),
+
+    // @Param: ORIGIN_ALT
+    // @DisplayName: Backup altitude (MSL) for EKF origin
+    // @Description:  Backup EKF origin altitude (MSL) used when not using a positioning system.
+    // @Units: m
+    // @User: Standard
+    AP_GROUPINFO("ORIGIN_ALT", 21, ParametersG2, backup_origin_alt, 0),
 
     AP_GROUPEND
 };
diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h
index 6e76cdfdd2..d40aa4a617 100644
--- a/ArduSub/Parameters.h
+++ b/ArduSub/Parameters.h
@@ -366,6 +366,9 @@ public:
     // control over servo output ranges
     SRV_Channels servo_channels;
 
+    AP_Float backup_origin_lat;
+    AP_Float backup_origin_lon;
+    AP_Float backup_origin_alt;
 };
 
 extern const AP_Param::Info        var_info[];
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index b375d2f671..909a28c517 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -439,6 +439,8 @@ private:
     float get_alt_rel() const WARN_IF_UNUSED;
     float get_alt_msl() const WARN_IF_UNUSED;
     void exit_mission();
+    void set_origin(const Location& loc);
+    bool ensure_ekf_origin();
     bool verify_loiter_unlimited();
     bool verify_loiter_time();
     bool verify_wait_delay();