From 15601e413982d6f362a870d67586e1510c5ad2b1 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 15 May 2023 19:15:07 +0100 Subject: [PATCH] Tools: Web: FilterReview: add notch tracking overlay --- .../Tools/FilterReview/FilterReview.js | 444 +++++++++++++++++- .../Tools/FilterReview/index.html | 84 +++- 2 files changed, 513 insertions(+), 15 deletions(-) diff --git a/Tools/autotest/web-firmware/Tools/FilterReview/FilterReview.js b/Tools/autotest/web-firmware/Tools/FilterReview/FilterReview.js index 6f4a8895b5..4475937458 100644 --- a/Tools/autotest/web-firmware/Tools/FilterReview/FilterReview.js +++ b/Tools/autotest/web-firmware/Tools/FilterReview/FilterReview.js @@ -1,5 +1,293 @@ // A js tool for plotting ArduPilot batch log data +// Generic Class to hold source for notch target +class NotchTarget { + constructor(log, msg_name, key_name, name, mode_value) { + this.name = name + this.mode_value = mode_value + this.data = [] + + // Don't always need log data (static notch) + if (log == null) { + return + } + + // Grab data from log + log.parseAtOffset(msg_name) + if ((log.messages[msg_name] == null) || (log.messages[msg_name].length == 0)) { + return + } + + // Convert ket into array if needed + if (!Array.isArray(key_name)) { + key_name = [key_name] + } + + // Grab all given keys to data struct + this.data.time = [] + for (var j=0; j 0) { + return math.max(config.freq, rpm * config.ref * (1.0/60.0)) + } + return config.freq + } +} + +class ESCTarget extends NotchTarget { + constructor(log) { + super(null, null, null, "ESC", 3) + + // Grab data from log, have to do it a little differently to get instances + const msg = "ESC" + log.parseAtOffset(msg) + if ((log.messages[msg] == null) || (log.messages[msg].length == 0)) { + return + } + + // Individual RPM + var instances = 0 + for (let i=0;i<16;i++) { + const inst_msg = msg + "[" + i + "]" + if (log.messages[inst_msg] != null) { + this.data[i] = { time:[], freq:[] } + for (var j=0; j < log.messages[inst_msg].time_boot_ms.length; j++) { + this.data[i].time[j] = log.messages[inst_msg].time_boot_ms[j] / 1000 + this.data[i].freq[j] = log.messages[inst_msg].RPM[j] / 60 + } + instances++ + } + } + + // Average RPM + this.data.avg_freq = [] + this.data.avg_time = [] + var inst = [] + for (let i=0;i 1000)) { + inst[j].time_ms = null + inst[j].rpm = null + } + } + + // If a full set of valid instances take average + var expected_count = 0 + var count = 0 + var sum = 0 + for (let j=0;j 0) && (count == expected_count)) { + this.data.avg_freq.push((sum / count) / 60) + this.data.avg_time.push(time_ms / 1000) + + // Invalidate used values + for (let j=0;j 0) && (energy_y > 0)) { + // Weighted by relative energy + this.data[i].freq[j] = (freq_x*energy_x + freq_y*energy_y) / (energy_x + energy_y) + } else { + // Just take average + this.data[i].freq[j] = (freq_x + freq_y) * 0.5 + } + this.data[i].time[j] = log.messages[inst_msg].time_boot_ms[j] / 1000 + } + } + } + } + + get_target_freq(config) { + const dynamic = (config.options & (1<<1)) != 0 + if (dynamic) { + // Tracking multiple peaks + let freq = [] + let time = [] + + for (let i = 0; i < this.data.length; i++) { + const start_index = find_start_index(this.data[i].time) + const end_index = find_end_index(this.data[i].time) + + let inst_freq = this.data[i].freq.slice(start_index, end_index) + for (let j = 0; j < inst_freq.length; j++) { + inst_freq[j] = math.max(inst_freq[j], config.freq) + } + + time.push(...this.data[i].time.slice(start_index, end_index)) + freq.push(...inst_freq) + + // Add NAN to remove line from end back to the start + time.push(NaN) + freq.push(NaN) + } + return { freq:freq, time:time } + } + + // Just center peak + const start_index = find_start_index(this.data.time) + const end_index = find_end_index(this.data.time) + + let freq = this.data.PkAvg.slice(start_index, end_index) + for (let j = 0; j < freq.length; j++) { + freq[j] = math.max(freq[j], config.freq) + } + + return { freq:freq, time:this.data.time.slice(start_index, end_index) } + } +} + // return hanning window array of given length (in tensorflow format) function hanning(len) { const half = tf.scalar(0.5) @@ -141,6 +429,7 @@ function run_batch_fft(data_set) { // Setup plots with no data var Spectrogram = {} var fft_plot = {} +const max_num_harmonics = 8 function reset() { let axis = ["X" , "Y", "Z"] @@ -160,9 +449,6 @@ function reset() { // Clear extra text document.getElementById("FFTWindowInfo").innerHTML = "" - document.getElementById("Gyro0_info").innerHTML = "
" - document.getElementById("Gyro1_info").innerHTML = "
" - document.getElementById("Gyro2_info").innerHTML = "
" document.getElementById("Gyro0_FFT_info").innerHTML = "


