compass_demo.rs

# Add libm to your Cargo dependencies for floating-point operations.

#![no_main]
#![no_std]

extern crate panic_halt as _;

use core::{f32::consts::PI, fmt::Write, primitive};
use cortex_m_rt::entry;
use libm;
use lsm303agr::Lsm303agr;
use microbit::{
    display::blocking,
    hal::{
        self,
        uarte::{Baudrate, Parity, Uarte},
    },
    Board,
};

enum Direction {
    Up,
    UpRight,
    Right,
    DownRight,
    Down,
    DownLeft,
    Left,
    UpLeft,
}

fn mags_to_direction(x: i32, y: i32) -> Direction {
    let x = x as f32;
    let y = y as f32;
    let theta = libm::atan2f(y, x);
    let theta_plus_pi = theta + PI + PI / 8.0;
    let octant = libm::floorf(8.0 * theta_plus_pi / (2.0 * PI));
    if octant == 0.0 || octant == 8.0 {
        Direction::Left
    } else if octant == 1.0 {
        Direction::DownLeft
    } else if octant == 2.0 {
        Direction::Down
    } else if octant == 3.0 {
        Direction::DownRight
    } else if octant == 4.0 {
        Direction::Right
    } else if octant == 5.0 {
        Direction::UpRight
    } else if octant == 6.0 {
        Direction::Up
    } else if octant == 7.0 {
        Direction::UpLeft
    } else {
        panic!()
    }
}

#[entry]
fn main() -> ! {
    let board = Board::take().unwrap();

    // Configure serial port.
    let mut serial = Uarte::new(
        board.UARTE0,
        board.uart.into(),
        Parity::EXCLUDED,
        Baudrate::BAUD115200,
    );

    let arrows = [
        /* Up */
        [
            [0, 0, 1, 0, 0],
            [0, 1, 1, 1, 0],
            [1, 0, 1, 0, 1],
            [0, 0, 1, 0, 0],
            [0, 0, 1, 0, 0],
        ],
        /* UpRight */
        [
            [1, 1, 1, 1, 1],
            [0, 0, 0, 1, 1],
            [0, 0, 1, 0, 1],
            [0, 1, 0, 0, 1],
            [1, 0, 0, 0, 1],
        ],
        /* Right */
        [
            [0, 0, 1, 0, 0],
            [0, 0, 0, 1, 0],
            [1, 1, 1, 1, 1],
            [0, 0, 0, 1, 0],
            [0, 0, 1, 0, 0],
        ],
        /* DownRight */
        [
            [1, 0, 0, 0, 1],
            [0, 1, 0, 0, 1],
            [0, 0, 1, 0, 1],
            [0, 0, 0, 1, 1],
            [1, 1, 1, 1, 1],
        ],
        /* Down */
        [
            [0, 0, 1, 0, 0],
            [0, 0, 1, 0, 0],
            [1, 0, 1, 0, 1],
            [0, 1, 1, 1, 0],
            [0, 0, 1, 0, 0],
        ],
        /* DownLeft */
        [
            [1, 0, 0, 0, 1],
            [1, 0, 0, 1, 0],
            [1, 0, 1, 0, 0],
            [1, 1, 0, 0, 0],
            [1, 1, 1, 1, 1],
        ],
        /* Left */
        [
            [0, 0, 1, 0, 0],
            [0, 1, 0, 0, 0],
            [1, 1, 1, 1, 1],
            [0, 1, 0, 0, 0],
            [0, 0, 1, 0, 0],
        ],
        /* UpLeft */
        [
            [1, 1, 1, 1, 1],
            [1, 1, 0, 0, 0],
            [1, 0, 1, 0, 0],
            [1, 0, 0, 1, 0],
            [1, 0, 0, 0, 1],
        ],
    ];

    let mut timer = hal::Timer::new(board.TIMER0);
    let mut display = blocking::Display::new(board.display_pins);

    // Set up the I2C controller and Inertial Measurement Unit.
    let i2c = microbit::hal::Twim::new(
        board.TWIM0,
        board.i2c_internal.into(),
        microbit::hal::twim::Frequency::K100,
    );
    let mut sensor = Lsm303agr::new_with_i2c(i2c);
    sensor.init().unwrap();
    // let mut sensor = sensor.into_mag_continuous().ok().unwrap();
    // sensor
    //     .set_mag_odr(lsm303agr::MagOutputDataRate::Hz100)
    //     .unwrap();
    sensor
        .set_accel_odr(lsm303agr::AccelOutputDataRate::Hz10)
        .unwrap();

    writeln!(serial, "Ready.").unwrap();

    loop {
        // Read compass data and log it to the serial port.
        if sensor.accel_status().unwrap().xyz_new_data {
        // if sensor.mag_status().unwrap().xyz_new_data {
            // let data = sensor.mag_data().unwrap();
            let data = sensor.accel_data().unwrap();
            writeln!(serial, "Compass: x {} y {} z {}", data.x, data.y, data.z).unwrap();
            let direction = mags_to_direction(data.x, data.y);
            let arrow = &arrows[direction as usize];
            display.show(&mut timer, *arrow, 100);
            display.clear();
            // timer.delay(250u32);
        }
    }
}