Sub: create backup origin for gps-less operation with WMM

Co-authored-by: Clyde McQueen <clyde@mcqueen.net>

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;
 }

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);

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
 };

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[];

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();