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
//! 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.
//! 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
//! ("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.
@@ -101,7 +102,12 @@ pub struct Calibration {
}
/// `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);
// 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));
}
let mut down_from = "the level stretches".to_string();
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, 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!({
"forward": r3(rows[0]), "right": r3(rows[1]), "down": r3(rows[2]),
"level": level,
"down_from": down_from,
"slope_deg": slope.map(|x| (x * 10.0).round() / 10.0),
"forward_from": forward_from,
"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("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 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]);
@@ -220,11 +234,15 @@ mod tests {
#[test]
fn without_driving_forward_falls_back_and_motion_is_refused() {
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"));
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]);
assert!(calibrate(&moving, &[st("27 imu level A", 0.0, 10.0)], [0.0, 1.0, 0.0]).is_err());
assert!(calibrate(&samples, &[], [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], 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 result = calibrate::load_samples(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 {
Ok(c) => c,
Err(e) => {