Skip to content

FilterReview: only use armed raw IMU data if disarmed logging is rate limited #367

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
j-w9 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

FilterReview: only use armed raw IMU data if disarmed logging is rate limited #367

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 38 additions & 4 deletions FilterReview/FilterReview.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2228,6 +2228,21 @@ function load_from_batch(log, num_gyro, gyro_rate, get_param) {
}
}

// Return true for each time at which the vehicle was armed, from ARM log messages
function armed_mask(time, ARM) {
const mask = new Array(time.length)
let armed = false
let k = 0
for (let j = 0; j < time.length; j++) {
while ((k < ARM.TimeUS.length) && (ARM.TimeUS[k] <= time[j])) {
armed = ARM.ArmState[k] != 0
k++
}
mask[j] = armed
}
return mask
}

// Log from raw sensor logging
function load_from_raw_log(log, num_gyro, gyro_rate, get_param) {
Gyro_batch = []
Expand All @@ -2248,6 +2263,14 @@ function load_from_raw_log(log, num_gyro, gyro_rate, get_param) {
post_filter = false
}

// Logging may be rate limited while disarmed, the change in sample rate breaks the FFT.
// If so only use samples logged while armed, the gap where disarmed data is removed then splits the batches.
let ARM = null
const LOG_DARM_RATEMAX = get_param("LOG_DARM_RATEMAX", false)
if ((LOG_DARM_RATEMAX != null) && (LOG_DARM_RATEMAX > 0) && ("ARM" in log.messageTypes)) {
ARM = log.get("ARM")
}

// Load in a one massive batch, split for large gaps in log
for (const inst of Object.keys(log.messageTypes.GYR.instances)) {
const i = parseFloat(inst)
Expand All @@ -2268,10 +2291,21 @@ function load_from_raw_log(log, num_gyro, gyro_rate, get_param) {
continue
}

const time = log.get_instance("GYR", inst, "SampleUS")
const GyrX = log.get_instance("GYR", inst, "GyrX")
const GyrY = log.get_instance("GYR", inst, "GyrY")
const GyrZ = log.get_instance("GYR", inst, "GyrZ")
let time = log.get_instance("GYR", inst, "SampleUS")
let GyrX = log.get_instance("GYR", inst, "GyrX")
let GyrY = log.get_instance("GYR", inst, "GyrY")
let GyrZ = log.get_instance("GYR", inst, "GyrZ")

if (ARM != null) {
const armed = armed_mask(time, ARM)
if (armed.includes(true)) {
const keep = (_, k) => armed[k]
time = time.filter(keep)
GyrX = GyrX.filter(keep)
GyrY = GyrY.filter(keep)
GyrZ = GyrZ.filter(keep)
}
}

let sample_rate_sum = 0
let sample_rate_count = 0
Expand Down
Loading