" document.getElementById("Gyro1_FFT_info").innerHTML = "


" document.getElementById("Gyro2_FFT_info").innerHTML = "


" @@ -191,29 +477,52 @@ function reset() { fft_plot.layout = { xaxis: {title: {text: ""}, type: "linear"}, yaxis: {title: {text: ""}}, + showlegend: true, + legend: {itemclick: false, itemdoubleclick: false }, + margin: { b: 50, l: 50, r: 50, t: 20 } } var FFTPlot = document.getElementById("FFTPlot") Plotly.purge(FFTPlot) Plotly.newPlot(FFTPlot, fft_plot.data, fft_plot.layout, {displaylogo: false}); - // Disable legend click (could probably hook this into the set the ticky boxes) - FFTPlot.on('plotly_legendclick', function(data) { return false }) - // Spectrogram setup // Add surface Spectrogram.data = [{ type:"heatmap", - colorbar: {title: {side: "right", text: ""}}, + colorbar: {title: {side: "right", text: ""}, orientation: "h"}, transpose: true, zsmooth: "best", hovertemplate: "" }] + // Add tracking lines + // Two harmonic notch filters each with upto 8 harmonics + for (let i=0;i<2;i++) { + let Group_name = "Notch " + (i+1) + let dash = (i == 0) ? "solid" : "dot" + for (let j=0;j" + name, + legendgroup: i, + legendgrouptitle: { text: "" } + }) + } + } + // Define Layout Spectrogram.layout = { xaxis: {title: {text: "Time (s)"}}, yaxis: {title: {text: ""}, type: "linear"}, + showlegend: true, + legend: {itemclick: false, itemdoubleclick: false }, + margin: { b: 50, l: 50, r: 50, t: 20 } } var SpectrogramPlot = document.getElementById("Spectrogram") @@ -473,6 +782,57 @@ function redraw_Spectrogram() { Spectrogram.data[0].z[j] = amplitude_scale.fun(math.dotMultiply(Gyro_batch[batch_instance].FFT[axis][index], window_correction)) } + // Setup tracking lines + const tracking_hovertemplate = "%{meta}
" + "%{x:.2f} s
" + frequency_scale.hover("y") + for (let i=0;i 0 + for (const [key, value] of Object.entries(HNotch_params[i])) { + if (key != "enable") { + document.getElementById(value).disabled = !enable_input + } + } + } + + // Load values into HNotch object + for (let i = 0; i < HNotch_params.length; i++) { + HNotch_setup[i] = [] + for (const [key, value] of Object.entries(HNotch_params[i])) { + HNotch_setup[i][key] = parseFloat(document.getElementById(value).value) + } + } +} + var Gyro_batch +var tracking_methods +var HNotch_setup = [] function load(log_file) { const start = performance.now() - // Clear and reset state var log = new DataflashParser() log.processData(log_file) @@ -622,9 +1020,6 @@ function load(log_file) { return } - // setup/reset plot and options - reset() - log.parseAtOffset('PARM') // Try and decode device IDs @@ -632,7 +1027,7 @@ function load(log_file) { 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) { + if ((ID != null) && (ID > 0)) { const decoded = decode_devid(ID, DEVICE_TYPE_IMU) if (decoded != null) { document.getElementById("Gyro" + i + "_info").innerHTML = decoded.name + " via " + decoded.bus_type @@ -663,6 +1058,31 @@ function load(log_file) { // setup/reset plot and options reset() + + // Load potential sources of notch tracking targets + tracking_methods = [new StaticTarget(), + new ThrottleTarget(log), + new RPMTarget(log, 1, 2), + new ESCTarget(log), + new FFTTarget(log), + new RPMTarget(log, 2, 5)] + + // Read from log into HTML box + const HNotch_params = get_HNotch_param_names() + for (let i = 0; i < HNotch_params.length; i++) { + for (const param of Object.values(HNotch_params[i])) { + const value = get_param_value(log.messages.PARM, param) + if (value != null) { + document.getElementById(param).value = value + } + } + // Allow enable values to be changed + document.getElementById(HNotch_params[i].enable).disabled = false + } + + // Load notch params + HNotch_param_read() + // Update ranges of start and end time var start_time var end_time diff --git a/Tools/autotest/web-firmware/Tools/FilterReview/index.html b/Tools/autotest/web-firmware/Tools/FilterReview/index.html index 278676e7ae..7dc0101805 100644 --- a/Tools/autotest/web-firmware/Tools/FilterReview/index.html +++ b/Tools/autotest/web-firmware/Tools/FilterReview/index.html @@ -15,6 +15,19 @@ + + @@ -210,7 +223,7 @@ + + -
-
+
Spectrogram Options

@@ -248,9 +261,74 @@

-
+
+
+
+ First Notch Filter +

+ + +

+

+ + +

+

+ + +

+

+ + +

+

+ + +

+

+ + +

+

+ + +

+
+
+
+ Second Notch Filter +

+ + +

+

+ + +

+

+ + +

+

+ + +

+

+ + +

+

+ + +

+

+ + +

+