bofh

diff --git a/src/lib.rs b/src/lib.rs
index ecabc08..84e5233 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -257,6 +257,13 @@ where
         })
     }
 
+    /// Self test for gyroscope and accelerometer
+    pub fn self_test(&mut self) -> Result<(), E> {

bofh

dps = 250.0 # Based on chip settings
d2r = 3.14159265359 / 180.0

gyro_scale = dps / 32767.5 * d2r
gyro_x = np.array(gyro_x) * gyro_scale
gyro_y = np.array(gyro_y) * gyro_scale
gyro_z = np.array(gyro_z) * gyro_scale

ar = 2.0 # Based on chip settings
acc_scale = 9.807 * ar / 32767.5

bofh

    let device = hal::stm32f30x::Peripherals::take().unwrap();
    let mut core = Peripherals::take().unwrap();

    // Configure PC13 as an input
    device.RCC.ahbenr.modify(|_, w| w.iopcen().set_bit());
    device.GPIOC.moder.modify(
        |_, w| w.moder13().input()
    );
    device.GPIOC.pupdr.modify(|_, w| unsafe {
        w.pupdr13().bits(0b01)  // Pull-up

bofh

#![no_std]
#![no_main]
#![feature(core_intrinsics)]

use core::intrinsics;
use core::panic::PanicInfo;
use cortex_m_rt::entry;
use hal::stm32f30x::{self, interrupt};

use hal::serial;

bofh

#![no_std]
#![no_main]
#![feature(core_intrinsics)]

use core::intrinsics;
use core::panic::PanicInfo;
use cortex_m_rt::entry;
use hal::stm32f30x::{self, interrupt};
use hal::gpio::PullUp;

bofh

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

extern crate panic_semihosting;

use rtfm::app;

#[app(device = stm32f30x)]
const APP: () = {

bofh

/* Linker script for the STM32F303K8 */
MEMORY
{
  /* NOTE 1 K = 1 KiBi = 1024 bytes */
  FLASH : ORIGIN = 0x08000000, LENGTH = 64K
  RAM : ORIGIN = 0x20000000, LENGTH = 12K
}

bofh

0.08140577 0.18196583 10.211635 0.0037284256 -0.0055926386 0.04474111 0.007977625 -3.0261583 -0.0062152203 0.015819557
0.11492579 0.16041724 10.1422 0.0030626352 -0.0059921127 0.048203215 0.007977937 -3.0253944 -0.0062229135 0.015827162
0.093377195 0.1292915 10.185298 0.004261058 -0.004394216 0.04926848 0.007978375 -3.0246198 -0.0062241764 0.015838083
0.047885742 0.1676001 10.216423 0.002530003 -0.004394216 0.047404267 0.0079784375 -3.0238547 -0.006220216 0.015833206
0.090982914 0.16041724 10.149384 0.002530003 -0.0034621095 0.047537427 0.007977937 -3.0230792 -0.006229325 0.015833184
0.08140577 0.15562867 10.185298 0.002130529 -0.006524745 0.047670584 0.007978188 -3.022323 -0.006257707 0.015832609
0.052674316 0.17478296 10.221211 0.0015978967 -0.0029294773 0.048869006 0.0079785 -3.0215502 -0.006241512 0.0158118
0.062251467 0.15323438 10.137412 0.0041278997 -0.004660532 0.04926848 0.0079775 -3.0207667 -0.0062190653 0.015825406
0.05028003 0.15084009 10.137412 0.0026631611 -0.00479369 0.048469532 0.007977937 -3.

bofh

0.08140577 0.18196583 10.211635 0.0037284256 -0.0055926386 0.04474111 0.007977625 0.00035692778 -0.005511612 0.012298794 
0.11492579 0.16041724 10.1422 0.0030626352 -0.0059921127 0.048203215 0.007977937 0.00074087887 -0.0082964115 0.013977079 
0.093377195 0.1292915 10.185298 0.004261058 -0.004394216 0.04926848 0.007978375 0.0011334465 -0.0085896 0.013607653 
0.047885742 0.1676001 10.216423 0.002530003 -0.004394216 0.047404267 0.0079784375 0.0015111603 -0.0077588395 0.014238404 
0.090982914 0.16041724 10.149384 0.002530003 -0.0034621095 0.047537427 0.007977937 0.0018899906 -0.008037852 0.014580941 
0.08140577 0.15562867 10.185298 0.002130529 -0.006524745 0.047670584 0.007978188 0.002269528 -0.008078649 0.014705296 
0.052674316 0.17478296 10.221211 0.0015978967 -0.0029294773 0.048869006 0.0079785 0.002659056 -0.007726158 0.015022709 
0.062251467 0.15323438 10.137412 0.0041278997 -0.004660532 0.04926848 0.0079775 0.0030515043 -0.007542186 0.015061276 
0.05028003 0.15084009 10.137412 0.0026631611 -0.00479369 0.04

bofh

0.00035692778,-0.005511612,0.012298794,0.00035692778,-0.005511612,0.012298794,
0.00074087887,-0.0082964115,0.013977079,0.00074087887,-0.0082964115,0.013977079,
0.0011334465,-0.0085896,0.013607653,0.0011334465,-0.0085896,0.013607653,
0.0015111603,-0.0077588395,0.014238404,0.0015111603,-0.0077588395,0.014238404,
0.0018899906,-0.008037852,0.014580941,0.0018899906,-0.008037852,0.014580941,
0.002269528,-0.008078649,0.014705296,0.002269528,-0.008078649,0.014705296,
0.002659056,-0.007726158,0.015022709,0.002659056,-0.007726158,0.015022709,
0.0030515043,-0.007542186,0.015061276,0.0030515043,-0.007542186,0.015061276,
0.003437582,-0.0072640767,0.015054976,0.003437582,-0.0072640767,0.015054976,
0.0038237115,-0.0073329946,0.015249286,0.0038237115,-0.0073329946,0.015249286,