imu --calibrate: take down from the current mount file when a session has no level stretch
/ crosscheck (push) Successful in 1s
Rust (L4T) / Clippy, test, build, glibc (push) Successful in 15s

Level (home) and forward (a drive) can now be captured on different days.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01DDwfd6Ki8UfG4hK6W5YpJz
This commit is contained in:
2026-10-15 20:11:38 +09:00
parent 26d5e3cbe4
commit 7355b0d26b
2 changed files with 26 additions and 8 deletions
+25 -7
View File
@@ -7,7 +7,8 @@
//! - **Down** from the two level stretches, parked facing one way ("imu level A") and //! - **Down** from the two level stretches, parked facing one way ("imu level A") and
//! turned round ("imu level B"). Averaged, a slope's tilt cancels, since turning //! turned round ("imu level B"). Averaged, a slope's tilt cancels, since turning
//! round reverses it; the difference between them is reported as the slope. //! round reverses it; the difference between them is reported as the slope.
//! One stretch alone still works, slope included. //! One stretch alone still works, slope included. A session with neither keeps the
//! down of the current mount file, so level and forward can be captured on different days.
//! - **Forward** from straight-line pull-aways ("imu pull away", forward) and stops //! - **Forward** from straight-line pull-aways ("imu pull away", forward) and stops
//! ("imu brake", backward): the force's horizontal part, signed by which stretch it //! ("imu brake", backward): the force's horizontal part, signed by which stretch it
//! is in, summed. Without them, forward is the `--forward` axis laid flat, as before. //! is in, summed. Without them, forward is the `--forward` axis laid flat, as before.
@@ -101,7 +102,12 @@ pub struct Calibration {
} }
/// `fallback_forward`: the configured forward axis, used when no driving was captured. /// `fallback_forward`: the configured forward axis, used when no driving was captured.
pub fn calibrate(samples: &[Sample], stretches: &[Stretch], fallback_forward: [f64; 3]) -> Result<Calibration, String> { pub fn calibrate(
samples: &[Sample],
stretches: &[Stretch],
fallback_forward: [f64; 3],
fallback_down: Option<[f64; 3]>,
) -> Result<Calibration, String> {
let has = |s: &Stretch, k: &str| s.label.contains(k); let has = |s: &Stretch, k: &str| s.label.contains(k);
// Down: the mean force while still is "up"; down is its opposite. // Down: the mean force while still is "up"; down is its opposite.
@@ -120,8 +126,15 @@ pub fn calibrate(samples: &[Sample], stretches: &[Stretch], fallback_forward: [f
} }
ups.push(unit(m)); ups.push(unit(m));
} }
let mut down_from = "the level stretches".to_string();
let up = match ups.as_slice() { let up = match ups.as_slice() {
[] => return Err("no 'imu level A' or 'imu level B' stretch with samples".into()), [] => match fallback_down {
Some(d) => {
down_from = "the current mount file (no level stretch in this session)".into();
unit(d.map(|x| -x))
}
None => return Err("no 'imu level A' or 'imu level B' stretch with samples, and no mount file to take down from".into()),
},
[a] => *a, [a] => *a,
[a, b, ..] => unit([a[0] + b[0], a[1] + b[1], a[2] + b[2]]), [a, b, ..] => unit([a[0] + b[0], a[1] + b[1], a[2] + b[2]]),
}; };
@@ -170,6 +183,7 @@ pub fn calibrate(samples: &[Sample], stretches: &[Stretch], fallback_forward: [f
report: json!({ report: json!({
"forward": r3(rows[0]), "right": r3(rows[1]), "down": r3(rows[2]), "forward": r3(rows[0]), "right": r3(rows[1]), "down": r3(rows[2]),
"level": level, "level": level,
"down_from": down_from,
"slope_deg": slope.map(|x| (x * 10.0).round() / 10.0), "slope_deg": slope.map(|x| (x * 10.0).round() / 10.0),
"forward_from": forward_from, "forward_from": forward_from,
"forward_spread_deg": spread.map(|x| (x * 10.0).round() / 10.0), "forward_spread_deg": spread.map(|x| (x * 10.0).round() / 10.0),
@@ -206,7 +220,7 @@ mod tests {
st("29 imu pull away #1", 40.0, 43.0), st("29 imu pull away #1", 40.0, 43.0),
st("30 imu brake #1", 50.0, 53.0), st("30 imu brake #1", 50.0, 53.0),
]; ];
let r = calibrate(&samples, &stretches, [0.0, 1.0, 0.0]).unwrap(); let r = calibrate(&samples, &stretches, [0.0, 1.0, 0.0], None).unwrap();
let rows = r.mount.rows(); let rows = r.mount.rows();
let close = |a: [f64; 3], b: [f64; 3]| (0..3).all(|i| (a[i] - b[i]).abs() < 1e-6); let close = |a: [f64; 3], b: [f64; 3]| (0..3).all(|i| (a[i] - b[i]).abs() < 1e-6);
assert!(close(rows[2], [-1.0, 0.0, 0.0]), "down {:?}", rows[2]); assert!(close(rows[2], [-1.0, 0.0, 0.0]), "down {:?}", rows[2]);
@@ -220,11 +234,15 @@ mod tests {
#[test] #[test]
fn without_driving_forward_falls_back_and_motion_is_refused() { fn without_driving_forward_falls_back_and_motion_is_refused() {
let samples = steady(0.0, 10.0, [0.0, 0.0, 1.0]); let samples = steady(0.0, 10.0, [0.0, 0.0, 1.0]);
let r = calibrate(&samples, &[st("27 imu level A", 0.0, 10.0)], [0.0, 1.0, 0.0]).unwrap(); let r = calibrate(&samples, &[st("27 imu level A", 0.0, 10.0)], [0.0, 1.0, 0.0], None).unwrap();
assert!(r.report["forward_from"].as_str().unwrap().starts_with("the --forward axis")); assert!(r.report["forward_from"].as_str().unwrap().starts_with("the --forward axis"));
let mut moving = steady(0.0, 10.0, [0.0, 0.0, 1.0]); let mut moving = steady(0.0, 10.0, [0.0, 0.0, 1.0]);
moving.iter_mut().for_each(|s| s.gyro = [0.0, 0.0, 20.0]); moving.iter_mut().for_each(|s| s.gyro = [0.0, 0.0, 20.0]);
assert!(calibrate(&moving, &[st("27 imu level A", 0.0, 10.0)], [0.0, 1.0, 0.0]).is_err()); assert!(calibrate(&moving, &[st("27 imu level A", 0.0, 10.0)], [0.0, 1.0, 0.0], None).is_err());
assert!(calibrate(&samples, &[], [0.0, 1.0, 0.0]).is_err()); assert!(calibrate(&samples, &[], [0.0, 1.0, 0.0], None).is_err());
// Forward only, down from an earlier calibration.
let drive = steady(0.0, 3.0, [0.0, 0.3, 1.0]);
let r = calibrate(&drive, &[st("30 imu brake #1", 0.0, 3.0)], [0.0, 1.0, 0.0], Some([0.0, 0.0, -1.0])).unwrap();
assert!((r.mount.rows()[0][1] + 1.0).abs() < 1e-6, "{}", r.report); // braking force +Y: forward is -Y
} }
} }
+1 -1
View File
@@ -223,7 +223,7 @@ fn calibrate_session(o: &Opts, dir: &str) -> ExitCode {
let dir = std::path::Path::new(dir); let dir = std::path::Path::new(dir);
let result = calibrate::load_samples(dir) let result = calibrate::load_samples(dir)
.and_then(|s| Ok((s, calibrate::load_stretches(dir)?))) .and_then(|s| Ok((s, calibrate::load_stretches(dir)?)))
.and_then(|(s, st)| calibrate::calibrate(&s, &st, o.forward.unit())); .and_then(|(s, st)| calibrate::calibrate(&s, &st, o.forward.unit(), mount_file(&o.mount_file).map(|(m, _)| m.rows()[2])));
let c = match result { let c = match result {
Ok(c) => c, Ok(c) => c,
Err(e) => { Err(e) => {