Added cartesian gps coords relative home point as additional computed…

… fields (#773) * added gps coords transform module gps_transform.js * Added computed fields for coords in cartesian system * added coords values at curves legend * The home distance id added as computed field * added friendly names for gps cartesian coords * added using of gps home points altitude if it exists in log file * gps coord names are changed * Added example graphs for GPS Cartesian coords data * code issues are resolved * code issues are resolved * code issues are resolved * code style improvement * Field name is improved Co-authored-by: Mark Haslinghuis <[email protected]> * Code style improvement Co-authored-by: Mark Haslinghuis <[email protected]> * Code style improvement Co-authored-by: Mark Haslinghuis <[email protected]> * Code style improvement Co-authored-by: Mark Haslinghuis <[email protected]> * Code style improvement Co-authored-by: Mark Haslinghuis <[email protected]> * Code style improvement Co-authored-by: Mark Haslinghuis <[email protected]> * Update gps_transform.js * Code style improvement * Code style improvement * Code style improvement --------- Co-authored-by: Mark Haslinghuis <[email protected]>
betaflight · Sep 28, 2024 · 62978cc · 62978cc
1 parent f3b680b
commit 62978cc
Show file tree

Hide file tree

Showing 4 changed files with 125 additions and 7 deletions.
diff --git a/src/flightlog.js b/src/flightlog.js
@@ -1,5 +1,6 @@
 import { FlightLogIndex } from "./flightlog_index";
 import { FlightLogParser } from "./flightlog_parser";
+import { GPS_transform } from "./gps_transform";
 import {
   MAX_MOTOR_NUMBER,
   DSHOT_MIN_VALUE,
@@ -29,7 +30,7 @@ import {
  * Window based smoothing of fields is offered.
  */
 export function FlightLog(logData) {
-  let ADDITIONAL_COMPUTED_FIELD_COUNT = 15 /** attitude + PID_SUM + PID_ERROR + RCCOMMAND_SCALED **/,
+  let ADDITIONAL_COMPUTED_FIELD_COUNT = 19 /** attitude + PID_SUM + PID_ERROR + RCCOMMAND_SCALED + GPS coord**/,
     that = this,
     logIndex = 0,
     logIndexes = new FlightLogIndex(logData),
@@ -44,7 +45,8 @@ export function FlightLog(logData) {
     // Map from field indexes to smoothing window size in microseconds
     fieldSmoothing = {},
     maxSmoothing = 0,
-    smoothedCache = new FIFOCache(2);
+    smoothedCache = new FIFOCache(2),
+    gpsTransform = null;
 
   //Public fields:
   this.parser = parser;
@@ -130,7 +132,7 @@ export function FlightLog(logData) {
     index = index ?? logIndex;
     return logIndexes.getIntraframeDirectory(index).maxTime;
   };
-  
+
   this.getActualLoggedTime = function (index) {
     index = index ?? logIndex;
     const directory = logIndexes.getIntraframeDirectory(index);
@@ -281,6 +283,9 @@ export function FlightLog(logData) {
     if (!that.isFieldDisabled().GYRO && !that.isFieldDisabled().SETPOINT) {
       fieldNames.push("axisError[0]", "axisError[1]", "axisError[2]"); // Custom calculated error field
     }
+    if (!that.isFieldDisabled().GPS) {
+      fieldNames.push("gpsCartesianCoords[0]", "gpsCartesianCoords[1]", "gpsCartesianCoords[2]", "gpsDistance"); // GPS coords in cartesian system
+    }
 
     fieldNameToIndex = {};
     for (let i = 0; i < fieldNames.length; i++) {
@@ -518,10 +523,11 @@ export function FlightLog(logData) {
                   lastSlow[i] = frame[i];
                 }
                 break;
-              case "H":
-              // TODO
-              // contains coordinates only
-              // should be handled separately
+              case "H": {
+                const homeAltitude = frame.length > 2 ? frame[2] / 10 : 0; // will work after BF firmware improvement
+                gpsTransform = new GPS_transform(frame[0] / 10000000, frame[1] / 10000000, homeAltitude, 0.0);
+                break;
+              }
               case "G":
                 // The frameValid can be false, when no GPS home (the G frames contains GPS position as diff of GPS Home position).
                 // But other data from the G frame can be valid (time, num sats)
@@ -631,6 +637,11 @@ export function FlightLog(logData) {
       fieldNameToIndex["setpoint[2]"],
       fieldNameToIndex["setpoint[3]"],
     ];
+    let gpsCoord = [
+      fieldNameToIndex["GPS_coord[0]"],
+      fieldNameToIndex["GPS_coord[1]"],
+      fieldNameToIndex["GPS_altitude"],
+    ];
 
     const flightModeFlagsIndex = fieldNameToIndex["flightModeFlags"]; // This points to the flightmode data
 
@@ -690,6 +701,11 @@ export function FlightLog(logData) {
       axisPID = false;
     }
 
+    if (!gpsCoord[0]) {
+      gpsCoord = false;
+    }
+
+
     sourceChunkIndex = 0;
     destChunkIndex = 0;
 
@@ -830,6 +846,22 @@ export function FlightLog(logData) {
             }
           }
 
+          // Calculate cartesian coords by GPS
+          if (!that.isFieldDisabled().GPS) {
+            if (gpsTransform && gpsCoord && srcFrame[gpsCoord[0]]) {
+              const gpsCartesianCoords = gpsTransform.WGS_BS(srcFrame[gpsCoord[0]] / 10000000, srcFrame[gpsCoord[1]] / 10000000, srcFrame[gpsCoord[2]] / 10);
+              destFrame[fieldIndex++] = gpsCartesianCoords.x;
+              destFrame[fieldIndex++] = gpsCartesianCoords.y;
+              destFrame[fieldIndex++] = gpsCartesianCoords.z;
+              destFrame[fieldIndex++] = Math.sqrt(gpsCartesianCoords.x * gpsCartesianCoords.x + gpsCartesianCoords.z * gpsCartesianCoords.z);
+            } else {
+              destFrame[fieldIndex++] = 0;
+              destFrame[fieldIndex++] = 0;
+              destFrame[fieldIndex++] = 0;
+              destFrame[fieldIndex++] = 0;
+            }
+          }
+
           // Remove empty fields at the end
           destFrame.splice(fieldIndex);
         }

diff --git a/src/flightlog_fields_presenter.js b/src/flightlog_fields_presenter.js
@@ -124,6 +124,12 @@ const FRIENDLY_FIELD_NAMES = {
   GPS_altitude: "GPS Altitude ASL",
   GPS_speed: "GPS Speed",
   GPS_ground_course: "GPS Heading",
+
+  "gpsCartesianCoords[all]": "GPS Coords",
+  "gpsCartesianCoords[0]": "GPS Coords [X]",
+  "gpsCartesianCoords[1]": "GPS Coords [Y]",
+  "gpsCartesianCoords[2]": "GPS Coords [Z]",
+  gpsDistance:     "GPS Home distance",
 };
 
 const DEBUG_FRIENDLY_FIELD_NAMES_INITIAL = {
@@ -1639,6 +1645,12 @@ FlightLogFieldPresenter.decodeFieldToFriendly = function (
       }
     case "GPS_ground_course":
       return `${(value / 10).toFixed(1)} °`;
+
+    case "gpsCartesianCoords[0]":
+    case "gpsCartesianCoords[1]":
+    case "gpsCartesianCoords[2]":
+    case "gpsDistance":
+        return `${value.toFixed(0)} m`;
 
     case "debug[0]":
     case "debug[1]":

diff --git a/src/gps_transform.js b/src/gps_transform.js
@@ -0,0 +1,67 @@
+export function GPS_transform(Lat0, Lon0, H0, Heading) {
+
+  function deg2rad(deg) {
+    return deg * Math.PI / 180.0;
+  }
+
+  Lat0 = deg2rad(Lat0);
+  Lon0 = deg2rad(Lon0);
+  const Semimajor = 6378137.0,
+        Flat = 1.0 / 298.257223563,
+        Ecc_2 = Flat * (2 - Flat),
+        SinB = Math.sin(Lat0),
+        CosB = Math.cos(Lat0),
+        SinL = Math.sin(Lon0),
+        CosL = Math.cos(Lon0),
+        N = Semimajor / Math.sqrt(1.0 - Ecc_2 * SinB * SinB),
+
+        a11 = -SinB * CosL,
+        a12 = -SinB * SinL,
+        a13 = CosB,
+        a21 = -SinL,
+        a22 = CosL,
+        a23 = 0,
+        a31 = CosL * CosB,
+        a32 = CosB * SinL,
+        a33 = SinB,
+
+        X0 = (N + H0) * CosB * CosL,
+        Y0 = (N + H0) * CosB * SinL,
+        Z0 = (N + H0 - Ecc_2 * N) * SinB,
+        c11 = Math.cos( deg2rad(Heading) ),
+        c12 = Math.sin( deg2rad(Heading) ),
+        c21 = -c12,
+        c22 = c11;
+
+  this.WGS_ECEF = function (Lat, Lon, H) {
+    Lat = deg2rad(Lat);
+    Lon = deg2rad(Lon);
+    const 
+      SinB = Math.sin(Lat),
+      CosB = Math.cos(Lat),
+      SinL = Math.sin(Lon),
+      CosL = Math.cos(Lon),
+      N = Semimajor / Math.sqrt(1 - Ecc_2 * SinB * SinB);
+
+    return {
+      x: (N + H) * CosB * CosL,
+      y: (N + H) * CosB * SinL,
+      z: (N + H - Ecc_2 * N) * SinB,
+    };
+  };
+
+  this.ECEF_BS = function (pos) {
+    const PosX1= a11 * (pos.x - X0) + a12 * (pos.y - Y0) + a13 * (pos.z - Z0);
+    const PosZ1= a21 * (pos.x - X0) + a22 * (pos.y - Y0) + a23 * (pos.z - Z0);
+
+    return {
+      x: c11 * PosX1 + c12 * PosZ1,
+      y: a31 * (pos.x - X0) + a32 * (pos.y - Y0) + a33 * (pos.z - Z0),
+      z: c21 * PosX1 + c22 * PosZ1,
+    };
+  };
+
+  this.WGS_BS = function (Lat, Lon, H) {
+    return this.ECEF_BS(this.WGS_ECEF(Lat, Lon, H));
+  };
+}
diff --git a/src/graph_config.js b/src/graph_config.js
@@ -1537,6 +1537,13 @@ GraphConfig.getExampleGraphConfigs = function (flightLog, graphNames) {
         "GPS_coord[all]",
       ],
     });
+    EXAMPLE_GRAPHS.push({
+      label: "GPS Cartesian coords",
+      fields: [
+        "gpsCartesianCoords[all]",
+        "gpsDistance",
+      ],
+    });
   }
 
   for (const srcGraph of EXAMPLE_GRAPHS) {