From c1fe95f726a34534c1d8ef774748c851d4315f04 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 11 May 2023 03:06:55 +0100 Subject: [PATCH] Tools: Web: Add FFT batch log review tool --- .../Tools/FilterReview/DecodeDevID.js | 154 +++ .../Tools/FilterReview/FilterReview.js | 828 ++++++++++++++++ .../web-firmware/Tools/FilterReview/Readme.md | 6 + .../Tools/FilterReview/index.html | 275 ++++++ .../web-firmware/Tools/FilterReview/parser.js | 897 ++++++++++++++++++ 5 files changed, 2160 insertions(+) create mode 100644 Tools/autotest/web-firmware/Tools/FilterReview/DecodeDevID.js create mode 100644 Tools/autotest/web-firmware/Tools/FilterReview/FilterReview.js create mode 100644 Tools/autotest/web-firmware/Tools/FilterReview/Readme.md create mode 100644 Tools/autotest/web-firmware/Tools/FilterReview/index.html create mode 100644 Tools/autotest/web-firmware/Tools/FilterReview/parser.js diff --git a/Tools/autotest/web-firmware/Tools/FilterReview/DecodeDevID.js b/Tools/autotest/web-firmware/Tools/FilterReview/DecodeDevID.js new file mode 100644 index 0000000000..26359c3c17 --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterReview/DecodeDevID.js @@ -0,0 +1,154 @@ +/* + Translated from Tools/scripts/decode_devid.py +*/ + +const DEVICE_TYPE_COMPASS = 0 +const DEVICE_TYPE_IMU = 1 +const DEVICE_TYPE_BARO = 2 +const DEVICE_TYPE_AIRSPEED = 3 + +function decode_devid(ID, type) { + +const bus_type = ID & 0x07 +const bus = (ID>>3) & 0x1F +const address = (ID>>8) & 0xFF +const devtype = (ID>>16) + +bustypes = { + 1: "I2C", + 2: "SPI", + 3: "DRONECAN", + 4: "SITL", + 5: "MSP", + 6: "SERIAL", +} + +compass_types = { + 0x01 : "HMC5883_OLD", + 0x07 : "HMC5883", + 0x02 : "LSM303D", + 0x04 : "AK8963 ", + 0x05 : "BMM150 ", + 0x06 : "LSM9DS1", + 0x08 : "LIS3MDL", + 0x09 : "AK09916", + 0x0A : "IST8310", + 0x0B : "ICM20948", + 0x0C : "MMC3416", + 0x0D : "QMC5883L", + 0x0E : "MAG3110", + 0x0F : "SITL", + 0x10 : "IST8308", + 0x11 : "RM3100_OLD", + 0x12 : "RM3100", + 0x13 : "MMC5883", + 0x14 : "AK09918", +} + +imu_types = { + 0x09 : "BMI160", + 0x10 : "L3G4200D", + 0x11 : "ACC_LSM303D", + 0x12 : "ACC_BMA180", + 0x13 : "ACC_MPU6000", + 0x16 : "ACC_MPU9250", + 0x17 : "ACC_IIS328DQ", + 0x21 : "GYR_MPU6000", + 0x22 : "GYR_L3GD20", + 0x24 : "GYR_MPU9250", + 0x25 : "GYR_I3G4250D", + 0x26 : "GYR_LSM9DS1", + 0x27 : "ICM20789", + 0x28 : "ICM20689", + 0x29 : "BMI055", + 0x2A : "SITL", + 0x2B : "BMI088", + 0x2C : "ICM20948", + 0x2D : "ICM20648", + 0x2E : "ICM20649", + 0x2F : "ICM20602", + 0x30 : "ICM20601", + 0x31 : "ADIS1647x", + 0x32 : "SERIAL", + 0x33 : "ICM40609", + 0x34 : "ICM42688", + 0x35 : "ICM42605", + 0x36 : "ICM40605", + 0x37 : "IIM42652", + 0x38 : "BMI270", + 0x39 : "BMI085", + 0x3A : "ICM42670", +} + +baro_types = { + 0x01 : "SITL", + 0x02 : "BMP085", + 0x03 : "BMP280", + 0x04 : "BMP388", + 0x05 : "DPS280", + 0x06 : "DPS310", + 0x07 : "FBM320", + 0x08 : "ICM20789", + 0x09 : "KELLERLD", + 0x0A : "LPS2XH", + 0x0B : "MS5611", + 0x0C : "SPL06", + 0x0D : "DRONECAN", + 0x0E : "MSP", + 0x0F : "ICP101XX", + 0x10 : "ICP201XX", + 0x11 : "MS5607", + 0x12 : "MS5837", + 0x13 : "MS5637", + 0x14 : "BMP390", +} + +airspeed_types = { + 0x01 : "SITL", + 0x02 : "MS4525", + 0x03 : "MS5525", + 0x04 : "DLVR", + 0x05 : "MSP", + 0x06 : "SDP3X", + 0x07 : "DRONECAN", + 0x08 : "ANALOG", + 0x09 : "NMEA", + 0x0A : "ASP5033", +} + +function get(lookup, index) { + if (lookup[index] != null) { + return lookup[index] + } + return "Unknown" +} + +var name +switch (type) { + case DEVICE_TYPE_COMPASS: + name = "Compass: " + get(compass_types, devtype) + break + case DEVICE_TYPE_IMU: + name = "IMU: " + get(imu_types, devtype) + break + case DEVICE_TYPE_BARO: + name = "Baro: " + get(baro_types, devtype) + break + case DEVICE_TYPE_AIRSPEED: + name = "Airspeed: " + get(airspeed_types, devtype) + break + default: + console.error("Unknown type"); + return +} + +const bus_type_string = get(bustypes, bus_type) + +if (bus_type == 3) { + // dronecan devtype represents sensor_id + return { bus_type: bus_type_string, bus: bus, address: address, sensor_id: devtype-1, name: name } +} + +return { bus_type: bus_type_string, bus: bus, address: address, devtype: devtype, name: name } + +} diff --git a/Tools/autotest/web-firmware/Tools/FilterReview/FilterReview.js b/Tools/autotest/web-firmware/Tools/FilterReview/FilterReview.js new file mode 100644 index 0000000000..72962268b2 --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterReview/FilterReview.js @@ -0,0 +1,828 @@ +// A js tool for plotting ArduPilot batch log data + +// return hanning window array of given length +function hanning(len) { + w = [] + for (let i=0;i= len/2) { + freq[i] -= len + } + freq[i] /= len * d + } + return freq +} + +function run_fft(data, window_size, window_spacing, windowing_function, positive_index) { + const num_points = data.x.length + + var fft_x = [] + var fft_y = [] + var fft_z = [] + + var center_sample = [] + + const num_windows = Math.floor((num_points-window_size)/window_spacing) + 1 + for (var i=0;i" + amplitude_scale.hover("y") + + for (let i = 0; i < Gyro_batch.length; i++) { + if ((Gyro_batch[i] == null) || (Gyro_batch[i].FFT == null)) { + continue + } + + // Find the start and end index + const start_index = find_start_index(Gyro_batch[i].FFT.time) + const end_index = find_end_index(Gyro_batch[i].FFT.time) + + // Number of windows to plot + const plot_length = end_index - start_index + + // Windowing amplitude correction depends on spectrum of interest + const window_correction = amplitude_scale.use_PSD ? (Gyro_batch[i].FFT.correction.energy * math.sqrt(1/2)) : Gyro_batch[i].FFT.correction.linear + + // Take mean from start to end + var fft_mean_x = [] + var fft_mean_y = [] + var fft_mean_z = [] + for (let j=start_index;j" + frequency_scale.hover("x") + "
" + amplitude_scale.hover("z") + Spectrogram.data[0].colorbar.title.text = amplitude_scale.label + + // Find the start and end index + const start_index = find_start_index(Gyro_batch[batch_instance].FFT.time) + const end_index = find_end_index(Gyro_batch[batch_instance].FFT.time) + + // Number of windows to plot + const plot_length = end_index - start_index + + // Setup xy data + Spectrogram.data[0].x = apply_scale_fun(Gyro_batch[batch_instance].FFT.bins, frequency_scale.fun) + Spectrogram.data[0].y = Gyro_batch[batch_instance].FFT.time.slice(start_index, end_index) + + // Windowing amplitude correction depends on spectrum of interest + const window_correction = amplitude_scale.use_PSD ? (Gyro_batch[batch_instance].FFT.correction.energy * math.sqrt(1/2)) : Gyro_batch[batch_instance].FFT.correction.linear + + // Setup z data + Spectrogram.data[0].z = [] + for (j = 0; j= log.messages.ISBD.N.length) { + // This is expected at the end of a log, no more msgs to add, break here + console.log("Could not find next sequence " + i + " of " + log.messages.ISBH.N.length-1) + decode_complete = true + break + } + } + if (decode_complete) { + break + } + + let x = [] + let y = [] + let z = [] + const num_samples = log.messages.ISBH.smp_cnt[i] + const num_data_msg = num_samples / 32 + for (let j = 0; j < num_data_msg; j++) { + // Read in expected number of samples + if ((log.messages.ISBD.N[data_index] != seq_num) || (log.messages.ISBD.seqno[data_index] != j)) { + console.log("Missing or extra data msg") + return + } + + // Accumulate data for this batch + x.push(...log.messages.ISBD.x[data_index]) + y.push(...log.messages.ISBD.y[data_index]) + z.push(...log.messages.ISBD.z[data_index]) + + data_index++ + if (data_index >= log.messages.ISBD.N.length) { + console.log("sequence incomplete " + i + " of " + log.messages.ISBH.N.length-1) + decode_complete = true + break + } + } + if (decode_complete) { + break + } + + if ((x.length != num_samples) || (y.length != num_samples) || (z.length != num_samples)) { + console.log("sample length wrong") + return + } + + // Remove logging scale factor + const mul = 1/log.messages.ISBH.mul[i] + for (let j = 0; j < num_samples; j++) { + x[j] *= mul + y[j] *= mul + z[j] *= mul + } + + // Add to batches for this instance + Gyro_batch[instance].push({ sample_time: log.messages.ISBH.SampleUS[i] / 1000000, + sample_rate: log.messages.ISBH.smp_rate[i], + x: x, + y: y, + z: z }) + } + + if (Gyro_batch.length == 0) { + console.log("no data") + return + } + + // Work out if logging is pre/post from param value + log.parseAtOffset('PARM') + const INS_LOG_BAT_OPT = get_param_value(log.messages.PARM, "INS_LOG_BAT_OPT") + const _doing_sensor_rate_logging = (INS_LOG_BAT_OPT & (1 << 0)) != 0 + const _doing_post_filter_logging = (INS_LOG_BAT_OPT & (1 << 1)) != 0 + const _doing_pre_post_filter_logging = (INS_LOG_BAT_OPT & (1 << 2)) != 0 + const use_instance_offset = _doing_pre_post_filter_logging || (_doing_post_filter_logging && _doing_sensor_rate_logging) + const instance_offset = Gyro_batch.length/2 + for (let i = 0; i < Gyro_batch.length; i++) { + if (Gyro_batch[i] == null) { + continue + } + if (use_instance_offset && (i >= instance_offset)) { + Gyro_batch[i].sensor_num = i - instance_offset + Gyro_batch[i].post_filter = true + } else { + Gyro_batch[i].sensor_num = i + Gyro_batch[i].post_filter = _doing_post_filter_logging && !_doing_pre_post_filter_logging + } + } + + // setup/reset plot and options + reset() + + // Update ranges of start and end time + var start_time + var end_time + for (let i = 0; i < Gyro_batch.length; i++) { + if (Gyro_batch[i] == null) { + continue + } + + const batch_start = Gyro_batch[i][0].sample_time + if ((start_time == null) || (batch_start < start_time)) { + start_time = batch_start + } + + const batch_end = Gyro_batch[i][Gyro_batch[i].length - 1].sample_time + if ((end_time == null) || (batch_end > end_time)) { + end_time = batch_end + } + } + start_time = math.floor(start_time) + end_time = math.ceil(end_time) + + var start_input = document.getElementById("TimeStart") + start_input.disabled = false; + start_input.min = start_time + start_input.value = start_time + start_input.max = end_time + + var end_input = document.getElementById("TimeEnd") + end_input.disabled = false; + end_input.min = start_time + end_input.value = end_time + end_input.max = end_time + + // Enable checkboxes for sensors which are present + var first_gyro + var have_pre = false + for (let i = 0; i < Gyro_batch.length; i++) { + if (Gyro_batch[i] == null) { + continue + } + const prepost = Gyro_batch[i].post_filter ? "Post" : "Pre" + let axis = ["X" , "Y", "Z"] + for (let j = 0; j < 3; j++) { + var fft_check = document.getElementById("Gyro" + Gyro_batch[i].sensor_num + prepost + axis[j]) + fft_check.disabled = false + fft_check.checked = true + } + + // Track which sensors are present for spectrogram + if (first_gyro == null || (Gyro_batch[i].sensor_num < first_gyro)) { + first_gyro = Gyro_batch[i].sensor_num + } + if (Gyro_batch[i].post_filter == false) { + document.getElementById("SpecGyroPre").disabled = false + have_pre = true + } else { + document.getElementById("SpecGyroPost").disabled = false + } + document.getElementById("SpecGyroInst" + Gyro_batch[i].sensor_num).disabled = false + document.getElementById("SpecGyroAxisX").checked = true + document.getElementById("SpecGyroAxisY").disabled = false + document.getElementById("SpecGyroAxisZ").disabled = false + } + + // Default spectrograph to first sensor, pre if available and X axis + document.getElementById("SpecGyroInst" + first_gyro).checked = true + document.getElementById("SpecGyro" + (have_pre ? "Pre" : "Post")).checked = true + document.getElementById("SpecGyroAxisX").disabled = false + + // Calculate FFT + calculate() + + // Plot + redraw() + + // Enable the calculate button + document.getElementById("calculate").disabled = false + + // Set FFT info + var set_batch_len_msg = false + for (let i = 0; i < 3; i++) { + let sample_rate = 0 + let window_size = 0 + let count = 0 + for (let j = 0; j < Gyro_batch.length; j++) { + if ((Gyro_batch[j] == null) || Gyro_batch[j].sensor_num != i) { + continue + } + sample_rate += Gyro_batch[j].FFT.average_sample_rate + window_size += Gyro_batch[j].FFT.window_size + count++ + } + if (count == 0) { + continue + } + sample_rate /= count + window_size /= count + + document.getElementById("Gyro" + i + "_FFT_info").innerHTML = "Logging rate : " + (sample_rate).toFixed(2) + " Hz

" + + "Frequency resolution : " + (sample_rate/window_size).toFixed(2) + " Hz" + + if (set_batch_len_msg == false) { + set_batch_len_msg = true + document.getElementById("FFTWindowInfo").innerHTML = "Window size: " + window_size + } + } + + // Try and decode device IDs + for (let i = 0; i < 3; i++) { + const ID_param = i == 0 ? "INS_GYR_ID" : "INS_GYR" + (i + 1) + "_ID" + const ID = get_param_value(log.messages.PARM, ID_param) + if (ID != null) { + const decoded = decode_devid(ID, DEVICE_TYPE_IMU) + if (decoded != null) { + document.getElementById("Gyro" + i + "_info").innerHTML = decoded.name + " via " + decoded.bus_type + } + } + + } + + const end = performance.now(); + console.log(`Load took: ${end - start} ms`); +} diff --git a/Tools/autotest/web-firmware/Tools/FilterReview/Readme.md b/Tools/autotest/web-firmware/Tools/FilterReview/Readme.md new file mode 100644 index 0000000000..041bdd838a --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterReview/Readme.md @@ -0,0 +1,6 @@ +Test with `python -m http.server --bind 127.0.0.1` + +Uses log parser from https://github.com/Williangalvani/JsDataflashParser + +WIP! + diff --git a/Tools/autotest/web-firmware/Tools/FilterReview/index.html b/Tools/autotest/web-firmware/Tools/FilterReview/index.html new file mode 100644 index 0000000000..6f9341382e --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterReview/index.html @@ -0,0 +1,275 @@ + + + + +ArduPilot Filter Review Tool + + + + + + + +

ArduPilot Filter Review Tool

+ + + + + + + + +
+
+ Setup + + + + + + + + +
+
+ Amplitude scale + +
+ +
+
+
+
+ Spectrum scale + +
+ +
+
+
+
+ Frequency scale + + + + + +
+ +
+ +
+
+ +
+ +
+
+
+
+
+ FFT Settings + + +

+ +
+
+
+ Analysis time + + + s

+ + + s +
+
+

+ Bin log: + + +

+
+
+

+

+

+ + + + + + + +
+
+ Gyro 1 + +

+

+ Pre-filter + + + + + + + + +
+

+

+

+ Post-filter + + + + + + + + +
+

+ +
+
+
+ Gyro 2 + +

+

+ Pre-filter + + + + + + + + +
+

+

+

+ Post-filter + + + + + + + + +
+

+ +
+
+
+ Gyro 3 + +

+

+ Pre-filter + + + + + + + + +
+

+

+

+ Post-filter + + + + + + + + +
+

+ +
+
+ + +

+

+

+ + + + + + + +
+
+ Spectrogram Options +

+

+ Gyro instance + + + + + + + + +
+

+

+

+ Filtering + + + + + +
+

+

+

+ Axis + + + + + + + + +
+

+
+
+ + + + + + + + diff --git a/Tools/autotest/web-firmware/Tools/FilterReview/parser.js b/Tools/autotest/web-firmware/Tools/FilterReview/parser.js new file mode 100644 index 0000000000..f24c81a0c9 --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterReview/parser.js @@ -0,0 +1,897 @@ +const MAV_TYPE_GENERIC = 0 // Generic micro air vehicle. +const MAV_TYPE_FIXED_WING = 1 // Fixed wing aircraft. +const MAV_TYPE_QUADROTOR = 2 // Quadrotor +const MAV_TYPE_COAXIAL = 3 // Coaxial helicopter +const MAV_TYPE_HELICOPTER = 4 // Normal helicopter with tail rotor. +const MAV_TYPE_ANTENNA_TRACKER = 5 // Ground installation +const MAV_TYPE_GCS = 6 // Operator control unit / ground control station +const MAV_TYPE_AIRSHIP = 7 // Airship, controlled +const MAV_TYPE_FREE_BALLOON = 8 // Free balloon, uncontrolled +const MAV_TYPE_ROCKET = 9 // Rocket +const MAV_TYPE_GROUND_ROVER = 10 // Ground rover +const MAV_TYPE_SURFACE_BOAT = 11 // Surface vessel, boat, ship +const MAV_TYPE_SUBMARINE = 12 // Submarine +const MAV_TYPE_HEXAROTOR = 13 // Hexarotor +const MAV_TYPE_OCTOROTOR = 14 // Octorotor +const MAV_TYPE_TRICOPTER = 15 // Tricopter +const MAV_TYPE_FLAPPING_WING = 16 // Flapping wing +const MAV_TYPE_KITE = 17 // Kite +const MAV_TYPE_ONBOARD_CONTROLLER = 18 // Onboard companion controller +const MAV_TYPE_VTOL_DUOROTOR = 19 // Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. +const MAV_TYPE_VTOL_QUADROTOR = 20 // Quad-rotor VTOL using a V-shaped quad config in vertical operation. +// Tailsitter. +const MAV_TYPE_VTOL_TILTROTOR = 21 // Tiltrotor VTOL +const MAV_TYPE_VTOL_RESERVED2 = 22 // VTOL reserved 2 +const MAV_TYPE_VTOL_RESERVED3 = 23 // VTOL reserved 3 +const MAV_TYPE_VTOL_RESERVED4 = 24 // VTOL reserved 4 +const MAV_TYPE_VTOL_RESERVED5 = 25 // VTOL reserved 5 +const MAV_TYPE_GIMBAL = 26 // Onboard gimbal +const MAV_TYPE_ADSB = 27 // Onboard ADSB peripheral +const MAV_TYPE_PARAFOIL = 28 // Steerable, nonrigid airfoil +const MAV_TYPE_DODECAROTOR = 29 // Dodecarotor +const MAV_TYPE_CAMERA = 30 // Camera +const MAV_TYPE_CHARGING_STATION = 31 // Charging station +const MAV_TYPE_FLARM = 32 // Onboard FLARM collision avoidance system +const MAV_TYPE_ENUM_END = 33 // + + +const modeMappingApm = { + 0: 'MANUAL', + 1: 'CIRCLE', + 2: 'STABILIZE', + 3: 'TRAINING', + 4: 'ACRO', + 5: 'FBWA', + 6: 'FBWB', + 7: 'CRUISE', + 8: 'AUTOTUNE', + 10: 'AUTO', + 11: 'RTL', + 12: 'LOITER', + 13: 'TAKEOFF', + 14: 'AVOID_ADSB', + 15: 'GUIDED', + 16: 'INITIALISING', + 17: 'QSTABILIZE', + 18: 'QHOVER', + 19: 'QLOITER', + 20: 'QLAND', + 21: 'QRTL', + 22: 'QAUTOTUNE', + 23: 'QACRO', + 24: 'THERMAL' +} +const modeMappingAcm = { + 0: 'STABILIZE', + 1: 'ACRO', + 2: 'ALT_HOLD', + 3: 'AUTO', + 4: 'GUIDED', + 5: 'LOITER', + 6: 'RTL', + 7: 'CIRCLE', + 9: 'LAND', + 11: 'DRIFT', + 13: 'SPORT', + 14: 'FLIP', + 15: 'AUTOTUNE', + 16: 'POSHOLD', + 17: 'BRAKE', + 18: 'THROW', + 19: 'AVOID_ADSB', + 20: 'GUIDED_NOGPS', + 21: 'SMART_RTL', + 22: 'FLOWHOLD', + 23: 'FOLLOW', + 24: 'ZIGZAG', + 25: 'SYSTEMID', + 26: 'AUTOROTATE' +} +const modeMappingRover = { + 0: 'MANUAL', + 1: 'ACRO', + 3: 'STEERING', + 4: 'HOLD', + 5: 'LOITER', + 6: 'FOLLOW', + 7: 'SIMPLE', + 10: 'AUTO', + 11: 'RTL', + 12: 'SMART_RTL', + 15: 'GUIDED', + 16: 'INITIALISING' +} +const modeMappingTracker = { + 0: 'MANUAL', + 1: 'STOP', + 2: 'SCAN', + 3: 'SERVO_TEST', + 10: 'AUTO', + 16: 'INITIALISING' +} +const modeMappingSub = { + 0: 'STABILIZE', + 1: 'ACRO', + 2: 'ALT_HOLD', + 3: 'AUTO', + 4: 'GUIDED', + 7: 'CIRCLE', + 9: 'SURFACE', + 16: 'POSHOLD', + 19: 'MANUAL', + 20: 'MOTOR_DETECT' +} + + +const multipliers = { + '-': 0, // no multiplier e.g. a string + '?': 1, // multipliers which haven't been worked out yet.... + // + 2: 1e2, + 1: 1e1, + 0: 1e0, + A: 1e-1, + B: 1e-2, + C: 1e-3, + D: 1e-4, + E: 1e-5, + F: 1e-6, + G: 1e-7, + // + '!': 3.6, // (ampere*second => milliampere*hour) and (km/h => m/s) + '/': 3600 // (ampere*second => ampere*hour) +} + +const multipliersTable = { + 0.000001: 'n', + 1000: 'M', + 0.001: 'm' +} + +const HEAD1 = 163 +const HEAD2 = 149 + +const units = { + '-': '', // no units e.g. Pi, or a string + '?': 'UNKNOWN', // Units which haven't been worked out yet.... + A: 'A', // Ampere + d: '°', // of the angular variety, -180 to 180 + b: 'B', // bytes + k: '°/s', // degrees per second. Degrees are NOT SI, but is some situations more user-friendly than radians + D: '°', // degrees of latitude + e: '°/s/s', // degrees per second per second. Degrees are NOT SI, but is some situations more user-friendly + E: 'rad/s', // radians per second + G: 'Gauss', // Gauss is not an SI unit, but 1 tesla = 10000 gauss so a simple replacement is not possible here + h: '°', // 0.? to 359.? + i: 'A.s', // Ampere second + J: 'W.s', // Joule (Watt second) + // { 'l', "l" }, // litres + L: 'rad/s/s', // radians per second per second + m: 'm', // metres + n: 'm/s', // metres per second + // { 'N', "N" }, // Newton + o: 'm/s/s', // metres per second per second + O: '°C', // degrees Celsius. Not SI, but Kelvin is too cumbersome for most users + '%': '%', // percent + S: 'satellites', // number of satellites + s: 's', // seconds + q: 'rpm', // rounds per minute. Not SI, but sometimes more intuitive than Hertz + r: 'rad', // radians + U: '°', // degrees of longitude + u: 'ppm', // pulses per minute + v: 'V', // Volt + P: 'Pa', // Pascal + w: 'Ohm', // Ohm + Y: 'us', // pulse width modulation in microseconds + z: 'Hz', // Hertz + '#': 'instance' // instance number for message +} + +function getModeMap (mavType) { + let map + if ([MAV_TYPE_QUADROTOR, + MAV_TYPE_HELICOPTER, + MAV_TYPE_HEXAROTOR, + MAV_TYPE_OCTOROTOR, + MAV_TYPE_COAXIAL, + MAV_TYPE_TRICOPTER].includes(mavType)) { + map = modeMappingAcm + } + if (mavType === MAV_TYPE_FIXED_WING) { + map = modeMappingApm + } + if (mavType === MAV_TYPE_GROUND_ROVER) { + map = modeMappingRover + } + if (mavType === MAV_TYPE_ANTENNA_TRACKER) { + map = modeMappingTracker + } + if (mavType === MAV_TYPE_SUBMARINE) { + map = modeMappingSub + } + if (map == null) { + return null + } + return map +} + +function assignColumn (obj) { + const ArrayOfString = obj.split(',') + return ArrayOfString +} + +// Converts from degrees to radians. +Math.radians = function (degrees) { + return degrees * Math.PI / 180 +} + +// Converts from radians to degrees. +Math.degrees = function (radians) { + return radians * 180 / Math.PI +} + +class DataflashParser { + constructor () { + this.time = null + this.timebase = null + this.buffer = null + this.data = null + this.FMT = [] + this.FMT[128] = { + Type: '128', + length: '89', + Name: 'FMT', + Format: 'BBnNZ', + Columns: 'Type,Length,Name,Format,Columns' + } + this.offset = 0 + this.msgType = [] + this.offsetArray = [] + this.totalSize = null + this.messages = {} + this.lastPercentage = 0 + this.sent = false + this.maxPercentageInterval = 0.05 + this.messageTypes = {} + this.alreadyParsed = [] + } + + FORMAT_TO_STRUCT (obj) { + let temp + const dict = { + name: obj.Name, + fieldnames: obj.Columns.split(',') + } + + const column = assignColumn(obj.Columns) + let low + let n + for (let i = 0; i < obj.Format.length; i++) { + temp = obj.Format.charAt(i) + switch (temp) { + case 'a': // int16_t[32] + dict[column[i]] = [] + for (let j = 0; j < 32; j++) { + dict[column[i]][j] = this.data.getInt16(this.offset, true) + this.offset += 2 + } + break + case 'b': + dict[column[i]] = this.data.getInt8(this.offset) + this.offset += 1 + break + case 'B': + dict[column[i]] = this.data.getUint8(this.offset) + this.offset += 1 + break + case 'h': + dict[column[i]] = this.data.getInt16(this.offset, true) + this.offset += 2 + break + case 'H': + dict[column[i]] = this.data.getUint16(this.offset, true) + this.offset += 2 + break + case 'i': + dict[column[i]] = this.data.getInt32(this.offset, true) + this.offset += 4 + break + case 'I': + dict[column[i]] = this.data.getUint32(this.offset, true) + this.offset += 4 + break + case 'f': + dict[column[i]] = this.data.getFloat32(this.offset, true) + this.offset += 4 + break + case 'd': + dict[column[i]] = this.data.getFloat64(this.offset, true) + this.offset += 8 + break + case 'Q': + low = this.data.getUint32(this.offset, true) + this.offset += 4 + n = this.data.getUint32(this.offset, true) * 4294967296.0 + low + if (low < 0) n += 4294967296 + dict[column[i]] = n + this.offset += 4 + break + case 'q': + low = this.data.getInt32(this.offset, true) + this.offset += 4 + n = this.data.getInt32(this.offset, true) * 4294967296.0 + low + if (low < 0) n += 4294967296 + dict[column[i]] = n + this.offset += 4 + break + case 'n': + // TODO: fix these regex and unsilent linter + // eslint-disable-next-line + dict[column[i]] = String.fromCharCode.apply(null, new Uint8Array(this.buffer, this.offset, 4)).replace(/\x00+$/g, '') + this.offset += 4 + break + case 'N': + // eslint-disable-next-line + dict[column[i]] = String.fromCharCode.apply(null, new Uint8Array(this.buffer, this.offset, 16)).replace(/\x00+$/g, '') + this.offset += 16 + break + case 'Z': + // eslint-disable-next-line + dict[column[i]] = String.fromCharCode.apply(null, new Uint8Array(this.buffer, this.offset, 64)).replace(/\x00+$/g, '') + this.offset += 64 + break + case 'c': + // this.this.data.setInt16(offset,true); + dict[column[i]] = this.data.getInt16(this.offset, true) / 100 + this.offset += 2 + break + case 'C': + // this.data.setUint16(offset,true); + dict[column[i]] = this.data.getUint16(this.offset, true) / 100 + this.offset += 2 + break + case 'E': + // this.data.setUint32(offset,true); + dict[column[i]] = this.data.getUint32(this.offset, true) / 100 + this.offset += 4 + break + case 'e': + // this.data.setInt32(offset,true); + dict[column[i]] = this.data.getInt32(this.offset, true) / 100 + this.offset += 4 + break + case 'L': + // this.data.setInt32(offset,true); + dict[column[i]] = this.data.getInt32(this.offset, true) + this.offset += 4 + break + case 'M': + // this.data.setInt32(offset,true); + dict[column[i]] = this.data.getUint8(this.offset) + this.offset += 1 + break + } + } + return dict + } + + gpstimetoTime (week, msec) { + const epoch = 86400 * (10 * 365 + (1980 - 1969) / 4 + 1 + 6 - 2) + return epoch + 86400 * 7 * week + msec * 0.001 - 15 + } + + setTimeBase (base) { + this.timebase = base + } + + findTimeBase (gps) { + const temp = this.gpstimetoTime(parseInt(gps.GWk), parseInt(gps.GMS)) + this.setTimeBase(parseInt(temp - gps.TimeUS * 0.000001)) + } + + getMsgType (element) { + for (let i = 0; i < this.FMT.length; i++) { + if (this.FMT[i] != null) { + // eslint-disable-next-line + if (this.FMT[i].Name == element) { + return i + } + } + } + } + + onMessage (message) { + if (this.totalSize == null) { // for percentage calculation + this.totalSize = this.buffer.byteLength + } + if (message.name in this.messages) { + this.messages[message.name].push(this.fixData(message)) + } else { + this.messages[message.name] = [this.fixData(message)] + } + const percentage = 100 * this.offset / this.totalSize + if ((percentage - this.lastPercentage) > this.maxPercentageInterval) { + self.postMessage({ percentage: percentage }) + this.lastPercentage = percentage + } + } + + messageHasInstances (name) { + const type = this.FMT.find(msg => msg !== undefined && msg.Name === name) + return type !== undefined && type.units !== undefined && type.units.indexOf('instance') !== -1 + } + + getInstancesFieldName (name) { + const type = this.FMT.find(msg => msg !== undefined && msg.Name === name) + if (type.units === undefined) { + return null + } + return type.Columns.split(',')[type.units.indexOf('instance')] + } + + // Next three functions are used for transfering data on postmessage, instead of cloning + isTypedArray (arr) { + return ArrayBuffer.isView(arr) && !(arr instanceof DataView) + } + + getType (arr) { + return this.isTypedArray(arr) && arr.constructor.name + } + + postData (data) { + data.dataType = {} + const transferables = [] + for (const field of Object.keys(data.messageList)) { + const arrayType = this.getType(data.messageList[field]) + if (arrayType) { + transferables.push(data.messageList[field].buffer) + } + // Apparently it is magically decoded on the other end, no need for metadata + // data['dataType'][field] = arrayType + } + self.postMessage(data, transferables) + } + + extractTimeMetadataFromGPS () { + const metadata = { + startTime: this.extractStartTime() + } + self.postMessage({ metadata: metadata }) + } + + parseAtOffset (name) { + const type = this.getMsgType(name) + const parsed = [] + for (let i = 0; i < this.msgType.length; i++) { + if (type === this.msgType[i]) { + this.offset = this.offsetArray[i] + try { + const temp = this.FORMAT_TO_STRUCT(this.FMT[this.msgType[i]]) + if (temp.name != null) { + parsed.push(this.fixData(temp)) + } + } catch (e) { + console.log('reached log end?') + console.log(e) + } + } + if (i % 100000 === 0) { + const perc = 100 * i / this.msgType.length + self.postMessage({ percentage: perc }) + } + } + delete this.messages[name] + this.messages[name] = parsed + + self.postMessage({ percentage: 100 }) + console.log(name, this.messageHasInstances(name) ? 'has instances' : 'has no instances') + if (parsed.length && this.messageHasInstances(name)) { + const instanceField = this.getInstancesFieldName(name) + const instances = {} + for (const msg of parsed) { + try { + instances[msg[instanceField]].push(msg) + } catch (e) { + instances[msg[instanceField]] = [msg] + } + } + if (Object.keys(instances).length === 1) { + this.fixDataOnce(name) + this.simplifyData(name) + + if (name === 'GPS') { + this.extractTimeMetadataFromGPS() + } + this.postData({ messageType: name, messageList: this.messages[name] }) + return parsed + } + for (const [index, messages] of Object.entries(instances)) { + const newName = name + '[' + index + ']' + this.messages[newName] = messages + this.fixDataOnce(newName) + this.simplifyData(newName) + this.postData({ + messageType: newName, + messageList: this.messages[newName] + }) + } + } else if (parsed.length) { + this.fixDataOnce(name) + this.simplifyData(name) + if (name === 'GPS') { + this.extractTimeMetadataFromGPS() + } + this.postData({ messageType: name, messageList: this.messages[name] }) + } + this.alreadyParsed.push(name) + return parsed + } + + checkNumberOfInstances (name) { + // Similar to parseOffset, but finishes earlier and updates messageTypes + const type = this.getMsgType(name) + const availableInstances = [] + const instanceField = this.getInstancesFieldName(name) + if (instanceField === null) { + return [1] + } + let repeats = 0 + for (let i = 0; i < this.msgType.length; i++) { + if (type === this.msgType[i]) { + this.offset = this.offsetArray[i] + try { + const temp = this.FORMAT_TO_STRUCT(this.FMT[this.msgType[i]]) + if (temp.name != null) { + const msg = temp + if (msg[instanceField] === undefined) { + break + } + // we do an early return after we get 20 repeated instance numbers. should we? + const instance = msg[instanceField] + if (availableInstances.indexOf(instance) !== -1) { + repeats += 1 + if (repeats > 20) { + return availableInstances + } + } else { + availableInstances.push(instance) + } + } + } catch (e) { + console.log(e) + } + } + } + return availableInstances + } + + timestamp (TimeUs) { + const temp = this.timebase + TimeUs * 0.000001 + if (temp > 0) { + TimeUs = temp + } + let date = new Date(TimeUs * 1000) + const hours = date.getHours() + const minutes = '0' + date.getMinutes() + const seconds = '0' + date.getSeconds() + const formattedTime = hours + ':' + minutes.substr(-2) + ':' + seconds.substr(-2) + date = date.toString() + const time = date.split(' ') + if (time[0] !== 'Invalid') { + this.time = time[0] + ' ' + time[1] + ' ' + time[2] + ' ' + time[3] + } + return formattedTime + } + + DfReader () { + let lastOffset = 0 + while (this.offset < (this.buffer.byteLength - 3)) { + if (this.data.getUint8(this.offset) !== HEAD1 || this.data.getUint8(this.offset + 1) !== HEAD2) { + this.offset += 1 + continue + } + this.offset += 2 + + const attribute = this.data.getUint8(this.offset) + if (this.FMT[attribute] != null) { + this.offset += 1 + this.offsetArray.push(this.offset) + this.msgType.push(attribute) + let value + try { + value = this.FORMAT_TO_STRUCT(this.FMT[attribute]) + if (this.FMT[attribute].Name === 'GPS') { + this.findTimeBase(value) + } + } catch (e) { + // console.log('reached log end?') + // console.log(e) + this.offset += 1 + continue + } + if (attribute === 128) { + this.FMT[value.Type] = { + Type: value.Type, + length: value.Length, + Name: value.Name, + Format: value.Format, + Columns: value.Columns + } + } + // this.onMessage(value) + } else { + this.offset += 1 + } + if (this.offset - lastOffset > 50000) { + const perc = 100 * this.offset / this.buffer.byteLength + self.postMessage({ percentage: perc }) + lastOffset = this.offset + } + } + self.postMessage({ percentage: 100 }) + self.postMessage({ messages: this.messages }) + this.sent = true + } + + getModeString (cmode) { + let mavtype + const msgs = this.messages.MSG + for (const i in msgs.Message) { + if (msgs.Message[i].toLowerCase().includes('arduplane')) { + mavtype = MAV_TYPE_FIXED_WING + return getModeMap(mavtype)[cmode] + } else if (msgs.Message[i].toLowerCase().includes('arducopter')) { + mavtype = MAV_TYPE_QUADROTOR + return getModeMap(mavtype)[cmode] + } else if (msgs.Message[i].toLowerCase().includes('ardusub')) { + mavtype = MAV_TYPE_SUBMARINE + return getModeMap(mavtype)[cmode] + } else if (msgs.Message[i].toLowerCase().includes('rover')) { + mavtype = MAV_TYPE_GROUND_ROVER + return getModeMap(mavtype)[cmode] + } else if (msgs.Message[i].toLowerCase().includes('tracker')) { + mavtype = MAV_TYPE_ANTENNA_TRACKER + return getModeMap(mavtype)[cmode] + } + } + console.log('defaulting to quadcopter') + return getModeMap(MAV_TYPE_QUADROTOR)[cmode] + } + + fixData (message) { + if (message.name === 'MODE') { + message.asText = this.getModeString(message.Mode) + } + // eslint-disable-next-line + message.time_boot_ms = message.TimeUS / 1000 + delete message.TimeUS + delete message.fieldnames + return message + } + + fixDataOnce (name) { + if (['GPS', 'ATT', 'AHR2', 'MODE'].indexOf(name) === -1) { + if (this.messageTypes[name]) { + const fields = this.messages[name][0].fieldnames + if (this.messageTypes[name].multipliers) { + for (const message in this.messages[name]) { + for (let i = 1; i < fields.length; i++) { + const fieldname = fields[i] + if (!isNaN(this.messageTypes[name].multipliers[i])) { + this.messages[name][message][fieldname] *= this.messageTypes[name].multipliers[i] + } + } + } + } + } + } + } + + concatTypedArrays (a, b) { // a, b TypedArray of same type + const c = new (a.constructor)(a.length + b.length) + c.set(a, 0) + c.set(b, a.length) + return c + } + + createUint8ArrayFromString (str) { + const array = new Uint8Array(str.length) + for (let i = 0, strLen = str.length; i < strLen; i++) { + array[i] = str.charCodeAt(i) + } + return array + } + + processFiles () { + this.files = {} + for (const msg of this.messages.FILE) { + if (!this.files[msg.FileName]) { + this.files[msg.FileName] = this.createUint8ArrayFromString(msg.Data) + } else { + this.files[msg.FileName] = this.concatTypedArrays( + this.files[msg.FileName], this.createUint8ArrayFromString(msg.Data) + ) + } + } + self.postMessage({ files: this.files }) + } + + simplifyData (name) { + if (name === 'MODE') { + this.messageTypes[name].expressions.push('asText') + } + if (name === 'FILE') { + return + } + if (['FMTU'].indexOf(name) === -1) { + if (this.messageTypes[name]) { + const fields = this.messageTypes[name].expressions + if (!fields.includes('time_boot_ms')) { + fields.push('time_boot_ms') + } + const mergedData = {} + for (const field of fields) { + mergedData[field] = [] + } + for (const message of this.messages[name]) { + for (let i = 1; i < fields.length; i++) { + const fieldname = fields[i] + mergedData[fieldname].push(message[fieldname]) + } + } + delete this.messages[name] + this.messages[name] = mergedData + for (const field of this.messageTypes[name].expressions) { + if (this.messages[name][field] && !isNaN(this.messages[name][field][0])) { + const newData = new Float64Array(this.messages[name][field]) + delete this.messages[name][field] + this.messages[name][field] = newData + } + } + } + } + } + + populateUnits () { + // console.log(this.messages['FMTU']) + for (const msg of this.messages.FMTU) { + this.FMT[msg.FmtType].units = [] + for (const unit of msg.UnitIds) { + this.FMT[msg.FmtType].units.push(units[unit]) + } + this.FMT[msg.FmtType].multipliers = [] + for (const mult of msg.MultIds) { + this.FMT[msg.FmtType].multipliers.push(multipliers[mult]) + } + } + } + + extractStartTime () { + const msgs = this.messages.GPS + for (const i in msgs.time_boot_ms) { + if (msgs.GWk[i] > 1000) { // lousy validation + const weeks = msgs.GWk[i] + const ms = msgs.GMS[i] + let d = new Date((315964800.0 + ((60 * 60 * 24 * 7) * weeks) + ms / 1000.0) * 1000.0) + // adjusting for leap seconds + d = new Date(d.getTime() - this.leapSecondsGPS(d.getUTCFullYear(), d.getUTCMonth() + 1) * 1000) + return d + } + } + } + + leapSecondsGPS (year, month) { + return this.leapSecondsTAI(year, month) - 19 + } + + leapSecondsTAI (year, month) { + const yyyymm = year * 100 + month + if (yyyymm >= 201701) return 37 + if (yyyymm >= 201507) return 36 + if (yyyymm >= 201207) return 35 + if (yyyymm >= 200901) return 34 + if (yyyymm >= 200601) return 33 + if (yyyymm >= 199901) return 32 + if (yyyymm >= 199707) return 31 + if (yyyymm >= 199601) return 30 + + return 0 + } + + processData (data) { + this.buffer = data + this.data = new DataView(this.buffer) + this.DfReader() + const messageTypes = {} + this.parseAtOffset('FMTU') + this.populateUnits() + const typeSet = new Set(this.msgType) + for (const msg of this.FMT) { + if (msg) { + if (typeSet.has(msg.Type)) { + const fields = msg.Columns.split(',') + // expressions = expressions.filter(e => e !== 'TimeUS') + const complexFields = {} + if (msg.units) { + for (const field in fields) { + complexFields[fields[field]] = { + name: fields[field], + units: (multipliersTable[msg.multipliers[field]] || '') + msg.units[field], + multiplier: msg.multipliers[field] + } + } + } else { + for (const field in fields) { + complexFields[fields[field]] = { + name: fields[field], + units: '?', + multiplier: 1 + } + } + } + const availableInstances = this.checkNumberOfInstances(msg.Name) + if (availableInstances.length > 1) { + for (const instance of availableInstances) { + messageTypes[msg.Name + '[' + instance + ']'] = { + expressions: fields, + units: msg.units, + multipiers: msg.multipliers, + complexFields: complexFields + } + } + } else { + messageTypes[msg.Name] = { + expressions: fields, + units: msg.units, + multipiers: msg.multipliers, + complexFields: complexFields + } + } + } + } + } + self.postMessage({ availableMessages: messageTypes }) + this.messageTypes = messageTypes + this.parseAtOffset('CMD') + this.parseAtOffset('MSG') + this.parseAtOffset('FILE') + this.processFiles() + this.parseAtOffset('MODE') + this.parseAtOffset('AHR2') + this.parseAtOffset('ATT') + this.parseAtOffset('GPS') + this.parseAtOffset('POS') + this.parseAtOffset('XKQ1') + this.parseAtOffset('XKQ') + this.parseAtOffset('NKQ1') + this.parseAtOffset('NKQ2') + this.parseAtOffset('XKQ2') + this.parseAtOffset('PARM') + this.parseAtOffset('MSG') + this.parseAtOffset('STAT') + this.parseAtOffset('EV') + this.parseAtOffset('FNCE') + + self.postMessage({ messagesDoneLoading: true }) + return { types: this.messageTypes, messages: this.messages } + } + + loadType (type) { + this.parseAtOffset(type) + console.log('done') + } +} + +self.addEventListener('message', function (event) { + if (event.data === null) { + console.log('got bad file message!') + } else if (event.data.action === 'parse') { + parser = new DataflashParser() + let data = event.data.file + parser.processData(data) + } else if (event.data.action === 'loadType') { + parser.loadType(event.data.type.split('[')[0]) + } else if (event.data.action === 'trimFile') { + parser.trimFile(event.data.time) + } +})