diff --git a/Tools/autotest/web-firmware/Tools/FilterTool/FileSaver.js b/Tools/autotest/web-firmware/Tools/FilterTool/FileSaver.js new file mode 100644 index 0000000000..5d204aee15 --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterTool/FileSaver.js @@ -0,0 +1,171 @@ +/* +* FileSaver.js +* A saveAs() FileSaver implementation. +* +* By Eli Grey, http://eligrey.com +* +* License : https://github.com/eligrey/FileSaver.js/blob/master/LICENSE.md (MIT) +* source : http://purl.eligrey.com/github/FileSaver.js +*/ + +// The one and only way of getting global scope in all environments +// https://stackoverflow.com/q/3277182/1008999 +var _global = typeof window === 'object' && window.window === window + ? window : typeof self === 'object' && self.self === self + ? self : typeof global === 'object' && global.global === global + ? global + : this + +function bom (blob, opts) { + if (typeof opts === 'undefined') opts = { autoBom: false } + else if (typeof opts !== 'object') { + console.warn('Deprecated: Expected third argument to be a object') + opts = { autoBom: !opts } + } + + // prepend BOM for UTF-8 XML and text/* types (including HTML) + // note: your browser will automatically convert UTF-16 U+FEFF to EF BB BF + if (opts.autoBom && /^\s*(?:text\/\S*|application\/xml|\S*\/\S*\+xml)\s*;.*charset\s*=\s*utf-8/i.test(blob.type)) { + return new Blob([String.fromCharCode(0xFEFF), blob], { type: blob.type }) + } + return blob +} + +function download (url, name, opts) { + var xhr = new XMLHttpRequest() + xhr.open('GET', url) + xhr.responseType = 'blob' + xhr.onload = function () { + saveAs(xhr.response, name, opts) + } + xhr.onerror = function () { + console.error('could not download file') + } + xhr.send() +} + +function corsEnabled (url) { + var xhr = new XMLHttpRequest() + // use sync to avoid popup blocker + xhr.open('HEAD', url, false) + try { + xhr.send() + } catch (e) {} + return xhr.status >= 200 && xhr.status <= 299 +} + +// `a.click()` doesn't work for all browsers (#465) +function click (node) { + try { + node.dispatchEvent(new MouseEvent('click')) + } catch (e) { + var evt = document.createEvent('MouseEvents') + evt.initMouseEvent('click', true, true, window, 0, 0, 0, 80, + 20, false, false, false, false, 0, null) + node.dispatchEvent(evt) + } +} + +// Detect WebView inside a native macOS app by ruling out all browsers +// We just need to check for 'Safari' because all other browsers (besides Firefox) include that too +// https://www.whatismybrowser.com/guides/the-latest-user-agent/macos +var isMacOSWebView = _global.navigator && /Macintosh/.test(navigator.userAgent) && /AppleWebKit/.test(navigator.userAgent) && !/Safari/.test(navigator.userAgent) + +var saveAs = _global.saveAs || ( + // probably in some web worker + (typeof window !== 'object' || window !== _global) + ? function saveAs () { /* noop */ } + + // Use download attribute first if possible (#193 Lumia mobile) unless this is a macOS WebView + : ('download' in HTMLAnchorElement.prototype && !isMacOSWebView) + ? function saveAs (blob, name, opts) { + var URL = _global.URL || _global.webkitURL + var a = document.createElement('a') + name = name || blob.name || 'download' + + a.download = name + a.rel = 'noopener' // tabnabbing + + // TODO: detect chrome extensions & packaged apps + // a.target = '_blank' + + if (typeof blob === 'string') { + // Support regular links + a.href = blob + if (a.origin !== location.origin) { + corsEnabled(a.href) + ? download(blob, name, opts) + : click(a, a.target = '_blank') + } else { + click(a) + } + } else { + // Support blobs + a.href = URL.createObjectURL(blob) + setTimeout(function () { URL.revokeObjectURL(a.href) }, 4E4) // 40s + setTimeout(function () { click(a) }, 0) + } + } + + // Use msSaveOrOpenBlob as a second approach + : 'msSaveOrOpenBlob' in navigator + ? function saveAs (blob, name, opts) { + name = name || blob.name || 'download' + + if (typeof blob === 'string') { + if (corsEnabled(blob)) { + download(blob, name, opts) + } else { + var a = document.createElement('a') + a.href = blob + a.target = '_blank' + setTimeout(function () { click(a) }) + } + } else { + navigator.msSaveOrOpenBlob(bom(blob, opts), name) + } + } + + // Fallback to using FileReader and a popup + : function saveAs (blob, name, opts, popup) { + // Open a popup immediately do go around popup blocker + // Mostly only available on user interaction and the fileReader is async so... + popup = popup || open('', '_blank') + if (popup) { + popup.document.title = + popup.document.body.innerText = 'downloading...' + } + + if (typeof blob === 'string') return download(blob, name, opts) + + var force = blob.type === 'application/octet-stream' + var isSafari = /constructor/i.test(_global.HTMLElement) || _global.safari + var isChromeIOS = /CriOS\/[\d]+/.test(navigator.userAgent) + + if ((isChromeIOS || (force && isSafari) || isMacOSWebView) && typeof FileReader !== 'undefined') { + // Safari doesn't allow downloading of blob URLs + var reader = new FileReader() + reader.onloadend = function () { + var url = reader.result + url = isChromeIOS ? url : url.replace(/^data:[^;]*;/, 'data:attachment/file;') + if (popup) popup.location.href = url + else location = url + popup = null // reverse-tabnabbing #460 + } + reader.readAsDataURL(blob) + } else { + var URL = _global.URL || _global.webkitURL + var url = URL.createObjectURL(blob) + if (popup) popup.location = url + else location.href = url + popup = null // reverse-tabnabbing #460 + setTimeout(function () { URL.revokeObjectURL(url) }, 4E4) // 40s + } + } +) + +_global.saveAs = saveAs.saveAs = saveAs + +if (typeof module !== 'undefined') { + module.exports = saveAs; +} diff --git a/Tools/autotest/web-firmware/Tools/FilterTool/filters.js b/Tools/autotest/web-firmware/Tools/FilterTool/filters.js new file mode 100644 index 0000000000..94104b7ab6 --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterTool/filters.js @@ -0,0 +1,571 @@ +function calc_lowpass_alpha_dt(dt, cutoff_freq) +{ + if (dt <= 0.0 || cutoff_freq <= 0.0) { + return 1.0; + } + var rc = 1.0/(Math.PI*2*cutoff_freq); + return dt/(dt+rc); +} + +function LPF_1P(sample_rate,cutoff) { + this.reset = function(sample) { + this.value = sample; + } + if (cutoff <= 0) { + this.apply = function(sample) { + return sample; + } + return this; + } + this.alpha = calc_lowpass_alpha_dt(1.0/sample_rate,cutoff) + this.value = 0.0; + this.apply = function(sample) { + this.value += this.alpha * (sample - this.value); + return this.value; + } + return this; +} + +function DigitalBiquadFilter(sample_freq, cutoff_freq) { + this.delay_element_1 = 0; + this.delay_element_2 = 0; + this.cutoff_freq = cutoff_freq; + + if (cutoff_freq <= 0) { + // zero cutoff means pass-thru + this.reset = function(sample) { + } + this.apply = function(sample) { + return sample; + } + return this; + } + + var fr = sample_freq/cutoff_freq; + var ohm = Math.tan(Math.PI/fr); + var c = 1.0+2.0*Math.cos(Math.PI/4.0)*ohm + ohm*ohm; + + this.b0 = ohm*ohm/c; + this.b1 = 2.0*this.b0; + this.b2 = this.b0; + this.a1 = 2.0*(ohm*ohm-1.0)/c; + this.a2 = (1.0-2.0*Math.cos(Math.PI/4.0)*ohm+ohm*ohm)/c; + this.initialised = false; + + this.apply = function(sample) { + if (!this.initialised) { + this.reset(sample); + this.initialised = true; + } + var delay_element_0 = sample - this.delay_element_1 * this.a1 - this.delay_element_2 * this.a2; + var output = delay_element_0 * this.b0 + this.delay_element_1 * this.b1 + this.delay_element_2 * this.b2; + + this.delay_element_2 = this.delay_element_1; + this.delay_element_1 = delay_element_0; + return output; + } + + this.reset = function(sample) { + this.delay_element_1 = this.delay_element_2 = sample * (1.0 / (1 + this.a1 + this.a2)); + } + + return this; +} + +function sq(v) { + return v*v; +} + +function constrain_float(v,vmin,vmax) { + if (v < vmin) { + return vmin; + } + if (v > vmax) { + return vmax; + } + return v; +} + +function NotchFilter(sample_freq,center_freq_hz,bandwidth_hz,attenuation_dB) { + this.sample_freq = sample_freq; + this.center_freq_hz = center_freq_hz; + this.bandwidth_hz = bandwidth_hz; + this.attenuation_dB = attenuation_dB; + this.need_reset = true; + this.initialised = false; + + this.calculate_A_and_Q = function() { + this.A = Math.pow(10.0, -this.attenuation_dB / 40.0); + if (this.center_freq_hz > 0.5 * this.bandwidth_hz) { + var octaves = Math.log2(this.center_freq_hz / (this.center_freq_hz - this.bandwidth_hz / 2.0)) * 2.0; + this.Q = Math.sqrt(Math.pow(2.0, octaves)) / (Math.pow(2.0, octaves) - 1.0); + } else { + this.Q = 0.0; + } + } + + this.init_with_A_and_Q = function() { + if ((this.center_freq_hz > 0.0) && (this.center_freq_hz < 0.5 * this.sample_freq) && (this.Q > 0.0)) { + var omega = 2.0 * Math.PI * this.center_freq_hz / this.sample_freq; + var alpha = Math.sin(omega) / (2 * this.Q); + this.b0 = 1.0 + alpha*sq(this.A); + this.b1 = -2.0 * Math.cos(omega); + this.b2 = 1.0 - alpha*sq(this.A); + this.a0_inv = 1.0/(1.0 + alpha); + this.a1 = this.b1; + this.a2 = 1.0 - alpha; + this.initialised = true; + } else { + this.initialised = false; + } + } + + // check center frequency is in the allowable range + if ((center_freq_hz > 0.5 * bandwidth_hz) && (center_freq_hz < 0.5 * sample_freq)) { + this.calculate_A_and_Q(); + this.init_with_A_and_Q(); + } else { + this.initialised = false; + } + + this.apply = function(sample) { + if (!this.initialised || this.need_reset) { + // if we have not been initialised when return the input + // sample as output and update delayed samples + this.signal1 = sample; + this.signal2 = sample; + this.ntchsig = sample; + this.ntchsig1 = sample; + this.ntchsig2 = sample; + this.need_reset = false; + return sample; + } + this.ntchsig2 = this.ntchsig1; + this.ntchsig1 = this.ntchsig; + this.ntchsig = sample; + var output = (this.ntchsig*this.b0 + this.ntchsig1*this.b1 + this.ntchsig2*this.b2 - this.signal1*this.a1 - this.signal2*this.a2) * this.a0_inv; + this.signal2 = this.signal1; + this.signal1 = output; + return output; + } + + this.reset = function(sample) { + this.need_reset = true; + this.apply(sample); + } + + return this; +} + +function HarmonicNotchFilter(sample_freq,enable,mode,freq,bw,att,ref,fm_rat,hmncs,opts) { + this.notches = [] + var chained = 1; + var dbl = false; + if (opts & 1) { + dbl = true; + } + + this.reset = function(sample) { + for (n in this.notches) { + this.notches[n].reset(sample); + } + } + + if (enable <= 0) { + this.apply = function(sample) { + return sample; + } + return this; + } + + if (mode == 0) { + // fixed notch + } + if (mode == 1) { + var motors_throttle = Math.max(0,get_form("Throttle")); + var throttle_freq = freq * Math.max(fm_rat,Math.sqrt(motors_throttle / ref)); + freq = throttle_freq; + } + if (mode == 2) { + var rpm = get_form("RPM1"); + freq = Math.max(rpm/60.0,freq) * ref; + } + if (mode == 5) { + var rpm = get_form("RPM2"); + freq = Math.max(rpm/60.0,freq) * ref; + } + if (mode == 3) { + if (opts & 2) { + chained = get_form("NUM_MOTORS"); + } + var rpm = get_form("ESC_RPM"); + freq = Math.max(rpm/60.0,freq) * ref; + } + for (var n=0;n<8;n++) { + var fmul = n+1; + if (hmncs & (1< samples/10) { + integral_in += Math.abs(sample); + } + if (sample >= 0 && last_in < 0) { + crossing_in_i = i; + } + last_in = sample; + for (var j=0;j samples/10) { + integral_out += Math.abs(sample); + } + if (sample >= 0 && last_out < 0) { + crossing_sum += (i-crossing_in_i); + crossing_count ++; + } + last_out = sample; + } + var ratio = integral_out/integral_in; + var avg_lag = crossing_sum / crossing_count; + var lag = (360.0 * avg_lag * freq) / sample_rate; + return [ratio,lag]; +} + +var chart; + +function calculate_filter() { + var sample_rate = get_form("GyroSampleRate"); + var filters = [] + var freq_max = get_form("MaxFreq"); + var samples = 100000; + var freq_step = 1; + filters.push(new HarmonicNotchFilter(sample_rate, + get_form("INS_HNTCH_ENABLE"), + get_form("INS_HNTCH_MODE"), + get_form("INS_HNTCH_FREQ"), + get_form("INS_HNTCH_BW"), + get_form("INS_HNTCH_ATT"), + get_form("INS_HNTCH_REF"), + get_form("INS_HNTCH_FM_RAT"), + get_form("INS_HNTCH_HMNCS"), + get_form("INS_HNTCH_OPTS"))); + filters.push(new HarmonicNotchFilter(sample_rate, + get_form("INS_HNTC2_ENABLE"), + get_form("INS_HNTC2_MODE"), + get_form("INS_HNTC2_FREQ"), + get_form("INS_HNTC2_BW"), + get_form("INS_HNTC2_ATT"), + get_form("INS_HNTC2_REF"), + get_form("INS_HNTC2_FM_RAT"), + get_form("INS_HNTC2_HMNCS"), + get_form("INS_HNTC2_OPTS"))); + filters.push(new DigitalBiquadFilter(sample_rate,get_form("INS_GYRO_FILTER"))); + filters.push(new LPF_1P(sample_rate,get_form("FLTD"))); + + var num_notches = 0; + for (const f in filters) { + if ('notches' in filters[f]) { + num_notches += filters[f].notches.length; + } + } + samples /= (num_notches+1); + samples /= Math.max(1, freq_max/150.0); + // console.log("samples: " + samples) + + var attenuation = [] + var phase_lag = [] + var max_phase_lag = 0.0; + var phase_wrap = 0.0; + for (freq=1; freq<=freq_max; freq++) { + var v = run_filters(filters, freq, sample_rate, samples); + attenuation.push({x:freq, y:v[0]}); + var phase = v[1] + phase_wrap; + if (phase < max_phase_lag-100) { + // we have wrapped + phase_wrap += 360.0; + phase += 360.0; + } + phase_lag.push({x:freq, y:-phase}); + if (phase > max_phase_lag) { + max_phase_lag = phase; + } + } + max_phase_lag = Math.ceil((max_phase_lag+10)/10)*10; + max_phase_lag = Math.min(get_form("MaxPhaseLag"), max_phase_lag); + if (chart) { + chart.data.datasets[0].data = attenuation; + chart.data.datasets[1].data = phase_lag; + chart.options.scales.xAxes[0].ticks.max = freq_max; + chart.options.scales.yAxes[1].ticks.min = -max_phase_lag; + chart.options.scales.yAxes[1].ticks.max = 0; + chart.update(); + } else { + chart = new Chart("Attenuation", { + type : "scatter", + data: { + datasets: [ + { + label: 'Attenuation', + yAxisID: 'Attenuation', + pointRadius: 4, + borderColor: "rgba(0,0,255,1.0)", + pointBackgroundColor: "rgba(0,0,255,1.0)", + data: attenuation, + showLine: true, + fill: false + }, + { + label: 'PhaseLag', + yAxisID: 'PhaseLag', + pointRadius: 4, + borderColor: "rgba(255,0,0,1.0)", + pointBackgroundColor: "rgba(255,0,0,1.0)", + data: phase_lag, + showLine: true, + fill: false + } + ] + }, + options: { + legend: {display: true}, + scales: { + yAxes: [ + { + scaleLabel: { display: true, labelString: "Attenuation" }, + id: 'Attenuation', + position: 'left', + ticks: {min:0, max:1.0, stepSize:0.1} + }, + { + scaleLabel: { display: true, labelString: "Phase Lag(deg)" }, + id: 'PhaseLag', + position: 'right', + ticks: {min:-max_phase_lag, max:0, stepSize:10} + } + ], + xAxes: [ + { + scaleLabel: { display: true, labelString: "Frequency(Hz)" }, + ticks: {min:0, max:freq_max, stepSize:10} + } + ], + } + } + }); + } + //console.log("At 15Hz: " + attenuation[14].y + " " + phase_lag[14].y); + //console.log("At 30Hz: " + attenuation[29].y + " " + phase_lag[29].y); +} + +function setCookie(c_name, value) { + var exdate = new Date(); + var exdays = 365; + exdate.setDate(exdate.getDate() + exdays); + var c_value = escape(value) + ";expires=" + exdate.toUTCString(); + document.cookie = c_name + "=" + c_value + ";path=/"; +} + +function getCookie(c_name, def_value) { + let name = c_name + "="; + let decodedCookie = decodeURIComponent(document.cookie); + let ca = decodedCookie.split(';'); + for(let i = 0; i -1 ? cookie.substr(0, eqPos) : cookie; + document.cookie = name + "=;expires=Thu, 01 Jan 1970 00:00:00 GMT"; + } +} + +function save_parameters() { + var params = ""; + var inputs = document.forms["params"].getElementsByTagName("input"); + for (const v in inputs) { + var name = "" + inputs[v].name; + if (name.startsWith("INS_")) { + var value = inputs[v].value; + params += name + "=" + value + "\n"; + } + } + var blob = new Blob([params], { type: "text/plain;charset=utf-8" }); + saveAs(blob, "filter.param"); +} + +async function load_parameters(file) { + var text = await file.text(); + var lines = text.split('\n'); + for (i in lines) { + var line = lines[i]; + if (line.startsWith("INS_")) { + v = line.split(/[\s,=\t]+/); + if (v.length >= 2) { + var vname = v[0]; + var value = v[1]; + var fvar = document.getElementById(vname); + if (fvar) { + fvar.value = value; + console.log("set " + vname + "=" + value); + } + } + } + } + fill_docs(); + calculate_filter(); +} + +function fill_docs() +{ + var inputs = document.forms["params"].getElementsByTagName("input"); + for (const v in inputs) { + var name = inputs[v].name; + var doc = document.getElementById(name + ".doc"); + if (!doc) { + continue; + } + inputs[v].onchange = fill_docs; + var value = parseFloat(inputs[v].value); + if (name.endsWith("_ENABLE")) { + if (value >= 1) { + doc.innerHTML = "Enabled"; + } else { + doc.innerHTML = "Disabled"; + } + } else if (name.endsWith("_MODE")) { + switch (Math.floor(value)) { + case 0: + doc.innerHTML = "Fixed notch"; + break; + case 1: + doc.innerHTML = "Throttle"; + break; + case 2: + doc.innerHTML = "RPM Sensor 1"; + break; + case 3: + doc.innerHTML = "ESC Telemetry"; + break; + case 4: + doc.innerHTML = "Dynamic FFT"; + break; + case 5: + doc.innerHTML = "RPM Sensor 2"; + break; + default: + doc.innerHTML = "INVALID"; + break; + } + } else if (name.endsWith("_OPTS")) { + var ival = Math.floor(value); + var bits = []; + if (ival & 1) { + bits.push("Double Notch"); + } + if (ival & 2) { + bits.push("Dynamic Harmonic"); + } + if (ival & 4) { + bits.push("Loop Rate"); + } + if (ival & 8) { + bits.push("All IMUs Rate"); + } + doc.innerHTML = bits.join(", "); + } else if (name.endsWith("_HMNCS")) { + var ival = Math.floor(value); + var bits = []; + if (ival & 1) { + bits.push("Fundamental"); + } + if (ival & 2) { + bits.push("1st Harmonic"); + } + if (ival & 4) { + bits.push("2nd Harmonic"); + } + if (ival & 8) { + bits.push("3rd Harmonic"); + } + if (ival & 16) { + bits.push("4th Harmonic"); + } + if (ival & 32) { + bits.push("5th Harmonic"); + } + if (ival & 64) { + bits.push("6th Harmonic"); + } + doc.innerHTML = bits.join(", "); + } + + } +} diff --git a/Tools/autotest/web-firmware/Tools/FilterTool/index.html b/Tools/autotest/web-firmware/Tools/FilterTool/index.html new file mode 100644 index 0000000000..0f80369157 --- /dev/null +++ b/Tools/autotest/web-firmware/Tools/FilterTool/index.html @@ -0,0 +1,180 @@ + + + + +ArduPilot Filter Analysis + + + + + +

