//! Madgwick's orientation filter
//!
//! This demo runs Madgwick's orientation filter and logs the orientation of the board as
//! quaternions via the ITM. The data is encoded in binary format and logged as COBS frame. The
//! binary format is as follows:
//!
//! - `w`: `f32`, LE (Little Endian), 4 bytes
//! - `x`: `f32`, LE, 4 bytes
//! - `y`: `f32`, LE, 4 bytes
//! - `z`: `f32`, LE, 4 bytes
//!
//! where the quaternion is the tuple `(w, x, y, z)`
//!
//! The suggested way to receive this data is to connect the F3 SWO pin to a UART to USB converter
//! and then to read out the associated device file using `itmdump`. Make sure you configure the
//! serial device before calling `itmdump`. The commands to run are:
//!
//! ``` console
//! $ stty -F /dev/ttyUSB0 raw 2000000 -echo
//!
//! $ itmdump -f /dev/ttyUSB0 > data.txt
//! ```
//!
//! You can pipe the quaternions through the `viz` program (shipped with this crate) to get real
//! time [visualization]. The command to run is:
//!
//! [visualization]: https://mobile.twitter.com/japaricious/status/962770003325005824
//!
//! ``` console
//! $ itmdump -f /dev/ttyUSB0 | viz
//! ```
//!
//! ```
//! // #![deny(unsafe_code)]
//! #![deny(warnings)]
//! #![no_std]
//! 
//! extern crate aligned;
//! extern crate byteorder;
//! extern crate cast;
//! extern crate cobs;
//! extern crate cortex_m;
//! extern crate f3;
//! extern crate madgwick;
//! #[macro_use(block)]
//! extern crate nb;
//! 
//! use core::ptr;
//! use core::f32::consts::PI;
//! 
//! use aligned::Aligned;
//! use byteorder::{ByteOrder, LE};
//! use cast::{f32, i32};
//! use cortex_m::itm;
//! use f3::hal::i2c::I2c;
//! use f3::hal::prelude::*;
//! use f3::hal::spi::Spi;
//! use f3::hal::stm32f30x;
//! use f3::hal::timer::Timer;
//! use f3::l3gd20::{self, Odr};
//! use f3::lsm303dlhc::{AccelOdr, MagOdr};
//! use f3::{L3gd20, Lsm303dlhc};
//! use madgwick::{F32x3, Marg};
//! 
//! // Number of samples to use for gyroscope calibration
//! const NSAMPLES: i32 = 256;
//! 
//! // Magnetometer calibration parameters
//! // NOTE you need to use the right parameters for *your* magnetometer
//! // You can use the `log-sensors` example to calibrate your magnetometer. The producer is explained
//! // in https://github.com/kriswiner/MPU6050/wiki/Simple-and-Effective-Magnetometer-Calibration
//! const M_BIAS_X: f32 = -34.;
//! const M_SCALE_X: f32 = 650.;
//! 
//! const M_BIAS_Y: f32 = -70.;
//! const M_SCALE_Y: f32 = 636.;
//! 
//! const M_BIAS_Z: f32 = -37.5;
//! const M_SCALE_Z: f32 = 589.5;
//! 
//! // Sensitivities of the accelerometer and gyroscope, respectively
//! const K_G: f32 = 2. / (1 << 15) as f32; // LSB -> g
//! const K_AR: f32 = 8.75e-3 * PI / 180.; // LSB -> rad/s
//! 
//! // Madgwick filter parameters
//! const SAMPLE_FREQ: u32 = 220;
//! const BETA: f32 = 1e-3;
//! 
//! fn main() {
//!     let mut cp = cortex_m::Peripherals::take().unwrap();
//!     let dp = stm32f30x::Peripherals::take().unwrap();
//! 
//!     let mut flash = dp.FLASH.constrain();
//!     let mut rcc = dp.RCC.constrain();
//! 
//!     let clocks = rcc.cfgr
//!         .sysclk(64.mhz())
//!         .pclk1(32.mhz())
//!         .freeze(&mut flash.acr);
//! 
//!     // enable ITM
//!     // TODO this should be some high level API in the cortex-m crate
//!     unsafe {
//!         // enable TPIU and ITM
//!         cp.DCB.demcr.modify(|r| r | (1 << 24));
//! 
//!         // prescaler
//!         let swo_freq = 2_000_000;
//!         cp.TPIU.acpr.write((clocks.sysclk().0 / swo_freq) - 1);
//! 
//!         // SWO NRZ
//!         cp.TPIU.sppr.write(2);
//! 
//!         cp.TPIU.ffcr.modify(|r| r & !(1 << 1));
//! 
//!         // STM32 specific: enable tracing in the DBGMCU_CR register
//!         const DBGMCU_CR: *mut u32 = 0xe0042004 as *mut u32;
//!         let r = ptr::read_volatile(DBGMCU_CR);
//!         ptr::write_volatile(DBGMCU_CR, r | (1 << 5));
//! 
//!         // unlock the ITM
//!         cp.ITM.lar.write(0xC5ACCE55);
//! 
//!         cp.ITM.tcr.write(
//!             (0b000001 << 16) | // TraceBusID
//!             (1 << 3) | // enable SWO output
//!             (1 << 0), // enable the ITM
//!         );
//! 
//!         // enable stimulus port 0
//!         cp.ITM.ter[0].write(1);
//!     }
//! 
//!     let mut gpioa = dp.GPIOA.split(&mut rcc.ahb);
//!     let mut gpiob = dp.GPIOB.split(&mut rcc.ahb);
//!     let mut gpioe = dp.GPIOE.split(&mut rcc.ahb);
//! 
//!     let mut nss = gpioe
//!         .pe3
//!         .into_push_pull_output(&mut gpioe.moder, &mut gpioe.otyper);
//!     nss.set_high();
//!     let mut led = gpioe
//!         .pe9
//!         .into_push_pull_output(&mut gpioe.moder, &mut gpioe.otyper);
//! 
//!     let sck = gpioa.pa5.into_af5(&mut gpioa.moder, &mut gpioa.afrl);
//!     let miso = gpioa.pa6.into_af5(&mut gpioa.moder, &mut gpioa.afrl);
//!     let mosi = gpioa.pa7.into_af5(&mut gpioa.moder, &mut gpioa.afrl);
//! 
//!     let spi = Spi::spi1(
//!         dp.SPI1,
//!         (sck, miso, mosi),
//!         l3gd20::MODE,
//!         1.mhz(),
//!         clocks,
//!         &mut rcc.apb2,
//!     );
//! 
//!     let mut l3gd20 = L3gd20::new(spi, nss).unwrap();
//! 
//!     l3gd20.set_odr(Odr::Hz380).unwrap();
//! 
//!     let scl = gpiob.pb6.into_af4(&mut gpiob.moder, &mut gpiob.afrl);
//!     let sda = gpiob.pb7.into_af4(&mut gpiob.moder, &mut gpiob.afrl);
//! 
//!     let i2c = I2c::i2c1(dp.I2C1, (scl, sda), 400.khz(), clocks, &mut rcc.apb1);
//! 
//!     let mut lsm303dlhc = Lsm303dlhc::new(i2c).unwrap();
//! 
//!     lsm303dlhc.accel_odr(AccelOdr::Hz400).unwrap();
//!     lsm303dlhc.mag_odr(MagOdr::Hz220).unwrap();
//! 
//!     let mut timer = Timer::tim2(dp.TIM2, 380.hz(), clocks, &mut rcc.apb1);
//! 
//!     // Calibrate the gyroscope
//!     let mut ar_bias_x = 0;
//!     let mut ar_bias_y = 0;
//!     let mut ar_bias_z = 0;
//!     for _ in 0..NSAMPLES {
//!         block!(timer.wait()).unwrap();
//! 
//!         let ar = l3gd20.gyro().unwrap();
//! 
//!         ar_bias_x += i32(ar.x);
//!         ar_bias_y += i32(ar.y);
//!         ar_bias_z += i32(ar.z);
//!     }
//!     let ar_bias_x = (ar_bias_x / NSAMPLES) as i16;
//!     let ar_bias_y = (ar_bias_y / NSAMPLES) as i16;
//!     let ar_bias_z = (ar_bias_z / NSAMPLES) as i16;
//! 
//!     // Turn on the LED after calibrating the gyroscope
//!     led.set_high();
//! 
//!     let mut marg = Marg::new(BETA, 1. / f32(SAMPLE_FREQ));
//!     let mut timer = Timer::tim2(timer.free(), SAMPLE_FREQ.hz(), clocks, &mut rcc.apb1);
//! 
//!     let mut tx_buf: Aligned<u32, [u8; 18]> = Aligned([0; 18]);
//!     loop {
//!         block!(timer.wait()).unwrap();
//! 
//!         let m = lsm303dlhc.mag().unwrap();
//!         let ar = l3gd20.gyro().unwrap();
//!         let g = lsm303dlhc.accel().unwrap();
//! 
//!         let m_x = (f32(m.x) - M_BIAS_X) / M_SCALE_X;
//!         let m_y = (f32(m.y) - M_BIAS_Y) / M_SCALE_Y;
//!         let m_z = (f32(m.z) - M_BIAS_Z) / M_SCALE_Z;
//! 
//!         // Fix the X Y Z components of the magnetometer so they match the gyro axes
//!         let m = F32x3 {
//!             x: m_y,
//!             y: -m_x,
//!             z: m_z,
//!         };
//! 
//!         let ar_x = f32(ar.x - ar_bias_x) * K_AR;
//!         let ar_y = f32(ar.y - ar_bias_y) * K_AR;
//!         let ar_z = f32(ar.z - ar_bias_z) * K_AR;
//!         let ar = F32x3 {
//!             x: ar_x,
//!             y: ar_y,
//!             z: ar_z,
//!         };
//! 
//!         // Fix the X Y Z components of the accelerometer so they match the gyro axes
//!         let g_x = f32(g.x) * K_G;
//!         let g_y = f32(g.y) * K_G;
//!         let g_z = f32(g.z) * K_G;
//!         let g = F32x3 {
//!             x: g_y,
//!             y: -g_x,
//!             z: g_z,
//!         };
//! 
//!         // Run the filter
//!         let quat = marg.update(m, ar, g);
//! 
//!         // Serialize the quaternion
//!         let mut start = 0;
//!         let mut buf = [0; 16];
//!         LE::write_f32(&mut buf[start..start + 4], quat.0);
//!         start += 4;
//!         LE::write_f32(&mut buf[start..start + 4], quat.1);
//!         start += 4;
//!         LE::write_f32(&mut buf[start..start + 4], quat.2);
//!         start += 4;
//!         LE::write_f32(&mut buf[start..start + 4], quat.3);
//!         // start += 4;
//! 
//!         // Log data
//!         cobs::encode(&buf, &mut tx_buf.array);
//! 
//!         itm::write_aligned(&mut cp.ITM.stim[0], &tx_buf);
//!     }
//! }
//! ```
// Auto-generated. Do not modify.