Tools: Web: FilterReview: add notch tracking overlay

This commit is contained in:
Iampete1 2023-05-15 19:15:07 +01:00 committed by Andrew Tridgell
parent 6443eab963
commit 15601e4139
2 changed files with 513 additions and 15 deletions

View File

@ -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<key_name.length; j++) {
this.data[key_name[j]] = []
}
for (var i=0; i < log.messages[msg_name].time_boot_ms.length; i++) {
this.data.time[i] = log.messages[msg_name].time_boot_ms[i] / 1000
for (var j=0; j<key_name.length; j++) {
this.data[key_name[j]][i] = log.messages[msg_name][key_name[j]][i]
}
}
}
get_target_freq(config) {
if ((Object.keys(this.data).length == 0) || (this.data.time == null)) {
return
}
const start_index = find_start_index(this.data.time)
const end_index = find_end_index(this.data.time)
if (config.ref == 0) {
return { freq:[config.freq, config.freq], time:[this.data.time[start_index], this.data.time[end_index]] }
}
var freq = []
for (let j=start_index;j<end_index;j++) {
freq.push(this.get_target_freq_index(config, j))
}
return { freq:freq, time:this.data.time.slice(start_index, end_index) }
}
}
// Tracking mode specific classes
class StaticTarget extends NotchTarget {
constructor(log) {
super(null, null, null, "Static", 0)
}
get_target_freq(config) {
const start_time = parseFloat(document.getElementById("TimeStart").value)
const end_time = parseFloat(document.getElementById("TimeEnd").value)
return { freq:[config.freq, config.freq], time:[start_time, end_time] }
}
}
class ThrottleTarget extends NotchTarget {
constructor(log) {
super(log, "RATE", "AOut", "Throttle", 1)
}
get_target_freq_index(config, index) {
const motors_throttle = math.max(0, this.data.AOut[index])
return config.freq * math.max(config.min_ratio, math.sqrt(motors_throttle / config.ref))
}
}
class RPMTarget extends NotchTarget {
constructor(log, instance, mode_value) {
const key = "rpm" + instance
super(log, "RPM", key, "RPM" + instance, mode_value)
this.key = key
}
get_target_freq_index(config, index) {
const rpm = this.data[this.key][index]
if (rpm > 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<instances;i++) {
inst[i] = { rpm:null, time_ms:null }
}
for (let i=0;i<log.messages[msg].length;i++) {
// Update instance
const instance = log.messages[msg][i].Instance
const time_ms = log.messages[msg][i].time_boot_ms
inst[instance].rpm = log.messages[msg][i].RPM
inst[instance].time_ms = time_ms
// Invalidate any instance that has timed out
for (let j=0;j<instances;j++) {
if ((j != instance) && (inst[j].time_ms != null) && ((time_ms - inst[j].time_ms) > 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<instances;j++) {
if (inst[j].rpm != null) {
count++
sum += inst[j].rpm
}
if (inst[j].time_ms != null) {
expected_count++
}
}
if ((count > 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<instances;j++) {
inst[j].rpm = null
}
}
}
}
get_target_freq(config) {
if (this.data.length == 0) {
return
}
const dynamic = (config.options & (1<<1)) != 0
if (dynamic) {
// Tracking individual motor RPM's
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 }
}
// Tracking average motor rpm
const start_index = find_start_index(this.data.avg_time)
const end_index = find_end_index(this.data.avg_time)
let freq = this.data.avg_freq.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.avg_time.slice(start_index, end_index) }
}
}
class FFTTarget extends NotchTarget {
constructor(log) {
super(log, "FTN1", "PkAvg", "FFT", 4)
// Grab data from log, have to do it a little differently to get instances
const msg = "FTN2"
log.parseAtOffset(msg)
if ((log.messages[msg] == null) || (log.messages[msg].length == 0)) {
return
}
for (let i=0;i<3;i++) {
// FFT can track three peaks
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++) {
// Do noise weighting between axis to get a single frequency
// Same as `get_weighted_freq_hz` function in AP_GyroFFT
const energy_x = log.messages[inst_msg].EnX[j]
const energy_y = log.messages[inst_msg].EnY[j]
const freq_x = log.messages[inst_msg].PkX[j]
const freq_y = log.messages[inst_msg].PkY[j]
if ((energy_x > 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 = "<br>"
document.getElementById("Gyro1_info").innerHTML = "<br>"
document.getElementById("Gyro2_info").innerHTML = "<br>"
document.getElementById("Gyro0_FFT_info").innerHTML = "<br><br><br>"
document.getElementById("Gyro1_FFT_info").innerHTML = "<br><br><br>"
document.getElementById("Gyro2_FFT_info").innerHTML = "<br><br><br>"
@ -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<max_num_harmonics;j++) {
let name = (j == 0) ? "Fundamental" : "Harmonic " + j
Spectrogram.data.push({
type:"scatter",
mode: "lines",
line: { width: 4, dash: dash },
visible: false,
name: name,
meta: Group_name + "<br>" + 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 = "<extra></extra>%{meta}<br>" + "%{x:.2f} s<br>" + frequency_scale.hover("y")
for (let i=0;i<HNotch_setup.length;i++) {
// Plus one for the spectrogram plot
const plot_offset = i * max_num_harmonics + 1
// Hide all
for (let j=0;j<max_num_harmonics;j++) {
Spectrogram.data[plot_offset + j].visible = false
}
// Notch disabled, nothing to do
if ((HNotch_setup[i].enable <= 0) || (HNotch_setup[i].harmonics <= 0)) {
continue
}
// Find configured tracking source
let tracking
for (let j=0;j<tracking_methods.length;j++) {
if (HNotch_setup[i].mode == tracking_methods[j].mode_value) {
tracking = tracking_methods[j]
}
}
// Invalid mode
if (tracking == null) {
continue
}
const Group_name = "Notch " + (i+1) + ": " + tracking.name
const fundamental = tracking.get_target_freq(HNotch_setup[i])
// Enable each harmonic
for (let j=0;j<max_num_harmonics;j++) {
if ((HNotch_setup[i].harmonics & (1<<j)) == 0) {
continue
}
const harmonic_freq = math.dotMultiply(fundamental.freq, j+1)
if (harmonic_freq == null) {
break
}
Spectrogram.data[plot_offset + j].visible = true
Spectrogram.data[plot_offset + j].x = fundamental.time
Spectrogram.data[plot_offset + j].y = frequency_scale.fun(harmonic_freq)
Spectrogram.data[plot_offset + j].hovertemplate = tracking_hovertemplate
Spectrogram.data[plot_offset + j].legendgrouptitle.text = Group_name
}
}
Plotly.redraw("Spectrogram")
}
@ -520,12 +880,50 @@ function get_param_value(param_log, name) {
return value
}
function get_HNotch_param_names() {
prefix = ["INS_HNTCH_", "INS_HNTC2_"]
ret = []
for (let i = 0; i < prefix.length; i++) {
ret[i] = {enable: prefix[i] + "ENABLE",
mode: prefix[i] + "MODE",
freq: prefix[i] + "FREQ",
ref: prefix[i] + "REF",
min_ratio: prefix[i] + "FM_RAT",
harmonics: prefix[i] + "HMNCS",
options: prefix[i] + "OPTS"}
}
return ret
}
function HNotch_param_read() {
const HNotch_params = get_HNotch_param_names("INS_HNTCH_")
for (let i = 0; i < HNotch_params.length; i++) {
// Enable all params in group if enable is set
const enable_input = parseFloat(document.getElementById(HNotch_params[i].enable).value) > 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

View File

@ -15,6 +15,19 @@
<body>
<style>
div.plotly-notifier {
visibility: hidden;
}
label.parameter_input_label {
display: inline-block;
width: 160px;
text-align: right;
margin:5px 0px;
}
</style>
<table>
<tr>
<td style="width: 30px;"></td>
@ -210,7 +223,7 @@
<tr>
<td style="width: 60px;"></td>
<td>
<fieldset style="width:300px">
<fieldset style="width:300px;height:350px">
<legend>Spectrogram Options</legend>
<p>
<fieldset style="width:300px">
@ -250,7 +263,72 @@
</p>
</fieldset>
</td>
<td>
<fieldset style="width:300px;height:350px">
<legend>First Notch Filter</legend>
<p>
<label class="parameter_input_label" for="INS_HNTCH_ENABLE">INS_HNTCH_ENABLE</label>
<input id="INS_HNTCH_ENABLE" name="INS_HNTCH_ENABLE" type="number" step="1" value="0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTCH_MODE">INS_HNTCH_MODE</label>
<input id="INS_HNTCH_MODE" name="INS_HNTCH_MODE" type="number" step="1" value="0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTCH_FREQ">INS_HNTCH_FREQ</label>
<input id="INS_HNTCH_FREQ" name="INS_HNTCH_FREQ" type="number" step="0.1" value="0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTCH_REF">INS_HNTCH_REF</label>
<input id="INS_HNTCH_REF" name="INS_HNTCH_REF" type="number" step="0.01" value="0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTCH_FM_RAT">INS_HNTCH_FM_RAT</label>
<input id="INS_HNTCH_FM_RAT" name="INS_HNTCH_FM_RAT" type="number" step="0.01" value="1.0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTCH_HMNCS">INS_HNTCH_HMNCS</label>
<input id="INS_HNTCH_HMNCS" name="INS_HNTCH_HMNCS" type="number" step="1" value="1" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTCH_OPTS">INS_HNTCH_OPTS</label>
<input id="INS_HNTCH_OPTS" name="INS_HNTCH_OPTS" type="number" step="1" value="0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
</fieldset>
</td>
<td>
<fieldset style="width:300px;height:350px">
<legend>Second Notch Filter</legend>
<p>
<label class="parameter_input_label" for="INS_HNTC2_ENABLE">INS_HNTC2_ENABLE</label>
<input id="INS_HNTC2_ENABLE" name="INS_HNTC2_ENABLE" type="number" step="1" value="0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTC2_MODE">INS_HNTC2_MODE</label>
<input id="INS_HNTC2_MODE" name="INS_HNTC2_MODE" type="number" step="1" value="0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTC2_FREQ">INS_HNTC2_FREQ</label>
<input id="INS_HNTC2_FREQ" name="INS_HNTC2_FREQ" type="number" step="0.1" value="0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTC2_REF">INS_HNTC2_REF</label>
<input id="INS_HNTC2_REF" name="INS_HNTC2_REF" type="number" step="0.01" value="0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTC2_FM_RAT">INS_HNTC2_FM_RAT</label>
<input id="INS_HNTC2_FM_RAT" name="INS_HNTC2_FM_RAT" type="number" step="0.01" value="1.0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTC2_HMNCS">INS_HNTC2_HMNCS</label>
<input id="INS_HNTC2_HMNCS" name="INS_HNTC2_HMNCS" type="number" step="1" value="1" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
<p>
<label class="parameter_input_label" for="INS_HNTC2_OPTS">INS_HNTC2_OPTS</label>
<input id="INS_HNTC2_OPTS" name="INS_HNTC2_OPTS" type="number" step="1" value="0" onchange="HNotch_param_read(); redraw_Spectrogram();" style="width: 100px" disabled/>
</p>
</fieldset>
</td>
</tr>
</table>