MAGFit: option to use earth field along the flight track

MAGFit/index.html

@@ -77,16 +77,23 @@ <h1 style="text-align:center"><a href="" style="color: #000000; text-decoration:
         </td>
         <td style="width: 10px;"></td>
         <td>
-            <fieldset style="width:200px;height:80px">
+            <fieldset style="width:200px;min-height:80px">
                 <legend>Location
                     <img id="TT" src="../images/question-circle.svg" style="width: 1em; vertical-align: bottom" 
-                    data-tippy-content='Location used to get the expected earth field from the World Magnetic Model. Filled in from the EKF origin or last position in the log, or from the AHRS_ORIGIN_LAT and AHRS_ORIGIN_LON parameters. If the log has no location, for example when flying without GPS, enter it here. The Calculate button must be clicked after any change of location.'
+                    data-tippy-content='Location, altitude above sea level and date used to get the expected earth field from the World Magnetic Model (WMM2025). Location and altitude are filled in from the EKF origin or last position in the log, or from the AHRS_ORIGIN parameters. The date is taken from GPS time in the log, or is the current date if there is none. If the log has no location, for example when flying without GPS, enter it here. If the log has a position track, "Use position along flight" gets the earth field at the logged position throughout the flight rather than only at this location, this gives better results for long range flights. The Calculate button must be clicked after any change.'
                     data-tippy-maxWidth='750px'/>
                 </legend>
                 <label for="Lat">Latitude</label>
                 <input id="Lat" name="Lat" type="number" min="-90" max="90" step="any" onchange="set_need_calc(true)" style="width:90px"/><br><br>
                 <label for="Lng">Longitude</label>
-                <input id="Lng" name="Lng" type="number" min="-180" max="180" step="any" onchange="set_need_calc(true)" style="width:90px"/>
+                <input id="Lng" name="Lng" type="number" min="-180" max="180" step="any" onchange="set_need_calc(true)" style="width:90px"/><br><br>
+                <label for="Alt">Altitude (m)</label>
+                <input id="Alt" name="Alt" type="number" step="any" value="0" onchange="set_need_calc(true)" style="width:70px"/><br><br>
+                <label for="Date">Date</label>
+                <input id="Date" name="Date" type="date" onchange="set_need_calc(true)"/>
+                <br><br>
+                <input id="UseTrack" name="UseTrack" type="checkbox" onchange="set_need_calc(true)" disabled/>
+                <label for="UseTrack">Use position along flight</label>
             </fieldset>
         </td>
         <td style="width: 10px;"></td>

MAGFit/magfit.js

