From 97b8b7fbd070c924a6ffcdc468581fb3eeb907ba Mon Sep 17 00:00:00 2001
From: Nicolas Rabault <nicolas.rabault@luos.io>
Date: Sat, 24 Aug 2024 15:11:49 +0200
Subject: [PATCH] [IMU] massive cleanup - Luos msgHandler are not in IRQ since
 Luos 2.0 - Complete Robus config is done on RobusHAL - Do not poll IMU
 without detection - Remove warnings

---
 examples/projects/l0/imu/include/main.h       |   18 -
 examples/projects/l0/imu/lib/Imu/imu.c        |   42 +-
 .../core/driver/eMPL/inv_mpu.c                | 1523 +++++++++--------
 .../driver/eMPL/inv_mpu_dmp_motion_driver.c   |  728 ++++----
 .../core/driver/include/mlinclude.h           |    4 +-
 .../l0/imu/lib/Imu/mpu_configuration.c        |  148 +-
 .../l0/imu/lib/Imu/mpu_configuration.h        |    9 +-
 examples/projects/l0/imu/node_config.h        |    3 +-
 examples/projects/l0/imu/src/gpio.c           |  149 +-
 examples/projects/l0/imu/src/main.c           |    2 -
 examples/projects/l0/imu/src/stm32f0xx_it.c   |   91 +-
 examples/projects/l0/imu/src/usart.c          |   80 -
 12 files changed, 1350 insertions(+), 1447 deletions(-)
 delete mode 100644 examples/projects/l0/imu/src/usart.c

diff --git a/examples/projects/l0/imu/include/main.h b/examples/projects/l0/imu/include/main.h
index 1b5015a58..22220b1eb 100644
--- a/examples/projects/l0/imu/include/main.h
+++ b/examples/projects/l0/imu/include/main.h
@@ -93,30 +93,12 @@ void Error_Handler(void);
 #define INVEN_INT_EXTI_IRQn EXTI0_1_IRQn
 #define CS_Pin GPIO_PIN_1
 #define CS_GPIO_Port GPIOA
-#define POWER_SENSOR_Pin GPIO_PIN_2
-#define POWER_SENSOR_GPIO_Port GPIOA
 #define LED_Pin GPIO_PIN_3
 #define LED_GPIO_Port GPIOA
-#define COM_LVL_DOWN_Pin GPIO_PIN_5
-#define COM_LVL_DOWN_GPIO_Port GPIOA
-#define COM_LVL_UP_Pin GPIO_PIN_6
-#define COM_LVL_UP_GPIO_Port GPIOA
 #define SCL_Pin GPIO_PIN_10
 #define SCL_GPIO_Port GPIOB
 #define SDA_Pin GPIO_PIN_11
 #define SDA_GPIO_Port GPIOB
-#define PTPB_Pin GPIO_PIN_13
-#define PTPB_GPIO_Port GPIOB
-#define RX_EN_Pin GPIO_PIN_14
-#define RX_EN_GPIO_Port GPIOB
-#define TX_EN_Pin GPIO_PIN_15
-#define TX_EN_GPIO_Port GPIOB
-#define PTPA_Pin GPIO_PIN_8
-#define PTPA_GPIO_Port GPIOA
-#define COM_TX_Pin GPIO_PIN_9
-#define COM_TX_GPIO_Port GPIOA
-#define COM_RX_Pin GPIO_PIN_10
-#define COM_RX_GPIO_Port GPIOA
 /* USER CODE BEGIN Private defines */
 #define FIRM_REV "0.5.1"
     void gyro_data_ready_cb(void);
diff --git a/examples/projects/l0/imu/lib/Imu/imu.c b/examples/projects/l0/imu/lib/Imu/imu.c
index 597ecd08a..1b986eb21 100644
--- a/examples/projects/l0/imu/lib/Imu/imu.c
+++ b/examples/projects/l0/imu/lib/Imu/imu.c
@@ -6,7 +6,8 @@
  ******************************************************************************/
 #include "main.h"
 #include "imu.h"
-#include <mpu_configuration.h>
+#include "mpu_configuration.h"
+#include "data_builder.h"
 
 /*******************************************************************************
  * Definitions
@@ -18,10 +19,6 @@
 volatile uint32_t hal_timestamp = 0;
 unsigned char *mpl_key          = (unsigned char *)"eMPL 5.1";
 
-service_t *service_pointer;
-volatile msg_t pub_msg;
-volatile int pub = LUOS_LAST_STD_CMD;
-
 /*******************************************************************************
  * Function
  ******************************************************************************/
@@ -36,6 +33,7 @@ void Imu_Init(void)
 {
     revision_t revision = {.major = 1, .minor = 0, .build = 0};
     mpu_setup();
+    HAL_GPIO_WritePin(GPIOA, LED_Pin, GPIO_PIN_RESET);
     hal.report.quat = 1;
     Luos_CreateService(Imu_MsgHandler, IMU_TYPE, "Imu", revision);
 }
@@ -46,11 +44,15 @@ void Imu_Init(void)
  ******************************************************************************/
 void Imu_Loop(void)
 {
+    if (!Luos_IsDetected())
+    {
+        return;
+    }
+
     // *********************IMU management*******************************
     unsigned long sensor_timestamp;
-    unsigned long timestamp;
+    unsigned long timestamp       = Luos_GetSystick();
     int new_data                  = 0;
-    timestamp                     = HAL_GetTick();
     static unsigned char new_temp = 0;
 #ifdef COMPASS_ENABLED
     static unsigned char new_compass = 0;
@@ -226,11 +228,6 @@ void Imu_Loop(void)
          * rate requested by the host.
          */
     }
-    if (hal.update_request == 1)
-    {
-        read_from_mpl(service_pointer);
-        hal.update_request = 0;
-    }
 }
 /******************************************************************************
  * @brief Msg Handler call back when a msg receive for this service
@@ -242,19 +239,15 @@ static void Imu_MsgHandler(service_t *service, const msg_t *msg)
 {
     if (msg->header.cmd == GET_CMD)
     {
-        // fill the message infos
-        hal.update_request = 1;
-        service_pointer    = service;
-        hal.source_id      = msg->header.source;
-        pub                = LUOS_LAST_STD_CMD;
+        // Fill the message infos
+        hal.source_id = msg->header.source;
+        read_from_mpl(service);
         return;
     }
     if (msg->header.cmd == PARAMETERS)
     {
-        service_pointer = service;
-        // fill the message infos
+        // Get the message infos
         memcpy(&hal.report, msg->data, sizeof(short));
-        pub = LUOS_LAST_STD_CMD;
         return;
     }
 }
@@ -266,12 +259,3 @@ void gyro_data_ready_cb(void)
 {
     hal.new_gyro = 1;
 }
-
-void HAL_SYSTICK_Callback(void)
-{
-    if (pub != LUOS_LAST_STD_CMD)
-    {
-        Luos_SendMsg(service_pointer, (msg_t *)&pub_msg);
-        pub = LUOS_LAST_STD_CMD;
-    }
-}
diff --git a/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/eMPL/inv_mpu.c b/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/eMPL/inv_mpu.c
index 833c3dfd8..42a35dcc6 100644
--- a/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/eMPL/inv_mpu.c
+++ b/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/eMPL/inv_mpu.c
@@ -37,89 +37,95 @@
  * min(int a, int b)
  */
 #if defined EMPL_TARGET_STM32F4
-#include "i2c.h"   
-#include "main.h"
-#include "log.h"
-   
-#define i2c_write   Sensors_I2C_WriteRegister
-#define i2c_read    Sensors_I2C_ReadRegister 
-#define delay_ms    mdelay
-#define get_ms      get_tick_count
-#define log_i       MPL_LOGI
-#define log_e       MPL_LOGE
-#define min(a,b) ((a<b)?a:b)
-   
+    #include "i2c.h"
+    #include "main.h"
+    #include "log.h"
+
+    #define i2c_write Sensors_I2C_WriteRegister
+    #define i2c_read  Sensors_I2C_ReadRegister
+    #define delay_ms  mdelay
+    #define get_ms    get_systick_count
+    #define log_i     MPL_LOGI
+    #define log_e     MPL_LOGE
+    #define min(a, b) ((a < b) ? a : b)
+
 #elif defined MOTION_DRIVER_TARGET_MSP430
-#include "msp430.h"
-#include "msp430_i2c.h"
-#include "msp430_clock.h"
-#include "msp430_interrupt.h"
-#define i2c_write   msp430_i2c_write
-#define i2c_read    msp430_i2c_read
-#define delay_ms    msp430_delay_ms
-#define get_ms      msp430_get_clock_ms
+    #include "msp430.h"
+    #include "msp430_i2c.h"
+    #include "msp430_clock.h"
+    #include "msp430_interrupt.h"
+    #define i2c_write msp430_i2c_write
+    #define i2c_read  msp430_i2c_read
+    #define delay_ms  msp430_delay_ms
+    #define get_ms    msp430_get_clock_ms
 static inline int reg_int_cb(struct int_param_s *int_param)
 {
     return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
-        int_param->active_low);
+                             int_param->active_low);
 }
-#define log_i(...)     do {} while (0)
-#define log_e(...)     do {} while (0)
-/* labs is already defined by TI's toolchain. */
-/* fabs is for doubles. fabsf is for floats. */
-#define fabs        fabsf
-#define min(a,b) ((a<b)?a:b)
+    #define log_i(...) \
+        do             \
+        {              \
+        } while (0)
+    #define log_e(...) \
+        do             \
+        {              \
+        } while (0)
+    /* labs is already defined by TI's toolchain. */
+    /* fabs is for doubles. fabsf is for floats. */
+    #define fabs      fabsf
+    #define min(a, b) ((a < b) ? a : b)
 #elif defined EMPL_TARGET_MSP430
-#include "msp430.h"
-#include "msp430_i2c.h"
-#include "msp430_clock.h"
-#include "msp430_interrupt.h"
-#include "log.h"
-#define i2c_write   msp430_i2c_write
-#define i2c_read    msp430_i2c_read
-#define delay_ms    msp430_delay_ms
-#define get_ms      msp430_get_clock_ms
+    #include "msp430.h"
+    #include "msp430_i2c.h"
+    #include "msp430_clock.h"
+    #include "msp430_interrupt.h"
+    #include "log.h"
+    #define i2c_write msp430_i2c_write
+    #define i2c_read  msp430_i2c_read
+    #define delay_ms  msp430_delay_ms
+    #define get_ms    msp430_get_clock_ms
 static inline int reg_int_cb(struct int_param_s *int_param)
 {
     return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
-        int_param->active_low);
+                             int_param->active_low);
 }
-#define log_i       MPL_LOGI
-#define log_e       MPL_LOGE
-/* labs is already defined by TI's toolchain. */
-/* fabs is for doubles. fabsf is for floats. */
-#define fabs        fabsf
-#define min(a,b) ((a<b)?a:b)
+    #define log_i     MPL_LOGI
+    #define log_e     MPL_LOGE
+    /* labs is already defined by TI's toolchain. */
+    /* fabs is for doubles. fabsf is for floats. */
+    #define fabs      fabsf
+    #define min(a, b) ((a < b) ? a : b)
 #elif defined EMPL_TARGET_UC3L0
-/* Instead of using the standard TWI driver from the ASF library, we're using
- * a TWI driver that follows the slave address + register address convention.
- */
-#include "twi.h"
-#include "delay.h"
-#include "sysclk.h"
-#include "log.h"
-#include "sensors_xplained.h"
-#include "uc3l0_clock.h"
-#define i2c_write(a, b, c, d)   twi_write(a, b, d, c)
-#define i2c_read(a, b, c, d)    twi_read(a, b, d, c)
-/* delay_ms is a function already defined in ASF. */
-#define get_ms  uc3l0_get_clock_ms
+    /* Instead of using the standard TWI driver from the ASF library, we're using
+     * a TWI driver that follows the slave address + register address convention.
+     */
+    #include "twi.h"
+    #include "delay.h"
+    #include "sysclk.h"
+    #include "log.h"
+    #include "sensors_xplained.h"
+    #include "uc3l0_clock.h"
+    #define i2c_write(a, b, c, d) twi_write(a, b, d, c)
+    #define i2c_read(a, b, c, d)  twi_read(a, b, d, c)
+    /* delay_ms is a function already defined in ASF. */
+    #define get_ms                uc3l0_get_clock_ms
 static inline int reg_int_cb(struct int_param_s *int_param)
 {
     sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg);
     return 0;
 }
-#define log_i       MPL_LOGI
-#define log_e       MPL_LOGE
-/* UC3 is a 32-bit processor, so abs and labs are equivalent. */
-#define labs        abs
-#define fabs(x)     (((x)>0)?(x):-(x))
+    #define log_i                 MPL_LOGI
+    #define log_e                 MPL_LOGE
+    /* UC3 is a 32-bit processor, so abs and labs are equivalent. */
+    #define labs                  abs
+    #define fabs(x)               (((x) > 0) ? (x) : -(x))
 #else
-#error  Gyro driver is missing the system layer implementations.
+    #error Gyro driver is missing the system layer implementations.
 #endif
 
 #if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
-#error  Which gyro are you using? Define MPUxxxx in your compiler options.
+    #error Which gyro are you using? Define MPUxxxx in your compiler options.
 #endif
 
 /* Time for some messy macro work. =]
@@ -134,35 +140,41 @@ static inline int reg_int_cb(struct int_param_s *int_param)
  * #define AK8963_SECONDARY
  */
 #if defined MPU9150
-#ifndef MPU6050
-#define MPU6050
-#endif                          /* #ifndef MPU6050 */
-#if defined AK8963_SECONDARY
-#error "MPU9150 and AK8963_SECONDARY cannot both be defined."
-#elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
-#define AK8975_SECONDARY
-#endif                          /* #if defined AK8963_SECONDARY */
-#elif defined MPU9250           /* #if defined MPU9150 */
-#ifndef MPU6500
-#define MPU6500
-#endif                          /* #ifndef MPU6500 */
-#if defined AK8975_SECONDARY
-#error "MPU9250 and AK8975_SECONDARY cannot both be defined."
-#elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
-#define AK8963_SECONDARY
-#endif                          /* #if defined AK8975_SECONDARY */
-#endif                          /* #if defined MPU9150 */
+    #ifndef MPU6050
+        #define MPU6050
+    #endif /* #ifndef MPU6050 */
+    #if defined AK8963_SECONDARY
+        #error "MPU9150 and AK8963_SECONDARY cannot both be defined."
+    #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
+        #define AK8975_SECONDARY
+    #endif            /* #if defined AK8963_SECONDARY */
+#elif defined MPU9250 /* #if defined MPU9150 */
+    #ifndef MPU6500
+        #define MPU6500
+    #endif /* #ifndef MPU6500 */
+    #if defined AK8975_SECONDARY
+        #error "MPU9250 and AK8975_SECONDARY cannot both be defined."
+    #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
+        #define AK8963_SECONDARY
+    #endif /* #if defined AK8975_SECONDARY */
+#endif     /* #if defined MPU9150 */
 
 #if defined AK8975_SECONDARY || defined AK8963_SECONDARY
-#define AK89xx_SECONDARY
+    #define AK89xx_SECONDARY
 #else
 /* #warning "No compass = less profit for Invensense. Lame." */
 #endif
 
 static int set_int_enable(unsigned char enable);
 
+static void get_systick_count(unsigned long *count)
+{
+    *count = HAL_GetTick();
+}
+
 /* Hardware registers needed by driver. */
