テイク 2

今回は、数学を使って、磁場が磁力計の X 軸および Y 軸となす正確な角度を求めます。

ここでは atan2 関数を使います。この関数は、-PI から PI の範囲の角度を返します。以下の図は、この角度がどのように測定されるかを示しています。

この図では明示的には示されていませんが、X 軸は右向きで、Y 軸は上向きです。

以下がスターターコードです。ラジアン単位の theta はすでに計算されています。theta の値に基づいて、どの LED を点灯させるかを選ぶ必要があります。

#![deny(unsafe_code)]
#![no_main]
#![no_std]

use cortex_m_rt::entry;
use panic_rtt_target as _;
use rtt_target::{rprintln, rtt_init_print};

mod calibration;
use crate::calibration::calc_calibration;
use crate::calibration::calibrated_measurement;

mod led;
use crate::led::Direction;
use crate::led::direction_to_led;

// これは便利です ;-)
use core::f32::consts::PI;
use libm::atan2f;

use microbit::{display::blocking::Display, hal::Timer};

#[cfg(feature = "v1")]
use microbit::{hal::twi, pac::twi0::frequency::FREQUENCY_A};

#[cfg(feature = "v2")]
use microbit::{hal::twim, pac::twim0::frequency::FREQUENCY_A};

use lsm303agr::{AccelOutputDataRate, Lsm303agr, MagOutputDataRate};

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

    #[cfg(feature = "v1")]
    let i2c = { twi::Twi::new(board.TWI0, board.i2c.into(), FREQUENCY_A::K100) };

    #[cfg(feature = "v2")]
    let i2c = { twim::Twim::new(board.TWIM0, board.i2c_internal.into(), FREQUENCY_A::K100) };

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

    let mut sensor = Lsm303agr::new_with_i2c(i2c);
    sensor.init().unwrap();
    sensor.set_mag_odr(MagOutputDataRate::Hz10).unwrap();
    sensor.set_accel_odr(AccelOutputDataRate::Hz10).unwrap();
    let mut sensor = sensor.into_mag_continuous().ok().unwrap();

    let calibration = calc_calibration(&mut sensor, &mut display, &mut timer);
    rprintln!("Calibration: {:?}", calibration);
    rprintln!("Calibration done, entering busy loop");
    loop {
        while !sensor.mag_status().unwrap().xyz_new_data {}
        let mut data = sensor.mag_data().unwrap();
        data = calibrated_measurement(data, &calibration);

        // これはまだ core にないので libm の atan2f を使います
        let theta = atan2f(data.y as f32, data.x as f32);

        // theta に基づいて方角を判定する
        let dir = Direction::NorthEast;

        display.show(&mut timer, direction_to_led(dir), 100);
    }
}

ヒント:

• 1 回転は 360 度です。
• PI ラジアンは 180 度に相当します。
• theta が 0 なら、どの方向を指していますか？
• 逆に theta が 0 に非常に近い値なら、どの方向を指していますか？
• theta が増え続ける場合、どの値で方角を切り替えますか