imu --calibrate: take down from the current mount file when a session has no level stretch
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:
+25
-7
@@ -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
@@ -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) => {
|
||||||
|
|||||||
Reference in New Issue
Block a user