Indicate heading using isosceles triangle

[chrisduerr]

While rotating the map would take a little more effort due to the extra rendering work, it should be a pretty nice incremental step to indicate GPS heading using an isosceles triangle, pointing in the direction of travel. This would allow implementing the compass logic without having to bother with the map rotation (yet).

[chrisduerr]

I was hoping that it would be relatively simple to get the current heading, however it turned out to be more complicated than expected.

The following code seems to somewhat get there, but seems both unreliable and inaccurate (not pointing anywhere near North):

Naive Attempt

use std::error::Error;
use std::str::FromStr;
use udev::Device;
use udev::Enumerator;

fn main() {
    let mut enumerator = Enumerator::new().unwrap();
    enumerator.match_is_initialized().unwrap();
    enumerator.match_subsystem("iio").unwrap();

    let mut devices = enumerator.scan_devices().unwrap();
    let compass = devices
        .find(|d| d.property_value("OF_NAME").is_some_and(|n| n == "compass"))
        .unwrap();

    let matrix = udev_attribute::<Matrix3x3<f64>>(&compass, "in_mount_matrix")
        .or_else(|| udev_attribute::<Matrix3x3<f64>>(&compass, "mount_matrix"))
        .unwrap();

    let syspath = compass.syspath().to_path_buf();

    let scale = udev_attribute::<f64>(&compass, "in_magn_scale");
    let scale_available = udev_attribute::<CompassScale>(&compass, "in_magn_scale_available");
    let x_scale = udev_attribute::<f64>(&compass, "in_magn_x_scale")
        .or_else(|| scale_available.and_then(|scale| scale.x))
        .or(scale)
        .unwrap();
    let y_scale = udev_attribute::<f64>(&compass, "in_magn_y_scale")
        .or_else(|| scale_available.and_then(|scale| scale.y))
        .or(scale)
        .unwrap();
    let z_scale = udev_attribute::<f64>(&compass, "in_magn_z_scale")
        .or_else(|| scale_available.and_then(|scale| scale.z))
        .or(scale)
        .unwrap();

    let mut min_x = f64::MAX;
    let mut min_y = f64::MAX;
    let mut min_z = f64::MAX;

    let mut max_x = f64::MIN;
    let mut max_y = f64::MIN;
    let mut max_z = f64::MIN;

    loop {
        let compass = Device::from_syspath(&syspath).unwrap();

        // Calculate x/y/z values in Microtesla.
        let x = udev_attribute::<i32>(&compass, "in_magn_x_raw").unwrap() as f64;
        let y = udev_attribute::<i32>(&compass, "in_magn_y_raw").unwrap() as f64;
        let z = udev_attribute::<i32>(&compass, "in_magn_z_raw").unwrap() as f64;
        let (x, y, z) = matrix.mul(x * x_scale, y * y_scale, z * z_scale);

        min_x = min_x.min(x);
        min_y = min_y.min(y);
        min_z = min_z.min(z);

        max_x = max_x.max(x);
        max_y = max_y.max(y);
        max_z = max_z.max(z);

        let x = (x - min_x) / (max_x - min_x) - 0.5;
        let y = (y - min_y) / (max_y - min_y) - 0.5;
        let z = (z - min_z) / (max_z - min_z) - 0.5;

        // let gauss_x = x * 100.;
        // let gauss_y = y * 100.;
        // let gauss_z = z * 100.;

        // TODO: This seems to work-ish for the phone being vertical (north is wrong though)?
        // TODO: min/max stuff seems necessary?
        // See also https://github.com/pimoroni/icm20948-python/blob/988f284b/examples/magnetometer.py
        let azimuth = x.atan2(z);
        let bearing = azimuth.to_degrees().rem_euclid(360.);

        println!("{:+.5} {:+.5} {:+.5} = {:+.2}°", x, y, z, bearing);
        std::thread::sleep_ms(250);
    }
}

/// Get a udev device attribute.
fn udev_attribute<T: FromStr>(device: &Device, attribute: &str) -> Option<T> {
    let value = device.attribute_value(attribute)?.to_string_lossy();
    T::from_str(&value).ok()
}

#[derive(Debug, Clone, PartialEq, Eq)]
pub struct Matrix3x3<T: Copy> {
    storage: [T; 9],
}

impl Matrix3x3<f64> {
    fn mul(&self, x: f64, y: f64, z: f64) -> (f64, f64, f64) {
        let x = self.storage[0] * x + self.storage[1] * y + self.storage[2] * z;
        let y = self.storage[3] * x + self.storage[4] * y + self.storage[5] * z;
        let z = self.storage[6] * x + self.storage[7] * y + self.storage[8] * z;
        (x, y, z)
    }
}

impl TryFrom<Vec<f64>> for Matrix3x3<f64> {
    type Error = Box<dyn Error>;

    fn try_from(data: Vec<f64>) -> Result<Self, Self::Error> {
        if data.len() == 9 {
            let mut storage = [0.; 9];
            storage.copy_from_slice(&data);
            Ok(Self { storage })
        } else {
            let error = format!(
                "Mismatched size when creating Matrix3x3 from Vec, expected length 9, got {}",
                data.len()
            );
            Err(error.into())
        }
    }
}

// The expected format is "0, 0, 0; 0, 0, 0; 0, 0, 0".
impl FromStr for Matrix3x3<f64> {
    type Err = Box<dyn Error>;

    fn from_str(s: &str) -> Result<Self, Self::Err> {
        let rows = s
            .split(';')
            // Ensure that we process only 3 rows.
            .take(3)
            // Take only 3 element from each row.
            .flat_map(|row| row.split(',').take(3))
            .map(|ch| f64::from_str(ch.trim()))
            .collect::<Result<Vec<_>, _>>()?;

        Self::try_from(rows)
    }
}

#[derive(Copy, Clone)]
struct CompassScale {
    x: Option<f64>,
    y: Option<f64>,
    z: Option<f64>,
}

impl FromStr for CompassScale {
    type Err = Box<dyn Error>;

    fn from_str(s: &str) -> Result<Self, Self::Err> {
        let mut scales = s.split(' ');

        let x = match scales.next() {
            Some(scale) => Some(f64::from_str(scale)?),
            None => None,
        };

        let y = match scales.next() {
            Some(scale) => Some(f64::from_str(scale)?),
            None => None,
        };

        let z = match scales.next() {
            Some(scale) => Some(f64::from_str(scale)?),
            None => None,
        };

        Ok(Self { x, y, z })
    }
}

This was inspired by the following examples:
https://github.com/pimoroni/icm20948-python/blob/988f284b/examples/magnetometer.py
https://gitlab.freedesktop.org/hadess/iio-sensor-proxy/-/merge_requests/316

Unfortunately iio-sensor-proxy does not provide this data, as evident from the previously linked pull request.

It is possible that combining this implementation with the accelerometer readings to project the magnetometer vector on the ground plane would lead to decent results, but I'm doubtful it would actually point north. Determining the correct solution without access to multiple devices/geographic locations seems difficult, since it's not entirely clear if the compass is off by a fixed degree or if the calculations are entirely incorrect.