From 846380388b5b4f7e844568d80072d30414e20f84 Mon Sep 17 00:00:00 2001 From: demvlad Date: Wed, 13 Nov 2024 13:42:44 +0300 Subject: [PATCH 1/7] added computing of heading[] fields from new flight attitude quatrernion fields instead of internal IMU --- src/flightlog.js | 82 +++++++++++++++++++++++++++--------------------- 1 file changed, 47 insertions(+), 35 deletions(-) diff --git a/src/flightlog.js b/src/flightlog.js index a70ad230..cc8a6736 100644 --- a/src/flightlog.js +++ b/src/flightlog.js @@ -610,20 +610,13 @@ export function FlightLog(logData) { * sourceChunks and destChunks can be the same array. */ function injectComputedFields(sourceChunks, destChunks) { - let gyroADC = [ - fieldNameToIndex["gyroADC[0]"], - fieldNameToIndex["gyroADC[1]"], - fieldNameToIndex["gyroADC[2]"], - ]; - let accSmooth = [ - fieldNameToIndex["accSmooth[0]"], - fieldNameToIndex["accSmooth[1]"], - fieldNameToIndex["accSmooth[2]"], - ]; - let magADC = [ - fieldNameToIndex["magADC[0]"], - fieldNameToIndex["magADC[1]"], - fieldNameToIndex["magADC[2]"], + let gyroADC = [fieldNameToIndex["gyroADC[0]"], fieldNameToIndex["gyroADC[1]"], fieldNameToIndex["gyroADC[2]"]]; + let accSmooth = [fieldNameToIndex["accSmooth[0]"], fieldNameToIndex["accSmooth[1]"], fieldNameToIndex["accSmooth[2]"]]; + let magADC = [fieldNameToIndex["magADC[0]"], fieldNameToIndex["magADC[1]"], fieldNameToIndex["magADC[2]"]]; + let imuQuaternion = [ + fieldNameToIndex["imuQuaternion[0]"], + fieldNameToIndex["imuQuaternion[1]"], + fieldNameToIndex["imuQuaternion[2]"], ]; let rcCommand = [ fieldNameToIndex["rcCommand[0]"], @@ -676,7 +669,7 @@ export function FlightLog(logData) { return; } - // Do we have mag fields? If not mark that data as absent +// Do we have mag fields? If not mark that data as absent if (!magADC[0]) { magADC = false; } @@ -689,6 +682,10 @@ export function FlightLog(logData) { accSmooth = false; } + if (!imuQuaternion[0]) { + imuQuaternion = false; + } + if (!rcCommand[0]) { rcCommand = false; } @@ -726,33 +723,48 @@ export function FlightLog(logData) { if (!destChunk.hasAdditionalFields) { destChunk.hasAdditionalFields = true; - - let chunkIMU = new IMU(sourceChunks[sourceChunkIndex].initialIMU); + const chunkIMU = new IMU(sourceChunk.initialIMU); for (let i = 0; i < sourceChunk.frames.length; i++) { let srcFrame = sourceChunk.frames[i], destFrame = destChunk.frames[i], fieldIndex = destFrame.length - ADDITIONAL_COMPUTED_FIELD_COUNT; - if (!that.isFieldDisabled().GYRO) { - //don't calculate attitude if no gyro data - attitude = chunkIMU.updateEstimatedAttitude( - [ - srcFrame[gyroADC[0]], - srcFrame[gyroADC[1]], - srcFrame[gyroADC[2]], - ], - [ - srcFrame[accSmooth[0]], - srcFrame[accSmooth[1]], - srcFrame[accSmooth[2]], - ], - srcFrame[FlightLogParser.prototype.FLIGHT_LOG_FIELD_INDEX_TIME], - sysConfig.acc_1G, - sysConfig.gyroScale, - magADC - ); + if (imuQuaternion) { + const scaleFromFixedInt16 = 0x7FFF; // 0x7FFF = 2^15 - 1 + const q = { + x: srcFrame[imuQuaternion[0]] / scaleFromFixedInt16, + y: srcFrame[imuQuaternion[1]] / scaleFromFixedInt16, + z: srcFrame[imuQuaternion[2]] / scaleFromFixedInt16, + w: 1.0, + }; + const xx = q.x * q.x, + xy = q.x * q.y, + xz = q.x * q.z, + wx = q.w * q.x, + yy = q.y * q.y, + yz = q.y * q.z, + wy = q.w * q.y, + zz = q.z * q.z, + wz = q.w * q.z; + let roll = Math.atan2((+2.0 * (wx + yz)), (+1.0 - 2.0 * (xx + yy))); + let pitch = ((0.5 * Math.PI) - Math.acos(+2.0 * (wy - xz))); + let heading = -Math.atan2((+2.0 * (wz + xy)), (+1.0 - 2.0 * (yy + zz))); + if (heading < 0) { + heading += 2.0 * Math.PI; + } + destFrame[fieldIndex++] = roll; + destFrame[fieldIndex++] = pitch; + destFrame[fieldIndex++] = heading; + } else { + const attitude = chunkIMU.updateEstimatedAttitude( + [srcFrame[gyroADC[0]], srcFrame[gyroADC[1]], srcFrame[gyroADC[2]]], + [srcFrame[accSmooth[0]], srcFrame[accSmooth[1]], srcFrame[accSmooth[2]]], + srcFrame[FlightLogParser.prototype.FLIGHT_LOG_FIELD_INDEX_TIME], + sysConfig.acc_1G, + sysConfig.gyroScale, + magADC); destFrame[fieldIndex++] = attitude.roll; destFrame[fieldIndex++] = attitude.pitch; destFrame[fieldIndex++] = attitude.heading; From b7c83622d3c7ab25f7d691a388e53938c9530e52 Mon Sep 17 00:00:00 2001 From: demvlad Date: Wed, 13 Nov 2024 17:35:34 +0300 Subject: [PATCH 2/7] removed unused variable --- src/flightlog.js | 1 - 1 file changed, 1 deletion(-) diff --git a/src/flightlog.js b/src/flightlog.js index cc8a6736..0b477a4d 100644 --- a/src/flightlog.js +++ b/src/flightlog.js @@ -661,7 +661,6 @@ export function FlightLog(logData) { let sourceChunkIndex; let destChunkIndex; - let attitude; const sysConfig = that.getSysConfig(); From f742272e4374c62bbe83d6b2cf7da1ba24cbab42 Mon Sep 17 00:00:00 2001 From: demvlad Date: Wed, 13 Nov 2024 18:21:25 +0300 Subject: [PATCH 3/7] resolved quaternion issue --- src/flightlog.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/flightlog.js b/src/flightlog.js index 0b477a4d..910d4753 100644 --- a/src/flightlog.js +++ b/src/flightlog.js @@ -735,7 +735,7 @@ export function FlightLog(logData) { x: srcFrame[imuQuaternion[0]] / scaleFromFixedInt16, y: srcFrame[imuQuaternion[1]] / scaleFromFixedInt16, z: srcFrame[imuQuaternion[2]] / scaleFromFixedInt16, - w: 1.0, + w: 0.0, }; const xx = q.x * q.x, xy = q.x * q.y, From 84820d7491b7877dc07b698ae735d0eb4c6458f9 Mon Sep 17 00:00:00 2001 From: demvlad Date: Wed, 13 Nov 2024 20:05:43 +0300 Subject: [PATCH 4/7] improve quaternion operation --- src/flightlog.js | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/flightlog.js b/src/flightlog.js index 910d4753..f6efbd17 100644 --- a/src/flightlog.js +++ b/src/flightlog.js @@ -735,8 +735,9 @@ export function FlightLog(logData) { x: srcFrame[imuQuaternion[0]] / scaleFromFixedInt16, y: srcFrame[imuQuaternion[1]] / scaleFromFixedInt16, z: srcFrame[imuQuaternion[2]] / scaleFromFixedInt16, - w: 0.0, + w: 1.0, }; + q.w = Math.sqrt(1.0 - (q.x * q.x + q.y * q.y + q.z * q.z)); const xx = q.x * q.x, xy = q.x * q.y, xz = q.x * q.z, From 602963777a455c6601815c4168eeab8dccb4ef7a Mon Sep 17 00:00:00 2001 From: demvlad Date: Wed, 13 Nov 2024 20:12:27 +0300 Subject: [PATCH 5/7] code style improvement: tab to space replacement --- src/flightlog.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/flightlog.js b/src/flightlog.js index f6efbd17..5caa4e6d 100644 --- a/src/flightlog.js +++ b/src/flightlog.js @@ -737,7 +737,7 @@ export function FlightLog(logData) { z: srcFrame[imuQuaternion[2]] / scaleFromFixedInt16, w: 1.0, }; - q.w = Math.sqrt(1.0 - (q.x * q.x + q.y * q.y + q.z * q.z)); + q.w = Math.sqrt(1.0 - (q.x * q.x + q.y * q.y + q.z * q.z)); const xx = q.x * q.x, xy = q.x * q.y, xz = q.x * q.z, From 7e9d2e833ca646a8f9cb9dec7c05283c90dab3b8 Mon Sep 17 00:00:00 2001 From: Vladimir Demidov Date: Thu, 14 Nov 2024 00:09:36 +0300 Subject: [PATCH 6/7] Code style improvement Co-authored-by: Mark Haslinghuis --- src/flightlog.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/flightlog.js b/src/flightlog.js index 5caa4e6d..2953434f 100644 --- a/src/flightlog.js +++ b/src/flightlog.js @@ -737,7 +737,7 @@ export function FlightLog(logData) { z: srcFrame[imuQuaternion[2]] / scaleFromFixedInt16, w: 1.0, }; - q.w = Math.sqrt(1.0 - (q.x * q.x + q.y * q.y + q.z * q.z)); + q.w = Math.sqrt(1.0 - (q.x ** 2 + q.y ** 2 + q.z ** 2)); const xx = q.x * q.x, xy = q.x * q.y, xz = q.x * q.z, From 9708ed81bb5bf31917065635031f0757d907028b Mon Sep 17 00:00:00 2001 From: Vladimir Demidov Date: Thu, 14 Nov 2024 00:10:08 +0300 Subject: [PATCH 7/7] Code style improvement Co-authored-by: Mark Haslinghuis --- src/flightlog.js | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/flightlog.js b/src/flightlog.js index 2953434f..3c959f2a 100644 --- a/src/flightlog.js +++ b/src/flightlog.js @@ -738,14 +738,14 @@ export function FlightLog(logData) { w: 1.0, }; q.w = Math.sqrt(1.0 - (q.x ** 2 + q.y ** 2 + q.z ** 2)); - const xx = q.x * q.x, + const xx = q.x ** 2, xy = q.x * q.y, xz = q.x * q.z, wx = q.w * q.x, - yy = q.y * q.y, + yy = q.y ** 2, yz = q.y * q.z, wy = q.w * q.y, - zz = q.z * q.z, + zz = q.z ** 2, wz = q.w * q.z; let roll = Math.atan2((+2.0 * (wx + yz)), (+1.0 - 2.0 * (xx + yy))); let pitch = ((0.5 * Math.PI) - Math.acos(+2.0 * (wy - xz)));