Bare Metal Rust Morning Exercise

Compass

(back to exercise)

  1. #![no_main]
  2. #![no_std]
  4. extern crate panic_halt as _;
  6. use core::fmt::Write;
  7. use cortex_m_rt::entry;
  8. use core::cmp::{max, min};
  9. use embedded_hal::digital::InputPin;
  10. use lsm303agr::{
  11.     AccelMode, AccelOutputDataRate, Lsm303agr, MagMode, MagOutputDataRate,
  12. };
  13. use microbit::display::blocking::Display;
  14. use microbit::hal::twim::Twim;
  15. use microbit::hal::uarte::{Baudrate, Parity, Uarte};
  16. use microbit::hal::{Delay, Timer};
  17. use microbit::pac::twim0::frequency::FREQUENCY_A;
  18. use microbit::Board;
  20. const COMPASS_SCALE: i32 = 30000;
  21. const ACCELEROMETER_SCALE: i32 = 700;
  23. #[entry]
  24. fn main() -> ! {
  25.     let mut board = Board::take().unwrap();
  27.     // Configure serial port.
  28.     let mut serial = Uarte::new(
  29.         board.UARTE0,
  30.         board.uart.into(),
  31.         Parity::EXCLUDED,
  32.         Baudrate::BAUD115200,
  33.     );
  35.     // Use the system timer as a delay provider.
  36.     let mut delay = Delay::new(board.SYST);
  38.     // Set up the I2C controller and Inertial Measurement Unit.
  39.     writeln!(serial, "Setting up IMU...").unwrap();
  40.     let i2c = Twim::new(board.TWIM0, board.i2c_internal.into(), FREQUENCY_A::K100);
  41.     let mut imu = Lsm303agr::new_with_i2c(i2c);
  42.     imu.init().unwrap();
  43.     imu.set_mag_mode_and_odr(
  44.         &mut delay,
  45.         MagMode::HighResolution,
  46.         MagOutputDataRate::Hz50,
  47.     )
  48.     .unwrap();
  49.     imu.set_accel_mode_and_odr(
  50.         &mut delay,
  51.         AccelMode::Normal,
  52.         AccelOutputDataRate::Hz50,
  53.     )
  54.     .unwrap();
  55.     let mut imu = imu.into_mag_continuous().ok().unwrap();
  57.     // Set up display and timer.
  58.     let mut timer = Timer::new(board.TIMER0);
  59.     let mut display = Display::new(board.display_pins);
  61.     let mut mode = Mode::Compass;
  62.     let mut button_pressed = false;
  64.     writeln!(serial, "Ready.").unwrap();
  66.     loop {
  67.         // Read compass data and log it to the serial port.
  68.         while !(imu.mag_status().unwrap().xyz_new_data()
  69.             && imu.accel_status().unwrap().xyz_new_data())
  70.         {}
  71.         let compass_reading = imu.magnetic_field().unwrap();
  72.         let accelerometer_reading = imu.acceleration().unwrap();
  73.         writeln!(
  74.             serial,
  75.             "{},{},{}\t{},{},{}",
  76.             compass_reading.x_nt(),
  77.             compass_reading.y_nt(),
  78.             compass_reading.z_nt(),
  79.             accelerometer_reading.x_mg(),
  80.             accelerometer_reading.y_mg(),
  81.             accelerometer_reading.z_mg(),
  82.         )
  83.         .unwrap();
  85.         let mut image = [[0; 5]; 5];
  86.         let (x, y) = match mode {
  87.             Mode::Compass => (
  88.                 scale(-compass_reading.x_nt(), -COMPASS_SCALE, COMPASS_SCALE, 0, 4)
  89.                     as usize,
  90.                 scale(compass_reading.y_nt(), -COMPASS_SCALE, COMPASS_SCALE, 0, 4)
  91.                     as usize,
  92.             ),
  93.             Mode::Accelerometer => (
  94.                 scale(
  95.                     accelerometer_reading.x_mg(),
  96.                     -ACCELEROMETER_SCALE,
  97.                     ACCELEROMETER_SCALE,
  98.                     0,
  99.                     4,
  100.                 ) as usize,
  101.                 scale(
  102.                     -accelerometer_reading.y_mg(),
  103.                     -ACCELEROMETER_SCALE,
  104.                     ACCELEROMETER_SCALE,
  105.                     0,
  106.                     4,
  107.                 ) as usize,
  108.             ),
  109.         };
  110.         image[y][x] = 255;
  111.         display.show(&mut timer, image, 100);
  113.         // If button A is pressed, switch to the next mode and briefly blink all LEDs
  114.         // on.
  115.         if board.buttons.button_a.is_low().unwrap() {
  116.             if !button_pressed {
  117.                 mode = mode.next();
  118.                 display.show(&mut timer, [[255; 5]; 5], 200);
  119.             }
  120.             button_pressed = true;
  121.         } else {
  122.             button_pressed = false;
  123.         }
  124.     }
  125. }
  127. #[derive(Copy, Clone, Debug, Eq, PartialEq)]
  128. enum Mode {
  129.     Compass,
  130.     Accelerometer,
  131. }
  133. impl Mode {
  134.     fn next(self) -> Self {
  135.         match self {
  136.             Self::Compass => Self::Accelerometer,
  137.             Self::Accelerometer => Self::Compass,
  138.         }
  139.     }
  140. }
  142. fn scale(value: i32, min_in: i32, max_in: i32, min_out: i32, max_out: i32) -> i32 {
  143.     let range_in = max_in - min_in;
  144.     let range_out = max_out - min_out;
  145.     cap(min_out + range_out * (value - min_in) / range_in, min_out, max_out)
  146. }
  148. fn cap(value: i32, min_value: i32, max_value: i32) -> i32 {
  149.     max(min_value, min(value, max_value))
  150. }