-struct gyro_reg_s {
+struct gyro_reg_s
+{
     unsigned char who_am_i;
     unsigned char rate_div;
     unsigned char lpf;
@@ -211,7 +223,8 @@ struct gyro_reg_s {
 };
 
 /* Information specific to a particular device. */
-struct hw_s {
+struct hw_s
+{
     unsigned char addr;
     unsigned short max_fifo;
     unsigned char num_reg;
@@ -227,7 +240,8 @@ struct hw_s {
  * previous state so that it can be restored at a later time.
  * TODO: This is tacky. Fix it.
  */
-struct motion_int_cache_s {
+struct motion_int_cache_s
+{
     unsigned short gyro_fsr;
     unsigned char accel_fsr;
     unsigned short lpf;
@@ -240,7 +254,8 @@ struct motion_int_cache_s {
 /* Cached chip configuration data.
  * TODO: A lot of these can be handled with a bitmask.
  */
-struct chip_cfg_s {
+struct chip_cfg_s
+{
     /* Matches gyro_cfg >> 3 & 0x03 */
     unsigned char gyro_fsr;
     /* Matches accel_cfg >> 3 & 0x03 */
@@ -287,7 +302,8 @@ struct chip_cfg_s {
 };
 
 /* Information for self-test. */
-struct test_s {
+struct test_s
+{
     unsigned long gyro_sens;
     unsigned long accel_sens;
     unsigned char reg_rate_div;
@@ -309,7 +325,8 @@ struct test_s {
 };
 
 /* Gyro driver state variables. */
-struct gyro_state_s {
+struct gyro_state_s
+{
     const struct gyro_reg_s *reg;
     const struct hw_s *hw;
     struct chip_cfg_s chip_cfg;
@@ -317,7 +334,8 @@ struct gyro_state_s {
 };
 
 /* Filter configurations. */
-enum lpf_e {
+enum lpf_e
+{
     INV_FILTER_256HZ_NOLPF2 = 0,
     INV_FILTER_188HZ,
     INV_FILTER_98HZ,
@@ -330,7 +348,8 @@ enum lpf_e {
 };
 
 /* Full scale ranges. */
-enum gyro_fsr_e {
+enum gyro_fsr_e
+{
     INV_FSR_250DPS = 0,
     INV_FSR_500DPS,
     INV_FSR_1000DPS,
@@ -339,7 +358,8 @@ enum gyro_fsr_e {
 };
 
 /* Full scale ranges. */
-enum accel_fsr_e {
+enum accel_fsr_e
+{
     INV_FSR_2G = 0,
     INV_FSR_4G,
     INV_FSR_8G,
@@ -348,14 +368,16 @@ enum accel_fsr_e {
 };
 
 /* Clock sources. */
-enum clock_sel_e {
+enum clock_sel_e
+{
     INV_CLK_INTERNAL = 0,
     INV_CLK_PLL,
     NUM_CLK
 };
 
 /* Low-power accel wakeup rates. */
-enum lp_accel_rate_e {
+enum lp_accel_rate_e
+{
 #if defined MPU6050
     INV_LPA_1_25HZ,
     INV_LPA_5HZ,
@@ -420,39 +442,39 @@ enum lp_accel_rate_e {
 #define BIT_STBY_XYZG       (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
 
 #if defined AK8975_SECONDARY
-#define SUPPORTS_AK89xx_HIGH_SENS   (0x00)
-#define AK89xx_FSR                  (9830)
+    #define SUPPORTS_AK89xx_HIGH_SENS (0x00)
+    #define AK89xx_FSR                (9830)
 #elif defined AK8963_SECONDARY
-#define SUPPORTS_AK89xx_HIGH_SENS   (0x10)
-#define AK89xx_FSR                  (4915)
+    #define SUPPORTS_AK89xx_HIGH_SENS (0x10)
+    #define AK89xx_FSR                (4915)
 #endif
 
 #ifdef AK89xx_SECONDARY
-#define AKM_REG_WHOAMI      (0x00)
+    #define AKM_REG_WHOAMI (0x00)
 
-#define AKM_REG_ST1         (0x02)
-#define AKM_REG_HXL         (0x03)
-#define AKM_REG_ST2         (0x09)
+    #define AKM_REG_ST1 (0x02)
+    #define AKM_REG_HXL (0x03)
+    #define AKM_REG_ST2 (0x09)
 
-#define AKM_REG_CNTL        (0x0A)
-#define AKM_REG_ASTC        (0x0C)
-#define AKM_REG_ASAX        (0x10)
-#define AKM_REG_ASAY        (0x11)
-#define AKM_REG_ASAZ        (0x12)
+    #define AKM_REG_CNTL (0x0A)
+    #define AKM_REG_ASTC (0x0C)
+    #define AKM_REG_ASAX (0x10)
+    #define AKM_REG_ASAY (0x11)
+    #define AKM_REG_ASAZ (0x12)
 
-#define AKM_DATA_READY      (0x01)
-#define AKM_DATA_OVERRUN    (0x02)
-#define AKM_OVERFLOW        (0x80)
-#define AKM_DATA_ERROR      (0x40)
+    #define AKM_DATA_READY   (0x01)
+    #define AKM_DATA_OVERRUN (0x02)
+    #define AKM_OVERFLOW     (0x80)
+    #define AKM_DATA_ERROR   (0x40)
 
-#define AKM_BIT_SELF_TEST   (0x40)
+    #define AKM_BIT_SELF_TEST (0x40)
 
-#define AKM_POWER_DOWN          (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
-#define AKM_SINGLE_MEASUREMENT  (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
-#define AKM_FUSE_ROM_ACCESS     (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
-#define AKM_MODE_SELF_TEST      (0x08 | SUPPORTS_AK89xx_HIGH_SENS)
+    #define AKM_POWER_DOWN         (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
+    #define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
+    #define AKM_FUSE_ROM_ACCESS    (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
+    #define AKM_MODE_SELF_TEST     (0x08 | SUPPORTS_AK89xx_HIGH_SENS)
 
-#define AKM_WHOAMI      (0x48)
+    #define AKM_WHOAMI (0x48)
 #endif
 
 #if defined MPU6050
@@ -484,8 +506,9 @@ const struct gyro_reg_s reg = {
     .bank_sel       = 0x6D,
     .mem_start_addr = 0x6E,
     .prgm_start_h   = 0x70
-#ifdef AK89xx_SECONDARY
-    ,.raw_compass   = 0x49,
+    #ifdef AK89xx_SECONDARY
+    ,
+    .raw_compass    = 0x49,
     .yg_offs_tc     = 0x01,
     .s0_addr        = 0x25,
     .s0_reg         = 0x26,
@@ -497,42 +520,41 @@ const struct gyro_reg_s reg = {
     .s0_do          = 0x63,
     .s1_do          = 0x64,
     .i2c_delay_ctrl = 0x67
-#endif
+    #endif
 };
 const struct hw_s hw = {
-    .addr           = 0x68,
-    .max_fifo       = 1024,
-    .num_reg        = 118,
-    .temp_sens      = 340,
-    .temp_offset    = -521,
-    .bank_size      = 256
-#if defined AK89xx_SECONDARY
-    ,.compass_fsr    = AK89xx_FSR
-#endif
+    .addr        = 0x68,
+    .max_fifo    = 1024,
+    .num_reg     = 118,
+    .temp_sens   = 340,
+    .temp_offset = -521,
+    .bank_size   = 256
+    #if defined AK89xx_SECONDARY
+    ,
+    .compass_fsr = AK89xx_FSR
+    #endif
 };
 
 const struct test_s test = {
-    .gyro_sens      = 32768/250,
-    .accel_sens     = 32768/16,
-    .reg_rate_div   = 0,    /* 1kHz. */
-    .reg_lpf        = 1,    /* 188Hz. */
-    .reg_gyro_fsr   = 0,    /* 250dps. */
-    .reg_accel_fsr  = 0x18, /* 16g. */
-    .wait_ms        = 50,
-    .packet_thresh  = 5,    /* 5% */
-    .min_dps        = 10.f,
-    .max_dps        = 105.f,
-    .max_gyro_var   = 0.14f,
-    .min_g          = 0.3f,
-    .max_g          = 0.95f,
-    .max_accel_var  = 0.14f
-};
+    .gyro_sens     = 32768 / 250,
+    .accel_sens    = 32768 / 16,
+    .reg_rate_div  = 0,    /* 1kHz. */
+    .reg_lpf       = 1,    /* 188Hz. */
+    .reg_gyro_fsr  = 0,    /* 250dps. */
+    .reg_accel_fsr = 0x18, /* 16g. */
+    .wait_ms       = 50,
+    .packet_thresh = 5, /* 5% */
+    .min_dps       = 10.f,
+    .max_dps       = 105.f,
+    .max_gyro_var  = 0.14f,
+    .min_g         = 0.3f,
+    .max_g         = 0.95f,
+    .max_accel_var = 0.14f};
 
 static struct gyro_state_s st = {
-    .reg = &reg,
-    .hw = &hw,
-    .test = &test
-};
+    .reg  = &reg,
+    .hw   = &hw,
+    .test = &test};
 #elif defined MPU6500
 const struct gyro_reg_s reg = {
     .who_am_i       = 0x75,
@@ -565,8 +587,9 @@ const struct gyro_reg_s reg = {
     .bank_sel       = 0x6D,
     .mem_start_addr = 0x6E,
     .prgm_start_h   = 0x70
-#ifdef AK89xx_SECONDARY
-    ,.raw_compass   = 0x49,
+    #ifdef AK89xx_SECONDARY
+    ,
+    .raw_compass    = 0x49,
     .s0_addr        = 0x25,
     .s0_reg         = 0x26,
     .s0_ctrl        = 0x27,
@@ -577,54 +600,54 @@ const struct gyro_reg_s reg = {
     .s0_do          = 0x63,
     .s1_do          = 0x64,
     .i2c_delay_ctrl = 0x67
-#endif
+    #endif
 };
 const struct hw_s hw = {
-    .addr           = 0x68,
-    .max_fifo       = 1024,
-    .num_reg        = 128,
-    .temp_sens      = 321,
-    .temp_offset    = 0,
-    .bank_size      = 256
-#if defined AK89xx_SECONDARY
-    ,.compass_fsr    = AK89xx_FSR
-#endif
+    .addr        = 0x68,
+    .max_fifo    = 1024,
+    .num_reg     = 128,
+    .temp_sens   = 321,
+    .temp_offset = 0,
+    .bank_size   = 256
+    #if defined AK89xx_SECONDARY
+    ,
+    .compass_fsr = AK89xx_FSR
+    #endif
 };
 
 const struct test_s test = {
-    .gyro_sens      = 32768/250,
-    .accel_sens     = 32768/2,  //FSR = +-2G = 16384 LSB/G
-    .reg_rate_div   = 0,    /* 1kHz. */
-    .reg_lpf        = 2,    /* 92Hz low pass filter*/
-    .reg_gyro_fsr   = 0,    /* 250dps. */
-    .reg_accel_fsr  = 0x0,  /* Accel FSR setting = 2g. */
-    .wait_ms        = 200,   //200ms stabilization time
-    .packet_thresh  = 200,    /* 200 samples */
-    .min_dps        = 20.f,  //20 dps for Gyro Criteria C
-    .max_dps        = 60.f, //Must exceed 60 dps threshold for Gyro Criteria B
-    .max_gyro_var   = .5f, //Must exceed +50% variation for Gyro Criteria A
-    .min_g          = .225f, //Accel must exceed Min 225 mg for Criteria B
-    .max_g          = .675f, //Accel cannot exceed Max 675 mg for Criteria B
-    .max_accel_var  = .5f,  //Accel must be within 50% variation for Criteria A
-    .max_g_offset   = .5f,   //500 mg for Accel Criteria C
-    .sample_wait_ms = 10    //10ms sample time wait
+    .gyro_sens      = 32768 / 250,
+    .accel_sens     = 32768 / 2, // FSR = +-2G = 16384 LSB/G
+    .reg_rate_div   = 0,         /* 1kHz. */
+    .reg_lpf        = 2,         /* 92Hz low pass filter*/
+    .reg_gyro_fsr   = 0,         /* 250dps. */
+    .reg_accel_fsr  = 0x0,       /* Accel FSR setting = 2g. */
+    .wait_ms        = 200,       // 200ms stabilization time
+    .packet_thresh  = 200,       /* 200 samples */
+    .min_dps        = 20.f,      // 20 dps for Gyro Criteria C
+    .max_dps        = 60.f,      // Must exceed 60 dps threshold for Gyro Criteria B
+    .max_gyro_var   = .5f,       // Must exceed +50% variation for Gyro Criteria A
+    .min_g          = .225f,     // Accel must exceed Min 225 mg for Criteria B
+    .max_g          = .675f,     // Accel cannot exceed Max 675 mg for Criteria B
+    .max_accel_var  = .5f,       // Accel must be within 50% variation for Criteria A
+    .max_g_offset   = .5f,       // 500 mg for Accel Criteria C
+    .sample_wait_ms = 10         // 10ms sample time wait
 };
 
 static struct gyro_state_s st = {
-    .reg = &reg,
-    .hw = &hw,
-    .test = &test
-};
+    .reg  = &reg,
+    .hw   = &hw,
+    .test = &test};
 #endif
 
 #define MAX_PACKET_LENGTH (12)
 #ifdef MPU6500
-#define HWST_MAX_PACKET_LENGTH (512)
+    #define HWST_MAX_PACKET_LENGTH (512)
 #endif
 
 #ifdef AK89xx_SECONDARY
 static int setup_compass(void);
-#define MAX_COMPASS_SAMPLE_RATE (100)
+    #define MAX_COMPASS_SAMPLE_RATE (100)
 #endif
 
 /**
@@ -638,7 +661,8 @@ static int set_int_enable(unsigned char enable)
 {
     unsigned char tmp;
 
-    if (st.chip_cfg.dmp_on) {
+    if (st.chip_cfg.dmp_on)
+    {
         if (enable)
             tmp = BIT_DMP_INT_EN;
         else
@@ -646,7 +670,9 @@ static int set_int_enable(unsigned char enable)
         if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
             return -1;
         st.chip_cfg.int_enable = tmp;
-    } else {
+    }
+    else
+    {
         if (!st.chip_cfg.sensors)
             return -1;
         if (enable && st.chip_cfg.int_enable)
@@ -671,7 +697,8 @@ int mpu_reg_dump(void)
     unsigned char ii;
     unsigned char data;
 
-    for (ii = 0; ii < st.hw->num_reg; ii++) {
+    for (ii = 0; ii < st.hw->num_reg; ii++)
+    {
         if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w)
             continue;
         if (i2c_read(st.hw->addr, ii, 1, &data))
@@ -725,7 +752,7 @@ int mpu_init(struct int_param_s *int_param)
     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
         return -1;
 
-   st.chip_cfg.accel_half = 0;
+    st.chip_cfg.accel_half = 0;
 
 #ifdef MPU6500
     /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
@@ -737,10 +764,10 @@ int mpu_init(struct int_param_s *int_param)
 #endif
 
     /* Set to invalid values to ensure no I2C writes are skipped. */
-    st.chip_cfg.sensors = 0xFF;
-    st.chip_cfg.gyro_fsr = 0xFF;
-    st.chip_cfg.accel_fsr = 0xFF;
-    st.chip_cfg.lpf = 0xFF;
+    st.chip_cfg.sensors     = 0xFF;
+    st.chip_cfg.gyro_fsr    = 0xFF;
+    st.chip_cfg.accel_fsr   = 0xFF;
+    st.chip_cfg.lpf         = 0xFF;
     st.chip_cfg.sample_rate = 0xFFFF;
     st.chip_cfg.fifo_enable = 0xFF;
     st.chip_cfg.bypass_mode = 0xFF;
@@ -750,13 +777,13 @@ int mpu_init(struct int_param_s *int_param)
     /* mpu_set_sensors always preserves this setting. */
     st.chip_cfg.clk_src = INV_CLK_PLL;
     /* Handled in next call to mpu_set_bypass. */
-    st.chip_cfg.active_low_int = 1;
-    st.chip_cfg.latched_int = 0;
+    st.chip_cfg.active_low_int  = 1;
+    st.chip_cfg.latched_int     = 0;
     st.chip_cfg.int_motion_only = 0;
-    st.chip_cfg.lp_accel_mode = 0;
+    st.chip_cfg.lp_accel_mode   = 0;
     memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
-    st.chip_cfg.dmp_on = 0;
-    st.chip_cfg.dmp_loaded = 0;
+    st.chip_cfg.dmp_on          = 0;
+    st.chip_cfg.dmp_loaded      = 0;
     st.chip_cfg.dmp_sample_rate = 0;
 
     if (mpu_set_gyro_fsr(2000))
@@ -770,7 +797,7 @@ int mpu_init(struct int_param_s *int_param)
     if (mpu_configure_fifo(0))
         return -1;
 
-#ifndef EMPL_TARGET_STM32F4    
+#ifndef EMPL_TARGET_STM32F4
     if (int_param)
         reg_int_cb(int_param);
 #endif
@@ -811,7 +838,8 @@ int mpu_lp_accel_mode(unsigned short rate)
     if (rate > 40)
         return -1;
 
-    if (!rate) {
+    if (!rate)
+    {
         mpu_set_int_latched(0);
         tmp[0] = 0;
         tmp[1] = BIT_STBY_XYZG;
@@ -830,16 +858,23 @@ int mpu_lp_accel_mode(unsigned short rate)
     mpu_set_int_latched(1);
 #if defined MPU6050
     tmp[0] = BIT_LPA_CYCLE;
-    if (rate == 1) {
+    if (rate == 1)
+    {
         tmp[1] = INV_LPA_1_25HZ;
         mpu_set_lpf(5);
-    } else if (rate <= 5) {
+    }
+    else if (rate <= 5)
+    {
         tmp[1] = INV_LPA_5HZ;
         mpu_set_lpf(5);
-    } else if (rate <= 20) {
+    }
+    else if (rate <= 20)
+    {
         tmp[1] = INV_LPA_20HZ;
         mpu_set_lpf(10);
-    } else {
+    }
+    else
+    {
         tmp[1] = INV_LPA_40HZ;
         mpu_set_lpf(20);
     }
@@ -874,8 +909,8 @@ int mpu_lp_accel_mode(unsigned short rate)
     if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
         return -1;
 #endif
-    st.chip_cfg.sensors = INV_XYZ_ACCEL;
-    st.chip_cfg.clk_src = 0;
+    st.chip_cfg.sensors       = INV_XYZ_ACCEL;
+    st.chip_cfg.clk_src       = 0;
     st.chip_cfg.lp_accel_mode = 1;
     mpu_configure_fifo(0);
 
@@ -924,7 +959,7 @@ int mpu_get_accel_reg(short *data, unsigned long *timestamp)
     data[1] = (tmp[2] << 8) | tmp[3];
     data[2] = (tmp[4] << 8) | tmp[5];
     if (timestamp)
-    	timestamp = HAL_GetTick();
+        *timestamp = HAL_GetTick();
     return 0;
 }
 
@@ -946,7 +981,7 @@ int mpu_get_temperature(long *data, unsigned long *timestamp)
         return -1;
     raw = (tmp[0] << 8) | tmp[1];
     if (timestamp)
-    	timestamp = HAL_GetTick();
+        *timestamp = HAL_GetTick();
 
     data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L);
     return 0;
@@ -955,59 +990,62 @@ int mpu_get_temperature(long *data, unsigned long *timestamp)
 /**
  *  @brief      Read biases to the accel bias 6500 registers.
  *  This function reads from the MPU6500 accel offset cancellations registers.
- *  The format are G in +-8G format. The register is initialized with OTP 
+ *  The format are G in +-8G format. The register is initialized with OTP
  *  factory trim values.
  *  @param[in]  accel_bias  returned structure with the accel bias
  *  @return     0 if successful.
  */
-int mpu_read_6500_accel_bias(long *accel_bias) {
-	unsigned char data[6];
-	if (i2c_read(st.hw->addr, 0x77, 2, &data[0]))
-		return -1;
-	if (i2c_read(st.hw->addr, 0x7A, 2, &data[2]))
-		return -1;
-	if (i2c_read(st.hw->addr, 0x7D, 2, &data[4]))
-		return -1;
-	accel_bias[0] = ((long)data[0]<<8) | data[1];
-	accel_bias[1] = ((long)data[2]<<8) | data[3];
-	accel_bias[2] = ((long)data[4]<<8) | data[5];
-	return 0;
+int mpu_read_6500_accel_bias(long *accel_bias)
+{
+    unsigned char data[6];
+    if (i2c_read(st.hw->addr, 0x77, 2, &data[0]))
+        return -1;
+    if (i2c_read(st.hw->addr, 0x7A, 2, &data[2]))
+        return -1;
+    if (i2c_read(st.hw->addr, 0x7D, 2, &data[4]))
+        return -1;
+    accel_bias[0] = ((long)data[0] << 8) | data[1];
+    accel_bias[1] = ((long)data[2] << 8) | data[3];
+    accel_bias[2] = ((long)data[4] << 8) | data[5];
+    return 0;
 }
 
 /**
  *  @brief      Read biases to the accel bias 6050 registers.
  *  This function reads from the MPU6050 accel offset cancellations registers.
- *  The format are G in +-8G format. The register is initialized with OTP 
+ *  The format are G in +-8G format. The register is initialized with OTP
  *  factory trim values.
  *  @param[in]  accel_bias  returned structure with the accel bias
  *  @return     0 if successful.
  */
-int mpu_read_6050_accel_bias(long *accel_bias) {
-	unsigned char data[6];
-	if (i2c_read(st.hw->addr, 0x06, 2, &data[0]))
-		return -1;
-	if (i2c_read(st.hw->addr, 0x08, 2, &data[2]))
-		return -1;
-	if (i2c_read(st.hw->addr, 0x0A, 2, &data[4]))
-		return -1;
-	accel_bias[0] = ((long)data[0]<<8) | data[1];
-	accel_bias[1] = ((long)data[2]<<8) | data[3];
-	accel_bias[2] = ((long)data[4]<<8) | data[5];
-	return 0;
+int mpu_read_6050_accel_bias(long *accel_bias)
+{
+    unsigned char data[6];
+    if (i2c_read(st.hw->addr, 0x06, 2, &data[0]))
+        return -1;
+    if (i2c_read(st.hw->addr, 0x08, 2, &data[2]))
+        return -1;
+    if (i2c_read(st.hw->addr, 0x0A, 2, &data[4]))
+        return -1;
+    accel_bias[0] = ((long)data[0] << 8) | data[1];
+    accel_bias[1] = ((long)data[2] << 8) | data[3];
+    accel_bias[2] = ((long)data[4] << 8) | data[5];
+    return 0;
 }
 
-int mpu_read_6500_gyro_bias(long *gyro_bias) {
-	unsigned char data[6];
-	if (i2c_read(st.hw->addr, 0x13, 2, &data[0]))
-		return -1;
-	if (i2c_read(st.hw->addr, 0x15, 2, &data[2]))
-		return -1;
-	if (i2c_read(st.hw->addr, 0x17, 2, &data[4]))
-		return -1;
-	gyro_bias[0] = ((long)data[0]<<8) | data[1];
-	gyro_bias[1] = ((long)data[2]<<8) | data[3];
-	gyro_bias[2] = ((long)data[4]<<8) | data[5];
-	return 0;
+int mpu_read_6500_gyro_bias(long *gyro_bias)
+{
+    unsigned char data[6];
+    if (i2c_read(st.hw->addr, 0x13, 2, &data[0]))
+        return -1;
+    if (i2c_read(st.hw->addr, 0x15, 2, &data[2]))
+        return -1;
+    if (i2c_read(st.hw->addr, 0x17, 2, &data[4]))
+        return -1;
+    gyro_bias[0] = ((long)data[0] << 8) | data[1];
+    gyro_bias[1] = ((long)data[2] << 8) | data[3];
+    gyro_bias[2] = ((long)data[4] << 8) | data[5];
+    return 0;
 }
 
 /**
@@ -1021,9 +1059,10 @@ int mpu_read_6500_gyro_bias(long *gyro_bias) {
 int mpu_set_gyro_bias_reg(long *gyro_bias)
 {
     unsigned char data[6] = {0, 0, 0, 0, 0, 0};
-    int i=0;
-    for(i=0;i<3;i++) {
-    	gyro_bias[i]= (-gyro_bias[i]);
+    int i                 = 0;
+    for (i = 0; i < 3; i++)
+    {
+        gyro_bias[i] = (-gyro_bias[i]);
     }
     data[0] = (gyro_bias[0] >> 8) & 0xff;
     data[1] = (gyro_bias[0]) & 0xff;
@@ -1048,11 +1087,12 @@ int mpu_set_gyro_bias_reg(long *gyro_bias)
  *  @param[in]  accel_bias  New biases.
  *  @return     0 if successful.
  */
-int mpu_set_accel_bias_6050_reg(const long *accel_bias) {
-    unsigned char data[6] = {0, 0, 0, 0, 0, 0};
+int mpu_set_accel_bias_6050_reg(const long *accel_bias)
+{
+    unsigned char data[6]  = {0, 0, 0, 0, 0, 0};
     long accel_reg_bias[3] = {0, 0, 0};
 
-    if(mpu_read_6050_accel_bias(accel_reg_bias))
+    if (mpu_read_6050_accel_bias(accel_reg_bias))
         return -1;
 
     accel_reg_bias[0] -= (accel_bias[0] & ~1);
@@ -1076,8 +1116,6 @@ int mpu_set_accel_bias_6050_reg(const long *accel_bias) {
     return 0;
 }
 
-
-
 /**
  *  @brief      Push biases to the accel bias 6500 registers.
  *  This function expects biases relative to the current sensor output, and
@@ -1086,11 +1124,12 @@ int mpu_set_accel_bias_6050_reg(const long *accel_bias) {
  *  @param[in]  accel_bias  New biases.
  *  @return     0 if successful.
  */
-int mpu_set_accel_bias_6500_reg(const long *accel_bias) {
-    unsigned char data[6] = {0, 0, 0, 0, 0, 0};
+int mpu_set_accel_bias_6500_reg(const long *accel_bias)
+{
+    unsigned char data[6]  = {0, 0, 0, 0, 0, 0};
     long accel_reg_bias[3] = {0, 0, 0};
 
-    if(mpu_read_6500_accel_bias(accel_reg_bias))
+    if (mpu_read_6500_accel_bias(accel_reg_bias))
         return -1;
 
     // Preserve bit 0 of factory value (for temperature compensation)
@@ -1115,7 +1154,6 @@ int mpu_set_accel_bias_6500_reg(const long *accel_bias) {
     return 0;
 }
 
-
 /**
  *  @brief  Reset FIFO read/write pointers.
  *  @return 0 if successful.
@@ -1135,7 +1173,8 @@ int mpu_reset_fifo(void)
     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
         return -1;
 
-    if (st.chip_cfg.dmp_on) {
+    if (st.chip_cfg.dmp_on)
+    {
         data = BIT_FIFO_RST | BIT_DMP_RST;
         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
             return -1;
@@ -1154,7 +1193,9 @@ int mpu_reset_fifo(void)
         data = 0;
         if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
             return -1;
-    } else {
+    }
+    else
+    {
         data = BIT_FIFO_RST;
         if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
             return -1;
@@ -1184,22 +1225,23 @@ int mpu_reset_fifo(void)
  */
 int mpu_get_gyro_fsr(unsigned short *fsr)
 {
-    switch (st.chip_cfg.gyro_fsr) {
-    case INV_FSR_250DPS:
-        fsr[0] = 250;
-        break;
-    case INV_FSR_500DPS:
-        fsr[0] = 500;
-        break;
-    case INV_FSR_1000DPS:
-        fsr[0] = 1000;
-        break;
-    case INV_FSR_2000DPS:
-        fsr[0] = 2000;
-        break;
-    default:
-        fsr[0] = 0;
-        break;
+    switch (st.chip_cfg.gyro_fsr)
+    {
+        case INV_FSR_250DPS:
+            fsr[0] = 250;
+            break;
+        case INV_FSR_500DPS:
+            fsr[0] = 500;
+            break;
+        case INV_FSR_1000DPS:
+            fsr[0] = 1000;
+            break;
+        case INV_FSR_2000DPS:
+            fsr[0] = 2000;
+            break;
+        default:
+            fsr[0] = 0;
+            break;
     }
     return 0;
 }
@@ -1216,21 +1258,22 @@ int mpu_set_gyro_fsr(unsigned short fsr)
     if (!(st.chip_cfg.sensors))
         return -1;
 
-    switch (fsr) {
-    case 250:
-        data = INV_FSR_250DPS << 3;
-        break;
-    case 500:
-        data = INV_FSR_500DPS << 3;
-        break;
-    case 1000:
-        data = INV_FSR_1000DPS << 3;
-        break;
-    case 2000:
-        data = INV_FSR_2000DPS << 3;
-        break;
-    default:
-        return -1;
+    switch (fsr)
+    {
+        case 250:
+            data = INV_FSR_250DPS << 3;
+            break;
+        case 500:
+            data = INV_FSR_500DPS << 3;
+            break;
+        case 1000:
+            data = INV_FSR_1000DPS << 3;
+            break;
+        case 2000:
+            data = INV_FSR_2000DPS << 3;
+            break;
+        default:
+            return -1;
     }
 
     if (st.chip_cfg.gyro_fsr == (data >> 3))
@@ -1248,21 +1291,22 @@ int mpu_set_gyro_fsr(unsigned short fsr)
  */
 int mpu_get_accel_fsr(unsigned char *fsr)
 {
-    switch (st.chip_cfg.accel_fsr) {
-    case INV_FSR_2G:
-        fsr[0] = 2;
-        break;
-    case INV_FSR_4G:
-        fsr[0] = 4;
-        break;
-    case INV_FSR_8G:
-        fsr[0] = 8;
-        break;
-    case INV_FSR_16G:
-        fsr[0] = 16;
-        break;
-    default:
-        return -1;
+    switch (st.chip_cfg.accel_fsr)
+    {
+        case INV_FSR_2G:
+            fsr[0] = 2;
+            break;
+        case INV_FSR_4G:
+            fsr[0] = 4;
+            break;
+        case INV_FSR_8G:
+            fsr[0] = 8;
+            break;
+        case INV_FSR_16G:
+            fsr[0] = 16;
+            break;
+        default:
+            return -1;
     }
     if (st.chip_cfg.accel_half)
         fsr[0] <<= 1;
@@ -1281,21 +1325,22 @@ int mpu_set_accel_fsr(unsigned char fsr)
     if (!(st.chip_cfg.sensors))
         return -1;
 
-    switch (fsr) {
-    case 2:
-        data = INV_FSR_2G << 3;
-        break;
-    case 4:
-        data = INV_FSR_4G << 3;
-        break;
-    case 8:
-        data = INV_FSR_8G << 3;
-        break;
-    case 16:
-        data = INV_FSR_16G << 3;
-        break;
-    default:
-        return -1;
+    switch (fsr)
+    {
+        case 2:
+            data = INV_FSR_2G << 3;
+            break;
+        case 4:
+            data = INV_FSR_4G << 3;
+            break;
+        case 8:
+            data = INV_FSR_8G << 3;
+            break;
+        case 16:
+            data = INV_FSR_16G << 3;
+            break;
+        default:
+            return -1;
     }
 
     if (st.chip_cfg.accel_fsr == (data >> 3))
@@ -1313,30 +1358,31 @@ int mpu_set_accel_fsr(unsigned char fsr)
  */
 int mpu_get_lpf(unsigned short *lpf)
 {
-    switch (st.chip_cfg.lpf) {
-    case INV_FILTER_188HZ:
-        lpf[0] = 188;
-        break;
-    case INV_FILTER_98HZ:
-        lpf[0] = 98;
-        break;
-    case INV_FILTER_42HZ:
-        lpf[0] = 42;
-        break;
-    case INV_FILTER_20HZ:
-        lpf[0] = 20;
-        break;
-    case INV_FILTER_10HZ:
-        lpf[0] = 10;
-        break;
-    case INV_FILTER_5HZ:
-        lpf[0] = 5;
-        break;
-    case INV_FILTER_256HZ_NOLPF2:
-    case INV_FILTER_2100HZ_NOLPF:
-    default:
-        lpf[0] = 0;
-        break;
+    switch (st.chip_cfg.lpf)
+    {
+        case INV_FILTER_188HZ:
+            lpf[0] = 188;
+            break;
+        case INV_FILTER_98HZ:
+            lpf[0] = 98;
+            break;
+        case INV_FILTER_42HZ:
+            lpf[0] = 42;
+            break;
+        case INV_FILTER_20HZ:
+            lpf[0] = 20;
+            break;
+        case INV_FILTER_10HZ:
+            lpf[0] = 10;
+            break;
+        case INV_FILTER_5HZ:
+            lpf[0] = 5;
+            break;
+        case INV_FILTER_256HZ_NOLPF2:
+        case INV_FILTER_2100HZ_NOLPF:
+        default:
+            lpf[0] = 0;
+            break;
     }
     return 0;
 }
@@ -1404,9 +1450,12 @@ int mpu_set_sample_rate(unsigned short rate)
 
     if (st.chip_cfg.dmp_on)
         return -1;
-    else {
-        if (st.chip_cfg.lp_accel_mode) {
-            if (rate && (rate <= 40)) {
+    else
+    {
+        if (st.chip_cfg.lp_accel_mode)
+        {
+            if (rate && (rate <= 40))
+            {
                 /* Just stay in low-power accel mode. */
                 mpu_lp_accel_mode(rate);
                 return 0;
@@ -1488,21 +1537,22 @@ int mpu_set_compass_sample_rate(unsigned short rate)
  */
 int mpu_get_gyro_sens(float *sens)
 {
-    switch (st.chip_cfg.gyro_fsr) {
-    case INV_FSR_250DPS:
-        sens[0] = 131.f;
-        break;
-    case INV_FSR_500DPS:
-        sens[0] = 65.5f;
-        break;
-    case INV_FSR_1000DPS:
-        sens[0] = 32.8f;
-        break;
-    case INV_FSR_2000DPS:
-        sens[0] = 16.4f;
-        break;
-    default:
-        return -1;
+    switch (st.chip_cfg.gyro_fsr)
+    {
+        case INV_FSR_250DPS:
+            sens[0] = 131.f;
+            break;
+        case INV_FSR_500DPS:
+            sens[0] = 65.5f;
+            break;
+        case INV_FSR_1000DPS:
+            sens[0] = 32.8f;
+            break;
+        case INV_FSR_2000DPS:
+            sens[0] = 16.4f;
+            break;
+        default:
+            return -1;
     }
     return 0;
 }
@@ -1514,21 +1564,22 @@ int mpu_get_gyro_sens(float *sens)
  */
 int mpu_get_accel_sens(unsigned short *sens)
 {
-    switch (st.chip_cfg.accel_fsr) {
-    case INV_FSR_2G:
-        sens[0] = 16384;
-        break;
-    case INV_FSR_4G:
-        sens[0] = 8192;
-        break;
-    case INV_FSR_8G:
-        sens[0] = 4096;
-        break;
-    case INV_FSR_16G:
-        sens[0] = 2048;
-        break;
-    default:
-        return -1;
+    switch (st.chip_cfg.accel_fsr)
+    {
+        case INV_FSR_2G:
+            sens[0] = 16384;
+            break;
+        case INV_FSR_4G:
+            sens[0] = 8192;
+            break;
+        case INV_FSR_8G:
+            sens[0] = 4096;
+            break;
+        case INV_FSR_16G:
+            sens[0] = 2048;
+            break;
+        default:
+            return -1;
     }
     if (st.chip_cfg.accel_half)
         sens[0] >>= 1;
@@ -1569,10 +1620,11 @@ int mpu_configure_fifo(unsigned char sensors)
 
     if (st.chip_cfg.dmp_on)
         return 0;
-    else {
+    else
+    {
         if (!(st.chip_cfg.sensors))
             return -1;
-        prev = st.chip_cfg.fifo_enable;
+        prev                    = st.chip_cfg.fifo_enable;
         st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;
         if (st.chip_cfg.fifo_enable != sensors)
             /* You're not getting what you asked for. Some sensors are
@@ -1585,8 +1637,10 @@ int mpu_configure_fifo(unsigned char sensors)
             set_int_enable(1);
         else
             set_int_enable(0);
-        if (sensors) {
-            if (mpu_reset_fifo()) {
+        if (sensors)
+        {
+            if (mpu_reset_fifo())
+            {
                 st.chip_cfg.fifo_enable = prev;
                 return -1;
             }
@@ -1633,7 +1687,8 @@ int mpu_set_sensors(unsigned char sensors)
         data = 0;
     else
         data = BIT_SLEEP;
-    if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {
+    if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data))
+    {
         st.chip_cfg.sensors = 0;
         return -1;
     }
@@ -1648,7 +1703,8 @@ int mpu_set_sensors(unsigned char sensors)
         data |= BIT_STBY_ZG;
     if (!(sensors & INV_XYZ_ACCEL))
         data |= BIT_STBY_XYZA;
-    if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {
+    if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data))
+    {
         st.chip_cfg.sensors = 0;
         return -1;
     }
@@ -1658,19 +1714,22 @@ int mpu_set_sensors(unsigned char sensors)
         mpu_set_int_latched(0);
 
 #ifdef AK89xx_SECONDARY
-#ifdef AK89xx_BYPASS
+    #ifdef AK89xx_BYPASS
     if (sensors & INV_XYZ_COMPASS)
         mpu_set_bypass(1);
     else
         mpu_set_bypass(0);
-#else
+    #else
     if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
         return -1;
     /* Handle AKM power management. */
-    if (sensors & INV_XYZ_COMPASS) {
+    if (sensors & INV_XYZ_COMPASS)
+    {
         data = AKM_SINGLE_MEASUREMENT;
         user_ctrl |= BIT_AUX_IF_EN;
-    } else {
+    }
+    else
+    {
         data = AKM_POWER_DOWN;
         user_ctrl &= ~BIT_AUX_IF_EN;
     }
@@ -1683,10 +1742,10 @@ int mpu_set_sensors(unsigned char sensors)
     /* Enable/disable I2C master mode. */
     if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
         return -1;
-#endif
+    #endif
 #endif
 
-    st.chip_cfg.sensors = sensors;
+    st.chip_cfg.sensors       = sensors;
     st.chip_cfg.lp_accel_mode = 0;
     HAL_Delay(50);
     return 0;
@@ -1727,7 +1786,7 @@ int mpu_get_int_status(short *status)
  *  @return     0 if successful.
  */
 int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
-        unsigned char *sensors, unsigned char *more)
+                  unsigned char *sensors, unsigned char *more)
 {
     /* Assumes maximum packet size is gyro (6) + accel (6). */
     unsigned char data[MAX_PACKET_LENGTH];
@@ -1757,12 +1816,14 @@ int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
     fifo_count = (data[0] << 8) | data[1];
     if (fifo_count < packet_size)
         return 0;
-//    log_i("FIFO count: %hd\n", fifo_count);
-    if (fifo_count > (st.hw->max_fifo >> 1)) {
+    //    log_i("FIFO count: %hd\n", fifo_count);
+    if (fifo_count > (st.hw->max_fifo >> 1))
+    {
         /* FIFO is 50% full, better check overflow bit. */
         if (i2c_read(st.hw->addr, st.reg->int_status, 1, data))
             return -1;
-        if (data[0] & BIT_FIFO_OVERFLOW) {
+        if (data[0] & BIT_FIFO_OVERFLOW)
+        {
             mpu_reset_fifo();
             return -2;
         }
@@ -1771,28 +1832,32 @@ int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
 
     if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data))
         return -1;
-    more[0] = fifo_count / packet_size - 1;
+    more[0]    = fifo_count / packet_size - 1;
     sensors[0] = 0;
 
-    if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
-        accel[0] = (data[index+0] << 8) | data[index+1];
-        accel[1] = (data[index+2] << 8) | data[index+3];
-        accel[2] = (data[index+4] << 8) | data[index+5];
+    if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
+    {
+        accel[0] = (data[index + 0] << 8) | data[index + 1];
+        accel[1] = (data[index + 2] << 8) | data[index + 3];
+        accel[2] = (data[index + 4] << 8) | data[index + 5];
         sensors[0] |= INV_XYZ_ACCEL;
         index += 6;
     }
-    if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {
-        gyro[0] = (data[index+0] << 8) | data[index+1];
+    if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO)
+    {
+        gyro[0] = (data[index + 0] << 8) | data[index + 1];
         sensors[0] |= INV_X_GYRO;
         index += 2;
     }
-    if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {
-        gyro[1] = (data[index+0] << 8) | data[index+1];
+    if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO)
+    {
+        gyro[1] = (data[index + 0] << 8) | data[index + 1];
         sensors[0] |= INV_Y_GYRO;
         index += 2;
     }
-    if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {
-        gyro[2] = (data[index+0] << 8) | data[index+1];
+    if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO)
+    {
+        gyro[2] = (data[index + 0] << 8) | data[index + 1];
         sensors[0] |= INV_Z_GYRO;
         index += 2;
     }
@@ -1808,7 +1873,7 @@ int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
  *  @param[in]  more    Number of remaining packets.
  */
 int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
-    unsigned char *more)
+                         unsigned char *more)
 {
     unsigned char tmp[2];
     unsigned short fifo_count;
@@ -1820,15 +1885,18 @@ int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
     if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
         return -1;
     fifo_count = (tmp[0] << 8) | tmp[1];
-    if (fifo_count < length) {
+    if (fifo_count < length)
+    {
         more[0] = 0;
         return -1;
     }
-    if (fifo_count > (st.hw->max_fifo >> 1)) {
+    if (fifo_count > (st.hw->max_fifo >> 1))
+    {
         /* FIFO is 50% full, better check overflow bit. */
         if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
             return -1;
-        if (tmp[0] & BIT_FIFO_OVERFLOW) {
+        if (tmp[0] & BIT_FIFO_OVERFLOW)
+        {
             mpu_reset_fifo();
             return -2;
         }
@@ -1852,7 +1920,8 @@ int mpu_set_bypass(unsigned char bypass_on)
     if (st.chip_cfg.bypass_mode == bypass_on)
         return 0;
 
-    if (bypass_on) {
+    if (bypass_on)
+    {
         if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
             return -1;
         tmp &= ~BIT_AUX_IF_EN;
@@ -1866,7 +1935,9 @@ int mpu_set_bypass(unsigned char bypass_on)
             tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
         if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
             return -1;
-    } else {
+    }
+    else
+    {
         /* Enable I2C master mode if compass is being used. */
         if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
             return -1;
@@ -1938,8 +2009,10 @@ static int get_accel_prod_shift(float *st_shift)
     shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
     shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
     shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
-    for (ii = 0; ii < 3; ii++) {
-        if (!shift_code[ii]) {
+    for (ii = 0; ii < 3; ii++)
+    {
+        if (!shift_code[ii])
+        {
             st_shift[ii] = 0.f;
             continue;
         }
@@ -1959,14 +2032,16 @@ static int accel_self_test(long *bias_regular, long *bias_st)
     float st_shift[3], st_shift_cust, st_shift_var;
 
     get_accel_prod_shift(st_shift);
-    for(jj = 0; jj < 3; jj++) {
+    for (jj = 0; jj < 3; jj++)
+    {
         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
-        if (st_shift[jj]) {
+        if (st_shift[jj])
+        {
             st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
             if (fabs(st_shift_var) > test.max_accel_var)
                 result |= 1 << jj;
-        } else if ((st_shift_cust < test.min_g) ||
-            (st_shift_cust > test.max_g))
+        }
+        else if ((st_shift_cust < test.min_g) || (st_shift_cust > test.max_g))
             result |= 1 << jj;
     }
 
@@ -1986,29 +2061,31 @@ static int gyro_self_test(long *bias_regular, long *bias_st)
     tmp[1] &= 0x1F;
     tmp[2] &= 0x1F;
 
-    for (jj = 0; jj < 3; jj++) {
+    for (jj = 0; jj < 3; jj++)
+    {
         st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
-        if (tmp[jj]) {
+        if (tmp[jj])
+        {
             st_shift = 3275.f / test.gyro_sens;
             while (--tmp[jj])
                 st_shift *= 1.046f;
             st_shift_var = st_shift_cust / st_shift - 1.f;
             if (fabs(st_shift_var) > test.max_gyro_var)
                 result |= 1 << jj;
-        } else if ((st_shift_cust < test.min_dps) ||
-            (st_shift_cust > test.max_dps))
+        }
+        else if ((st_shift_cust < test.min_dps) || (st_shift_cust > test.max_dps))
             result |= 1 << jj;
     }
     return result;
 }
 
-#endif 
+#endif
 #ifdef AK89xx_SECONDARY
 static int compass_self_test(void)
 {
     unsigned char tmp[6];
     unsigned char tries = 10;
-    int result = 0x07;
+    int result          = 0x07;
     short data;
 
     mpu_set_bypass(1);
@@ -2023,8 +2100,9 @@ static int compass_self_test(void)
     if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
         goto AKM_restore;
 
-    do {
-    	HAL_Delay(10);
+    do
+    {
+        HAL_Delay(10);
         if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
             goto AKM_restore;
         if (tmp[0] & AKM_DATA_READY)
@@ -2037,7 +2115,7 @@ static int compass_self_test(void)
         goto AKM_restore;
 
     result = 0;
-#if defined MPU9150
+    #if defined MPU9150
     data = (short)(tmp[1] << 8) | tmp[0];
     if ((data > 100) || (data < -100))
         result |= 0x01;
@@ -2047,17 +2125,17 @@ static int compass_self_test(void)
     data = (short)(tmp[5] << 8) | tmp[4];
     if ((data > -300) || (data < -1000))
         result |= 0x04;
-#elif defined MPU9250
+    #elif defined MPU9250
     data = (short)(tmp[1] << 8) | tmp[0];
-    if ((data > 200) || (data < -200))  
+    if ((data > 200) || (data < -200))
         result |= 0x01;
     data = (short)(tmp[3] << 8) | tmp[2];
-    if ((data > 200) || (data < -200))  
+    if ((data > 200) || (data < -200))
         result |= 0x02;
     data = (short)(tmp[5] << 8) | tmp[4];
-    if ((data > -800) || (data < -3200))  
+    if ((data > -800) || (data < -3200))
         result |= 0x04;
-#endif
+    #endif
 AKM_restore:
     tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS;
     i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
@@ -2114,7 +2192,7 @@ static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
     if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
         return -1;
     if (hw_test)
-    	HAL_Delay(200);
+        HAL_Delay(200);
 
     /* Fill FIFO for test.wait_ms milliseconds. */
     data[0] = BIT_FIFO_EN;
@@ -2132,12 +2210,13 @@ static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
     if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
         return -1;
 
-    fifo_count = (data[0] << 8) | data[1];
+    fifo_count   = (data[0] << 8) | data[1];
     packet_count = fifo_count / MAX_PACKET_LENGTH;
     gyro[0] = gyro[1] = gyro[2] = 0;
     accel[0] = accel[1] = accel[2] = 0;
 
-    for (ii = 0; ii < packet_count; ii++) {
+    for (ii = 0; ii < packet_count; ii++)
+    {
         short accel_cur[3], gyro_cur[3];
         if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
             return -1;
@@ -2155,29 +2234,24 @@ static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
         gyro[2] += (long)gyro_cur[2];
     }
 #ifdef EMPL_NO_64BIT
-    gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
-    gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
-    gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
-    if (has_accel) {
-        accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens /
-            packet_count);
-        accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens /
-            packet_count);
-        accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens /
-            packet_count);
+    gyro[0] = (long)(((float)gyro[0] * 65536.f) / test.gyro_sens / packet_count);
+    gyro[1] = (long)(((float)gyro[1] * 65536.f) / test.gyro_sens / packet_count);
+    gyro[2] = (long)(((float)gyro[2] * 65536.f) / test.gyro_sens / packet_count);
+    if (has_accel)
+    {
+        accel[0] = (long)(((float)accel[0] * 65536.f) / test.accel_sens / packet_count);
+        accel[1] = (long)(((float)accel[1] * 65536.f) / test.accel_sens / packet_count);
+        accel[2] = (long)(((float)accel[2] * 65536.f) / test.accel_sens / packet_count);
         /* Don't remove gravity! */
         accel[2] -= 65536L;
     }
 #else
-    gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count);
-    gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count);
-    gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count);
-    accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens /
-        packet_count);
-    accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens /
-        packet_count);
-    accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens /
-        packet_count);
+    gyro[0]  = (long)(((long long)gyro[0] << 16) / test.gyro_sens / packet_count);
+    gyro[1]  = (long)(((long long)gyro[1] << 16) / test.gyro_sens / packet_count);
+    gyro[2]  = (long)(((long long)gyro[2] << 16) / test.gyro_sens / packet_count);
+    accel[0] = (long)(((long long)accel[0] << 16) / test.accel_sens / packet_count);
+    accel[1] = (long)(((long long)accel[1] << 16) / test.accel_sens / packet_count);
+    accel[2] = (long)(((long long)accel[2] << 16) / test.accel_sens / packet_count);
     /* Don't remove gravity! */
     if (accel[2] > 0L)
         accel[2] -= 65536L;
@@ -2189,95 +2263,107 @@ static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
 }
 
 #ifdef MPU6500
-#define REG_6500_XG_ST_DATA     0x0
-#define REG_6500_XA_ST_DATA     0xD
+    #define REG_6500_XG_ST_DATA 0x0
+    #define REG_6500_XA_ST_DATA 0xD
 static const unsigned short mpu_6500_st_tb[256] = {
-	2620,2646,2672,2699,2726,2753,2781,2808, //7
-	2837,2865,2894,2923,2952,2981,3011,3041, //15
-	3072,3102,3133,3165,3196,3228,3261,3293, //23
-	3326,3359,3393,3427,3461,3496,3531,3566, //31
-	3602,3638,3674,3711,3748,3786,3823,3862, //39
-	3900,3939,3979,4019,4059,4099,4140,4182, //47
-	4224,4266,4308,4352,4395,4439,4483,4528, //55
-	4574,4619,4665,4712,4759,4807,4855,4903, //63
-	4953,5002,5052,5103,5154,5205,5257,5310, //71
-	5363,5417,5471,5525,5581,5636,5693,5750, //79
-	5807,5865,5924,5983,6043,6104,6165,6226, //87
-	6289,6351,6415,6479,6544,6609,6675,6742, //95
-	6810,6878,6946,7016,7086,7157,7229,7301, //103
-	7374,7448,7522,7597,7673,7750,7828,7906, //111
-	7985,8065,8145,8227,8309,8392,8476,8561, //119
-	8647,8733,8820,8909,8998,9088,9178,9270,
-	9363,9457,9551,9647,9743,9841,9939,10038,
-	10139,10240,10343,10446,10550,10656,10763,10870,
-	10979,11089,11200,11312,11425,11539,11654,11771,
-	11889,12008,12128,12249,12371,12495,12620,12746,
-	12874,13002,13132,13264,13396,13530,13666,13802,
-	13940,14080,14221,14363,14506,14652,14798,14946,
-	15096,15247,15399,15553,15709,15866,16024,16184,
-	16346,16510,16675,16842,17010,17180,17352,17526,
-	17701,17878,18057,18237,18420,18604,18790,18978,
-	19167,19359,19553,19748,19946,20145,20347,20550,
-	20756,20963,21173,21385,21598,21814,22033,22253,
-	22475,22700,22927,23156,23388,23622,23858,24097,
-	24338,24581,24827,25075,25326,25579,25835,26093,
-	26354,26618,26884,27153,27424,27699,27976,28255,
-	28538,28823,29112,29403,29697,29994,30294,30597,
-	30903,31212,31524,31839,32157,32479,32804,33132
-};
+    2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808, // 7
+    2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041, // 15
+    3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293, // 23
+    3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566, // 31
+    3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862, // 39
+    3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182, // 47
+    4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528, // 55
+    4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903, // 63
+    4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310, // 71
+    5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750, // 79
+    5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226, // 87
+    6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742, // 95
+    6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301, // 103
+    7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906, // 111
+    7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561, // 119
+    8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
+    9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
+    10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
+    10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
+    11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
+    12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
+    13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
+    15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
+    16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
+    17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
+    19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
+    20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
+    22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
+    24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
+    26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
+    28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
+    30903, 31212, 31524, 31839, 32157, 32479, 32804, 33132};
 static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
 {
     int i, result = 0, otp_value_zero = 0;
     float accel_st_al_min, accel_st_al_max;
     float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], accel_offset_max;
     unsigned char regs[3];
-    if (i2c_read(st.hw->addr, REG_6500_XA_ST_DATA, 3, regs)) {
-    	return 0x07;
+    if (i2c_read(st.hw->addr, REG_6500_XA_ST_DATA, 3, regs))
+    {
+        return 0x07;
+    }
+    for (i = 0; i < 3; i++)
+    {
+        if (regs[i] != 0)
+        {
+            ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
+            ct_shift_prod[i] *= 65536.f;
+            ct_shift_prod[i] /= test.accel_sens;
+        }
+        else
+        {
+            ct_shift_prod[i] = 0;
+            otp_value_zero   = 1;
+        }
+    }
+    if (otp_value_zero == 0)
+    {
+        for (i = 0; i < 3; i++)
+        {
+            st_shift_cust[i] = bias_st[i] - bias_regular[i];
+
+            st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f;
+
+            if (fabs(st_shift_ratio[i]) > test.max_accel_var)
+            {
+                result |= 1 << i; // Error condition
+            }
+        }
+    }
+    else
+    {
+        /* Self Test Pass/Fail Criteria B */
+        accel_st_al_min = test.min_g * 65536.f;
+        accel_st_al_max = test.max_g * 65536.f;
+
+        for (i = 0; i < 3; i++)
+        {
+            st_shift_cust[i] = bias_st[i] - bias_regular[i];
+            if (st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max)
+            {
+                result |= 1 << i; // Error condition
+            }
+        }
+    }
+
+    if (result == 0)
+    {
+        /* Self Test Pass/Fail Criteria C */
+        accel_offset_max = test.max_g_offset * 65536.f;
+        for (i = 0; i < 3; i++)
+        {
+            if (fabs(bias_regular[i]) > accel_offset_max)
+            {
+                result |= 1 << i; // Error condition
+            }
+        }
     }
-	for (i = 0; i < 3; i++) {
-		if (regs[i] != 0) {
-			ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
-			ct_shift_prod[i] *= 65536.f;
-			ct_shift_prod[i] /= test.accel_sens;
-		}
-		else {
-			ct_shift_prod[i] = 0;
-			otp_value_zero = 1;
-		}
-	}
-	if(otp_value_zero == 0) {
-		for (i = 0; i < 3; i++) {
-			st_shift_cust[i] = bias_st[i] - bias_regular[i];
-
-			st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f;
-
-			if (fabs(st_shift_ratio[i]) > test.max_accel_var) {
-				result |= 1 << i;	//Error condition
-			}
-		}
-	}
-	else {
-		/* Self Test Pass/Fail Criteria B */
-		accel_st_al_min = test.min_g * 65536.f;
-		accel_st_al_max = test.max_g * 65536.f;
-
-		for (i = 0; i < 3; i++) {
-			st_shift_cust[i] = bias_st[i] - bias_regular[i];
-			if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) {
-				result |= 1 << i;	//Error condition
-			}
-		}
-	}
-
-	if(result == 0) {
-	/* Self Test Pass/Fail Criteria C */
-		accel_offset_max = test.max_g_offset * 65536.f;
-		for (i = 0; i < 3; i++) {
-			if(fabs(bias_regular[i]) > accel_offset_max) {
-				result |= 1 << i;	//Error condition
-			}
-		}
-	}
 
     return result;
 }
@@ -2289,55 +2375,68 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
     float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], gyro_offset_max;
     unsigned char regs[3];
 
-    if (i2c_read(st.hw->addr, REG_6500_XG_ST_DATA, 3, regs)) {
+    if (i2c_read(st.hw->addr, REG_6500_XG_ST_DATA, 3, regs))
+    {
         return 0x07;
     }
 
-	for (i = 0; i < 3; i++) {
-		if (regs[i] != 0) {
-			ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
-			ct_shift_prod[i] *= 65536.f;
-			ct_shift_prod[i] /= test.gyro_sens;
-		}
-		else {
-			ct_shift_prod[i] = 0;
-			otp_value_zero = 1;
-		}
-	}
-
-	if(otp_value_zero == 0) {
-		/* Self Test Pass/Fail Criteria A */
-		for (i = 0; i < 3; i++) {
-			st_shift_cust[i] = bias_st[i] - bias_regular[i];
-
-			st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i];
-
-			if (fabs(st_shift_ratio[i]) < test.max_gyro_var) {
-				result |= 1 << i;	//Error condition
-			}
-		}
-	}
-	else {
-		/* Self Test Pass/Fail Criteria B */
-		gyro_st_al_max = test.max_dps * 65536.f;
-
-		for (i = 0; i < 3; i++) {
-			st_shift_cust[i] = bias_st[i] - bias_regular[i];
-			if(st_shift_cust[i] < gyro_st_al_max) {
-				result |= 1 << i;	//Error condition
-			}
-		}
-	}
-
-	if(result == 0) {
-	/* Self Test Pass/Fail Criteria C */
-		gyro_offset_max = test.min_dps * 65536.f;
-		for (i = 0; i < 3; i++) {
-			if(fabs(bias_regular[i]) > gyro_offset_max) {
-				result |= 1 << i;	//Error condition
-			}
-		}
-	}
+    for (i = 0; i < 3; i++)
+    {
+        if (regs[i] != 0)
+        {
+            ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
+            ct_shift_prod[i] *= 65536.f;
+            ct_shift_prod[i] /= test.gyro_sens;
+        }
+        else
+        {
+            ct_shift_prod[i] = 0;
+            otp_value_zero   = 1;
+        }
+    }
+
+    if (otp_value_zero == 0)
+    {
+        /* Self Test Pass/Fail Criteria A */
+        for (i = 0; i < 3; i++)
+        {
+            st_shift_cust[i] = bias_st[i] - bias_regular[i];
+
+            st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i];
+
+            if (fabs(st_shift_ratio[i]) < test.max_gyro_var)
+            {
+                result |= 1 << i; // Error condition
+            }
+        }
+    }
+    else
+    {
+        /* Self Test Pass/Fail Criteria B */
+        gyro_st_al_max = test.max_dps * 65536.f;
+
+        for (i = 0; i < 3; i++)
+        {
+            st_shift_cust[i] = bias_st[i] - bias_regular[i];
+            if (st_shift_cust[i] < gyro_st_al_max)
+            {
+                result |= 1 << i; // Error condition
+            }
+        }
+    }
+
+    if (result == 0)
+    {
+        /* Self Test Pass/Fail Criteria C */
+        gyro_offset_max = test.min_dps * 65536.f;
+        for (i = 0; i < 3; i++)
+        {
+            if (fabs(bias_regular[i]) > gyro_offset_max)
+            {
+                result |= 1 << i; // Error condition
+            }
+        }
+    }
     return result;
 }
 
@@ -2388,7 +2487,7 @@ static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, in
     if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
         return -1;
 
-    HAL_Delay(test.wait_ms);  //wait 200ms for sensors to stabilize
+    HAL_Delay(test.wait_ms); // wait 200ms for sensors to stabilize
 
     /* Enable FIFO */
     data[0] = BIT_FIFO_EN;
@@ -2398,55 +2497,57 @@ static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, in
     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
         return -1;
 
-    //initialize the bias return values
+    // initialize the bias return values
     gyro[0] = gyro[1] = gyro[2] = 0;
     accel[0] = accel[1] = accel[2] = 0;
 
-    //start reading samples
-    while (s < test.packet_thresh) {
-    	HAL_Delay(test.sample_wait_ms); //wait 10ms to fill FIFO
-		if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
-			return -1;
-		fifo_count = (data[0] << 8) | data[1];
-		packet_count = fifo_count / MAX_PACKET_LENGTH;
-		if ((test.packet_thresh - s) < packet_count)
-		            packet_count = test.packet_thresh - s;
-		read_size = packet_count * MAX_PACKET_LENGTH;
-
-		//burst read from FIFO
-		if (i2c_read(st.hw->addr, st.reg->fifo_r_w, read_size, data))
-						return -1;
-		ind = 0;
-		for (ii = 0; ii < packet_count; ii++) {
-			short accel_cur[3], gyro_cur[3];
-			accel_cur[0] = ((short)data[ind + 0] << 8) | data[ind + 1];
-			accel_cur[1] = ((short)data[ind + 2] << 8) | data[ind + 3];
-			accel_cur[2] = ((short)data[ind + 4] << 8) | data[ind + 5];
-			accel[0] += (long)accel_cur[0];
-			accel[1] += (long)accel_cur[1];
-			accel[2] += (long)accel_cur[2];
-			gyro_cur[0] = (((short)data[ind + 6] << 8) | data[ind + 7]);
-			gyro_cur[1] = (((short)data[ind + 8] << 8) | data[ind + 9]);
-			gyro_cur[2] = (((short)data[ind + 10] << 8) | data[ind + 11]);
-			gyro[0] += (long)gyro_cur[0];
-			gyro[1] += (long)gyro_cur[1];
-			gyro[2] += (long)gyro_cur[2];
-			ind += MAX_PACKET_LENGTH;
-		}
-		s += packet_count;
+    // start reading samples
+    while (s < test.packet_thresh)
+    {
+        HAL_Delay(test.sample_wait_ms); // wait 10ms to fill FIFO
+        if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
+            return -1;
+        fifo_count   = (data[0] << 8) | data[1];
+        packet_count = fifo_count / MAX_PACKET_LENGTH;
+        if ((test.packet_thresh - s) < packet_count)
+            packet_count = test.packet_thresh - s;
+        read_size = packet_count * MAX_PACKET_LENGTH;
+
+        // burst read from FIFO
+        if (i2c_read(st.hw->addr, st.reg->fifo_r_w, read_size, data))
+            return -1;
+        ind = 0;
+        for (ii = 0; ii < packet_count; ii++)
+        {
+            short accel_cur[3], gyro_cur[3];
+            accel_cur[0] = ((short)data[ind + 0] << 8) | data[ind + 1];
+            accel_cur[1] = ((short)data[ind + 2] << 8) | data[ind + 3];
+            accel_cur[2] = ((short)data[ind + 4] << 8) | data[ind + 5];
+            accel[0] += (long)accel_cur[0];
+            accel[1] += (long)accel_cur[1];
+            accel[2] += (long)accel_cur[2];
+            gyro_cur[0] = (((short)data[ind + 6] << 8) | data[ind + 7]);
+            gyro_cur[1] = (((short)data[ind + 8] << 8) | data[ind + 9]);
+            gyro_cur[2] = (((short)data[ind + 10] << 8) | data[ind + 11]);
+            gyro[0] += (long)gyro_cur[0];
+            gyro[1] += (long)gyro_cur[1];
+            gyro[2] += (long)gyro_cur[2];
+            ind += MAX_PACKET_LENGTH;
+        }
+        s += packet_count;
     }
 
-    //stop FIFO
+    // stop FIFO
     data[0] = 0;
     if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
         return -1;
 
-    gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / s);
-    gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / s);
-    gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / s);
-    accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / s);
-    accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / s);
-    accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / s);
+    gyro[0]  = (long)(((long long)gyro[0] << 16) / test.gyro_sens / s);
+    gyro[1]  = (long)(((long long)gyro[1] << 16) / test.gyro_sens / s);
+    gyro[2]  = (long)(((long long)gyro[2] << 16) / test.gyro_sens / s);
+    accel[0] = (long)(((long long)accel[0] << 16) / test.accel_sens / s);
+    accel[1] = (long)(((long long)accel[1] << 16) / test.accel_sens / s);
+    accel[2] = (long)(((long long)accel[2] << 16) / test.accel_sens / s);
     /* remove gravity from bias calculation */
     if (accel[2] > 0L)
         accel[2] -= 65536L;
@@ -2476,9 +2577,9 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug)
     const unsigned char tries = 2;
     long gyro_st[3], accel_st[3];
     unsigned char accel_result, gyro_result;
-#ifdef AK89xx_SECONDARY
+    #ifdef AK89xx_SECONDARY
     unsigned char compass_result;
-#endif
+    #endif
     int ii;
 
     int result;
@@ -2486,10 +2587,12 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug)
     unsigned short gyro_fsr, sample_rate, lpf;
     unsigned char dmp_was_on;
 
-    if (st.chip_cfg.dmp_on) {
+    if (st.chip_cfg.dmp_on)
+    {
         mpu_set_dmp_state(0);
         dmp_was_on = 1;
-    } else
+    }
+    else
         dmp_was_on = 0;
 
     /* Get initial settings. */
@@ -2503,7 +2606,8 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug)
     for (ii = 0; ii < tries; ii++)
         if (!get_st_6500_biases(gyro, accel, 0, debug))
             break;
-    if (ii == tries) {
+    if (ii == tries)
+    {
         /* If we reach this point, we most likely encountered an I2C error.
          * We'll just report an error for all three sensors.
          */
@@ -2515,7 +2619,8 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug)
     for (ii = 0; ii < tries; ii++)
         if (!get_st_6500_biases(gyro_st, accel_st, 1, debug))
             break;
-    if (ii == tries) {
+    if (ii == tries)
+    {
         /* Again, probably an I2C error. */
         result = 0;
         goto restore;
@@ -2531,36 +2636,36 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug)
     if (!accel_result)
         result |= 0x02;
 
-#ifdef AK89xx_SECONDARY
+    #ifdef AK89xx_SECONDARY
     compass_result = compass_self_test();
     if (!compass_result)
         result |= 0x04;
-#else
+    #else
     result |= 0x04;
-#endif
+    #endif
 restore:
-	/* Set to invalid values to ensure no I2C writes are skipped. */
-	st.chip_cfg.gyro_fsr = 0xFF;
-	st.chip_cfg.accel_fsr = 0xFF;
-	st.chip_cfg.lpf = 0xFF;
-	st.chip_cfg.sample_rate = 0xFFFF;
-	st.chip_cfg.sensors = 0xFF;
-	st.chip_cfg.fifo_enable = 0xFF;
-	st.chip_cfg.clk_src = INV_CLK_PLL;
-	mpu_set_gyro_fsr(gyro_fsr);
-	mpu_set_accel_fsr(accel_fsr);
-	mpu_set_lpf(lpf);
-	mpu_set_sample_rate(sample_rate);
-	mpu_set_sensors(sensors_on);
-	mpu_configure_fifo(fifo_sensors);
-
-	if (dmp_was_on)
-		mpu_set_dmp_state(1);
-
-	return result;
+    /* Set to invalid values to ensure no I2C writes are skipped. */
+    st.chip_cfg.gyro_fsr    = 0xFF;
+    st.chip_cfg.accel_fsr   = 0xFF;
+    st.chip_cfg.lpf         = 0xFF;
+    st.chip_cfg.sample_rate = 0xFFFF;
+    st.chip_cfg.sensors     = 0xFF;
+    st.chip_cfg.fifo_enable = 0xFF;
+    st.chip_cfg.clk_src     = INV_CLK_PLL;
+    mpu_set_gyro_fsr(gyro_fsr);
+    mpu_set_accel_fsr(accel_fsr);
+    mpu_set_lpf(lpf);
+    mpu_set_sample_rate(sample_rate);
+    mpu_set_sensors(sensors_on);
+    mpu_configure_fifo(fifo_sensors);
+
+    if (dmp_was_on)
+        mpu_set_dmp_state(1);
+
+    return result;
 }
 #endif
- /*
+/*
  *  \n This function must be called with the device either face-up or face-down
  *  (z-axis is parallel to gravity).
  *  @param[out] gyro        Gyro biases in q16 format.
@@ -2573,9 +2678,9 @@ int mpu_run_self_test(long *gyro, long *accel)
     const unsigned char tries = 2;
     long gyro_st[3], accel_st[3];
     unsigned char accel_result, gyro_result;
-#ifdef AK89xx_SECONDARY
+    #ifdef AK89xx_SECONDARY
     unsigned char compass_result;
-#endif
+    #endif
     int ii;
 #endif
     int result;
@@ -2583,10 +2688,12 @@ int mpu_run_self_test(long *gyro, long *accel)
     unsigned short gyro_fsr, sample_rate, lpf;
     unsigned char dmp_was_on;
 
-    if (st.chip_cfg.dmp_on) {
+    if (st.chip_cfg.dmp_on)
+    {
         mpu_set_dmp_state(0);
         dmp_was_on = 1;
-    } else
+    }
+    else
         dmp_was_on = 0;
 
     /* Get initial settings. */
@@ -2602,7 +2709,8 @@ int mpu_run_self_test(long *gyro, long *accel)
     for (ii = 0; ii < tries; ii++)
         if (!get_st_biases(gyro, accel, 0))
             break;
-    if (ii == tries) {
+    if (ii == tries)
+    {
         /* If we reach this point, we most likely encountered an I2C error.
          * We'll just report an error for all three sensors.
          */
@@ -2612,13 +2720,14 @@ int mpu_run_self_test(long *gyro, long *accel)
     for (ii = 0; ii < tries; ii++)
         if (!get_st_biases(gyro_st, accel_st, 1))
             break;
-    if (ii == tries) {
+    if (ii == tries)
+    {
         /* Again, probably an I2C error. */
         result = 0;
         goto restore;
     }
     accel_result = accel_self_test(accel, accel_st);
-    gyro_result = gyro_self_test(gyro, gyro_st);
+    gyro_result  = gyro_self_test(gyro, gyro_st);
 
     result = 0;
     if (!gyro_result)
@@ -2626,13 +2735,13 @@ int mpu_run_self_test(long *gyro, long *accel)
     if (!accel_result)
         result |= 0x02;
 
-#ifdef AK89xx_SECONDARY
+    #ifdef AK89xx_SECONDARY
     compass_result = compass_self_test();
     if (!compass_result)
         result |= 0x04;
-#else
-        result |= 0x04;
-#endif
+    #else
+    result |= 0x04;
+    #endif
 restore:
 #elif defined MPU6500
     /* For now, this function will return a "pass" result for all three sensors
@@ -2642,13 +2751,13 @@ int mpu_run_self_test(long *gyro, long *accel)
     result = 0x7;
 #endif
     /* Set to invalid values to ensure no I2C writes are skipped. */
-    st.chip_cfg.gyro_fsr = 0xFF;
-    st.chip_cfg.accel_fsr = 0xFF;
-    st.chip_cfg.lpf = 0xFF;
+    st.chip_cfg.gyro_fsr    = 0xFF;
+    st.chip_cfg.accel_fsr   = 0xFF;
+    st.chip_cfg.lpf         = 0xFF;
     st.chip_cfg.sample_rate = 0xFFFF;
-    st.chip_cfg.sensors = 0xFF;
+    st.chip_cfg.sensors     = 0xFF;
     st.chip_cfg.fifo_enable = 0xFF;
-    st.chip_cfg.clk_src = INV_CLK_PLL;
+    st.chip_cfg.clk_src     = INV_CLK_PLL;
     mpu_set_gyro_fsr(gyro_fsr);
     mpu_set_accel_fsr(accel_fsr);
     mpu_set_lpf(lpf);
@@ -2672,7 +2781,7 @@ int mpu_run_self_test(long *gyro, long *accel)
  *  @return     0 if successful.
  */
 int mpu_write_mem(unsigned short mem_addr, unsigned short length,
-        unsigned char *data)
+                  unsigned char *data)
 {
     unsigned char tmp[2];
 
@@ -2705,7 +2814,7 @@ int mpu_write_mem(unsigned short mem_addr, unsigned short length,
  *  @return     0 if successful.
  */
 int mpu_read_mem(unsigned short mem_addr, unsigned short length,
-        unsigned char *data)
+                 unsigned char *data)
 {
     unsigned char tmp[2];
 
@@ -2737,12 +2846,12 @@ int mpu_read_mem(unsigned short mem_addr, unsigned short length,
  *  @return     0 if successful.
  */
 int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
-    unsigned short start_addr, unsigned short sample_rate)
+                      unsigned short start_addr, unsigned short sample_rate)
 {
     unsigned short ii;
     unsigned short this_write;
     /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */
-#define LOAD_CHUNK  (16)
+#define LOAD_CHUNK (16)
     unsigned char cur[LOAD_CHUNK], tmp[2];
 
     if (st.chip_cfg.dmp_loaded)
@@ -2751,13 +2860,14 @@ int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
 
     if (!firmware)
         return -1;
-    for (ii = 0; ii < length; ii += this_write) {
+    for (ii = 0; ii < length; ii += this_write)
+    {
         this_write = min(LOAD_CHUNK, length - ii);
-        if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))
+        if (mpu_write_mem(ii, this_write, (unsigned char *)&firmware[ii]))
             return -1;
         if (mpu_read_mem(ii, this_write, cur))
             return -1;
-        if (memcmp(firmware+ii, cur, this_write))
+        if (memcmp(firmware + ii, cur, this_write))
             return -2;
     }
 
@@ -2767,7 +2877,7 @@ int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
     if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp))
         return -1;
 
-    st.chip_cfg.dmp_loaded = 1;
+    st.chip_cfg.dmp_loaded      = 1;
     st.chip_cfg.dmp_sample_rate = sample_rate;
     return 0;
 }
@@ -2783,7 +2893,8 @@ int mpu_set_dmp_state(unsigned char enable)
     if (st.chip_cfg.dmp_on == enable)
         return 0;
 
-    if (enable) {
+    if (enable)
+    {
         if (!st.chip_cfg.dmp_loaded)
             return -1;
         /* Disable data ready interrupt. */
@@ -2799,7 +2910,9 @@ int mpu_set_dmp_state(unsigned char enable)
         /* Enable DMP interrupt. */
         set_int_enable(1);
         mpu_reset_fifo();
-    } else {
+    }
+    else
+    {
         /* Disable DMP interrupt. */
         set_int_enable(0);
         /* Restore FIFO settings. */
@@ -2831,14 +2944,16 @@ static int setup_compass(void)
     mpu_set_bypass(1);
 
     /* Find compass. Possible addresses range from 0x0C to 0x0F. */
-    for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
+    for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++)
+    {
         int result;
         result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
         if (!result && (data[0] == AKM_WHOAMI))
             break;
     }
 
-    if (akm_addr > 0x0F) {
+    if (akm_addr > 0x0F)
+    {
         /* TODO: Handle this case in all compass-related functions. */
         return -1;
     }
@@ -2914,12 +3029,12 @@ static int setup_compass(void)
     if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
         return -1;
 
-#ifdef MPU9150
+    #ifdef MPU9150
     /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
     data[0] = BIT_I2C_MST_VDDIO;
     if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))
         return -1;
-#endif
+    #endif
 
     return 0;
 }
@@ -2939,30 +3054,30 @@ int mpu_get_compass_reg(short *data, unsigned long *timestamp)
     if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS))
         return -1;
 
-#ifdef AK89xx_BYPASS
+    #ifdef AK89xx_BYPASS
     if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
         return -1;
     tmp[8] = AKM_SINGLE_MEASUREMENT;
-    if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
+    if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp + 8))
         return -1;
-#else
+    #else
     if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp))
         return -1;
-#endif
+    #endif
 
-#if defined AK8975_SECONDARY
+    #if defined AK8975_SECONDARY
     /* AK8975 doesn't have the overrun error bit. */
     if (!(tmp[0] & AKM_DATA_READY))
         return -2;
     if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))
         return -3;
-#elif defined AK8963_SECONDARY
+    #elif defined AK8963_SECONDARY
     /* AK8963 doesn't have the data read error bit. */
     if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))
         return -2;
     if (tmp[7] & AKM_OVERFLOW)
         return -3;
-#endif
+    #endif
     data[0] = (tmp[2] << 8) | tmp[1];
     data[1] = (tmp[4] << 8) | tmp[3];
     data[2] = (tmp[6] << 8) | tmp[5];
@@ -2972,7 +3087,7 @@ int mpu_get_compass_reg(short *data, unsigned long *timestamp)
     data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8;
 
     if (timestamp)
-    	timestamp = HAL_GetTick();
+        *timestamp = HAL_GetTick();
     return 0;
 #else
     return -1;
@@ -3033,15 +3148,16 @@ int mpu_get_compass_fsr(unsigned short *fsr)
  *  @return     0 if successful.
  */
 int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
-    unsigned short lpa_freq)
+                            unsigned short lpa_freq)
 {
 
 #if defined MPU6500
     unsigned char data[3];
 #endif
-    if (lpa_freq) {
+    if (lpa_freq)
+    {
 #if defined MPU6500
-    	unsigned char thresh_hw;
+        unsigned char thresh_hw;
 
         /* 1LSb = 4mg. */
         if (thresh > 1020)
@@ -3064,12 +3180,15 @@ int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
             return -1;
 #endif
 
-        if (!st.chip_cfg.int_motion_only) {
+        if (!st.chip_cfg.int_motion_only)
+        {
             /* Store current settings for later. */
-            if (st.chip_cfg.dmp_on) {
+            if (st.chip_cfg.dmp_on)
+            {
                 mpu_set_dmp_state(0);
                 st.chip_cfg.cache.dmp_on = 1;
-            } else
+            }
+            else
                 st.chip_cfg.cache.dmp_on = 0;
             mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr);
             mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr);
@@ -3137,11 +3256,14 @@ int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
         st.chip_cfg.int_motion_only = 1;
         return 0;
 #endif
-    } else {
+    }
+    else
+    {
         /* Don't "restore" the previous state if no state has been saved. */
         unsigned int ii;
-        char *cache_ptr = (char*)&st.chip_cfg.cache;
-        for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) {
+        char *cache_ptr = (char *)&st.chip_cfg.cache;
+        for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++)
+        {
             if (cache_ptr[ii] != 0)
                 goto lp_int_restore;
         }
@@ -3150,13 +3272,13 @@ int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
     }
 lp_int_restore:
     /* Set to invalid values to ensure no I2C writes are skipped. */
-    st.chip_cfg.gyro_fsr = 0xFF;
-    st.chip_cfg.accel_fsr = 0xFF;
-    st.chip_cfg.lpf = 0xFF;
+    st.chip_cfg.gyro_fsr    = 0xFF;
+    st.chip_cfg.accel_fsr   = 0xFF;
+    st.chip_cfg.lpf         = 0xFF;
     st.chip_cfg.sample_rate = 0xFFFF;
-    st.chip_cfg.sensors = 0xFF;
+    st.chip_cfg.sensors     = 0xFF;
     st.chip_cfg.fifo_enable = 0xFF;
-    st.chip_cfg.clk_src = INV_CLK_PLL;
+    st.chip_cfg.clk_src     = INV_CLK_PLL;
     mpu_set_sensors(st.chip_cfg.cache.sensors_on);
     mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr);
     mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr);
@@ -3177,8 +3299,3 @@ int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
     st.chip_cfg.int_motion_only = 0;
     return 0;
 }
-
-/**
- *  @}
- */
-
diff --git a/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/eMPL/inv_mpu_dmp_motion_driver.c b/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/eMPL/inv_mpu_dmp_motion_driver.c
index bfd569ee7..12b84b70e 100644
--- a/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/eMPL/inv_mpu_dmp_motion_driver.c
+++ b/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/eMPL/inv_mpu_dmp_motion_driver.c
@@ -33,224 +33,230 @@
  * get_ms(unsigned long *count)
  */
 #if defined EMPL_TARGET_STM32F4
-#include "i2c.h"   
-#include "main.h"
-   
-#define i2c_write   Sensors_I2C_WriteRegister
-#define i2c_read    Sensors_I2C_ReadRegister
-#define get_ms      get_tick_count
+    #include "i2c.h"
+    #include "main.h"
+
+    #define i2c_write Sensors_I2C_WriteRegister
+    #define i2c_read  Sensors_I2C_ReadRegister
+    #define get_ms    get_tick_count
 
 #elif defined MOTION_DRIVER_TARGET_MSP430
-#include "msp430.h"
-#include "msp430_clock.h"
-#define delay_ms    msp430_delay_ms
-#define get_ms      msp430_get_clock_ms
-#define log_i(...)     do {} while (0)
-#define log_e(...)     do {} while (0)
+    #include "msp430.h"
+    #include "msp430_clock.h"
+    #define delay_ms msp430_delay_ms
+    #define get_ms   msp430_get_clock_ms
+    #define log_i(...) \
+        do             \
+        {              \
+        } while (0)
+    #define log_e(...) \
+        do             \
+        {              \
+        } while (0)
 
 #elif defined EMPL_TARGET_MSP430
-#include "msp430.h"
-#include "msp430_clock.h"
-#include "log.h"
-#define delay_ms    msp430_delay_ms
-#define get_ms      msp430_get_clock_ms
-#define log_i       MPL_LOGI
-#define log_e       MPL_LOGE
+    #include "msp430.h"
+    #include "msp430_clock.h"
+    #include "log.h"
+    #define delay_ms msp430_delay_ms
+    #define get_ms   msp430_get_clock_ms
+    #define log_i    MPL_LOGI
+    #define log_e    MPL_LOGE
 
 #elif defined EMPL_TARGET_UC3L0
-/* Instead of using the standard TWI driver from the ASF library, we're using
- * a TWI driver that follows the slave address + register address convention.
- */
-#include "delay.h"
-#include "sysclk.h"
-#include "log.h"
-#include "uc3l0_clock.h"
-/* delay_ms is a function already defined in ASF. */
-#define get_ms  uc3l0_get_clock_ms
-#define log_i       MPL_LOGI
-#define log_e       MPL_LOGE
+    /* Instead of using the standard TWI driver from the ASF library, we're using
+     * a TWI driver that follows the slave address + register address convention.
+     */
+    #include "delay.h"
+    #include "sysclk.h"
+    #include "log.h"
+    #include "uc3l0_clock.h"
+    /* delay_ms is a function already defined in ASF. */
+    #define get_ms uc3l0_get_clock_ms
+    #define log_i  MPL_LOGI
+    #define log_e  MPL_LOGE
 
 #else
-#error  Gyro driver is missing the system layer implementations.
+    #error Gyro driver is missing the system layer implementations.
 #endif
 
 /* These defines are copied from dmpDefaultMPU6050.c in the general MPL
  * releases. These defines may change for each DMP image, so be sure to modify
  * these values when switching to a new image.
  */
-#define CFG_LP_QUAT             (2712)
-#define END_ORIENT_TEMP         (1866)
-#define CFG_27                  (2742)
-#define CFG_20                  (2224)
-#define CFG_23                  (2745)
-#define CFG_FIFO_ON_EVENT       (2690)
-#define END_PREDICTION_UPDATE   (1761)
-#define CGNOTICE_INTR           (2620)
-#define X_GRT_Y_TMP             (1358)
-#define CFG_DR_INT              (1029)
-#define CFG_AUTH                (1035)
-#define UPDATE_PROP_ROT         (1835)
-#define END_COMPARE_Y_X_TMP2    (1455)
-#define SKIP_X_GRT_Y_TMP        (1359)
-#define SKIP_END_COMPARE        (1435)
-#define FCFG_3                  (1088)
-#define FCFG_2                  (1066)
-#define FCFG_1                  (1062)
-#define END_COMPARE_Y_X_TMP3    (1434)
-#define FCFG_7                  (1073)
-#define FCFG_6                  (1106)
-#define FLAT_STATE_END          (1713)
-#define SWING_END_4             (1616)
-#define SWING_END_2             (1565)
-#define SWING_END_3             (1587)
-#define SWING_END_1             (1550)
-#define CFG_8                   (2718)
-#define CFG_15                  (2727)
-#define CFG_16                  (2746)
-#define CFG_EXT_GYRO_BIAS       (1189)
-#define END_COMPARE_Y_X_TMP     (1407)
-#define DO_NOT_UPDATE_PROP_ROT  (1839)
-#define CFG_7                   (1205)
-#define FLAT_STATE_END_TEMP     (1683)
-#define END_COMPARE_Y_X         (1484)
-#define SKIP_SWING_END_1        (1551)
-#define SKIP_SWING_END_3        (1588)
-#define SKIP_SWING_END_2        (1566)
-#define TILTG75_START           (1672)
-#define CFG_6                   (2753)
-#define TILTL75_END             (1669)
-#define END_ORIENT              (1884)
-#define CFG_FLICK_IN            (2573)
-#define TILTL75_START           (1643)
-#define CFG_MOTION_BIAS         (1208)
-#define X_GRT_Y                 (1408)
-#define TEMPLABEL               (2324)
-#define CFG_ANDROID_ORIENT_INT  (1853)
-#define CFG_GYRO_RAW_DATA       (2722)
-#define X_GRT_Y_TMP2            (1379)
-
-#define D_0_22                  (22+512)
-#define D_0_24                  (24+512)
-
-#define D_0_36                  (36)
-#define D_0_52                  (52)
-#define D_0_96                  (96)
-#define D_0_104                 (104)
-#define D_0_108                 (108)
-#define D_0_163                 (163)
-#define D_0_188                 (188)
-#define D_0_192                 (192)
-#define D_0_224                 (224)
-#define D_0_228                 (228)
-#define D_0_232                 (232)
-#define D_0_236                 (236)
-
-#define D_1_2                   (256 + 2)
-#define D_1_4                   (256 + 4)
-#define D_1_8                   (256 + 8)
-#define D_1_10                  (256 + 10)
-#define D_1_24                  (256 + 24)
-#define D_1_28                  (256 + 28)
-#define D_1_36                  (256 + 36)
-#define D_1_40                  (256 + 40)
-#define D_1_44                  (256 + 44)
-#define D_1_72                  (256 + 72)
-#define D_1_74                  (256 + 74)
-#define D_1_79                  (256 + 79)
-#define D_1_88                  (256 + 88)
-#define D_1_90                  (256 + 90)
-#define D_1_92                  (256 + 92)
-#define D_1_96                  (256 + 96)
-#define D_1_98                  (256 + 98)
-#define D_1_106                 (256 + 106)
-#define D_1_108                 (256 + 108)
-#define D_1_112                 (256 + 112)
-#define D_1_128                 (256 + 144)
-#define D_1_152                 (256 + 12)
-#define D_1_160                 (256 + 160)
-#define D_1_176                 (256 + 176)
-#define D_1_178                 (256 + 178)
-#define D_1_218                 (256 + 218)
-#define D_1_232                 (256 + 232)
-#define D_1_236                 (256 + 236)
-#define D_1_240                 (256 + 240)
-#define D_1_244                 (256 + 244)
-#define D_1_250                 (256 + 250)
-#define D_1_252                 (256 + 252)
-#define D_2_12                  (512 + 12)
-#define D_2_96                  (512 + 96)
-#define D_2_108                 (512 + 108)
-#define D_2_208                 (512 + 208)
-#define D_2_224                 (512 + 224)
-#define D_2_236                 (512 + 236)
-#define D_2_244                 (512 + 244)
-#define D_2_248                 (512 + 248)
-#define D_2_252                 (512 + 252)
-
-#define CPASS_BIAS_X            (35 * 16 + 4)
-#define CPASS_BIAS_Y            (35 * 16 + 8)
-#define CPASS_BIAS_Z            (35 * 16 + 12)
-#define CPASS_MTX_00            (36 * 16)
-#define CPASS_MTX_01            (36 * 16 + 4)
-#define CPASS_MTX_02            (36 * 16 + 8)
-#define CPASS_MTX_10            (36 * 16 + 12)
-#define CPASS_MTX_11            (37 * 16)
-#define CPASS_MTX_12            (37 * 16 + 4)
-#define CPASS_MTX_20            (37 * 16 + 8)
-#define CPASS_MTX_21            (37 * 16 + 12)
-#define CPASS_MTX_22            (43 * 16 + 12)
-#define D_EXT_GYRO_BIAS_X       (61 * 16)
-#define D_EXT_GYRO_BIAS_Y       (61 * 16) + 4
-#define D_EXT_GYRO_BIAS_Z       (61 * 16) + 8
-#define D_ACT0                  (40 * 16)
-#define D_ACSX                  (40 * 16 + 4)
-#define D_ACSY                  (40 * 16 + 8)
-#define D_ACSZ                  (40 * 16 + 12)
-
-#define FLICK_MSG               (45 * 16 + 4)
-#define FLICK_COUNTER           (45 * 16 + 8)
-#define FLICK_LOWER             (45 * 16 + 12)
-#define FLICK_UPPER             (46 * 16 + 12)
-
-#define D_AUTH_OUT              (992)
-#define D_AUTH_IN               (996)
-#define D_AUTH_A                (1000)
-#define D_AUTH_B                (1004)
-
-#define D_PEDSTD_BP_B           (768 + 0x1C)
-#define D_PEDSTD_HP_A           (768 + 0x78)
-#define D_PEDSTD_HP_B           (768 + 0x7C)
-#define D_PEDSTD_BP_A4          (768 + 0x40)
-#define D_PEDSTD_BP_A3          (768 + 0x44)
-#define D_PEDSTD_BP_A2          (768 + 0x48)
-#define D_PEDSTD_BP_A1          (768 + 0x4C)
-#define D_PEDSTD_INT_THRSH      (768 + 0x68)
-#define D_PEDSTD_CLIP           (768 + 0x6C)
-#define D_PEDSTD_SB             (768 + 0x28)
-#define D_PEDSTD_SB_TIME        (768 + 0x2C)
-#define D_PEDSTD_PEAKTHRSH      (768 + 0x98)
-#define D_PEDSTD_TIML           (768 + 0x2A)
-#define D_PEDSTD_TIMH           (768 + 0x2E)
-#define D_PEDSTD_PEAK           (768 + 0X94)
-#define D_PEDSTD_STEPCTR        (768 + 0x60)
-#define D_PEDSTD_TIMECTR        (964)
-#define D_PEDSTD_DECI           (768 + 0xA0)
-
-#define D_HOST_NO_MOT           (976)
-#define D_ACCEL_BIAS            (660)
-
-#define D_ORIENT_GAP            (76)
-
-#define D_TILT0_H               (48)
-#define D_TILT0_L               (50)
-#define D_TILT1_H               (52)
-#define D_TILT1_L               (54)
-#define D_TILT2_H               (56)
-#define D_TILT2_L               (58)
-#define D_TILT3_H               (60)
-#define D_TILT3_L               (62)
-
-#define DMP_CODE_SIZE           (3062)
+#define CFG_LP_QUAT            (2712)
+#define END_ORIENT_TEMP        (1866)
+#define CFG_27                 (2742)
+#define CFG_20                 (2224)
+#define CFG_23                 (2745)
+#define CFG_FIFO_ON_EVENT      (2690)
+#define END_PREDICTION_UPDATE  (1761)
+#define CGNOTICE_INTR          (2620)
+#define X_GRT_Y_TMP            (1358)
+#define CFG_DR_INT             (1029)
+#define CFG_AUTH               (1035)
+#define UPDATE_PROP_ROT        (1835)
+#define END_COMPARE_Y_X_TMP2   (1455)
+#define SKIP_X_GRT_Y_TMP       (1359)
+#define SKIP_END_COMPARE       (1435)
+#define FCFG_3                 (1088)
+#define FCFG_2                 (1066)
+#define FCFG_1                 (1062)
+#define END_COMPARE_Y_X_TMP3   (1434)
+#define FCFG_7                 (1073)
+#define FCFG_6                 (1106)
+#define FLAT_STATE_END         (1713)
+#define SWING_END_4            (1616)
+#define SWING_END_2            (1565)
+#define SWING_END_3            (1587)
+#define SWING_END_1            (1550)
+#define CFG_8                  (2718)
+#define CFG_15                 (2727)
+#define CFG_16                 (2746)
+#define CFG_EXT_GYRO_BIAS      (1189)
+#define END_COMPARE_Y_X_TMP    (1407)
+#define DO_NOT_UPDATE_PROP_ROT (1839)
+#define CFG_7                  (1205)
+#define FLAT_STATE_END_TEMP    (1683)
+#define END_COMPARE_Y_X        (1484)
+#define SKIP_SWING_END_1       (1551)
+#define SKIP_SWING_END_3       (1588)
+#define SKIP_SWING_END_2       (1566)
+#define TILTG75_START          (1672)
+#define CFG_6                  (2753)
+#define TILTL75_END            (1669)
+#define END_ORIENT             (1884)
+#define CFG_FLICK_IN           (2573)
+#define TILTL75_START          (1643)
+#define CFG_MOTION_BIAS        (1208)
+#define X_GRT_Y                (1408)
+#define TEMPLABEL              (2324)
+#define CFG_ANDROID_ORIENT_INT (1853)
+#define CFG_GYRO_RAW_DATA      (2722)
+#define X_GRT_Y_TMP2           (1379)
+
+#define D_0_22 (22 + 512)
+#define D_0_24 (24 + 512)
+
+#define D_0_36  (36)
+#define D_0_52  (52)
+#define D_0_96  (96)
+#define D_0_104 (104)
+#define D_0_108 (108)
+#define D_0_163 (163)
+#define D_0_188 (188)
+#define D_0_192 (192)
+#define D_0_224 (224)
+#define D_0_228 (228)
+#define D_0_232 (232)
+#define D_0_236 (236)
+
+#define D_1_2   (256 + 2)
+#define D_1_4   (256 + 4)
+#define D_1_8   (256 + 8)
+#define D_1_10  (256 + 10)
+#define D_1_24  (256 + 24)
+#define D_1_28  (256 + 28)
+#define D_1_36  (256 + 36)
+#define D_1_40  (256 + 40)
+#define D_1_44  (256 + 44)
+#define D_1_72  (256 + 72)
+#define D_1_74  (256 + 74)
+#define D_1_79  (256 + 79)
+#define D_1_88  (256 + 88)
+#define D_1_90  (256 + 90)
+#define D_1_92  (256 + 92)
+#define D_1_96  (256 + 96)
+#define D_1_98  (256 + 98)
+#define D_1_106 (256 + 106)
+#define D_1_108 (256 + 108)
+#define D_1_112 (256 + 112)
+#define D_1_128 (256 + 144)
+#define D_1_152 (256 + 12)
+#define D_1_160 (256 + 160)
+#define D_1_176 (256 + 176)
+#define D_1_178 (256 + 178)
+#define D_1_218 (256 + 218)
+#define D_1_232 (256 + 232)
+#define D_1_236 (256 + 236)
+#define D_1_240 (256 + 240)
+#define D_1_244 (256 + 244)
+#define D_1_250 (256 + 250)
+#define D_1_252 (256 + 252)
+#define D_2_12  (512 + 12)
+#define D_2_96  (512 + 96)
+#define D_2_108 (512 + 108)
+#define D_2_208 (512 + 208)
+#define D_2_224 (512 + 224)
+#define D_2_236 (512 + 236)
+#define D_2_244 (512 + 244)
+#define D_2_248 (512 + 248)
+#define D_2_252 (512 + 252)
+
+#define CPASS_BIAS_X      (35 * 16 + 4)
+#define CPASS_BIAS_Y      (35 * 16 + 8)
+#define CPASS_BIAS_Z      (35 * 16 + 12)
+#define CPASS_MTX_00      (36 * 16)
+#define CPASS_MTX_01      (36 * 16 + 4)
+#define CPASS_MTX_02      (36 * 16 + 8)
+#define CPASS_MTX_10      (36 * 16 + 12)
+#define CPASS_MTX_11      (37 * 16)
+#define CPASS_MTX_12      (37 * 16 + 4)
+#define CPASS_MTX_20      (37 * 16 + 8)
+#define CPASS_MTX_21      (37 * 16 + 12)
+#define CPASS_MTX_22      (43 * 16 + 12)
+#define D_EXT_GYRO_BIAS_X (61 * 16)
+#define D_EXT_GYRO_BIAS_Y (61 * 16) + 4
+#define D_EXT_GYRO_BIAS_Z (61 * 16) + 8
+#define D_ACT0            (40 * 16)
+#define D_ACSX            (40 * 16 + 4)
+#define D_ACSY            (40 * 16 + 8)
+#define D_ACSZ            (40 * 16 + 12)
+
+#define FLICK_MSG     (45 * 16 + 4)
+#define FLICK_COUNTER (45 * 16 + 8)
+#define FLICK_LOWER   (45 * 16 + 12)
+#define FLICK_UPPER   (46 * 16 + 12)
+
+#define D_AUTH_OUT (992)
+#define D_AUTH_IN  (996)
+#define D_AUTH_A   (1000)
+#define D_AUTH_B   (1004)
+
+#define D_PEDSTD_BP_B      (768 + 0x1C)
+#define D_PEDSTD_HP_A      (768 + 0x78)
+#define D_PEDSTD_HP_B      (768 + 0x7C)
+#define D_PEDSTD_BP_A4     (768 + 0x40)
+#define D_PEDSTD_BP_A3     (768 + 0x44)
+#define D_PEDSTD_BP_A2     (768 + 0x48)
+#define D_PEDSTD_BP_A1     (768 + 0x4C)
+#define D_PEDSTD_INT_THRSH (768 + 0x68)
+#define D_PEDSTD_CLIP      (768 + 0x6C)
+#define D_PEDSTD_SB        (768 + 0x28)
+#define D_PEDSTD_SB_TIME   (768 + 0x2C)
+#define D_PEDSTD_PEAKTHRSH (768 + 0x98)
+#define D_PEDSTD_TIML      (768 + 0x2A)
+#define D_PEDSTD_TIMH      (768 + 0x2E)
+#define D_PEDSTD_PEAK      (768 + 0X94)
+#define D_PEDSTD_STEPCTR   (768 + 0x60)
+#define D_PEDSTD_TIMECTR   (964)
+#define D_PEDSTD_DECI      (768 + 0xA0)
+
+#define D_HOST_NO_MOT (976)
+#define D_ACCEL_BIAS  (660)
+
+#define D_ORIENT_GAP (76)
+
+#define D_TILT0_H (48)
+#define D_TILT0_L (50)
+#define D_TILT1_H (52)
+#define D_TILT1_L (54)
+#define D_TILT2_H (56)
+#define D_TILT2_L (58)
+#define D_TILT3_H (60)
+#define D_TILT3_L (62)
+
+#define DMP_CODE_SIZE (3062)
 
 static const unsigned char dmp_memory[DMP_CODE_SIZE] = {
     /* bank # 0 */
@@ -457,33 +463,32 @@ static const unsigned char dmp_memory[DMP_CODE_SIZE] = {
     0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00,
     0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10,
     0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88,
-    0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
-};
+    0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff};
 
 static const unsigned short sStartAddress = 0x0400;
 
 /* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */
 
-#define INT_SRC_TAP             (0x01)
-#define INT_SRC_ANDROID_ORIENT  (0x08)
+#define INT_SRC_TAP            (0x01)
+#define INT_SRC_ANDROID_ORIENT (0x08)
 
-#define DMP_FEATURE_SEND_ANY_GYRO   (DMP_FEATURE_SEND_RAW_GYRO | \
-                                     DMP_FEATURE_SEND_CAL_GYRO)
+#define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | DMP_FEATURE_SEND_CAL_GYRO)
 
-#define MAX_PACKET_LENGTH   (32)
+#define MAX_PACKET_LENGTH (32)
 
-#define DMP_SAMPLE_RATE     (200)
-#define GYRO_SF             (46850825LL * 200 / DMP_SAMPLE_RATE)
+#define DMP_SAMPLE_RATE (200)
+#define GYRO_SF         (46850825LL * 200 / DMP_SAMPLE_RATE)
 
 #define FIFO_CORRUPTION_CHECK
 #ifdef FIFO_CORRUPTION_CHECK
-#define QUAT_ERROR_THRESH       (1L<<24)
-#define QUAT_MAG_SQ_NORMALIZED  (1L<<28)
-#define QUAT_MAG_SQ_MIN         (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)
-#define QUAT_MAG_SQ_MAX         (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH)
+    #define QUAT_ERROR_THRESH      (1L << 24)
+    #define QUAT_MAG_SQ_NORMALIZED (1L << 28)
+    #define QUAT_MAG_SQ_MIN        (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)
+    #define QUAT_MAG_SQ_MAX        (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH)
 #endif
 
-struct dmp_s {
+struct dmp_s
+{
     void (*tap_cb)(unsigned char count, unsigned char direction);
     void (*android_orient_cb)(unsigned char orientation);
     unsigned short orient;
@@ -493,13 +498,12 @@ struct dmp_s {
 };
 
 static struct dmp_s dmp = {
-    .tap_cb = NULL,
+    .tap_cb            = NULL,
     .android_orient_cb = NULL,
-    .orient = 0,
-    .feature_mask = 0,
-    .fifo_rate = 0,
-    .packet_length = 0
-};
+    .orient            = 0,
+    .feature_mask      = 0,
+    .fifo_rate         = 0,
+    .packet_length     = 0};
 
 /**
  *  @brief  Load the DMP with this image.
@@ -508,7 +512,7 @@ static struct dmp_s dmp = {
 int dmp_load_motion_driver_firmware(void)
 {
     return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress,
-        DMP_SAMPLE_RATE);
+                             DMP_SAMPLE_RATE);
 }
 
 /**
@@ -521,14 +525,14 @@ int dmp_load_motion_driver_firmware(void)
 int dmp_set_orientation(unsigned short orient)
 {
     unsigned char gyro_regs[3], accel_regs[3];
-    const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C};
+    const unsigned char gyro_axes[3]  = {DINA4C, DINACD, DINA6C};
     const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C};
-    const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76};
+    const unsigned char gyro_sign[3]  = {DINA36, DINA56, DINA76};
     const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66};
 
-    gyro_regs[0] = gyro_axes[orient & 3];
-    gyro_regs[1] = gyro_axes[(orient >> 3) & 3];
-    gyro_regs[2] = gyro_axes[(orient >> 6) & 3];
+    gyro_regs[0]  = gyro_axes[orient & 3];
+    gyro_regs[1]  = gyro_axes[(orient >> 3) & 3];
+    gyro_regs[2]  = gyro_axes[(orient >> 6) & 3];
     accel_regs[0] = accel_axes[orient & 3];
     accel_regs[1] = accel_axes[(orient >> 3) & 3];
     accel_regs[2] = accel_axes[(orient >> 6) & 3];
@@ -541,15 +545,18 @@ int dmp_set_orientation(unsigned short orient)
 
     memcpy(gyro_regs, gyro_sign, 3);
     memcpy(accel_regs, accel_sign, 3);
-    if (orient & 4) {
+    if (orient & 4)
+    {
         gyro_regs[0] |= 1;
         accel_regs[0] |= 1;
     }
-    if (orient & 0x20) {
+    if (orient & 0x20)
+    {
         gyro_regs[1] |= 1;
         accel_regs[1] |= 1;
     }
-    if (orient & 0x100) {
+    if (orient & 0x100)
+    {
         gyro_regs[2] |= 1;
         accel_regs[2] |= 1;
     }
@@ -634,7 +641,7 @@ int dmp_set_accel_bias(long *bias)
 
     mpu_get_accel_sens(&accel_sens);
     accel_sf = (long long)accel_sens << 15;
-    __no_operation();
+    //__no_operation();
 
     accel_bias_body[0] = bias[dmp.orient & 3];
     if (dmp.orient & 4)
@@ -656,16 +663,16 @@ int dmp_set_accel_bias(long *bias)
     accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30);
 #endif
 
-    regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF);
-    regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF);
-    regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF);
-    regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF);
-    regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF);
-    regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF);
-    regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);
-    regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF);
-    regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF);
-    regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF);
+    regs[0]  = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF);
+    regs[1]  = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF);
+    regs[2]  = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF);
+    regs[3]  = (unsigned char)(accel_bias_body[0] & 0xFF);
+    regs[4]  = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF);
+    regs[5]  = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF);
+    regs[6]  = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);
+    regs[7]  = (unsigned char)(accel_bias_body[1] & 0xFF);
+    regs[8]  = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF);
+    regs[9]  = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF);
     regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF);
     regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF);
     return mpu_write_mem(D_ACCEL_BIAS, 12, regs);
@@ -680,18 +687,18 @@ int dmp_set_accel_bias(long *bias)
 int dmp_set_fifo_rate(unsigned short rate)
 {
     const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB,
-        0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF};
+                                        0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF};
     unsigned short div;
     unsigned char tmp[8];
 
     if (rate > DMP_SAMPLE_RATE)
         return -1;
-    div = DMP_SAMPLE_RATE / rate - 1;
+    div    = DMP_SAMPLE_RATE / rate - 1;
     tmp[0] = (unsigned char)((div >> 8) & 0xFF);
     tmp[1] = (unsigned char)(div & 0xFF);
     if (mpu_write_mem(D_0_22, 2, tmp))
         return -1;
-    if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end))
+    if (mpu_write_mem(CFG_6, 12, (unsigned char *)regs_end))
         return -1;
 
     dmp.fifo_rate = rate;
@@ -726,51 +733,55 @@ int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh)
     scaled_thresh = (float)thresh / DMP_SAMPLE_RATE;
 
     mpu_get_accel_fsr(&accel_fsr);
-    switch (accel_fsr) {
-    case 2:
-        dmp_thresh = (unsigned short)(scaled_thresh * 16384);
-        /* dmp_thresh * 0.75 */
-        dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288);
-        break;
-    case 4:
-        dmp_thresh = (unsigned short)(scaled_thresh * 8192);
-        /* dmp_thresh * 0.75 */
-        dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144);
-        break;
-    case 8:
-        dmp_thresh = (unsigned short)(scaled_thresh * 4096);
-        /* dmp_thresh * 0.75 */
-        dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072);
-        break;
-    case 16:
-        dmp_thresh = (unsigned short)(scaled_thresh * 2048);
-        /* dmp_thresh * 0.75 */
-        dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536);
-        break;
-    default:
-        return -1;
+    switch (accel_fsr)
+    {
+        case 2:
+            dmp_thresh = (unsigned short)(scaled_thresh * 16384);
+            /* dmp_thresh * 0.75 */
+            dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288);
+            break;
+        case 4:
+            dmp_thresh = (unsigned short)(scaled_thresh * 8192);
+            /* dmp_thresh * 0.75 */
+            dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144);
+            break;
+        case 8:
+            dmp_thresh = (unsigned short)(scaled_thresh * 4096);
+            /* dmp_thresh * 0.75 */
+            dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072);
+            break;
+        case 16:
+            dmp_thresh = (unsigned short)(scaled_thresh * 2048);
+            /* dmp_thresh * 0.75 */
+            dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536);
+            break;
+        default:
+            return -1;
     }
     tmp[0] = (unsigned char)(dmp_thresh >> 8);
     tmp[1] = (unsigned char)(dmp_thresh & 0xFF);
     tmp[2] = (unsigned char)(dmp_thresh_2 >> 8);
     tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF);
 
-    if (axis & TAP_X) {
+    if (axis & TAP_X)
+    {
         if (mpu_write_mem(DMP_TAP_THX, 2, tmp))
             return -1;
-        if (mpu_write_mem(D_1_36, 2, tmp+2))
+        if (mpu_write_mem(D_1_36, 2, tmp + 2))
             return -1;
     }
-    if (axis & TAP_Y) {
+    if (axis & TAP_Y)
+    {
         if (mpu_write_mem(DMP_TAP_THY, 2, tmp))
             return -1;
-        if (mpu_write_mem(D_1_40, 2, tmp+2))
+        if (mpu_write_mem(D_1_40, 2, tmp + 2))
             return -1;
     }
-    if (axis & TAP_Z) {
+    if (axis & TAP_Z)
+    {
         if (mpu_write_mem(DMP_TAP_THZ, 2, tmp))
             return -1;
-        if (mpu_write_mem(D_1_44, 2, tmp+2))
+        if (mpu_write_mem(D_1_44, 2, tmp + 2))
             return -1;
     }
     return 0;
@@ -823,8 +834,8 @@ int dmp_set_tap_time(unsigned short time)
     unsigned char tmp[2];
 
     dmp_time = time / (1000 / DMP_SAMPLE_RATE);
-    tmp[0] = (unsigned char)(dmp_time >> 8);
-    tmp[1] = (unsigned char)(dmp_time & 0xFF);
+    tmp[0]   = (unsigned char)(dmp_time >> 8);
+    tmp[1]   = (unsigned char)(dmp_time & 0xFF);
     return mpu_write_mem(DMP_TAPW_MIN, 2, tmp);
 }
 
@@ -839,8 +850,8 @@ int dmp_set_tap_time_multi(unsigned short time)
     unsigned char tmp[2];
 
     dmp_time = time / (1000 / DMP_SAMPLE_RATE);
-    tmp[0] = (unsigned char)(dmp_time >> 8);
-    tmp[1] = (unsigned char)(dmp_time & 0xFF);
+    tmp[0]   = (unsigned char)(dmp_time >> 8);
+    tmp[1]   = (unsigned char)(dmp_time & 0xFF);
     return mpu_write_mem(D_1_218, 2, tmp);
 }
 
@@ -855,10 +866,10 @@ int dmp_set_shake_reject_thresh(long sf, unsigned short thresh)
 {
     unsigned char tmp[4];
     long thresh_scaled = sf / 1000 * thresh;
-    tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF);
-    tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF);
-    tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF);
-    tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF);
+    tmp[0]             = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF);
+    tmp[1]             = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF);
+    tmp[2]             = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF);
+    tmp[3]             = (unsigned char)((long)thresh_scaled & 0xFF);
     return mpu_write_mem(D_1_92, 4, tmp);
 }
 
@@ -877,7 +888,7 @@ int dmp_set_shake_reject_time(unsigned short time)
     time /= (1000 / DMP_SAMPLE_RATE);
     tmp[0] = time >> 8;
     tmp[1] = time & 0xFF;
-    return mpu_write_mem(D_1_90,2,tmp);
+    return mpu_write_mem(D_1_90, 2, tmp);
 }
 
 /**
@@ -895,7 +906,7 @@ int dmp_set_shake_reject_timeout(unsigned short time)
     time /= (1000 / DMP_SAMPLE_RATE);
     tmp[0] = time >> 8;
     tmp[1] = time & 0xFF;
-    return mpu_write_mem(D_1_88,2,tmp);
+    return mpu_write_mem(D_1_88, 2, tmp);
 }
 
 /**
@@ -912,8 +923,7 @@ int dmp_get_pedometer_step_count(unsigned long *count)
     if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp))
         return -1;
 
-    count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
-        ((unsigned long)tmp[2] << 8) | tmp[3];
+    count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | ((unsigned long)tmp[2] << 8) | tmp[3];
     return 0;
 }
 
@@ -949,8 +959,7 @@ int dmp_get_pedometer_walk_time(unsigned long *time)
     if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp))
         return -1;
 
-    time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) |
-        ((unsigned long)tmp[2] << 8) | tmp[3]) * 20;
+    time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | ((unsigned long)tmp[2] << 8) | tmp[3]) * 20;
     return 0;
 }
 
@@ -1006,20 +1015,26 @@ int dmp_enable_feature(unsigned short mask)
 
     /* Send sensor data to the FIFO. */
     tmp[0] = 0xA3;
-    if (mask & DMP_FEATURE_SEND_RAW_ACCEL) {
+    if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
+    {
         tmp[1] = 0xC0;
         tmp[2] = 0xC8;
         tmp[3] = 0xC2;
-    } else {
+    }
+    else
+    {
         tmp[1] = 0xA3;
         tmp[2] = 0xA3;
         tmp[3] = 0xA3;
     }
-    if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
+    if (mask & DMP_FEATURE_SEND_ANY_GYRO)
+    {
         tmp[4] = 0xC4;
         tmp[5] = 0xCC;
         tmp[6] = 0xC6;
-    } else {
+    }
+    else
+    {
         tmp[4] = 0xA3;
         tmp[5] = 0xA3;
         tmp[6] = 0xA3;
@@ -1027,27 +1042,31 @@ int dmp_enable_feature(unsigned short mask)
     tmp[7] = 0xA3;
     tmp[8] = 0xA3;
     tmp[9] = 0xA3;
-    mpu_write_mem(CFG_15,10,tmp);
+    mpu_write_mem(CFG_15, 10, tmp);
 
     /* Send gesture data to the FIFO. */
     if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
         tmp[0] = DINA20;
     else
         tmp[0] = 0xD8;
-    mpu_write_mem(CFG_27,1,tmp);
+    mpu_write_mem(CFG_27, 1, tmp);
 
     if (mask & DMP_FEATURE_GYRO_CAL)
         dmp_enable_gyro_cal(1);
     else
         dmp_enable_gyro_cal(0);
 
-    if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
-        if (mask & DMP_FEATURE_SEND_CAL_GYRO) {
+    if (mask & DMP_FEATURE_SEND_ANY_GYRO)
+    {
+        if (mask & DMP_FEATURE_SEND_CAL_GYRO)
+        {
             tmp[0] = 0xB2;
             tmp[1] = 0x8B;
             tmp[2] = 0xB6;
             tmp[3] = 0x9B;
-        } else {
+        }
+        else
+        {
             tmp[0] = DINAC0;
             tmp[1] = DINA80;
             tmp[2] = DINAC2;
@@ -1056,7 +1075,8 @@ int dmp_enable_feature(unsigned short mask)
         mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp);
     }
 
-    if (mask & DMP_FEATURE_TAP) {
+    if (mask & DMP_FEATURE_TAP)
+    {
         /* Enable tap. */
         tmp[0] = 0xF8;
         mpu_write_mem(CFG_20, 1, tmp);
@@ -1069,14 +1089,18 @@ int dmp_enable_feature(unsigned short mask)
         dmp_set_shake_reject_thresh(GYRO_SF, 200);
         dmp_set_shake_reject_time(40);
         dmp_set_shake_reject_timeout(10);
-    } else {
+    }
+    else
+    {
         tmp[0] = 0xD8;
         mpu_write_mem(CFG_20, 1, tmp);
     }
 
-    if (mask & DMP_FEATURE_ANDROID_ORIENT) {
+    if (mask & DMP_FEATURE_ANDROID_ORIENT)
+    {
         tmp[0] = 0xD9;
-    } else
+    }
+    else
         tmp[0] = 0xD8;
     mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);
 
@@ -1129,10 +1153,13 @@ int dmp_get_enabled_features(unsigned short *mask)
  */
 int dmp_enable_gyro_cal(unsigned char enable)
 {
-    if (enable) {
+    if (enable)
+    {
         unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d};
         return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
-    } else {
+    }
+    else
+    {
         unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7};
         return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
     }
@@ -1148,7 +1175,8 @@ int dmp_enable_gyro_cal(unsigned char enable)
 int dmp_enable_lp_quat(unsigned char enable)
 {
     unsigned char regs[4];
-    if (enable) {
+    if (enable)
+    {
         regs[0] = DINBC0;
         regs[1] = DINBC2;
         regs[2] = DINBC4;
@@ -1172,12 +1200,14 @@ int dmp_enable_lp_quat(unsigned char enable)
 int dmp_enable_6x_lp_quat(unsigned char enable)
 {
     unsigned char regs[4];
-    if (enable) {
+    if (enable)
+    {
         regs[0] = DINA20;
         regs[1] = DINA28;
         regs[2] = DINA30;
         regs[3] = DINA38;
-    } else
+    }
+    else
         memset(regs, 0xA3, 4);
 
     mpu_write_mem(CFG_8, 4, regs);
@@ -1195,17 +1225,19 @@ static int decode_gesture(unsigned char *gesture)
     unsigned char tap, android_orient;
 
     android_orient = gesture[3] & 0xC0;
-    tap = 0x3F & gesture[3];
+    tap            = 0x3F & gesture[3];
 
-    if (gesture[1] & INT_SRC_TAP) {
+    if (gesture[1] & INT_SRC_TAP)
+    {
         unsigned char direction, count;
         direction = tap >> 3;
-        count = (tap % 8) + 1;
+        count     = (tap % 8) + 1;
         if (dmp.tap_cb)
             dmp.tap_cb(direction, count);
     }
 
-    if (gesture[1] & INT_SRC_ANDROID_ORIENT) {
+    if (gesture[1] & INT_SRC_ANDROID_ORIENT)
+    {
         if (dmp.android_orient_cb)
             dmp.android_orient_cb(android_orient >> 6);
     }
@@ -1224,20 +1256,19 @@ static int decode_gesture(unsigned char *gesture)
  */
 int dmp_set_interrupt_mode(unsigned char mode)
 {
-    const unsigned char regs_continuous[11] =
-        {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
-    const unsigned char regs_gesture[11] =
-        {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};
-
-    switch (mode) {
-    case DMP_INT_CONTINUOUS:
-        return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
-            (unsigned char*)regs_continuous);
-    case DMP_INT_GESTURE:
-        return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
-            (unsigned char*)regs_gesture);
-    default:
-        return -1;
+    const unsigned char regs_continuous[11] = {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
+    const unsigned char regs_gesture[11]    = {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};
+
+    switch (mode)
+    {
+        case DMP_INT_CONTINUOUS:
+            return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
+                                 (unsigned char *)regs_continuous);
+        case DMP_INT_GESTURE:
+            return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
+                                 (unsigned char *)regs_gesture);
+        default:
+            return -1;
     }
 }
 
@@ -1262,7 +1293,7 @@ int dmp_set_interrupt_mode(unsigned char mode)
  *  @return     0 if successful.
  */
 int dmp_read_fifo(short *gyro, short *accel, long *quat,
-    unsigned long *timestamp, short *sensors, unsigned char *more)
+                  unsigned long *timestamp, short *sensors, unsigned char *more)
 {
     unsigned char fifo_data[MAX_PACKET_LENGTH];
     unsigned char ii = 0;
@@ -1277,18 +1308,15 @@ int dmp_read_fifo(short *gyro, short *accel, long *quat,
         return -1;
 
     /* Parse DMP packet. */
-    if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {
+    if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))
+    {
 #ifdef FIFO_CORRUPTION_CHECK
         long quat_q14[4], quat_mag_sq;
 #endif
-        quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
-            ((long)fifo_data[2] << 8) | fifo_data[3];
-        quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
-            ((long)fifo_data[6] << 8) | fifo_data[7];
-        quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
-            ((long)fifo_data[10] << 8) | fifo_data[11];
-        quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
-            ((long)fifo_data[14] << 8) | fifo_data[15];
+        quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) | ((long)fifo_data[2] << 8) | fifo_data[3];
+        quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) | ((long)fifo_data[6] << 8) | fifo_data[7];
+        quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) | ((long)fifo_data[10] << 8) | fifo_data[11];
+        quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) | ((long)fifo_data[14] << 8) | fifo_data[15];
         ii += 16;
 #ifdef FIFO_CORRUPTION_CHECK
         /* We can detect a corrupted FIFO by monitoring the quaternion data and
@@ -1303,10 +1331,9 @@ int dmp_read_fifo(short *gyro, short *accel, long *quat,
         quat_q14[1] = quat[1] >> 16;
         quat_q14[2] = quat[2] >> 16;
         quat_q14[3] = quat[3] >> 16;
-        quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
-            quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
-        if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
-            (quat_mag_sq > QUAT_MAG_SQ_MAX)) {
+        quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] + quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
+        if ((quat_mag_sq < QUAT_MAG_SQ_MIN) || (quat_mag_sq > QUAT_MAG_SQ_MAX))
+        {
             /* Quaternion is outside of the acceptable threshold. */
             mpu_reset_fifo();
             sensors[0] = 0;
@@ -1316,18 +1343,20 @@ int dmp_read_fifo(short *gyro, short *accel, long *quat,
 #endif
     }
 
-    if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
-        accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
-        accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
-        accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
+    if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL)
+    {
+        accel[0] = ((short)fifo_data[ii + 0] << 8) | fifo_data[ii + 1];
+        accel[1] = ((short)fifo_data[ii + 2] << 8) | fifo_data[ii + 3];
+        accel[2] = ((short)fifo_data[ii + 4] << 8) | fifo_data[ii + 5];
         ii += 6;
         sensors[0] |= INV_XYZ_ACCEL;
     }
 
-    if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
-        gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
-        gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
-        gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
+    if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO)
+    {
+        gyro[0] = ((short)fifo_data[ii + 0] << 8) | fifo_data[ii + 1];
+        gyro[1] = ((short)fifo_data[ii + 2] << 8) | fifo_data[ii + 3];
+        gyro[2] = ((short)fifo_data[ii + 4] << 8) | fifo_data[ii + 5];
         ii += 6;
         sensors[0] |= INV_XYZ_GYRO;
     }
@@ -1338,7 +1367,7 @@ int dmp_read_fifo(short *gyro, short *accel, long *quat,
     if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
         decode_gesture(fifo_data + ii);
 
-    timestamp = HAL_GetTick();
+    *timestamp = HAL_GetTick();
     return 0;
 }
 
@@ -1374,4 +1403,3 @@ int dmp_register_android_orient_cb(void (*func)(unsigned char))
 /**
  *  @}
  */
-
diff --git a/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/include/mlinclude.h b/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/include/mlinclude.h
index 2cdac0a7c..69e3caa7a 100644
--- a/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/include/mlinclude.h
+++ b/examples/projects/l0/imu/lib/Imu/motion_driver_6.12/core/driver/include/mlinclude.h
@@ -6,7 +6,7 @@
 #ifndef INV_INCLUDE_H__
 #define INV_INCLUDE_H__
 
-#define INVENSENSE_FUNC_START  typedef int invensensePutFunctionCallsHere
+#define INVENSENSE_FUNC_START // typedef int invensensePutFunctionCallsHere
 
 #ifdef COVERAGE
 #include "utestCommon.h"
@@ -35,4 +35,4 @@ extern int functionExitLog(const char *file, const char *func);
 
 // #define return if ( functionExitLog(__FILE__, __FUNCTION__) ) return
 
-#endif //INV_INCLUDE_H__
\ No newline at end of file
+#endif //INV_INCLUDE_H__
diff --git a/examples/projects/l0/imu/lib/Imu/mpu_configuration.c b/examples/projects/l0/imu/lib/Imu/mpu_configuration.c
index e53f5fda6..9164f3465 100644
--- a/examples/projects/l0/imu/lib/Imu/mpu_configuration.c
+++ b/examples/projects/l0/imu/lib/Imu/mpu_configuration.c
@@ -1,10 +1,9 @@
 #include <mpu_configuration.h>
-#include "luos_engine.h"
-#include "robus_network.h"
 #include "invensense.h"
 #include "invensense_adv.h"
 #include "mpu.h"
 #include "main.h"
+#include <eMPL_outputs.h>
 
 /* The sensors can be mounted onto the board in any orientation. The mounting
  * matrix seen below tells the MPL how to rotate the raw data from the
@@ -34,6 +33,8 @@ void read_from_mpl(service_t *service)
     unsigned long timestamp;
     float float_data[3] = {0};
     msg_t msg;
+    msg.header.target_mode = SERVICEIDACK;
+    msg.header.target      = hal.source_id;
 
     if (inv_get_sensor_type_quat(data, &accuracy, (inv_time_t *)&timestamp))
     {
@@ -43,14 +44,9 @@ void read_from_mpl(service_t *service)
          */
         if (hal.report.quat)
         {
-            msg.header.cmd         = QUATERNION;
-            msg.header.target_mode = SERVICEID;
-            msg.header.target      = hal.source_id;
-            msg.header.size        = 4 * sizeof(float);
-            float quat[4]          = {(float)data[0] / (1 << 30),
-                             (float)data[1] / (1 << 30),
-                             (float)data[2] / (1 << 30),
-                             (float)data[3] / (1 << 30)};
+            msg.header.cmd  = QUATERNION;
+            msg.header.size = 4 * sizeof(float);
+            float quat[4]   = {(float)data[0] / (1 << 30), (float)data[1] / (1 << 30), (float)data[2] / (1 << 30), (float)data[3] / (1 << 30)};
             memcpy(msg.data, quat, msg.header.size);
             Luos_SendMsg(service, &msg);
         }
@@ -60,13 +56,9 @@ void read_from_mpl(service_t *service)
     {
         if (inv_get_sensor_type_accel(data, &accuracy, (inv_time_t *)&timestamp))
         {
-            msg.header.cmd         = ACCEL_3D;
-            msg.header.target_mode = SERVICEID;
-            msg.header.target      = hal.source_id;
-            msg.header.size        = 3 * sizeof(float);
-            float accell[3]        = {(float)data[0] / (1 << 16),
-                               (float)data[1] / (1 << 16),
-                               (float)data[2] / (1 << 16)};
+            msg.header.cmd  = ACCEL_3D;
+            msg.header.size = 3 * sizeof(float);
+            float accell[3] = {(float)data[0] / (1 << 16), (float)data[1] / (1 << 16), (float)data[2] / (1 << 16)};
             memcpy(msg.data, accell, msg.header.size);
             Luos_SendMsg(service, &msg);
         }
@@ -75,13 +67,9 @@ void read_from_mpl(service_t *service)
     {
         if (inv_get_sensor_type_gyro(data, &accuracy, (inv_time_t *)&timestamp))
         {
-            msg.header.cmd         = GYRO_3D;
-            msg.header.target_mode = SERVICEID;
-            msg.header.target      = hal.source_id;
-            msg.header.size        = 3 * sizeof(float);
-            float gyro[3]          = {(float)data[0] / (1 << 16),
-                             (float)data[1] / (1 << 16),
-                             (float)data[2] / (1 << 16)};
+            msg.header.cmd  = GYRO_3D;
+            msg.header.size = 3 * sizeof(float);
+            float gyro[3]   = {(float)data[0] / (1 << 16), (float)data[1] / (1 << 16), (float)data[2] / (1 << 16)};
             memcpy(msg.data, gyro, msg.header.size);
             Luos_SendMsg(service, &msg);
         }
@@ -91,13 +79,9 @@ void read_from_mpl(service_t *service)
     {
         if (inv_get_sensor_type_compass(data, &accuracy, (inv_time_t *)&timestamp))
         {
-            msg.header.cmd         = COMPASS_3D;
-            msg.header.target_mode = SERVICEID;
-            msg.header.target      = hal.source_id;
-            msg.header.size        = 3 * sizeof(float);
-            float compass[3]       = {(float)data[0] / (1 << 16),
-                                (float)data[1] / (1 << 16),
-                                (float)data[2] / (1 << 16)};
+            msg.header.cmd   = COMPASS_3D;
+            msg.header.size  = 3 * sizeof(float);
+            float compass[3] = {(float)data[0] / (1 << 16), (float)data[1] / (1 << 16), (float)data[2] / (1 << 16)};
             memcpy(msg.data, compass, msg.header.size);
             Luos_SendMsg(service, &msg);
         }
@@ -107,13 +91,9 @@ void read_from_mpl(service_t *service)
     {
         if (inv_get_sensor_type_euler(data, &accuracy, (inv_time_t *)&timestamp))
         {
-            msg.header.cmd         = EULER_3D;
-            msg.header.target_mode = SERVICEID;
-            msg.header.target      = hal.source_id;
-            msg.header.size        = 3 * sizeof(float);
-            float euler[3]         = {(float)data[0] / (1 << 16),
-                              (float)data[1] / (1 << 16),
-                              (float)data[2] / (1 << 16)};
+            msg.header.cmd  = EULER_3D;
+            msg.header.size = 3 * sizeof(float);
+            float euler[3]  = {(float)data[0] / (1 << 16), (float)data[1] / (1 << 16), (float)data[2] / (1 << 16)};
             memcpy(msg.data, euler, msg.header.size);
             Luos_SendMsg(service, &msg);
         }
@@ -122,19 +102,9 @@ void read_from_mpl(service_t *service)
     {
         if (inv_get_sensor_type_rot_mat(data, &accuracy, (inv_time_t *)&timestamp))
         {
-            msg.header.cmd         = ROT_MAT;
-            msg.header.target_mode = SERVICEID;
-            msg.header.target      = hal.source_id;
-            msg.header.size        = 9 * sizeof(float);
-            short tmp[9]           = {(float)data[0] / (1 << 14),
-                            (float)data[1] / (1 << 14),
-                            (float)data[2] / (1 << 14),
-                            (float)data[3] / (1 << 14),
-                            (float)data[4] / (1 << 14),
-                            (float)data[5] / (1 << 14),
-                            (float)data[6] / (1 << 14),
-                            (float)data[7] / (1 << 14),
-                            (float)data[8] / (1 << 14)};
+            msg.header.cmd  = ROT_MAT;
+            msg.header.size = 9 * sizeof(float);
+            short tmp[9]    = {(float)data[0] / (1 << 14), (float)data[1] / (1 << 14), (float)data[2] / (1 << 14), (float)data[3] / (1 << 14), (float)data[4] / (1 << 14), (float)data[5] / (1 << 14), (float)data[6] / (1 << 14), (float)data[7] / (1 << 14), (float)data[8] / (1 << 14)};
             memcpy(msg.data, tmp, msg.header.size);
             Luos_SendMsg(service, &msg);
         }
@@ -143,11 +113,9 @@ void read_from_mpl(service_t *service)
     {
         if (inv_get_sensor_type_heading(data, &accuracy, (inv_time_t *)&timestamp))
         {
-            msg.header.cmd         = HEADING;
-            msg.header.target_mode = SERVICEID;
-            msg.header.target      = hal.source_id;
-            msg.header.size        = sizeof(float);
-            float heading          = (float)data[0] / (1 << 16);
+            msg.header.cmd  = HEADING;
+            msg.header.size = sizeof(float);
+            float heading   = (float)data[0] / (1 << 16);
             memcpy(msg.data, &heading, msg.header.size);
             Luos_SendMsg(service, &msg);
         }
@@ -156,10 +124,8 @@ void read_from_mpl(service_t *service)
     {
         if (inv_get_sensor_type_linear_acceleration(float_data, &accuracy, (inv_time_t *)&timestamp))
         {
-            msg.header.cmd         = LINEAR_ACCEL;
-            msg.header.target_mode = SERVICEID;
-            msg.header.target      = hal.source_id;
-            msg.header.size        = 3 * sizeof(float);
+            msg.header.cmd  = LINEAR_ACCEL;
+            msg.header.size = 3 * sizeof(float);
             memcpy(msg.data, float_data, msg.header.size);
             Luos_SendMsg(service, &msg);
         }
@@ -169,10 +135,8 @@ void read_from_mpl(service_t *service)
         if (inv_get_sensor_type_gravity(float_data, &accuracy,
                                         (inv_time_t *)&timestamp))
         {
-            msg.header.cmd         = GRAVITY_VECTOR;
-            msg.header.target_mode = SERVICEID;
-            msg.header.target      = hal.source_id;
-            msg.header.size        = 3 * sizeof(float);
+            msg.header.cmd  = GRAVITY_VECTOR;
+            msg.header.size = 3 * sizeof(float);
             memcpy(msg.data, float_data, msg.header.size);
             Luos_SendMsg(service, &msg);
         }
@@ -180,18 +144,16 @@ void read_from_mpl(service_t *service)
     if (hal.report.pedo)
     {
         unsigned long timestamp;
-        timestamp = HAL_GetTick();
+        timestamp = Luos_GetSystick();
         if (timestamp > hal.next_pedo_ms)
         {
             hal.next_pedo_ms = timestamp + PEDO_READ_MS;
             unsigned long step_count, walk_time;
             dmp_get_pedometer_step_count(&step_count);
             dmp_get_pedometer_walk_time(&walk_time);
-            unsigned long pedo[2]  = {step_count, walk_time};
-            msg.header.cmd         = PEDOMETER;
-            msg.header.target_mode = SERVICEID;
-            msg.header.target      = hal.source_id;
-            msg.header.size        = 2 * sizeof(long);
+            unsigned long pedo[2] = {step_count, walk_time};
+            msg.header.cmd        = PEDOMETER;
+            msg.header.size       = 2 * sizeof(long);
             memcpy(msg.data, pedo, msg.header.size);
             Luos_SendMsg(service, &msg);
         }
@@ -211,40 +173,6 @@ void send_status_compass()
 }
 #endif
 
-/* Handle sensor on/off combinations. */
-static void setup_gyro(void)
-{
-    unsigned char mask = 0, lp_accel_was_on = 0;
-    if (hal.sensors & ACCEL_ON)
-        mask |= INV_XYZ_ACCEL;
-    if (hal.sensors & GYRO_ON)
-    {
-        mask |= INV_XYZ_GYRO;
-        lp_accel_was_on |= hal.lp_accel_mode;
-    }
-#ifdef COMPASS_ENABLED
-    if (hal.sensors & COMPASS_ON)
-    {
-        mask |= INV_XYZ_COMPASS;
-        lp_accel_was_on |= hal.lp_accel_mode;
-    }
-#endif
-    /* If you need a power transition, this function should be called with a
-     * mask of the sensors still enabled. The driver turns off any sensors
-     * excluded from this mask.
-     */
-    mpu_set_sensors(mask);
-    mpu_configure_fifo(mask);
-    if (lp_accel_was_on)
-    {
-        unsigned short rate;
-        hal.lp_accel_mode = 0;
-        /* Switching out of LP accel, notify MPL of new accel sampling rate. */
-        mpu_get_sample_rate(&rate);
-        inv_set_accel_sample_rate(1000000L / rate);
-    }
-}
-
 static void tap_cb(unsigned char direction, unsigned char count)
 {
     return;
@@ -321,13 +249,13 @@ static void android_orient_cb(unsigned char orientation)
 
 static inline void run_self_test(void)
 {
-    int result;
     long gyro[3], accel[3];
 
 #if defined(MPU6500) || defined(MPU9250)
-    result = mpu_run_6500_self_test(gyro, accel, 0);
+
+    mpu_run_6500_self_test(gyro, accel, 0);
 #elif defined(MPU6050) || defined(MPU9150)
-    result = mpu_run_self_test(gyro, accel);
+    mpu_run_self_test(gyro, accel);
 #endif
     /* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/
 
@@ -388,8 +316,7 @@ void mpu_setup(void)
     if (result)
     {
         // Could not initialize gyro.
-        while (1)
-            ;
+        LUOS_ASSERT(0);
     }
 
     /* If you're not using an MPU9150 AND you're not using DMP features, this
@@ -401,8 +328,7 @@ void mpu_setup(void)
     if (result)
     {
         // Could not initialize MPL.
-        while (1)
-            ;
+        LUOS_ASSERT(0);
     }
 
     /* Compute 6-axis and 9-axis quaternions. */
diff --git a/examples/projects/l0/imu/lib/Imu/mpu_configuration.h b/examples/projects/l0/imu/lib/Imu/mpu_configuration.h
index 10af26034..b985c07c1 100644
--- a/examples/projects/l0/imu/lib/Imu/mpu_configuration.h
+++ b/examples/projects/l0/imu/lib/Imu/mpu_configuration.h
@@ -1,11 +1,9 @@
 #ifndef __MPU_SETUP_H__
 #define __MPU_SETUP_H__
 
-#include "luos_engine.h"
-#include "robus_network.h"
-#include <eMPL_outputs.h>
 #include <inv_mpu_dmp_motion_driver.h>
 #include <inv_mpu.h>
+#include "luos_engine.h"
 
 #define SEND_ACCEL          (0x01)
 #define SEND_GYRO           (0x02)
@@ -28,8 +26,8 @@
 /* Starting sampling rate. (default 20) */
 #define DEFAULT_MPU_HZ (200)
 
-//#define FLASH_SIZE      (512)
-//#define FLASH_MEM_START ((void*)0x1800)
+// #define FLASH_SIZE      (512)
+// #define FLASH_MEM_START ((void*)0x1800)
 
 #define PEDO_READ_MS    (1000)
 #define TEMP_READ_MS    (500)
@@ -77,7 +75,6 @@ struct hal_s
     unsigned long next_compass_ms;
     imu_report_t report;
     unsigned short source_id;
-    unsigned char update_request;
     unsigned short dmp_features;
     struct rx_s rx;
 };
diff --git a/examples/projects/l0/imu/node_config.h b/examples/projects/l0/imu/node_config.h
index 1e4692a87..5caedd0dd 100644
--- a/examples/projects/l0/imu/node_config.h
+++ b/examples/projects/l0/imu/node_config.h
@@ -48,7 +48,8 @@
  ******************************************************************************/
 #define MAX_LOCAL_SERVICE_NUMBER 1
 #define MAX_MSG_NB               10
-#define MSG_BUFFER_SIZE          1024
+#define MSG_BUFFER_SIZE          512
+#define NO_RTB
 
 /*******************************************************************************
  * LUOS HAL LIBRARY DEFINITION
diff --git a/examples/projects/l0/imu/src/gpio.c b/examples/projects/l0/imu/src/gpio.c
index b154a6f0d..e338908bd 100644
--- a/examples/projects/l0/imu/src/gpio.c
+++ b/examples/projects/l0/imu/src/gpio.c
@@ -1,21 +1,21 @@
 /**
-  ******************************************************************************
-  * File Name          : gpio.c
-  * Description        : This file provides code for the configuration
-  *                      of all used GPIO pins.
-  ******************************************************************************
-  * @attention
-  *
-  * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
-  * All rights reserved.</center></h2>
-  *
-  * This software component is licensed by ST under BSD 3-Clause license,
-  * the "License"; You may not use this file except in compliance with the
-  * License. You may obtain a copy of the License at:
-  *                        opensource.org/licenses/BSD-3-Clause
-  *
-  ******************************************************************************
-  */
+ ******************************************************************************
+ * File Name          : gpio.c
+ * Description        : This file provides code for the configuration
+ *                      of all used GPIO pins.
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
+ * All rights reserved.</center></h2>
+ *
+ * This software component is licensed by ST under BSD 3-Clause license,
+ * the "License"; You may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at:
+ *                        opensource.org/licenses/BSD-3-Clause
+ *
+ ******************************************************************************
+ */
 
 /* Includes ------------------------------------------------------------------*/
 #include "gpio.h"
@@ -38,88 +38,39 @@
 void MX_GPIO_Init(void)
 {
 
-  GPIO_InitTypeDef GPIO_InitStruct = {0};
-
-  /* GPIO Ports Clock Enable */
-  __HAL_RCC_GPIOA_CLK_ENABLE();
-  __HAL_RCC_GPIOB_CLK_ENABLE();
-
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(GPIOA, CS_Pin|LED_Pin|COM_LVL_UP_Pin, GPIO_PIN_SET);
-
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(COM_LVL_DOWN_GPIO_Port, COM_LVL_DOWN_Pin, GPIO_PIN_RESET);
-
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(GPIOB, RX_EN_Pin|TX_EN_Pin, GPIO_PIN_RESET);
-
-  /*Configure GPIO pin : PtPin */
-  GPIO_InitStruct.Pin = INVEN_INT_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  HAL_GPIO_Init(INVEN_INT_GPIO_Port, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : PAPin PAPin */
-  GPIO_InitStruct.Pin = CS_Pin|LED_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-
-  /*Configure GPIO pin : PtPin */
-  GPIO_InitStruct.Pin = POWER_SENSOR_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  HAL_GPIO_Init(POWER_SENSOR_GPIO_Port, &GPIO_InitStruct);
-
-  /*Configure GPIO pin : PtPin */
-  GPIO_InitStruct.Pin = COM_LVL_DOWN_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(COM_LVL_DOWN_GPIO_Port, &GPIO_InitStruct);
-
-  /*Configure GPIO pin : PtPin */
-  GPIO_InitStruct.Pin = COM_LVL_UP_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(COM_LVL_UP_GPIO_Port, &GPIO_InitStruct);
-
-  /*Configure GPIO pin : PtPin */
-  GPIO_InitStruct.Pin = PTPB_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  HAL_GPIO_Init(PTPB_GPIO_Port, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : PBPin PBPin */
-  GPIO_InitStruct.Pin = RX_EN_Pin|TX_EN_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-
-  /*Configure GPIO pin : PtPin */
-  GPIO_InitStruct.Pin = PTPA_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  HAL_GPIO_Init(PTPA_GPIO_Port, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : PB3 PB4 */
-  GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_4;
-  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
-  GPIO_InitStruct.Alternate = GPIO_AF0_SPI1;
-  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-
-  /* EXTI interrupt init*/
-  HAL_NVIC_SetPriority(EXTI0_1_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(EXTI0_1_IRQn);
-
-  HAL_NVIC_SetPriority(EXTI4_15_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(EXTI4_15_IRQn);
-
+    GPIO_InitTypeDef GPIO_InitStruct = {0};
+
+    /* GPIO Ports Clock Enable */
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+
+    /*Configure GPIO pin Output Level */
+    HAL_GPIO_WritePin(GPIOA, CS_Pin | LED_Pin , GPIO_PIN_SET);
+
+    /*Configure GPIO pin : PtPin */
+    GPIO_InitStruct.Pin  = INVEN_INT_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
+    GPIO_InitStruct.Pull = GPIO_PULLUP;
+    HAL_GPIO_Init(INVEN_INT_GPIO_Port, &GPIO_InitStruct);
+
+    /*Configure GPIO pins : PAPin PAPin */
+    GPIO_InitStruct.Pin   = CS_Pin | LED_Pin;
+    GPIO_InitStruct.Mode  = GPIO_MODE_OUTPUT_PP;
+    GPIO_InitStruct.Pull  = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+    /*Configure GPIO pins : PB3 PB4 */
+    GPIO_InitStruct.Pin       = GPIO_PIN_3 | GPIO_PIN_4;
+    GPIO_InitStruct.Mode      = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull      = GPIO_NOPULL;
+    GPIO_InitStruct.Speed     = GPIO_SPEED_FREQ_HIGH;
+    GPIO_InitStruct.Alternate = GPIO_AF0_SPI1;
+    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+    /* EXTI interrupt init*/
+    HAL_NVIC_SetPriority(EXTI0_1_IRQn, 1, 1);
+    HAL_NVIC_EnableIRQ(EXTI0_1_IRQn);
 }
 
 /* USER CODE BEGIN 2 */
diff --git a/examples/projects/l0/imu/src/main.c b/examples/projects/l0/imu/src/main.c
index 9272d969c..7130fd264 100644
--- a/examples/projects/l0/imu/src/main.c
+++ b/examples/projects/l0/imu/src/main.c
@@ -41,7 +41,6 @@
 /* Includes ------------------------------------------------------------------*/
 #include "main.h"
 #include "i2c.h"
-#include "usart.h"
 #include "gpio.h"
 
 /* Private includes ----------------------------------------------------------*/
@@ -111,7 +110,6 @@ int main(void)
 
     /* Initialize all configured peripherals */
     MX_GPIO_Init();
-    MX_USART1_UART_Init();
     MX_I2C2_Init();
     /* USER CODE BEGIN 2 */
     Luos_Init();
diff --git a/examples/projects/l0/imu/src/stm32f0xx_it.c b/examples/projects/l0/imu/src/stm32f0xx_it.c
index 15ab7f7c9..fbd3d2835 100644
--- a/examples/projects/l0/imu/src/stm32f0xx_it.c
+++ b/examples/projects/l0/imu/src/stm32f0xx_it.c
@@ -1,36 +1,36 @@
 /* USER CODE BEGIN Header */
 /**
-  ******************************************************************************
-  * @file    stm32f0xx_it.c
-  * @brief   Interrupt Service Routines.
-  ******************************************************************************
-  *
-  * COPYRIGHT(c) 2019 STMicroelectronics
-  *
-  * Redistribution and use in source and binary forms, with or without modification,
-  * are permitted provided that the following conditions are met:
-  *   1. Redistributions of source code must retain the above copyright notice,
-  *      this list of conditions and the following disclaimer.
-  *   2. Redistributions in binary form must reproduce the above copyright notice,
-  *      this list of conditions and the following disclaimer in the documentation
-  *      and/or other materials provided with the distribution.
-  *   3. Neither the name of STMicroelectronics nor the names of its contributors
-  *      may be used to endorse or promote products derived from this software
-  *      without specific prior written permission.
-  *
-  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-  *
-  ******************************************************************************
-  */
+ ******************************************************************************
+ * @file    stm32f0xx_it.c
+ * @brief   Interrupt Service Routines.
+ ******************************************************************************
+ *
+ * COPYRIGHT(c) 2019 STMicroelectronics
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *   1. Redistributions of source code must retain the above copyright notice,
+ *      this list of conditions and the following disclaimer.
+ *   2. Redistributions in binary form must reproduce the above copyright notice,
+ *      this list of conditions and the following disclaimer in the documentation
+ *      and/or other materials provided with the distribution.
+ *   3. Neither the name of STMicroelectronics nor the names of its contributors
+ *      may be used to endorse or promote products derived from this software
+ *      without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
 /* USER CODE END Header */
 
 /* Includes ------------------------------------------------------------------*/
@@ -80,8 +80,8 @@
 /*           Cortex-M0 Processor Interruption and Exception Handlers          */
 /******************************************************************************/
 /**
-  * @brief This function handles Non maskable interrupt.
-  */
+ * @brief This function handles Non maskable interrupt.
+ */
 void NMI_Handler(void)
 {
     /* USER CODE BEGIN NonMaskableInt_IRQn 0 */
@@ -93,8 +93,8 @@ void NMI_Handler(void)
 }
 
 /**
-  * @brief This function handles Hard fault interrupt.
-  */
+ * @brief This function handles Hard fault interrupt.
+ */
 void HardFault_Handler(void)
 {
     /* USER CODE BEGIN HardFault_IRQn 0 */
@@ -108,8 +108,8 @@ void HardFault_Handler(void)
 }
 
 /**
-  * @brief This function handles System service call via SWI instruction.
-  */
+ * @brief This function handles System service call via SWI instruction.
+ */
 void SVC_Handler(void)
 {
     /* USER CODE BEGIN SVC_IRQn 0 */
@@ -121,8 +121,8 @@ void SVC_Handler(void)
 }
 
 /**
-  * @brief This function handles Pendable request for system service.
-  */
+ * @brief This function handles Pendable request for system service.
+ */
 void PendSV_Handler(void)
 {
     /* USER CODE BEGIN PendSV_IRQn 0 */
@@ -134,8 +134,8 @@ void PendSV_Handler(void)
 }
 
 /**
-  * @brief This function handles System tick timer.
-  */
+ * @brief This function handles System tick timer.
+ */
 void SysTick_Handler(void)
 {
     /* USER CODE BEGIN SysTick_IRQn 0 */
@@ -143,7 +143,6 @@ void SysTick_Handler(void)
     /* USER CODE END SysTick_IRQn 0 */
     HAL_IncTick();
     /* USER CODE BEGIN SysTick_IRQn 1 */
-    HAL_SYSTICK_Callback();
     /* USER CODE END SysTick_IRQn 1 */
 }
 
@@ -155,8 +154,8 @@ void SysTick_Handler(void)
 /******************************************************************************/
 
 /**
-  * @brief This function handles EXTI line 0 and 1 interrupts.
-  */
+ * @brief This function handles EXTI line 0 and 1 interrupts.
+ */
 void EXTI0_1_IRQHandler(void)
 {
     /* USER CODE BEGIN EXTI0_1_IRQn 0 */
@@ -172,8 +171,8 @@ void EXTI0_1_IRQHandler(void)
 }
 
 /**
-  * @brief This function handles EXTI line 4 to 15 interrupts.
-  */
+ * @brief This function handles EXTI line 4 to 15 interrupts.
+ */
 void EXTI4_15_IRQHandler(void)
 {
     /* USER CODE BEGIN EXTI4_15_IRQn 0 */
diff --git a/examples/projects/l0/imu/src/usart.c b/examples/projects/l0/imu/src/usart.c
deleted file mode 100644
index ca0b89c20..000000000
--- a/examples/projects/l0/imu/src/usart.c
+++ /dev/null
@@ -1,80 +0,0 @@
-/**
-  ******************************************************************************
-  * File Name          : USART.c
-  * Description        : This file provides code for the configuration
-  *                      of the USART instances.
-  ******************************************************************************
-  * @attention
-  *
-  * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
-  * All rights reserved.</center></h2>
-  *
-  * This software component is licensed by ST under BSD 3-Clause license,
-  * the "License"; You may not use this file except in compliance with the
-  * License. You may obtain a copy of the License at:
-  *                        opensource.org/licenses/BSD-3-Clause
-  *
-  ******************************************************************************
-  */
-
-/* Includes ------------------------------------------------------------------*/
-#include "usart.h"
-
-/* USER CODE BEGIN 0 */
-
-/* USER CODE END 0 */
-
-/* USART1 init function */
-
-void MX_USART1_UART_Init(void)
-{
-    LL_USART_InitTypeDef USART_InitStruct = {0};
-
-    LL_GPIO_InitTypeDef GPIO_InitStruct = {0};
-    /* Peripheral clock enable */
-    LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_USART1);
-
-    LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA);
-    /**USART1 GPIO Configuration
-  PA9   ------> USART1_TX
-  PA10   ------> USART1_RX
-  */
-    GPIO_InitStruct.Pin        = LL_GPIO_PIN_9;
-    GPIO_InitStruct.Mode       = LL_GPIO_MODE_ALTERNATE;
-    GPIO_InitStruct.Speed      = LL_GPIO_SPEED_FREQ_HIGH;
-    GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
-    GPIO_InitStruct.Pull       = LL_GPIO_PULL_UP;
-    GPIO_InitStruct.Alternate  = LL_GPIO_AF_1;
-    LL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-
-    GPIO_InitStruct.Pin        = LL_GPIO_PIN_10;
-    GPIO_InitStruct.Mode       = LL_GPIO_MODE_ALTERNATE;
-    GPIO_InitStruct.Speed      = LL_GPIO_SPEED_FREQ_HIGH;
-    GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
-    GPIO_InitStruct.Pull       = LL_GPIO_PULL_UP;
-    GPIO_InitStruct.Alternate  = LL_GPIO_AF_1;
-    LL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-
-    /* USART1 interrupt Init */
-    NVIC_SetPriority(USART1_IRQn, 0);
-    NVIC_EnableIRQ(USART1_IRQn);
-
-    USART_InitStruct.BaudRate            = 57600;
-    USART_InitStruct.DataWidth           = LL_USART_DATAWIDTH_8B;
-    USART_InitStruct.StopBits            = LL_USART_STOPBITS_1;
-    USART_InitStruct.Parity              = LL_USART_PARITY_NONE;
-    USART_InitStruct.TransferDirection   = LL_USART_DIRECTION_TX_RX;
-    USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE;
-    USART_InitStruct.OverSampling        = LL_USART_OVERSAMPLING_16;
-    LL_USART_Init(USART1, &USART_InitStruct);
-    LL_USART_DisableIT_CTS(USART1);
-    LL_USART_DisableOverrunDetect(USART1);
-    LL_USART_ConfigAsyncMode(USART1);
-    LL_USART_Enable(USART1);
-}
-
-/* USER CODE BEGIN 1 */
-
-/* USER CODE END 1 */
-
-/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/