diff --git a/Libraries/LogHelpers.js b/Libraries/LogHelpers.js index e55af0f6..c4d5847f 100644 --- a/Libraries/LogHelpers.js +++ b/Libraries/LogHelpers.js @@ -63,18 +63,22 @@ function get_version_and_board(log) { // Check we have bracketed the messages we need continue } + let types = [] + for (const type of Object.keys(build_types)) { + types.push("(?:" + type + ")") + } + const regex = new RegExp("(" + types.join("|") + ").+\\((.+)\\)", 'g') + const found = regex.exec(MSG.Message[i]) + if (found == null) { + continue + } if (fw_string == null) { - let types = [] - for (const type of Object.keys(build_types)) { - types.push("(?:" + type + ")") - } - const regex = new RegExp("(" + types.join("|") + ").+\\((.+)\\)", 'g') - const found = regex.exec(MSG.Message[i]) - if (found == null) { - continue - } fw_string = found[0] + } + if (build_type == null) { build_type = build_types[found[1]] + } + if (fw_hash == null) { fw_hash = found[2] } os_string = MSG.Message[i+1] diff --git a/PIDReview/ArduCopter_angle_rate_control_loop.drawio.png b/PIDReview/ArduCopter_angle_rate_control_loop.drawio.png new file mode 100644 index 00000000..b7ff2d1d Binary files /dev/null and b/PIDReview/ArduCopter_angle_rate_control_loop.drawio.png differ diff --git a/PIDReview/PIDReview.js b/PIDReview/PIDReview.js index 175a1ab2..24ee884c 100644 --- a/PIDReview/PIDReview.js +++ b/PIDReview/PIDReview.js @@ -4,7 +4,7 @@ var DataflashParser const import_done = import('../modules/JsDataflashParser/parser.js').then((mod) => { DataflashParser = mod.default }); // Keys in data object to run FFT of -const fft_keys = ["Tar", "Act", "Err", "P", "I", "D", "FF", "Out"] +const fft_keys = ["Tar", "Act", "Err", "P", "I", "D", "FF", "DFF", "Out"] function run_batch_fft(data_set) { @@ -64,7 +64,8 @@ function run_batch_fft(data_set) { // Log section is too short, skip continue } - var ret = run_fft(data_set[j][i], fft_keys, window_size, window_spacing, windowing_function, fft) + const valid_keys = fft_keys.filter(key => data_set[j][i][key] != null) + var ret = run_fft(data_set[j][i], valid_keys, window_size, window_spacing, windowing_function, fft) // Initialize arrays if (!have_data) { @@ -76,7 +77,7 @@ function run_batch_fft(data_set) { } data_set[j].FFT.time.push(...array_offset(array_scale(ret.center, sample_time), data_set[j][i].time[0])) - for (const key of fft_keys) { + for (const key of valid_keys) { data_set[j].FFT[key].push(...ret[key]) } } @@ -135,6 +136,14 @@ function reset() { Spectrogram.data[i].x = [] Spectrogram.data[i].y = [] } + for (let i = 0; i < target_filter_plot.data.length; i++) { + target_filter_plot.data[i].x = [] + target_filter_plot.data[i].y = [] + } + for (let i = 0; i < error_filter_plot.data.length; i++) { + error_filter_plot.data[i].x = [] + error_filter_plot.data[i].y = [] + } document.getElementById("calculate").disabled = true @@ -165,6 +174,8 @@ var TimeOutputs = {} var fft_plot = {} var step_plot = {} var Spectrogram = {} +var target_filter_plot = {} +var error_filter_plot = {} function setup_plots() { const time_scale_label = "Time (s)" @@ -272,7 +283,7 @@ function setup_plots() { Plotly.newPlot(plot, TimeInputs.data, TimeInputs.layout, {displaylogo: false}) - const pid_outputs = ["P","I","D","FF","Output"] + const pid_outputs = ["P","I","D","FF","D FF","Output"] TimeOutputs.data = [] for (const item of pid_outputs) { TimeOutputs.data.push({ mode: "lines", @@ -355,6 +366,42 @@ function setup_plots() { Plotly.purge(plot) Plotly.newPlot(plot, Spectrogram.data, Spectrogram.layout, {displaylogo: false}); + // Target filter plot setup + target_filter_plot.data = [ + { mode: "lines", name: "Unfiltered target (RATE)", meta: "Unfiltered target (RATE)", + showlegend: true, hovertemplate: "" }, + { mode: "lines", name: "Filtered target (PID)", meta: "Filtered target (PID)", + showlegend: true, hovertemplate: "" } + ] + target_filter_plot.layout = { + xaxis: {title: {text: frequency_scale.label }, type: "linear", zeroline: false, showline: true, mirror: true}, + yaxis: {title: {text: amplitude_scale.label }, zeroline: false, showline: true, mirror: true }, + showlegend: true, + legend: {itemclick: false, itemdoubleclick: false }, + margin: { b: 50, l: 50, r: 50, t: 20 }, + } + plot = document.getElementById("TargetFilterPlot") + Plotly.purge(plot) + Plotly.newPlot(plot, target_filter_plot.data, target_filter_plot.layout, {displaylogo: false}); + + // Error filter plot setup + error_filter_plot.data = [ + { mode: "lines", name: "Unfiltered error (Tar-Act)", meta: "Unfiltered error (Tar-Act)", + showlegend: true, hovertemplate: "" }, + { mode: "lines", name: "Filtered error (PID Err)", meta: "Filtered error (PID Err)", + showlegend: true, hovertemplate: "" } + ] + error_filter_plot.layout = { + xaxis: {title: {text: frequency_scale.label }, type: "linear", zeroline: false, showline: true, mirror: true}, + yaxis: {title: {text: amplitude_scale.label }, zeroline: false, showline: true, mirror: true }, + showlegend: true, + legend: {itemclick: false, itemdoubleclick: false }, + margin: { b: 50, l: 50, r: 50, t: 20 }, + } + plot = document.getElementById("ErrorFilterPlot") + Plotly.purge(plot) + Plotly.newPlot(plot, error_filter_plot.data, error_filter_plot.layout, {displaylogo: false}); + link_plots() } @@ -366,10 +413,14 @@ function link_plots() { document.getElementById("FFTPlot").removeAllListeners("plotly_relayout"); document.getElementById("Spectrogram").removeAllListeners("plotly_relayout"); document.getElementById("step_plot").removeAllListeners("plotly_relayout"); + document.getElementById("TargetFilterPlot").removeAllListeners("plotly_relayout"); + document.getElementById("ErrorFilterPlot").removeAllListeners("plotly_relayout"); // Link all frequency axis link_plot_axis_range([["FFTPlot", "x", "", fft_plot], + ["TargetFilterPlot", "x", "", target_filter_plot], + ["ErrorFilterPlot", "x", "", error_filter_plot], ["Spectrogram", "y", "", Spectrogram]]) // Link time axis @@ -383,12 +434,14 @@ function link_plots() { ["TimeOutputs", TimeOutputs], ["FFTPlot", fft_plot], ["step_plot", step_plot], - ["Spectrogram", Spectrogram]]) + ["Spectrogram", Spectrogram], + ["TargetFilterPlot", target_filter_plot], + ["ErrorFilterPlot", error_filter_plot]]) } // Add data sets to FFT plot -const plot_types = ["Target", "Actual", "Error", "P", "I", "D", "FF", "Output"] +const plot_types = ["Target", "Actual", "Error", "P", "I", "D", "FF", "D FF", "Output"] function get_FFT_data_index(set_num, plot_type) { return set_num*plot_types.length + plot_type } @@ -518,6 +571,7 @@ function clear_calculation() { } } PID_log_messages[i].sets.FFT = null + PID_log_messages[i].filter_fft = null } } @@ -533,6 +587,152 @@ function calculate() { run_batch_fft(PID_log_messages[i].sets) } + calculate_filter_ffts() + +} + +// Compute per-window FFT data for filter comparison plots. +// Stores results on each PIDR/PIDP/PIDY entry as .filter_fft +function calculate_filter_ffts() { + + const window_size = parseInt(document.getElementById("FFTWindow_size").value) + if (!Number.isInteger(Math.log2(window_size))) { + return + } + + const window_overlap = 0.5 + const window_spacing = Math.round(window_size * (1 - window_overlap)) + const windowing_fn = hanning(window_size) + const window_correction_obj = window_correction_factors(windowing_fn) + const fft_lib = new FFTJS(window_size) + const real_len = real_length(window_size) + + // FFT normalization scale (same as run_fft) + const end_scale = 1 / window_size + const mid_scale = 2 / window_size + const norm_scale = new Array(real_len) + norm_scale[0] = end_scale + for (let j = 1; j < real_len - 1; j++) norm_scale[j] = mid_scale + norm_scale[real_len - 1] = end_scale + + // Compute per-window FFT for a flat list of {time, data, sample_rate} batches. + // Returns {bins, average_sample_rate, window_size, correction, time, spectra} or null. + function run_windowed_fft(batches) { + let sample_rate_sum = 0, sample_rate_count = 0 + for (const b of batches) { + if (b.data.length >= window_size) { + sample_rate_sum += b.sample_rate + sample_rate_count++ + } + } + if (sample_rate_sum === 0) return null + + const sample_time = sample_rate_count / sample_rate_sum + const fft_buf = fft_lib.createComplexArray() + const time_arr = [] + const spectra_arr = [] + + for (const b of batches) { + if (b.data.length < window_size) continue + const num_win = Math.floor((b.data.length - window_size) / window_spacing) + 1 + for (let i = 0; i < num_win; i++) { + const ws = i * window_spacing + time_arr.push(b.time[0] + (ws + window_size * 0.5) * sample_time) + + const windowed = new Array(window_size) + for (let j = 0; j < window_size; j++) windowed[j] = b.data[ws + j] * windowing_fn[j] + + fft_lib.realTransform(fft_buf, windowed) + + const spectrum = [new Array(real_len), new Array(real_len)] + for (let j = 0; j < real_len; j++) { + spectrum[0][j] = fft_buf[j * 2] * norm_scale[j] + spectrum[1][j] = fft_buf[j * 2 + 1] * norm_scale[j] + } + spectra_arr.push(spectrum) + } + } + + if (time_arr.length === 0) return null + + return { + bins: rfft_freq(window_size, sample_time), + average_sample_rate: 1 / sample_time, + window_size: window_size, + correction: window_correction_obj, + time: time_arr, + spectra: spectra_arr + } + } + + // Collect flat batch list from sets array, optionally transforming data. + // key_fn: string key name or function(batch) => Array + // unit_scale: numeric multiplier applied to the data, or null/1 for no scaling + function get_batches(sets_arr, key_fn, unit_scale) { + const batches = [] + if (sets_arr == null) return batches + for (const set of sets_arr) { + if (set == null) continue + for (const batch of set) { + let d = typeof key_fn === 'function' ? key_fn(batch) : batch[key_fn] + if (!d) continue + if (unit_scale != null && unit_scale !== 1.0) { + d = array_scale(d, unit_scale) + } + batches.push({ time: batch.time, data: d, sample_rate: batch.sample_rate }) + } + } + return batches + } + + // Compute for each PIDR / PIDP / PIDY entry + for (const pid_entry of PID_log_messages) { + const id = pid_entry.id[0] + if (!["PIDR", "PIDP", "PIDY"].includes(id)) continue + + if (!pid_entry.have_data) { + pid_entry.filter_fft = null + continue + } + + // Axis letter: R, P, or Y + const axis = id.slice(-1) + + // Find corresponding RATE_x entry (may not exist for all vehicle types) + const rate_entry = PID_log_messages.find( + m => m.id[0] === "RATE" && m.id[1] === axis && m.have_data + ) || null + + pid_entry.filter_fft = {} + + // Unfiltered target: RATE.RDes / PDes / YDes, scaled to PID units + if (rate_entry != null) { + pid_entry.filter_fft.tar_unfiltered = run_windowed_fft( + get_batches(rate_entry.sets, "Tar", pid_entry.unitScale) + ) + } else { + pid_entry.filter_fft.tar_unfiltered = null + } + + // Filtered target: PIDR.Tar (already unit-scaled during load) + pid_entry.filter_fft.tar_filtered = run_windowed_fft( + get_batches(pid_entry.sets, "Tar", null) + ) + + // Unfiltered error: Tar - Act (before error filter, both already unit-scaled) + pid_entry.filter_fft.err_unfiltered = run_windowed_fft( + get_batches(pid_entry.sets, function(batch) { + const arr = new Array(batch.Tar.length) + for (let i = 0; i < arr.length; i++) arr[i] = batch.Tar[i] - batch.Act[i] + return arr + }, null) + ) + + // Filtered error: PIDR.Err (already unit-scaled during load) + pid_entry.filter_fft.err_filtered = run_windowed_fft( + get_batches(pid_entry.sets, "Err", null) + ) + } } // Get configured amplitude scale @@ -618,13 +818,13 @@ function add_param_sets() { set_cell_style(item) const names = get_PID_param_names(PID.params.prefix) - for (const [name, param_string] of Object.entries(names)) { + for (const [name, param] of Object.entries(names)) { let item = document.createElement("th") header.appendChild(item) set_cell_style(item) - item.appendChild(document.createTextNode(name.replace("_", " "))) - item.setAttribute('title', param_string) + item.appendChild(document.createTextNode(param.title)) + item.setAttribute('title', param.name) } // Add line for each param set @@ -667,21 +867,21 @@ function add_param_sets() { checkbox.disabled = (valid_sets == 1) || !valid item.appendChild(checkbox) - for (const name of Object.keys(names)) { + for (const [key, param] of Object.entries(names)) { let item = document.createElement("td") row.appendChild(item) set_cell_style(item, color) - const value = set[name] + const value = set[key] if (value == null) { continue } - const text = document.createTextNode(value.toFixed(4)) + const text = document.createTextNode(value.toFixed(param.decimalPlaces)) let changed = false if (i > 0) { - const last_value = PID.params.sets[i-1][name] + const last_value = PID.params.sets[i-1][key] if (value != last_value) { changed = true } @@ -724,6 +924,11 @@ function add_param_sets() { document.getElementById("Spec_D").disabled = !have_all document.getElementById("Spec_FF").disabled = !have_all + // DFF is only available in newer firmware logs + const have_DFF = have_all && PID.sets.some(set => set != null && set.some(batch => batch.DFF != null)) + document.getElementById("PIDX_DFF").disabled = !have_DFF + document.getElementById("Spec_DFF").disabled = !have_DFF + // Uncheck any that are disabled if (!have_all) { document.getElementById("PIDX_Err").checked = false @@ -731,17 +936,20 @@ function add_param_sets() { document.getElementById("PIDX_I").checked = false document.getElementById("PIDX_D").checked = false document.getElementById("PIDX_FF").checked = false + } + if (!have_DFF) { + document.getElementById("PIDX_DFF").checked = false + } - // Change to Out on spectrogram if disabled option is set - const disabled_checked = document.getElementById("Spec_Err").checked || - document.getElementById("Spec_P").checked || - document.getElementById("Spec_I").checked || - document.getElementById("Spec_D").checked || - document.getElementById("Spec_FF").checked - if (disabled_checked) { - document.getElementById("Spec_Out").checked = true - } - + // Change to Out on spectrogram if disabled option is set + const disabled_checked = document.getElementById("Spec_Err").checked || + document.getElementById("Spec_P").checked || + document.getElementById("Spec_I").checked || + document.getElementById("Spec_D").checked || + document.getElementById("Spec_FF").checked || + document.getElementById("Spec_DFF").checked + if ((!have_all || !have_DFF) && disabled_checked) { + document.getElementById("Spec_Out").checked = true } @@ -806,7 +1014,10 @@ function redraw() { if ("FF" in set[i]) { TimeOutputs.data[3].y = TimeOutputs.data[3].y.concat(set[i].FF) } - TimeOutputs.data[4].y = TimeOutputs.data[4].y.concat(set[i].Out) + if (set[i].DFF != null) { + TimeOutputs.data[4].y = TimeOutputs.data[4].y.concat(set[i].DFF) + } + TimeOutputs.data[5].y = TimeOutputs.data[5].y.concat(set[i].Out) } } @@ -841,6 +1052,7 @@ function redraw() { Plotly.redraw("TimeOutputs") if (PID.sets.FFT == null) { + redraw_filter_plots() return } @@ -920,6 +1132,8 @@ function redraw() { redraw_Spectrogram() redraw_step() + + redraw_filter_plots() } function redraw_Spectrogram() { @@ -1196,6 +1410,238 @@ function redraw_step() { } +// Redraw the target-filter and error-filter plots. +// Only shown when a Roll/Pitch/Yaw axis is selected and filter FFT data is available. +function redraw_filter_plots() { + + if ((PID_log_messages == null) || !PID_log_messages.have_data) { + document.getElementById("FilterPlotsSection").style.display = "none" + return + } + + const PID = PID_log_messages[get_axis_index()] + const id = PID.id[0] + + // Resolve the PIDR/PIDP/PIDY entry that carries the filter_fft data + let pid_entry = null + if (["PIDR", "PIDP", "PIDY"].includes(id)) { + pid_entry = PID + } else if (id === "RATE") { + // e.g. id[1] = "R" -> look for "PIDR" + const axis = PID.id[1] + pid_entry = PID_log_messages.find(m => m.id[0] === "PID" + axis && m.have_data) || null + } + + const show = (pid_entry != null) && (pid_entry.filter_fft != null) + document.getElementById("FilterPlotsSection").style.display = show ? "" : "none" + if (!show) return + + // --- Enable / disable frequency-line checkboxes --- + const filt_params = PID_log_messages.filt_params || {} + let has_lpf_tar = false, has_notch_tar = false + let has_lpf_err = false, has_notch_err = false + for (let i = 0; i < pid_entry.params.sets.length; i++) { + if (pid_entry.sets == null || pid_entry.sets[i] == null) continue + const s = pid_entry.params.sets[i] + if (s.Target_filter != null && s.Target_filter !== 0) has_lpf_tar = true + if (s.Error_filter != null && s.Error_filter !== 0) has_lpf_err = true + const ntf = s.Notch_target + if (ntf != null && ntf !== 0 && filt_params[Math.round(ntf)]?.NOTCH_FREQ) has_notch_tar = true + const nef = s.Notch_error + if (nef != null && nef !== 0 && filt_params[Math.round(nef)]?.NOTCH_FREQ) has_notch_err = true + } + const tar_lpf_cb = document.getElementById("Tar_ShowLPF") + const tar_notch_cb = document.getElementById("Tar_ShowNotch") + const err_lpf_cb = document.getElementById("Err_ShowLPF") + const err_notch_cb = document.getElementById("Err_ShowNotch") + // Default to checked the first time each checkbox becomes enabled (disabled → enabled transition) + if (tar_lpf_cb.disabled && has_lpf_tar) tar_lpf_cb.checked = true + if (tar_notch_cb.disabled && has_notch_tar) tar_notch_cb.checked = true + if (err_lpf_cb.disabled && has_lpf_err) err_lpf_cb.checked = true + if (err_notch_cb.disabled && has_notch_err) err_notch_cb.checked = true + tar_lpf_cb.disabled = !has_lpf_tar; if (!has_lpf_tar) tar_lpf_cb.checked = false + tar_notch_cb.disabled = !has_notch_tar; if (!has_notch_tar) tar_notch_cb.checked = false + err_lpf_cb.disabled = !has_lpf_err; if (!has_lpf_err) err_lpf_cb.checked = false + err_notch_cb.disabled = !has_notch_err; if (!has_notch_err) err_notch_cb.checked = false + + // Build vertical-line shapes (and notch bandwidth shading) for a filter plot. + // lpf_key / notch_key are property names in pid_entry.params.sets[i]. + function build_filter_shapes(lpf_key, notch_key, show_lpf, show_notch) { + const shapes = [] + const lpf_freqs = new Set() + const notch_data = new Map() // freq_Hz -> q (deduplicated by centre frequency) + for (let i = 0; i < pid_entry.params.sets.length; i++) { + if (pid_entry.sets == null || pid_entry.sets[i] == null) continue + const s = pid_entry.params.sets[i] + if (show_lpf) { + const f = s[lpf_key] + if (f != null && f !== 0) lpf_freqs.add(f) + } + if (show_notch) { + const n = s[notch_key] + if (n != null && n !== 0) { + const fp = filt_params[Math.round(n)] + if (fp?.NOTCH_FREQ && fp.NOTCH_FREQ !== 0) + notch_data.set(fp.NOTCH_FREQ, fp.NOTCH_Q ?? 1) + } + } + } + for (const f of lpf_freqs) { + const x = frequency_scale.fun([f])[0] + shapes.push({ type: 'line', x0: x, x1: x, y0: 0, y1: 1, yref: 'paper', + line: { color: 'rgba(30,120,255,0.75)', dash: 'dash', width: 1.5 } }) + } + for (const [f, q] of notch_data) { + const bw = f / Math.max(q, 0.01) + const x_lo = frequency_scale.fun([Math.max(f - bw, 0.01)])[0] + const x_hi = frequency_scale.fun([f + bw])[0] + const x_ctr = frequency_scale.fun([f])[0] + // Grey band covering the notch bandwidth + shapes.push({ type: 'rect', x0: x_lo, x1: x_hi, y0: 0, y1: 1, yref: 'paper', + fillcolor: 'rgba(150,150,150,0.2)', line: { width: 0 }, layer: 'below' }) + // Centre frequency line + shapes.push({ type: 'line', x0: x_ctr, x1: x_ctr, y0: 0, y1: 1, yref: 'paper', + line: { color: 'rgba(220,80,0,0.75)', dash: 'dot', width: 2 } }) + } + return shapes + } + + const fft_cache = pid_entry.filter_fft + + // Compute a mean amplitude spectrum for a fft_data object over the selected time range. + // Returns {x, y} ready for Plotly, or {x:[], y:[]} if no data. + function compute_mean_spectrum(fft_data) { + if (fft_data == null || fft_data.time.length === 0) return { x: [], y: [] } + + const FFT_resolution = fft_data.average_sample_rate / fft_data.window_size + const window_corr = amplitude_scale.window_correction(fft_data.correction, FFT_resolution) + const scaled_bins = frequency_scale.fun(fft_data.bins) + + const start_idx = find_start_index(fft_data.time) + const end_idx = find_end_index(fft_data.time) + 1 + const mean_len = end_idx - start_idx + if (mean_len <= 0 || start_idx >= fft_data.spectra.length) return { x: [], y: [] } + + const num_bins = fft_data.spectra[start_idx][0].length + let mean = new Array(num_bins).fill(0) + for (let k = start_idx; k < end_idx; k++) { + mean = array_add(mean, amplitude_scale.fun(complex_abs(fft_data.spectra[k]))) + } + + const corrected = array_scale(mean, window_corr / mean_len) + return { x: scaled_bins, y: amplitude_scale.scale(corrected) } + } + + const hovertemplate = "%{meta}
" + frequency_scale.hover("x") + "
" + amplitude_scale.hover("y") + + // --- Target filter plot --- + const tar_unfilt = compute_mean_spectrum(fft_cache.tar_unfiltered) + const tar_filt = compute_mean_spectrum(fft_cache.tar_filtered) + + target_filter_plot.data[0].x = tar_unfilt.x + target_filter_plot.data[0].y = tar_unfilt.y + target_filter_plot.data[0].hovertemplate = hovertemplate + + target_filter_plot.data[1].x = tar_filt.x + target_filter_plot.data[1].y = tar_filt.y + target_filter_plot.data[1].hovertemplate = hovertemplate + + target_filter_plot.layout.xaxis.type = frequency_scale.type + target_filter_plot.layout.xaxis.title.text = frequency_scale.label + target_filter_plot.layout.yaxis.title.text = amplitude_scale.label + target_filter_plot.layout.shapes = build_filter_shapes( + "Target_filter", "Notch_target", + tar_lpf_cb.checked, tar_notch_cb.checked) + + Plotly.redraw("TargetFilterPlot") + + // --- Target filter info --- + render_filter_info("TargetFilterInfo", pid_entry, "Target_filter", "Notch_target") + + // --- Error filter plot --- + const err_unfilt = compute_mean_spectrum(fft_cache.err_unfiltered) + const err_filt = compute_mean_spectrum(fft_cache.err_filtered) + + error_filter_plot.data[0].x = err_unfilt.x + error_filter_plot.data[0].y = err_unfilt.y + error_filter_plot.data[0].hovertemplate = hovertemplate + + error_filter_plot.data[1].x = err_filt.x + error_filter_plot.data[1].y = err_filt.y + error_filter_plot.data[1].hovertemplate = hovertemplate + + error_filter_plot.layout.xaxis.type = frequency_scale.type + error_filter_plot.layout.xaxis.title.text = frequency_scale.label + error_filter_plot.layout.yaxis.title.text = amplitude_scale.label + error_filter_plot.layout.shapes = build_filter_shapes( + "Error_filter", "Notch_error", + err_lpf_cb.checked, err_notch_cb.checked) + + Plotly.redraw("ErrorFilterPlot") + + // --- Error filter info --- + render_filter_info("ErrorFilterInfo", pid_entry, "Error_filter", "Notch_error") +} + +// Populate a filter-info div with LPF cut-off and optional notch parameters. +// lpf_key / notch_key are property names from get_PID_param_names(). +function render_filter_info(element_id, pid_entry, lpf_key, notch_key) { + const el = document.getElementById(element_id) + if (!el) return + + const filt_params = (PID_log_messages.filt_params) || {} + const sets = pid_entry.params.sets + const prefix = pid_entry.params.prefix || "" + + // Collect rows, one per param set that was actually logged (have data) + const rows = [] + for (let i = 0; i < sets.length; i++) { + if (pid_entry.sets == null || pid_entry.sets[i] == null) continue + + const set = sets[i] + const lpf_val = set[lpf_key] + const notch_idx = set[notch_key] + + const lpf_param_name = prefix + (lpf_key === "Target_filter" ? "FLTT" : "FLTE") + const notch_param_name = prefix + (notch_key === "Notch_target" ? "NTF" : "NEF") + + let html = "" + + // LPF line + if (lpf_val != null) { + html += `${lpf_param_name} = ${lpf_val.toFixed(1)} Hz — 1st-order LPF cut-off frequency` + } + + // Notch line (only when index != 0) + if (notch_idx != null && notch_idx !== 0) { + const n = Math.round(notch_idx) + const fp = filt_params[n] || {} + const freq = fp.NOTCH_FREQ != null ? fp.NOTCH_FREQ.toFixed(1) + " Hz" : "?" + const q = fp.NOTCH_Q != null ? fp.NOTCH_Q.toFixed(2) : "?" + const att = fp.NOTCH_ATT != null ? fp.NOTCH_ATT.toFixed(1) + " dB" : "?" + html += `
${notch_param_name} = ${n} — ` + html += `FILT${n}_NOTCH_FREQ = ${freq},  ` + html += `FILT${n}_NOTCH_Q = ${q},  ` + html += `FILT${n}_NOTCH_ATT = ${att}` + } + + rows.push({ label: sets.length > 1 ? `Test ${i + 1}` : null, html }) + } + + if (rows.length === 0) { el.innerHTML = ""; return } + + // Check whether all rows are identical (skip test labels in that case) + const all_same = rows.every(r => r.html === rows[0].html) + + let out = "" + for (const row of rows) { + if (!all_same && row.label) out += `${row.label}: ` + out += row.html + if (!all_same) out += "
" + } + el.innerHTML = out +} + // Update lines that are shown in FFT plot function update_hidden(source) { @@ -1204,7 +1650,7 @@ function update_hidden(source) { var index for (let j = 0; j < fft_keys.length; j++) { const key = fft_keys[j] - if (id.endsWith(key)) { + if (id.endsWith("_" + key)) { index = j break } @@ -1296,15 +1742,68 @@ function time_range_changed() { } function get_PID_param_names(prefix) { - return { KP: prefix + "P", - KI: prefix + "I", - KD: prefix + "D", - FF: prefix + "FF", - I_max: prefix + "IMAX", - Target_filter: prefix + "FLTT", - Error_filter: prefix + "FLTE", - D_filter: prefix + "FLTD", - Slew_max: prefix + "SMAX"} + return { + KP: { + title: "KP", + name: prefix + "P", + decimalPlaces: 4, + }, + KI: { + title: "KI", + name: prefix + "I", + decimalPlaces: 4, + }, + KD: { + title: "KD", + name: prefix + "D", + decimalPlaces: 4, + }, + FF: { + title: "KFF", + name: prefix + "FF", + decimalPlaces: 4, + }, + D_FF: { + title: "KDFF", + name: prefix + "D_FF", + decimalPlaces: 4, + }, + I_max: { + title: "I Max", + name: prefix + "IMAX", + decimalPlaces: 4, + }, + Target_filter: { + title: "Target Filter (Hz)", + name: prefix + "FLTT", + decimalPlaces: 4, + }, + Notch_target: { + title: "Target Notch Index", + name: prefix + "NTF", + decimalPlaces: 0, + }, + Error_filter: { + title: "Error Filter (Hz)", + name: prefix + "FLTE", + decimalPlaces: 4, + }, + Notch_error: { + title: "Error Notch Index", + name: prefix + "NEF", + decimalPlaces: 0, + }, + D_filter: { + title: "D Filter (Hz)", + name: prefix + "FLTD", + decimalPlaces: 4, + }, + Slew_max: { + title: "Slew Max", + name: prefix + "SMAX", + decimalPlaces: 4, + } + } } // Split use the given time array to return split points in log data @@ -1444,6 +1943,18 @@ async function load(log_file) { // Load params, split for any changes const PARM = log.get('PARM') + + // Scan for FILTn_NOTCH_FREQ / _Q / _ATT parameters (global, not per-axis) + PID_log_messages.filt_params = {} + for (let j = 0; j < PARM.Name.length; j++) { + const match = PARM.Name[j].match(/^FILT(\d+)_(NOTCH_FREQ|NOTCH_Q|NOTCH_ATT)$/) + if (match) { + const idx = parseInt(match[1]) + if (!PID_log_messages.filt_params[idx]) PID_log_messages.filt_params[idx] = {} + PID_log_messages.filt_params[idx][match[2]] = PARM.Value[j] + } + } + for (let i = 0; i < PID_log_messages.length; i++) { PID_log_messages[i].params = { prefix: null, sets: [] } for (const prefix of PID_log_messages[i].prefixes) { @@ -1459,14 +1970,14 @@ async function load(log_file) { let last_set_end for (let j = 0; j < PARM.Name.length; j++) { const param_name = PARM.Name[j] - for (const [name, param_string] of Object.entries(names)) { - if (param_name !== param_string) { + for (const [key, param] of Object.entries(names)) { + if (param_name !== param.name) { continue } const time = PARM.TimeUS[j] * US2S const value = PARM.Value[j] found_param = true - if (param_values[name] != null && (param_values[name] != value)) { + if (param_values[key] != null && (param_values[key] != value)) { if ((last_set_end == null) || (time - last_set_end > 1.0)) { // First param change for a second last_set_end = time @@ -1479,12 +1990,12 @@ async function load(log_file) { } else { // Very recent param change, combine with latest set, this leaves gap between sets - param_values[name] = value + param_values[key] = value param_values.start_time = time } } - param_values[name] = value + param_values[key] = value break } } @@ -1545,7 +2056,9 @@ async function load(log_file) { P: Array.from(log_msg.P.slice(batch.batch_start, batch.batch_end)), I: Array.from(log_msg.I.slice(batch.batch_start, batch.batch_end)), D: Array.from(log_msg.D.slice(batch.batch_start, batch.batch_end)), - FF: Array.from(log_msg.FF.slice(batch.batch_start, batch.batch_end))}) + FF: Array.from(log_msg.FF.slice(batch.batch_start, batch.batch_end)), + DFF: ("DFF" in log_msg) ? Array.from(log_msg.DFF.slice(batch.batch_start, batch.batch_end)) : null, + }) } } @@ -1610,7 +2123,7 @@ async function load(log_file) { const len = batch.P.length batch.Out = new Array(len) for (let i = 0; i