@@ -788,23 +788,44 @@ function find_end_index(time) {
 function update_earth_field() {
     const Lat = parseFloat(document.getElementById("Lat").value)
     const Lng = parseFloat(document.getElementById("Lng").value)
-    if (isNaN(Lat) || isNaN(Lng)) {
+    const Alt = parseFloat(document.getElementById("Alt").value)
+    const date = new Date(document.getElementById("Date").value)
+    if (isNaN(Lat) || isNaN(Lng) || isNaN(date.getTime())) {
         return false
     }
-    if ((earth_field != null) && (earth_field.Lat === Lat) && (earth_field.Lng === Lng)) {
+    const year = decimal_year(date)
+    const use_track = document.getElementById("UseTrack").checked && (position_track != null)
+    if ((earth_field != null) && (earth_field.Lat === Lat) && (earth_field.Lng === Lng) && (earth_field.Alt === Alt) && (earth_field.year === year) && (earth_field.use_track === use_track)) {
         // No change
         return true
     }
 
-    const field = expected_earth_field_lat_lon(Lat, Lng)
+    const field = expected_earth_field(Lat, Lng, Alt, year)
     if (field == null) {
         return false
     }
-    console.log("EF: " + field.vector[0] + ", " + field.vector[1] + ", " + field.vector[2] + " at Lat: " + Lat + " Lng: " + Lng)
+    console.log("EF: " + field.vector[0] + ", " + field.vector[1] + ", " + field.vector[2] + " at Lat: " + Lat + " Lng: " + Lng + " Alt: " + Alt + " Year: " + year)
 
     earth_field = field
     earth_field.Lat = Lat
     earth_field.Lng = Lng
+    earth_field.Alt = Alt
+    earth_field.year = year
+    earth_field.use_track = use_track
+
+    // Earth field along the flight track
+    earth_field.track = null
+    if (use_track) {
+        const len = position_track.time.length
+        let track = { time: position_track.time, x: new Array(len), y: new Array(len), z: new Array(len) }
+        for (let i = 0; i < len; i++) {
+            const vector = expected_earth_field(position_track.lat[i], position_track.lng[i], position_track.alt[i], year).vector
+            track.x[i] = vector[0]
+            track.y[i] = vector[1]
+            track.z[i] = vector[2]
+        }
+        earth_field.track = track
+    }
 
     // Expected body frame field must be re-calculated
     source = null
@@ -815,7 +836,7 @@ function update_earth_field() {
 function calculate() {
 
     if (!update_earth_field()) {
-        alert("Enter a valid latitude and longitude in the Location box")
+        alert("Enter a valid latitude, longitude and date in the Location box")
         return
     }
 
@@ -1145,13 +1166,37 @@ function check_orientation() {
     console.log(`Orientation check took: ${end - start} ms`);
 }
 
-function get_body_frame_ef(quaternion) {
+// Earth frame field at the given time, interpolated along the flight track if available
+function get_earth_field_vector(time, track_index) {
+    const track = earth_field.track
+    if (track == null) {
+        return earth_field.vector
+    }
+    const last = track.time.length - 1
+    if (time <= track.time[0]) {
+        return [track.x[0], track.y[0], track.z[0]]
+    }
+    if (time >= track.time[last]) {
+        return [track.x[last], track.y[last], track.z[last]]
+    }
+    while ((track_index.i < last - 1) && (track.time[track_index.i + 1] < time)) {
+        track_index.i++
+    }
+    const i = track_index.i
+    const ratio = (time - track.time[i]) / (track.time[i + 1] - track.time[i])
+    return [track.x[i] + (track.x[i + 1] - track.x[i]) * ratio,
+            track.y[i] + (track.y[i + 1] - track.y[i]) * ratio,
+            track.z[i] + (track.z[i + 1] - track.z[i]) * ratio]
+}
+
+function get_body_frame_ef(quaternion, time) {
 
     const len = quaternion.q1.length
 
     let ret = { x: new Array(len), y: new Array(len), z: new Array(len) }
 
     let q = new Quaternion()
+    let track_index = { i: 0 }
     for (let i = 0; i < len; i++) {
 
         // Invert and load into helper
@@ -1160,7 +1205,7 @@ function get_body_frame_ef(quaternion) {
         q.q3 = -quaternion.q3[i]
         q.q4 = -quaternion.q4[i]
 
-        const tmp = q.rotate(earth_field.vector)
+        const tmp = q.rotate(get_earth_field_vector(time[i], track_index))
 
         ret.x[i] = tmp[0]
         ret.y[i] = tmp[1]
@@ -1190,7 +1235,7 @@ function select_body_frame_attitude() {
     }
 
     // Calculate expected for this source
-    Object.assign(source, get_body_frame_ef(source.quaternion))
+    Object.assign(source, get_body_frame_ef(source.quaternion, source.quaternion.time))
 
     // Interpolate expected to logged compass and calculate error
     for (let i = 0; i < 3; i++) {
@@ -1270,7 +1315,7 @@ function select_body_frame_attitude() {
         }
 
         // Rotate earth field into body frame
-        MAG_Data[i].expected = get_body_frame_ef(MAG_Data[i].quaternion)
+        MAG_Data[i].expected = get_body_frame_ef(MAG_Data[i].quaternion, MAG_Data[i].time)
 
         // Error between existing calibration and expected
         MAG_Data[i].orig.error = calc_error(MAG_Data[i].expected, MAG_Data[i].orig)
@@ -1685,28 +1730,48 @@ function calc_error(A, B) {
     return ret
 }
 
+// Return latitude, longitude (deg) and altitude (m)
 function extractLatLon(log, get_param) {
-  var Lat, Lng
+  var Lat, Lng, Alt
   if (("ORGN" in log.messageTypes) && ("instances" in log.messageTypes.ORGN) && (0 in log.messageTypes.ORGN.instances)) {
     const ORGN = log.get_instance("ORGN", 0)
     Lat = ORGN.Lat[ORGN.Lat.length-1] * 10**-7
     Lng = ORGN.Lng[ORGN.Lng.length-1] * 10**-7
-    return [Lat, Lng]
+    Alt = ORGN.Alt[ORGN.Alt.length-1]
+    return [Lat, Lng, Alt]
   }
   console.warn("no ORGN message found")
   if ("POS" in log.messageTypes) {
     const POS = log.get("POS")
     Lat = POS.Lat[POS.Lat.length-1] * 10**-7
     Lng = POS.Lng[POS.Lng.length-1] * 10**-7
-    return [Lat, Lng]
+    Alt = POS.Alt[POS.Alt.length-1]
+    return [Lat, Lng, Alt]
   }
   // Recorded origin params, zero if not set
   const origin_lat = get_param("AHRS_ORIGIN_LAT")
   const origin_lng = get_param("AHRS_ORIGIN_LON")
   if ((origin_lat != null) && (origin_lng != null) && ((origin_lat != 0) || (origin_lng != 0))) {
-    return [origin_lat, origin_lng]
+    return [origin_lat, origin_lng, get_param("AHRS_ORIGIN_ALT")]
+  }
+  return [Lat, Lng, Alt]
+}
+
+// Get date of log from GPS time, returns undefined if there is no GPS time
+function get_log_date(log) {
+  if (!("GPS" in log.messageTypes) || !("instances" in log.messageTypes.GPS)) {
+    return
+  }
+  for (const inst of Object.keys(log.messageTypes.GPS.instances)) {
+    const GWk = log.get_instance("GPS", inst, "GWk")
+    const GMS = log.get_instance("GPS", inst, "GMS")
+    for (let i = 0; i < GWk.length; i++) {
+      if (GWk[i] > 0) {
+        // GPS epoch is 6 January 1980, ignore leap seconds as only the date is needed
+        return new Date(Date.UTC(1980, 0, 6) + (GWk[i] * 604800 * 1000) + GMS[i])
+      }
+    }
   }
-  return [Lat, Lng]
 }
 
 // Enable/disable calculate and save params button
@@ -1748,6 +1813,7 @@ var MAG_Data
 var fits
 var body_frame_earth_field
 var earth_field
+var position_track
 async function load(log_file) {
 
     // Make sure imports are fully loaded before starting
@@ -1988,10 +2054,45 @@ async function load(log_file) {
     // Use origin msg
     // Use last EKF origin for earth field
     // If there is no location in the log it can be entered manually
-    var [Lat, Lng] = extractLatLon(log, get_param)
+    var [Lat, Lng, Alt] = extractLatLon(log, get_param)
     earth_field = null
     document.getElementById("Lat").value = (Lat != null) ? Lat : ""
     document.getElementById("Lng").value = (Lng != null) ? Lng : ""
+    document.getElementById("Alt").value = (Alt != null) ? Math.round(Alt) : 0
+
+    // Position along the flight, used to get the earth field throughout the flight rather than at a single location
+    position_track = null
+    if ("POS" in log.messageTypes) {
+        const POS_time = TimeUS_to_seconds(log.get("POS", "TimeUS"))
+        const POS_lat = log.get("POS", "Lat")
+        const POS_lng = log.get("POS", "Lng")
+        const POS_alt = log.get("POS", "Alt")
+        position_track = { time: [], lat: [], lng: [], alt: [] }
+        for (let i = 0; i < POS_time.length; i++) {
+            if ((POS_lat[i] == 0) && (POS_lng[i] == 0)) {
+                // No position yet
+                continue
+            }
+            // One point per second is plenty, the field changes slowly with position
+            const last = position_track.time.length - 1
+            if ((last >= 0) && (POS_time[i] - position_track.time[last] < 1.0)) {
+                continue
+            }
+            position_track.time.push(POS_time[i])
+            position_track.lat.push(POS_lat[i] * 10**-7)
+            position_track.lng.push(POS_lng[i] * 10**-7)
+            position_track.alt.push(POS_alt[i])
+        }
+        if (position_track.time.length < 2) {
+            position_track = null
+        }
+    }
+    document.getElementById("UseTrack").checked = position_track != null
+    document.getElementById("UseTrack").disabled = position_track == null
+
+    // Date for magnetic model, use the current date if the log does not have GPS time
+    const log_date = get_log_date(log) || new Date()
+    document.getElementById("Date").value = log_date.toISOString().slice(0, 10)
 
     // Workout which attitude source to use, Note that this is not clever enough to deal with primary changing in flight
     const EKF_TYPE = get_param("AHRS_EKF_TYPE")