ArduPilot Filter Analysis

+ +The following form will display the attenuation and phase lag for an +ArduPilot 4.2 filter setup. + + +

+ + + + + +

+
+ INS Settings +

+ + +

+

+ + +

+

+ + +

+

+ + +

+
+
+ PID Settings +

+ + +

+
+
+ Throttle Based +

+ + +

+
+
+ ESC Telemetry +

+ + +

+

+ + +

+
+
+ RPM/EFI Based +

+ + +

+

+ + +

+
+
+ First Notch Filter +

+ + + +

+

+ + + +

+

+ + +

+

+ + +

+

+ + +

+

+ + +

+

+ + +

+

+ + + +

+

+ + + +

+
+
+ Second Notch Filter +

+ + + +

+

+ + + +

+

+ + +

+

+ + +

+

+ + +

+

+ + +

+

+ + +

+

+ + + +

+

+ + + +

+
+
+
+ + + + + diff --git a/Tools/autotest/web-firmware/images/filter.png b/Tools/autotest/web-firmware/images/filter.png new file mode 100644 index 0000000000..bc541d07d5 Binary files /dev/null and b/Tools/autotest/web-firmware/images/filter.png differ diff --git a/Tools/autotest/web-firmware/index.html b/Tools/autotest/web-firmware/index.html index 2e15d88e3c..4ceef07a40 100644 --- a/Tools/autotest/web-firmware/index.html +++ b/Tools/autotest/web-firmware/index.html @@ -98,6 +98,8 @@ The software and hardware we provide is only for use in unmanned alt="Companion">Companion - Companion Computer example code and Images

AP_PeriphAP_Periph - UAVCAN Peripheral Firmware

+FilterToolFilterTool - Filter Analysis Tool

Types of firmware available