mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 07:13:56 -04:00
Tools: Web: Add FFT batch log review tool
This commit is contained in:
parent
b4f9992ab7
commit
c1fe95f726
154
Tools/autotest/web-firmware/Tools/FilterReview/DecodeDevID.js
Normal file
154
Tools/autotest/web-firmware/Tools/FilterReview/DecodeDevID.js
Normal file
@ -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 }
|
||||
|
||||
}
|
828
Tools/autotest/web-firmware/Tools/FilterReview/FilterReview.js
Normal file
828
Tools/autotest/web-firmware/Tools/FilterReview/FilterReview.js
Normal file
@ -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;i++) {
|
||||
w[i] = 0.5 - 0.5 * Math.cos( (2*Math.PI*i) / (len - 1) )
|
||||
}
|
||||
return w
|
||||
}
|
||||
|
||||
// Calculate correction factors for linear and energy spectrum
|
||||
// linear: 1 / mean(w)
|
||||
// energy: 1 / sqrt(mean(w.^2))
|
||||
function window_correction_factors(w) {
|
||||
let window_sum = 0
|
||||
let window_sum_squared = 0
|
||||
for (let i=0;i<w.length;i++) {
|
||||
window_sum += w[i]
|
||||
window_sum_squared += w[i] * w[i]
|
||||
}
|
||||
const mean = window_sum / w.length
|
||||
const rms = Math.sqrt(window_sum_squared / w.length)
|
||||
return { linear: 1/mean, energy: 1/rms }
|
||||
}
|
||||
|
||||
// Frequency bins for given fft length and sample period
|
||||
function fft_freq(len, d) {
|
||||
freq = []
|
||||
for (var i=0;i<len;i++) {
|
||||
freq[i] = i
|
||||
if (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<num_windows;i++) {
|
||||
// Calculate the start of each window
|
||||
const window_start = i * window_spacing
|
||||
|
||||
// Get data and apply windowing function
|
||||
// Also take average time
|
||||
let x = []
|
||||
let y = []
|
||||
let z = []
|
||||
for (var j=0;j<window_size;j++) {
|
||||
x[j] = data.x[window_start + j] * windowing_function[j]
|
||||
y[j] = data.y[window_start + j] * windowing_function[j]
|
||||
z[j] = data.z[window_start + j] * windowing_function[j]
|
||||
}
|
||||
|
||||
// So caller can assign a time
|
||||
center_sample[i] = window_start + window_size * 0.5
|
||||
|
||||
// Run fft
|
||||
const p2_x = math.fft(x)
|
||||
const p2_y = math.fft(y)
|
||||
const p2_z = math.fft(z)
|
||||
|
||||
// Discard negative spectrum
|
||||
// Assumes a symmetrical spectrum due to running on real numbers
|
||||
let p1_x = p2_x.slice(0, positive_index)
|
||||
let p1_y = p2_y.slice(0, positive_index)
|
||||
let p1_z = p2_z.slice(0, positive_index)
|
||||
|
||||
// Take magnitude to give real number
|
||||
p1_x = math.abs(p1_x)
|
||||
p1_y = math.abs(p1_y)
|
||||
p1_z = math.abs(p1_z)
|
||||
|
||||
// double positive spectrum to account for discarded energy in the negative spectrum
|
||||
// Note that we don't scale the DC or Nyquist limit
|
||||
for (var j=1;j<positive_index-1;j++) {
|
||||
p1_x[j] *= 2
|
||||
p1_y[j] *= 2
|
||||
p1_z[j] *= 2
|
||||
}
|
||||
|
||||
// Scale all points by the window size
|
||||
fft_x[i] = [];
|
||||
fft_y[i] = [];
|
||||
fft_z[i] = [];
|
||||
for (var j=0;j<positive_index;j++) {
|
||||
fft_x[i][j] = p1_x[j] / window_size
|
||||
fft_y[i][j] = p1_y[j] / window_size
|
||||
fft_z[i][j] = p1_z[j] / window_size
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return {x:fft_x, y:fft_y, z:fft_z, center:center_sample}
|
||||
}
|
||||
|
||||
function run_batch_fft(data_set) {
|
||||
const num_batch = data_set.length
|
||||
const num_points = data_set[0].x.length
|
||||
|
||||
var sample_rate_sum = 0
|
||||
for (let i=0;i<num_batch;i++) {
|
||||
if ((data_set[i].x.length != num_points) || (data_set[i].y.length != num_points) || (data_set[i].z.length != num_points)) {
|
||||
console.log("Uneven batch sizes")
|
||||
return
|
||||
}
|
||||
sample_rate_sum += data_set[0].sample_rate
|
||||
}
|
||||
|
||||
// Average sample time
|
||||
const sample_time = num_batch / sample_rate_sum
|
||||
|
||||
// Must have at least one window
|
||||
const window_per_batch = math.max(parseInt(document.getElementById("FFTWindow").value), 1)
|
||||
|
||||
// Hard code 50% overlap
|
||||
const window_overlap = 0.5
|
||||
|
||||
// Calculate window size for given number of windows and overlap
|
||||
const window_size = math.floor(num_points / (1 + (window_per_batch - 1)*(1-window_overlap)))
|
||||
|
||||
const window_spacing = Math.round(window_size * (1 - window_overlap))
|
||||
const windowing_function = hanning(window_size)
|
||||
|
||||
// Get windowing correction factors for use later when plotting
|
||||
const window_correction = window_correction_factors(windowing_function)
|
||||
|
||||
// Get bins
|
||||
var bins = fft_freq(window_size, sample_time)
|
||||
|
||||
// discard negative spectrum
|
||||
const positive_index = Math.floor(window_size/2)
|
||||
bins = bins.slice(0, positive_index)
|
||||
|
||||
var x = []
|
||||
var y = []
|
||||
var z = []
|
||||
|
||||
var time = []
|
||||
|
||||
for (let i=0;i<num_batch;i++) {
|
||||
var ret = run_fft(data_set[i], window_size, window_spacing, windowing_function, positive_index)
|
||||
|
||||
for (let j=0;j<ret.center.length;j++) {
|
||||
time.push(data_set[i].sample_time + sample_time * ret.center[j])
|
||||
}
|
||||
|
||||
x.push(...ret.x)
|
||||
y.push(...ret.y)
|
||||
z.push(...ret.z)
|
||||
|
||||
}
|
||||
|
||||
return { bins: bins, time: time, average_sample_rate: 1/sample_time, window_size: window_size, correction: window_correction, x: x, y: y, z: z}
|
||||
}
|
||||
|
||||
// Setup plots with no data
|
||||
var Spectrogram = {}
|
||||
var fft_plot = {}
|
||||
function reset() {
|
||||
let axis = ["X" , "Y", "Z"]
|
||||
|
||||
// Disable all plot selection checkboxes
|
||||
for (let i = 0; i < 3; i++) {
|
||||
for (let j = 0; j < 3; j++) {
|
||||
document.getElementById("Gyro" + i + "Pre" + axis[j]).disabled = true
|
||||
document.getElementById("Gyro" + i + "Post" + axis[j]).disabled = true
|
||||
}
|
||||
}
|
||||
|
||||
// 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>"
|
||||
|
||||
// FFT plot setup
|
||||
fft_plot.data = []
|
||||
for (let i=0;i<Gyro_batch.length;i++) {
|
||||
// For each gyro
|
||||
const sensor_num = Gyro_batch[i].sensor_num + 1
|
||||
const pre_post = Gyro_batch[i].post_filter ? " Post-filter" : " Pre-filter"
|
||||
for (let j=0;j<3;j++) {
|
||||
// For each axis
|
||||
const name = sensor_num + axis[j] + pre_post
|
||||
fft_plot.data[i*3 + j] = { mode: "lines",
|
||||
visible: true,
|
||||
name: name,
|
||||
// this extra data allows us to put the name in the hover tool tip
|
||||
meta: name }
|
||||
}
|
||||
}
|
||||
|
||||
fft_plot.layout = {
|
||||
xaxis: {title: {text: ""}, type: "linear"},
|
||||
yaxis: {title: {text: ""}},
|
||||
}
|
||||
|
||||
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
|
||||
// Define type
|
||||
Spectrogram.data = [{
|
||||
type:"surface",
|
||||
colorbar: {title: {side: "right", text: ""}},
|
||||
contours: {
|
||||
x: {highlight: false},
|
||||
y: {highlight: false},
|
||||
z: {highlight: false},
|
||||
},
|
||||
hovertemplate: ""
|
||||
}];
|
||||
|
||||
// Define Layout
|
||||
Spectrogram.layout = {
|
||||
margin: {
|
||||
l: 10,
|
||||
r: 10,
|
||||
b: 10,
|
||||
t: 10,
|
||||
pad: 0
|
||||
},
|
||||
scene: {
|
||||
camera: {
|
||||
projection: { type: "orthographic"},
|
||||
eye: { x:0, y:0, z:1 },
|
||||
},
|
||||
aspectratio: { x:1.4, y:4.4, z:1.4 },
|
||||
xaxis: {showspikes: false, title: {text: ""}},
|
||||
yaxis: {showspikes: false, title: {text: "Time (s)"}, autorange: 'reversed'},
|
||||
zaxis: {showspikes: false, title: {text: ""}}
|
||||
},
|
||||
}
|
||||
|
||||
Plotly.purge("Spectrogram")
|
||||
Plotly.newPlot("Spectrogram", Spectrogram.data, Spectrogram.layout, {modeBarButtonsToRemove: ['resetCameraDefault3d', 'orbitRotation'], displaylogo: false});
|
||||
}
|
||||
|
||||
// Calculate if needed and re-draw, called from calculate button
|
||||
function re_calc() {
|
||||
|
||||
calculate()
|
||||
|
||||
redraw()
|
||||
}
|
||||
|
||||
// Force full re-calc on next run, on window size change
|
||||
function clear_calculation() {
|
||||
for (let i = 0; i < Gyro_batch.length; i++) {
|
||||
if (Gyro_batch[i] == null) {
|
||||
continue
|
||||
}
|
||||
Gyro_batch[i].FFT = null
|
||||
}
|
||||
}
|
||||
|
||||
// Re-run all FFT's
|
||||
function calculate() {
|
||||
for (let i = 0; i < Gyro_batch.length; i++) {
|
||||
if (Gyro_batch[i] == null) {
|
||||
continue
|
||||
}
|
||||
if (Gyro_batch[i].FFT == null) {
|
||||
Gyro_batch[i].FFT = run_batch_fft(Gyro_batch[i])
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Get configured amplitude scale
|
||||
function get_amplitude_scale() {
|
||||
|
||||
const use_DB = document.getElementById("ScaleLog").checked;
|
||||
const use_PSD = document.getElementById("SpectrumPSD").checked;
|
||||
|
||||
var ret = {}
|
||||
if (use_DB) {
|
||||
if (use_PSD) {
|
||||
ret.fun = function (x) { return 10.0 * Math.log10(x*x) }
|
||||
ret.label = "PSD (db)"
|
||||
} else {
|
||||
ret.fun = function (x) { return 10.0 * Math.log10(x) }
|
||||
ret.label = "Linear amplitude (db)"
|
||||
}
|
||||
ret.hover = function (axis) { return "%{" + axis + ":.2f} db" }
|
||||
|
||||
} else {
|
||||
if (use_PSD) {
|
||||
ret.fun = function (x) { return x*x }
|
||||
ret.label = "PSD"
|
||||
} else {
|
||||
ret.fun = function (x) { return x }
|
||||
ret.label = "Linear amplitude"
|
||||
}
|
||||
ret.hover = function (axis) { return "%{" + axis + ":.2f}" }
|
||||
|
||||
}
|
||||
|
||||
ret.use_PSD = use_PSD
|
||||
|
||||
return ret
|
||||
}
|
||||
|
||||
// Get configured frequency scale object
|
||||
function get_frequency_scale() {
|
||||
|
||||
const use_RPM = document.getElementById("freq_Scale_RPM").checked;
|
||||
|
||||
var ret = {}
|
||||
if (use_RPM) {
|
||||
ret.fun = function (x) { return 60.0 * x }
|
||||
ret.label = "RPM"
|
||||
ret.hover = function (axis) { return "%{" + axis + ":.2f} RPM" }
|
||||
|
||||
} else {
|
||||
ret.fun = function (x) { return x }
|
||||
ret.label = "Frequency (Hz)"
|
||||
ret.hover = function (axis) { return "%{" + axis + ":.2f} Hz" }
|
||||
}
|
||||
|
||||
ret.type = document.getElementById("freq_ScaleLog").checked ? "log" : "linear"
|
||||
|
||||
return ret
|
||||
}
|
||||
|
||||
// Helper to apply scale function to array
|
||||
function apply_scale_fun(data, scale_fun) {
|
||||
var x = []
|
||||
for (let i = 0; i < data.length; i++) {
|
||||
x[i] = scale_fun(data[i])
|
||||
}
|
||||
return x
|
||||
}
|
||||
|
||||
// Look through time array and return first index before start time
|
||||
function find_start_index(time) {
|
||||
const start_time = parseFloat(document.getElementById("TimeStart").value)
|
||||
|
||||
var start_index = 0
|
||||
for (j = 0; j<time.length; j++) {
|
||||
// Move forward start index while time is less than start time
|
||||
if (time[j] < start_time) {
|
||||
start_index = j
|
||||
}
|
||||
}
|
||||
return start_index
|
||||
}
|
||||
|
||||
// Look through time array and return first index after end time
|
||||
function find_end_index(time) {
|
||||
const end_time = parseFloat(document.getElementById("TimeEnd").value)
|
||||
|
||||
var end_index
|
||||
for (j = 0; j<time.length; j++) {
|
||||
// Move forward end index while time is less than end time
|
||||
if (time[j] <= end_time) {
|
||||
end_index = j
|
||||
}
|
||||
}
|
||||
return end_index + 1
|
||||
}
|
||||
|
||||
function redraw() {
|
||||
|
||||
// Graph config
|
||||
const amplitude_scale = get_amplitude_scale()
|
||||
const frequency_scale = get_frequency_scale()
|
||||
|
||||
// Setup axes
|
||||
fft_plot.layout.xaxis.type = frequency_scale.type
|
||||
fft_plot.layout.xaxis.title.text = frequency_scale.label
|
||||
fft_plot.layout.yaxis.title.text = amplitude_scale.label
|
||||
|
||||
const fft_hovertemplate = "<extra></extra>%{meta}<br>" + frequency_scale.hover("x") + "<br>" + 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<end_index;j++) {
|
||||
// Add to mean sum
|
||||
for (let k=0;k<Gyro_batch[i].FFT.x[j].length;k++) {
|
||||
if (j == start_index) {
|
||||
fft_mean_x[k] = amplitude_scale.fun(Gyro_batch[i].FFT.x[j][k] * window_correction)
|
||||
fft_mean_y[k] = amplitude_scale.fun(Gyro_batch[i].FFT.y[j][k] * window_correction)
|
||||
fft_mean_z[k] = amplitude_scale.fun(Gyro_batch[i].FFT.z[j][k] * window_correction)
|
||||
} else {
|
||||
fft_mean_x[k] += amplitude_scale.fun(Gyro_batch[i].FFT.x[j][k] * window_correction)
|
||||
fft_mean_y[k] += amplitude_scale.fun(Gyro_batch[i].FFT.y[j][k] * window_correction)
|
||||
fft_mean_z[k] += amplitude_scale.fun(Gyro_batch[i].FFT.z[j][k] * window_correction)
|
||||
}
|
||||
if (j == (end_index - 1)) {
|
||||
fft_mean_x[k] /= plot_length
|
||||
fft_mean_y[k] /= plot_length
|
||||
fft_mean_z[k] /= plot_length
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Set scaled y data
|
||||
fft_plot.data[i*3 + 0].y = fft_mean_x
|
||||
fft_plot.data[i*3 + 1].y = fft_mean_y
|
||||
fft_plot.data[i*3 + 2].y = fft_mean_z
|
||||
|
||||
// Set scaled x data
|
||||
const scaled_bins = apply_scale_fun(Gyro_batch[i].FFT.bins, frequency_scale.fun)
|
||||
fft_plot.data[i*3 + 0].x = scaled_bins
|
||||
fft_plot.data[i*3 + 1].x = scaled_bins
|
||||
fft_plot.data[i*3 + 2].x = scaled_bins
|
||||
|
||||
// Set hover over function
|
||||
fft_plot.data[i*3 + 0].hovertemplate = fft_hovertemplate
|
||||
fft_plot.data[i*3 + 1].hovertemplate = fft_hovertemplate
|
||||
fft_plot.data[i*3 + 2].hovertemplate = fft_hovertemplate
|
||||
|
||||
}
|
||||
|
||||
Plotly.redraw("FFTPlot")
|
||||
|
||||
redraw_Spectrogram()
|
||||
|
||||
}
|
||||
|
||||
// Find the instance of "Gyro_batch" that matches the selection
|
||||
function find_instance(gyro_instance, post_filter) {
|
||||
for (let i = 0; i < Gyro_batch.length; i++) {
|
||||
if ((Gyro_batch[i] == null) || (Gyro_batch[i].FFT == null)) {
|
||||
continue
|
||||
}
|
||||
if ((Gyro_batch[i].post_filter == post_filter) && (Gyro_batch[i].sensor_num == gyro_instance)) {
|
||||
return i
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
function redraw_Spectrogram() {
|
||||
|
||||
// Work out which index to plot
|
||||
var gyro_instance
|
||||
if (document.getElementById("SpecGyroInst0").checked) {
|
||||
gyro_instance = 0
|
||||
} else if (document.getElementById("SpecGyroInst1").checked) {
|
||||
gyro_instance = 1
|
||||
} else {
|
||||
gyro_instance = 2
|
||||
}
|
||||
const post_filter = document.getElementById("SpecGyroPost").checked
|
||||
|
||||
const batch_instance = find_instance(gyro_instance, post_filter)
|
||||
if (batch_instance == null) {
|
||||
console.log("Could not find matching dataset for checkbox: " + checkbox.id)
|
||||
return
|
||||
}
|
||||
|
||||
var axis
|
||||
if (document.getElementById("SpecGyroAxisX").checked) {
|
||||
axis = "x"
|
||||
} else if (document.getElementById("SpecGyroAxisY").checked) {
|
||||
axis = "y"
|
||||
} else {
|
||||
axis = "z"
|
||||
}
|
||||
|
||||
// Get scales
|
||||
const amplitude_scale = get_amplitude_scale()
|
||||
const frequency_scale = get_frequency_scale()
|
||||
|
||||
// Setup axes
|
||||
Spectrogram.layout.scene.xaxis.type = frequency_scale.type
|
||||
Spectrogram.layout.scene.xaxis.title.text = frequency_scale.label
|
||||
|
||||
Spectrogram.data[0].hovertemplate = "<extra></extra>" + "%{y:.2f} s<br>" + frequency_scale.hover("x") + "<br>" + 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<plot_length; j++) {
|
||||
Spectrogram.data[0].z[j] = []
|
||||
const index = start_index + j
|
||||
for (k = 0; k<Gyro_batch[batch_instance].FFT[axis][index].length; k++) {
|
||||
Spectrogram.data[0].z[j][k] = amplitude_scale.fun(Gyro_batch[batch_instance].FFT[axis][index][k] * window_correction)
|
||||
}
|
||||
}
|
||||
|
||||
Plotly.redraw("Spectrogram")
|
||||
}
|
||||
|
||||
// Update lines that are shown in FFT plot
|
||||
function update_hidden(checkbox) {
|
||||
|
||||
const gyro_instance = parseFloat(checkbox.id.match(/\d+/g));
|
||||
const post_filter = checkbox.id.includes("Post");
|
||||
|
||||
// Find the gyro data this box matches with
|
||||
const batch_instance = find_instance(gyro_instance, post_filter)
|
||||
if (batch_instance == null) {
|
||||
console.log("Could not find matching dataset for checkbox: " + checkbox.id)
|
||||
return
|
||||
}
|
||||
|
||||
const axi = checkbox.id.substr(checkbox.id.length - 1)
|
||||
|
||||
let axi_index
|
||||
let axis = ["X" , "Y", "Z"]
|
||||
for (let j=0;j<3;j++) {
|
||||
if (axis[j] == axi) {
|
||||
axi_index = j
|
||||
break
|
||||
}
|
||||
}
|
||||
|
||||
fft_plot.data[batch_instance*3 + axi_index].visible = checkbox.checked
|
||||
|
||||
Plotly.redraw("FFTPlot")
|
||||
|
||||
}
|
||||
|
||||
// Grab param from log
|
||||
function get_param_value(param_log, name) {
|
||||
var value
|
||||
for (let i = 0; i < param_log.Name.length; i++) {
|
||||
if (param_log.Name[i] === name) {
|
||||
if ((value != null) && (value != PARM_log.Value[i])) {
|
||||
console.log("Param changed in flight: " + name)
|
||||
}
|
||||
value = param_log.Value[i]
|
||||
}
|
||||
}
|
||||
return value
|
||||
}
|
||||
|
||||
var Gyro_batch
|
||||
function load(log_file) {
|
||||
|
||||
const start = performance.now()
|
||||
|
||||
// Clear and reset state
|
||||
var log = new DataflashParser()
|
||||
log.processData(log_file)
|
||||
|
||||
// Load batch messages
|
||||
log.parseAtOffset("ISBH")
|
||||
log.parseAtOffset("ISBD")
|
||||
if (log.messages.ISBH == null || log.messages.ISBD == null) {
|
||||
console.log("No Batch logging msg")
|
||||
return
|
||||
}
|
||||
|
||||
// Assign batches to each sensor
|
||||
// Only interested in gyro here
|
||||
const IMU_SENSOR_TYPE_GYRO = 1
|
||||
Gyro_batch = []
|
||||
let data_index = 0
|
||||
for (let i = 0; i < log.messages.ISBH.N.length; i++) {
|
||||
// Parse headers
|
||||
if (log.messages.ISBH.type[i] != IMU_SENSOR_TYPE_GYRO) {
|
||||
continue
|
||||
}
|
||||
|
||||
const instance = log.messages.ISBH.instance[i]
|
||||
if (Gyro_batch[instance] == null) {
|
||||
Gyro_batch[instance] = []
|
||||
}
|
||||
|
||||
let decode_complete = false
|
||||
|
||||
// Advance data index until sequence match
|
||||
const seq_num = log.messages.ISBH.N[i]
|
||||
while (log.messages.ISBD.N[data_index] != seq_num) {
|
||||
data_index++
|
||||
if (data_index >= 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<br><br>" +
|
||||
"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`);
|
||||
}
|
6
Tools/autotest/web-firmware/Tools/FilterReview/Readme.md
Normal file
6
Tools/autotest/web-firmware/Tools/FilterReview/Readme.md
Normal file
@ -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!
|
||||
|
275
Tools/autotest/web-firmware/Tools/FilterReview/index.html
Normal file
275
Tools/autotest/web-firmware/Tools/FilterReview/index.html
Normal file
@ -0,0 +1,275 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="utf-8" />
|
||||
<title>ArduPilot Filter Review Tool</title>
|
||||
<script src='https://cdn.plot.ly/plotly-2.20.0.min.js'></script>
|
||||
<script src="https://unpkg.com/mathjs/lib/browser/math.js"></script>
|
||||
<script type="text/javascript" src="parser.js"></script>
|
||||
<script type="text/javascript" src="DecodeDevID.js"></script>
|
||||
|
||||
</head>
|
||||
<a href="https://ardupilot.org"><img src="logo.png"></a>
|
||||
<h1>ArduPilot Filter Review Tool</h1>
|
||||
|
||||
<body>
|
||||
|
||||
<table>
|
||||
<tr>
|
||||
<td style="width: 30px;"></td>
|
||||
<td>
|
||||
<fieldset style="width:1100px">
|
||||
<legend>Setup</legend>
|
||||
<table>
|
||||
<tr>
|
||||
<td>
|
||||
<fieldset style="width:150px;height:80px">
|
||||
<legend>Amplitude scale</legend>
|
||||
<input type="radio" id="ScaleLog" name="Scale" checked>
|
||||
<label for="LogScale">dB</label><br>
|
||||
<input type="radio" id="ScaleLinear" name="Scale">
|
||||
<label for="LinearScale">Linear</label><br>
|
||||
</fieldset>
|
||||
</td>
|
||||
<td>
|
||||
<fieldset style="width:200px;height:80px">
|
||||
<legend>Spectrum scale</legend>
|
||||
<input type="radio" id="SpectrumLinear" name="SpectrumScale" checked>
|
||||
<label for="SpectrumLinear">Linear</label><br>
|
||||
<input type="radio" id="SpectrumPSD" name="SpectrumScale">
|
||||
<label for="SpectrumPSD">Power Spectral Density</label><br>
|
||||
</fieldset>
|
||||
</td>
|
||||
<td>
|
||||
<fieldset style="width:150px;height:80px">
|
||||
<legend>Frequency scale</legend>
|
||||
<table>
|
||||
<tr>
|
||||
<td>
|
||||
<input type="radio" id="freq_ScaleLinear" name="feq_scale" checked>
|
||||
<label for="LinearScale">Linear</label><br>
|
||||
<input type="radio" id="freq_ScaleLog" name="feq_scale">
|
||||
<label for="LogScale">Log</label><br>
|
||||
</td>
|
||||
<td>
|
||||
<input type="radio" id="freq_Scale_Hz" name="feq_unit" checked>
|
||||
<label for="Scale_unit_Hz">Hz</label><br>
|
||||
<input type="radio" id="freq_Scale_RPM" name="feq_unit">
|
||||
<label for="Scale_unit_RPM">RPM</label><br>
|
||||
</td>
|
||||
</tr>
|
||||
</table>
|
||||
</fieldset>
|
||||
</td>
|
||||
<td>
|
||||
<fieldset style="width:200px;height:80px">
|
||||
<legend>FFT Settings</legend>
|
||||
<label for="FFTWindow">Windows per batch</label>
|
||||
<input id="FFTWindow" name="FFTWindow" type="number" min="1" step="1" value="1" onchange="clear_calculation()" style="width:50px"/>
|
||||
<br><br>
|
||||
<label id="FFTWindowInfo"></label>
|
||||
</fieldset>
|
||||
</td>
|
||||
<td>
|
||||
<fieldset style="width:200px;height:80px">
|
||||
<legend>Analysis time</legend>
|
||||
<label for="TimeStart">Start</label>
|
||||
<input id="TimeStart" name="TimeStart" type="number" min="0" step="1" value="0" onchange="" style="width:50px" disabled/>
|
||||
s<br><br>
|
||||
<label for="TimeEnd">End</label>
|
||||
<input id="TimeEnd" name="TimeEnd" type="number" min="0" step="1" value="0" onchange="" style="width:50px" disabled/>
|
||||
s
|
||||
</fieldset>
|
||||
</td>
|
||||
</tr>
|
||||
</table>
|
||||
<p>
|
||||
Bin log:
|
||||
<input id="fileItem" type="file" accept=".bin" onchange="readFile(this)">
|
||||
<input type="button" id="calculate" value="Calculate" onclick="re_calc()" disabled>
|
||||
</p>
|
||||
</fieldset>
|
||||
</td>
|
||||
</tr>
|
||||
</table>
|
||||
<p>
|
||||
<div id="FFTPlot" style="width:1200px;height:450px"></div>
|
||||
</p>
|
||||
<table>
|
||||
<tr>
|
||||
<td style="width: 60px;"></td>
|
||||
<td>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Gyro 1</legend>
|
||||
<label id="Gyro0_info"><br></label>
|
||||
<p>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Pre-filter</legend>
|
||||
<input type="checkbox" id="Gyro0PreX" name="Gyro0PreX" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro0PreX">X</label>
|
||||
|
||||
<input type="checkbox" id="Gyro0PreY" name="Gyro0PreY" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro0PreY">Y</label>
|
||||
|
||||
<input type="checkbox" id="Gyro0PreZ" name="Gyro0PreZ" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro0PreZ">Z</label>
|
||||
</fieldset>
|
||||
</p>
|
||||
<p>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Post-filter</legend>
|
||||
<input type="checkbox" id="Gyro0PostX" name="Gyro0PostX" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro0PostX">X</label>
|
||||
|
||||
<input type="checkbox" id="Gyro0PostY" name="Gyro0PostY" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro0PostY">Y</label>
|
||||
|
||||
<input type="checkbox" id="Gyro0PostZ" name="Gyro0PostZ" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro0PostZ">Z</label>
|
||||
</fieldset>
|
||||
</p>
|
||||
<label id="Gyro0_FFT_info"><br><br><br></label>
|
||||
</fieldset>
|
||||
</td>
|
||||
<td>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Gyro 2</legend>
|
||||
<label id="Gyro1_info"><br></label>
|
||||
<p>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Pre-filter</legend>
|
||||
<input type="checkbox" id="Gyro1PreX" name="Gyro1PreX" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro1PreX">X</label>
|
||||
|
||||
<input type="checkbox" id="Gyro1PreY" name="Gyro1PreY" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro1PreY">Y</label>
|
||||
|
||||
<input type="checkbox" id="Gyro1PreZ" name="Gyro1PreZ" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro1PreZ">Z</label>
|
||||
</fieldset>
|
||||
</p>
|
||||
<p>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Post-filter</legend>
|
||||
<input type="checkbox" id="Gyro1PostX" name="Gyro1PostX" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro1PostX">X</label>
|
||||
|
||||
<input type="checkbox" id="Gyro1PostY" name="Gyro1PostY" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro1PostY">Y</label>
|
||||
|
||||
<input type="checkbox" id="Gyro1PostZ" name="Gyro1PostZ" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro1PostZ">Z</label>
|
||||
</fieldset>
|
||||
</p>
|
||||
<label id="Gyro1_FFT_info"><br><br><br></label>
|
||||
</fieldset>
|
||||
</td>
|
||||
<td>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Gyro 3</legend>
|
||||
<label id="Gyro2_info"><br></label>
|
||||
<p>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Pre-filter</legend>
|
||||
<input type="checkbox" id="Gyro2PreX" name="Gyro2PreX" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro2PreX">X</label>
|
||||
|
||||
<input type="checkbox" id="Gyro2PreY" name="Gyro2PreY" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro2PreY">Y</label>
|
||||
|
||||
<input type="checkbox" id="Gyro2PreZ" name="Gyro2PreZ" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro2PreZ">Z</label>
|
||||
</fieldset>
|
||||
</p>
|
||||
<p>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Post-filter</legend>
|
||||
<input type="checkbox" id="Gyro2PostX" name="Gyro2PostX" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro2PostX">X</label>
|
||||
|
||||
<input type="checkbox" id="Gyro2PostY" name="Gyro2PostY" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro2PostY">Y</label>
|
||||
|
||||
<input type="checkbox" id="Gyro2PostZ" name="Gyro2PostZ" onchange="update_hidden(this)" checked disabled>
|
||||
<label for="Gyro2PostZ">Z</label>
|
||||
</fieldset>
|
||||
</p>
|
||||
<label id="Gyro2_FFT_info"><br><br><br></label>
|
||||
</fieldset>
|
||||
</td>
|
||||
</tr>
|
||||
</table>
|
||||
|
||||
|
||||
<p>
|
||||
<div id="Spectrogram" style="width:1200px;height:450px"></div>
|
||||
</p>
|
||||
|
||||
<table>
|
||||
<tr>
|
||||
<td style="width: 60px;"></td>
|
||||
<td>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Spectrogram Options</legend>
|
||||
<p>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Gyro instance</legend>
|
||||
<input type="radio" id="SpecGyroInst0" name="SpecGyroInst" onchange="redraw_Spectrogram()" disabled>
|
||||
<label for="SpecGyroInst0">1</label>
|
||||
|
||||
<input type="radio" id="SpecGyroInst1" name="SpecGyroInst" onchange="redraw_Spectrogram()" disabled>
|
||||
<label for="SpecGyroInst1">2</label>
|
||||
|
||||
<input type="radio" id="SpecGyroInst2" name="SpecGyroInst" onchange="redraw_Spectrogram()" disabled>
|
||||
<label for="SpecGyroInst2">3</label>
|
||||
</fieldset>
|
||||
</p>
|
||||
<p>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Filtering</legend>
|
||||
<input type="radio" id="SpecGyroPre" name="SpecGyroPrePost" onchange="redraw_Spectrogram()" disabled>
|
||||
<label for="SpecGyroPre">Pre-filter</label>
|
||||
|
||||
<input type="radio" id="SpecGyroPost" name="SpecGyroPrePost" onchange="redraw_Spectrogram()" disabled>
|
||||
<label for="SpecGyroPost">Post-filter</label>
|
||||
</fieldset>
|
||||
</p>
|
||||
<p>
|
||||
<fieldset style="width:300px">
|
||||
<legend>Axis</legend>
|
||||
<input type="radio" id="SpecGyroAxisX" name="SpecGyroAxis" onchange="redraw_Spectrogram()" disabled>
|
||||
<label for="SpecGyroAxisX">X</label>
|
||||
|
||||
<input type="radio" id="SpecGyroAxisY" name="SpecGyroAxis" onchange="redraw_Spectrogram()" disabled>
|
||||
<label for="SpecGyroAxisY">Y</label>
|
||||
|
||||
<input type="radio" id="SpecGyroAxisZ" name="SpecGyroAxis" onchange="redraw_Spectrogram()" disabled>
|
||||
<label for="SpecGyroAxisZ">Z</label>
|
||||
</fieldset>
|
||||
</p>
|
||||
</fieldset>
|
||||
</td>
|
||||
|
||||
</tr>
|
||||
</table>
|
||||
|
||||
|
||||
|
||||
|
||||
</body>
|
||||
|
||||
<script type="text/javascript" src="FilterReview.js"></script>
|
||||
<script>
|
||||
function readFile(e) {
|
||||
const file = e.files[0]
|
||||
if (file == null) {
|
||||
return
|
||||
}
|
||||
let reader = new FileReader()
|
||||
reader.onload = function (e) {
|
||||
load(reader.result)
|
||||
}
|
||||
reader.readAsArrayBuffer(file)
|
||||
}
|
||||
|
||||
</script>
|
897
Tools/autotest/web-firmware/Tools/FilterReview/parser.js
Normal file
897
Tools/autotest/web-firmware/Tools/FilterReview/parser.js
Normal file
@ -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....
|
||||
// <leave a gap here, just in case....>
|
||||
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,
|
||||
// <leave a gap here, just in case....>
|
||||
'!': 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)
|
||||
}
|
||||
})
|
Loading…
Reference in New Issue
Block a user