diff --git a/src/flightlog.js b/src/flightlog.js index 89c122c5..8b1b7107 100644 --- 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 index fd21c49d..50ce336a 100644 --- 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 new file mode 100644 index 00000000..f9feaef3 --- /dev/null +++ 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 index 87092a3e..89670574 100644 --- 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) {