diff --git a/src/imu/calibrate.rs b/src/imu/calibrate.rs index 9966728..167b33e 100644 --- a/src/imu/calibrate.rs +++ b/src/imu/calibrate.rs @@ -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 { +pub fn calibrate( + samples: &[Sample], + stretches: &[Stretch], + fallback_forward: [f64; 3], + fallback_down: Option<[f64; 3]>, +) -> Result { 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 } } diff --git a/src/imu/main.rs b/src/imu/main.rs index 8e5495a..76eeb41 100644 --- a/src/imu/main.rs +++ b/src/imu/main.rs @@ -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) => {