openXsensor/inv_mpu_dmp_motion_driver.c

   1 /*
   2  $License:
   3     Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
   4     See included License.txt for License information.
   5  $
   6  */
   7 /**
   8  *  @addtogroup  DRIVERS Sensor Driver Layer
   9  *  @brief       Hardware drivers to communicate with sensors via I2C.
  10  *
  11  *  @{
  12  *      @file       inv_mpu_dmp_motion_driver.c
  13  *      @brief      DMP image and interface functions.
  14  *      @details    All functions are preceded by the dmp_ prefix to
  15  *                  differentiate among MPL and general driver function calls.
  16  */
  17
  18 #define EMPL_TARGET_ATMEGA328
  19 #define MPU6050 //MPU9150
  20
  21 #include <stdio.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24 //#include <string.h>
  25 //#include <math.h>
  26 #include "inv_mpu.h"
  27 #include "inv_mpu_dmp_motion_driver.h"
  28 #include "dmpKey.h"
  29 #include "dmpmap.h"
  30
  31 #include "oXs_config_basic.h"
  32 #include "oXs_config_macros.h"
  33
  34 /* The following functions must be defined for this platform:
  35  * i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data)
  36  * i2c_read(unsigned char slave_addr, unsigned char reg_addr,  unsigned char length, unsigned char *data)
  37  * delay_ms(unsigned long num_ms)
  38  * get_ms(unsigned long *count)
  39  *    The functions are defined in arduino_shim
  40  */
  41 #include "arduino_shim.h"
  42
  43 /* These defines are copied from dmpDefaultMPU6050.c in the general MPL
  44  * releases. These defines may change for each DMP image, so be sure to modify
  45  * these values when switching to a new image.
  46  */
  47 #define CFG_LP_QUAT             (2712)
  48 #define END_ORIENT_TEMP         (1866)
  49 #define CFG_27                  (2742)
  50 #define CFG_20                  (2224)
  51 #define CFG_23                  (2745)
  52 #define CFG_FIFO_ON_EVENT       (2690)
  53 #define END_PREDICTION_UPDATE   (1761)
  54 #define CGNOTICE_INTR           (2620)
  55 #define X_GRT_Y_TMP             (1358)
  56 #define CFG_DR_INT              (1029)
  57 #define CFG_AUTH                (1035)
  58 #define UPDATE_PROP_ROT         (1835)
  59 #define END_COMPARE_Y_X_TMP2    (1455)
  60 #define SKIP_X_GRT_Y_TMP        (1359)
  61 #define SKIP_END_COMPARE        (1435)
  62 #define FCFG_3                  (1088)
  63 #define FCFG_2                  (1066)
  64 #define FCFG_1                  (1062)
  65 #define END_COMPARE_Y_X_TMP3    (1434)
  66 #define FCFG_7                  (1073)
  67 #define FCFG_6                  (1106)
  68 #define FLAT_STATE_END          (1713)
  69 #define SWING_END_4             (1616)
  70 #define SWING_END_2             (1565)
  71 #define SWING_END_3             (1587)
  72 #define SWING_END_1             (1550)
  73 #define CFG_8                   (2718)
  74 #define CFG_15                  (2727)
  75 #define CFG_16                  (2746)
  76 #define CFG_EXT_GYRO_BIAS       (1189)
  77 #define END_COMPARE_Y_X_TMP     (1407)
  78 #define DO_NOT_UPDATE_PROP_ROT  (1839)
  79 #define CFG_7                   (1205)
  80 #define FLAT_STATE_END_TEMP     (1683)
  81 #define END_COMPARE_Y_X         (1484)
  82 #define SKIP_SWING_END_1        (1551)
  83 #define SKIP_SWING_END_3        (1588)
  84 #define SKIP_SWING_END_2        (1566)
  85 #define TILTG75_START           (1672)
  86 #define CFG_6                   (2753)
  87 #define TILTL75_END             (1669)
  88 #define END_ORIENT              (1884)
  89 #define CFG_FLICK_IN            (2573)
  90 #define TILTL75_START           (1643)
  91 #define CFG_MOTION_BIAS         (1208)
  92 #define X_GRT_Y                 (1408)
  93 #define TEMPLABEL               (2324)
  94 #define CFG_ANDROID_ORIENT_INT  (1853)
  95 #define CFG_GYRO_RAW_DATA       (2722)
  96 #define X_GRT_Y_TMP2            (1379)
  97
  98 #define D_0_22                  (22+512)
  99 #define D_0_24                  (24+512)
 100
 101 #define D_0_36                  (36)
 102 #define D_0_52                  (52)
 103 #define D_0_96                  (96)
 104 #define D_0_104                 (104)
 105 #define D_0_108                 (108)
 106 #define D_0_163                 (163)
 107 #define D_0_188                 (188)
 108 #define D_0_192                 (192)
 109 #define D_0_224                 (224)
 110 #define D_0_228                 (228)
 111 #define D_0_232                 (232)
 112 #define D_0_236                 (236)
 113
 114 #define D_1_2                   (256 + 2)
 115 #define D_1_4                   (256 + 4)
 116 #define D_1_8                   (256 + 8)
 117 #define D_1_10                  (256 + 10)
 118 #define D_1_24                  (256 + 24)
 119 #define D_1_28                  (256 + 28)
 120 #define D_1_36                  (256 + 36)
 121 #define D_1_40                  (256 + 40)
 122 #define D_1_44                  (256 + 44)
 123 #define D_1_72                  (256 + 72)
 124 #define D_1_74                  (256 + 74)
 125 #define D_1_79                  (256 + 79)
 126 #define D_1_88                  (256 + 88)
 127 #define D_1_90                  (256 + 90)
 128 #define D_1_92                  (256 + 92)
 129 #define D_1_96                  (256 + 96)
 130 #define D_1_98                  (256 + 98)
 131 #define D_1_106                 (256 + 106)
 132 #define D_1_108                 (256 + 108)
 133 #define D_1_112                 (256 + 112)
 134 #define D_1_128                 (256 + 144)
 135 #define D_1_152                 (256 + 12)
 136 #define D_1_160                 (256 + 160)
 137 #define D_1_176                 (256 + 176)
 138 #define D_1_178                 (256 + 178)
 139 #define D_1_218                 (256 + 218)
 140 #define D_1_232                 (256 + 232)
 141 #define D_1_236                 (256 + 236)
 142 #define D_1_240                 (256 + 240)
 143 #define D_1_244                 (256 + 244)
 144 #define D_1_250                 (256 + 250)
 145 #define D_1_252                 (256 + 252)
 146 #define D_2_12                  (512 + 12)
 147 #define D_2_96                  (512 + 96)
 148 #define D_2_108                 (512 + 108)
 149 #define D_2_208                 (512 + 208)
 150 #define D_2_224                 (512 + 224)
 151 #define D_2_236                 (512 + 236)
 152 #define D_2_244                 (512 + 244)
 153 #define D_2_248                 (512 + 248)
 154 #define D_2_252                 (512 + 252)
 155
 156 #define CPASS_BIAS_X            (35 * 16 + 4)
 157 #define CPASS_BIAS_Y            (35 * 16 + 8)
 158 #define CPASS_BIAS_Z            (35 * 16 + 12)
 159 #define CPASS_MTX_00            (36 * 16)
 160 #define CPASS_MTX_01            (36 * 16 + 4)
 161 #define CPASS_MTX_02            (36 * 16 + 8)
 162 #define CPASS_MTX_10            (36 * 16 + 12)
 163 #define CPASS_MTX_11            (37 * 16)
 164 #define CPASS_MTX_12            (37 * 16 + 4)
 165 #define CPASS_MTX_20            (37 * 16 + 8)
 166 #define CPASS_MTX_21            (37 * 16 + 12)
 167 #define CPASS_MTX_22            (43 * 16 + 12)
 168 #define D_EXT_GYRO_BIAS_X       (61 * 16)
 169 #define D_EXT_GYRO_BIAS_Y       (61 * 16) + 4
 170 #define D_EXT_GYRO_BIAS_Z       (61 * 16) + 8
 171 #define D_ACT0                  (40 * 16)
 172 #define D_ACSX                  (40 * 16 + 4)
 173 #define D_ACSY                  (40 * 16 + 8)
 174 #define D_ACSZ                  (40 * 16 + 12)
 175
 176 #define FLICK_MSG               (45 * 16 + 4)
 177 #define FLICK_COUNTER           (45 * 16 + 8)
 178 #define FLICK_LOWER             (45 * 16 + 12)
 179 #define FLICK_UPPER             (46 * 16 + 12)
 180
 181 #define D_AUTH_OUT              (992)
 182 #define D_AUTH_IN               (996)
 183 #define D_AUTH_A                (1000)
 184 #define D_AUTH_B                (1004)
 185
 186 #define D_PEDSTD_BP_B           (768 + 0x1C)
 187 #define D_PEDSTD_HP_A           (768 + 0x78)
 188 #define D_PEDSTD_HP_B           (768 + 0x7C)
 189 #define D_PEDSTD_BP_A4          (768 + 0x40)
 190 #define D_PEDSTD_BP_A3          (768 + 0x44)
 191 #define D_PEDSTD_BP_A2          (768 + 0x48)
 192 #define D_PEDSTD_BP_A1          (768 + 0x4C)
 193 #define D_PEDSTD_INT_THRSH      (768 + 0x68)
 194 #define D_PEDSTD_CLIP           (768 + 0x6C)
 195 #define D_PEDSTD_SB             (768 + 0x28)
 196 #define D_PEDSTD_SB_TIME        (768 + 0x2C)
 197 #define D_PEDSTD_PEAKTHRSH      (768 + 0x98)
 198 #define D_PEDSTD_TIML           (768 + 0x2A)
 199 #define D_PEDSTD_TIMH           (768 + 0x2E)
 200 #define D_PEDSTD_PEAK           (768 + 0X94)
 201 #define D_PEDSTD_STEPCTR        (768 + 0x60)
 202 #define D_PEDSTD_TIMECTR        (964)
 203 #define D_PEDSTD_DECI           (768 + 0xA0)
 204
 205 #define D_HOST_NO_MOT           (976)
 206 #define D_ACCEL_BIAS            (660)
 207
 208 #define D_ORIENT_GAP            (76)
 209
 210 #define D_TILT0_H               (48)
 211 #define D_TILT0_L               (50)
 212 #define D_TILT1_H               (52)
 213 #define D_TILT1_L               (54)
 214 #define D_TILT2_H               (56)
 215 #define D_TILT2_L               (58)
 216 #define D_TILT3_H               (60)
 217 #define D_TILT3_L               (62)
 218
 219 #define DMP_ACC_OFF_X ( (long long) ( ACC_OFFSET_X /2 ) )
 220 #define DMP_ACC_OFF_X0 ((unsigned char)(( ((long long) DMP_ACC_OFF_X) >> 24) & 0xFF) )
 221 #define DMP_ACC_OFF_X1 ((unsigned char)(( ((long long) DMP_ACC_OFF_X) >> 16) & 0xFF) )
 222 #define DMP_ACC_OFF_X2 ((unsigned char)(( ((long long) DMP_ACC_OFF_X) >>  8) & 0xFF) )
 223 #define DMP_ACC_OFF_X3 ((unsigned char)(( ((long long) DMP_ACC_OFF_X)      ) & 0xFF) )
 224 #define DMP_ACC_OFF_Y ( (long long) ( ACC_OFFSET_Y /2 ) )
 225 #define DMP_ACC_OFF_Y0 ((unsigned char)(( ((long long) DMP_ACC_OFF_Y) >> 24) & 0xFF) )
 226 #define DMP_ACC_OFF_Y1 ((unsigned char)(( ((long long) DMP_ACC_OFF_Y) >> 16) & 0xFF) )
 227 #define DMP_ACC_OFF_Y2 ((unsigned char)(( ((long long) DMP_ACC_OFF_Y) >>  8) & 0xFF) )
 228 #define DMP_ACC_OFF_Y3 ((unsigned char)(( ((long long) DMP_ACC_OFF_Y)      ) & 0xFF) )
 229 #define DMP_ACC_OFF_Z ( (long long) ( ACC_OFFSET_Z /2 ) )
 230 #define DMP_ACC_OFF_Z0 ((unsigned char)(( ((long long) DMP_ACC_OFF_Z) >> 24) & 0xFF) )
 231 #define DMP_ACC_OFF_Z1 ((unsigned char)(( ((long long) DMP_ACC_OFF_Z) >> 16) & 0xFF) )
 232 #define DMP_ACC_OFF_Z2 ((unsigned char)(( ((long long) DMP_ACC_OFF_Z) >>  8) & 0xFF) )
 233 #define DMP_ACC_OFF_Z3 ((unsigned char)(( ((long long) DMP_ACC_OFF_Z)      ) & 0xFF) )
 234
 235
 236
 237 #define DMP_CODE_SIZE           (3062)
 238 typedef unsigned char PROGMEM prog_uchar;  // added by Mstrens
 239
 240 const prog_uchar dmp_memory[DMP_CODE_SIZE] PROGMEM = {
 241     /* bank # 0 */
 242     0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, //0
 243     0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, //1
 244     0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, //2
 245     0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, //3
 246     0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, //4
 247     0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2, //5
 248     0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, //6 byte 8,9,10,11 set the dmp sample rate (default 200)
 249     0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, //7
 250     0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, //8
 251     0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, //9
 252     0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, //10
 253     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, //11
 254     0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, //12
 255     0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, //13
 256     0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00, //14
 257     0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, //15
 258     /* bank # 1 */
 259     0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f,
 260     0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00,
 261     0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00,
 262     0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 263     0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00,
 264     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00,
 265     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00,
 266     0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00,
 267     0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 268     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 269     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
 270     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00,
 271     0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83,
 272     0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28,
 273     0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
 274     0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00,
 275     /* bank # 2 */
 276     0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00,
 277     0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x03, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, // 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, initialy; changed byte 6 in 5 (instead of 3) in order to get 50hz in fifo (= 200 hz sample rate / 50 -1)
 278     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
 279     0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0,
 280     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 281     0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 282     0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 283     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 284     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 285     0x00, 0x00, 0x00, 0x00,
 286 #if defined (ACC_OFFSET_X ) && defined(ACC_OFFSET_Y ) && defined (ACC_OFFSET_Z )                                                        // This is the acceleration X, Y ,Z used by DMP to integrate into the quaternion
 287     DMP_ACC_OFF_X0 ,  DMP_ACC_OFF_X1 ,  DMP_ACC_OFF_X2 ,  DMP_ACC_OFF_X3 ,
 288     DMP_ACC_OFF_Y0 ,  DMP_ACC_OFF_Y1 ,  DMP_ACC_OFF_Y2 ,  DMP_ACC_OFF_Y3 ,
 289     DMP_ACC_OFF_Z0 ,  DMP_ACC_OFF_Z1 ,  DMP_ACC_OFF_Z2 ,  DMP_ACC_OFF_Z3 ,
 290
 291
 292 #else
 293                             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,// Offsest is O if not defined in config.h
 294 #endif
 295     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 296     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 297     0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e,
 298     0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c,
 299     0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,
 300     0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 301     /* bank # 3 */
 302     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 303     0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3,
 304     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c,
 305     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 306     0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82,
 307     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 308     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,
 309     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19,
 310     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 311     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00,
 312     0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 313     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 314     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 315     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 316     0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88,
 317     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 318
 319     /* bank # 4 */
 320     0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, // 0
 321     0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, // 1
 322     0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0x4C, 0xcd, 0x6C, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9, // 2  // in reg 1062 = bank 4, row 2, byte 6 , I replace (0xc5, 0xcd, 0xc7,) with ( 4C CD 6C)  for orientation
 323     0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96, // 3  // in reg 1073 = bank 4, row 3, byte 1 , , the orientation sign accel is already ok (for positive sign)
 324     0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, // 4  // in reg 1088 = bank 4, row 4, byte 0 , , the orientation sign gyro is already ok (for positive sign)
 325     0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, // 5
 326     0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, // 6
 327     0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, // 7
 328     0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, // 8
 329     0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8, // 0
 330     0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, //10
 331     0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, //11
 332     0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, //12
 333     0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2, //13
 334     0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, //14
 335     0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78, //15
 336     /* bank # 5 */
 337     0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba,
 338     0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8,
 339     0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88,
 340     0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7,
 341     0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1,
 342     0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1,
 343     0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29,
 344     0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a,
 345     0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7,
 346     0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e,
 347     0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c,
 348     0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8,
 349     0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88,
 350     0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94,
 351     0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2,
 352     0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82,
 353     /* bank # 6 */
 354     0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1,
 355     0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98,
 356     0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88,
 357     0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02,
 358     0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf,
 359     0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1,
 360     0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39,
 361     0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9,
 362     0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9,
 363     0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde,
 364     0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0,
 365     0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8,
 366     0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2,
 367     0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3,
 368     0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb,
 369     0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8,
 370     /* bank # 7 */
 371     0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb,
 372     0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28,
 373     0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2,
 374     0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a,
 375     0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb,
 376     0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6,
 377     0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8,
 378     0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1,
 379     0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99,
 380     0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb,
 381     0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9,
 382     0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9,
 383     0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac,
 384     0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6,
 385     0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0,
 386     0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf,
 387     /* bank # 8 */
 388     0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30,
 389     0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9,
 390     0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0,
 391     0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1,
 392     0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6,
 393     0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8,
 394     0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda,
 395     0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde,
 396     0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde,
 397     0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88,
 398     0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf,
 399     0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8,
 400     0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5,
 401     0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8,
 402     0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7,
 403     0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8,
 404     /* bank # 9 */
 405     0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0,
 406     0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b,
 407     0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab,
 408     0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0,
 409     0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0,
 410     0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59,
 411     0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31,
 412     0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28,
 413     0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0,
 414     0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9,
 415     0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c,
 416     0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e,
 417     0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0,
 418     0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf,
 419     0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2,
 420     0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83,
 421 // bank # 10      2     3     4     5     6     7     8     9     10    11    12    13    14    15
 422     0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6,  // 0
 423     0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad,  // 1
 424     0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb,  // 2
 425     0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,  // 3
 426     0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,  // 4
 427     0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,  // 5
 428     0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,  // 6
 429     0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84,  // 7
 430     0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0,  // 8
 431     0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0x20, 0x28,  // 9  // reg 2712  = bank 10, line 9 , byte 8 ; contains 8B = LP_QUAT not active (we use 6QUAT)
 432                                                                                                            //  reg 2718 = bank 10 , row 9 , byte 14 : we must replace 4*A3 by 20 28 30 38 in order to activate lp_6_Quat
 433                                                                                                           // reg 2722 = bank 10, line 10 , byte 2 ; not modified
 434     0x30, 0x38, 0xB2, 0x8B, 0xB6, 0x9B, 0xf2, 0xa3, 0xC0, 0xC8, 0xC2, 0xA3, 0xA3, 0xA3, 0xa3, 0xa3,  //10 //  reg 2727 : bank 10 , row 10, byte 7 (10 bytes) used to defined if accel and gyro have to be put in fifo; MS put C0 C8 C2 instead of 6*A3
 435     0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xD8, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9,  //11 // reg 2742 : bank 10 , row 11 , byte 6 (1 byte) D8 put instead of A3
 436     0xa3, 0xFE, 0xF2, 0xAB, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, 0xBB, 0xAF, 0xDF, 0xDF, 0xa3, 0xa3, 0xa3,  //12 // line was initially 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, ; it is filled by dmp_set_fifo_rate
 437     0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28,  //13
 438     0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90,  //14
 439     0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,  //15
 440     /* bank # 11 */
 441     0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83,
 442     0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19,
 443     0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39,
 444     0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7,
 445     0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80,
 446     0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9,
 447     0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26,
 448     0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89,
 449     0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8,
 450     0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8,
 451     0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22,
 452     0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2,
 453     0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00,
 454     0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10,
 455     0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88,
 456     0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
 457 };
 458
 459 static const unsigned short sStartAddress = 0x0400;
 460
 461 /* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */
 462
 463 #define INT_SRC_TAP             (0x01)
 464 #define INT_SRC_ANDROID_ORIENT  (0x08)
 465
 466 #define DMP_FEATURE_SEND_ANY_GYRO   (DMP_FEATURE_SEND_RAW_GYRO | \
 467                                      DMP_FEATURE_SEND_CAL_GYRO)
 468
 469 #define MAX_PACKET_LENGTH   (32)
 470
 471 #define DMP_SAMPLE_RATE     (200)
 472 #define GYRO_SF             (46850825LL * 200 / DMP_SAMPLE_RATE)                // for 200hz = 0x02CAE309
 473
 474 //#define FIFO_CORRUPTION_CHECK  // uncomment if a check on the quaternion has to be performed - this allows to detect some I2c errors (it requires about 200 bytes)
 475 #ifdef FIFO_CORRUPTION_CHECK
 476 #define QUAT_ERROR_THRESH       (1L<<24)
 477 #define QUAT_MAG_SQ_NORMALIZED  (1L<<28)
 478 #define QUAT_MAG_SQ_MIN         (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)
 479 #define QUAT_MAG_SQ_MAX         (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH)
 480 #endif
 481
 482 struct dmp_s {
 483 //    void (*tap_cb)(unsigned char count, unsigned char direction); // function to call for tap
 484 //    void (*android_orient_cb)(unsigned char orientation);         // function to call for android_orientation
 485 //    unsigned short orient;                                        // ????
 486 //    unsigned short feature_mask;                                  // bit mask to hold the list of features being activated
 487 //    unsigned short fifo_rate;                                     // define how often fifo data is filled by 6050 (should be 50 hz)
 488     unsigned char packet_length;                                  // length of a packet in FIFO
 489 };
 490
 491 static struct dmp_s dmp = {
 492 //    .tap_cb = NULL,
 493 //    .android_orient_cb = NULL,
 494 //    .orient = 0,
 495 //    .feature_mask = 0,
 496 //    .fifo_rate = 0,
 497     .packet_length = 0
 498 };
 499
 500 /**
 501  *  @brief  Load the DMP with this image.
 502  *  @return 0 if successful.
 503  */
 504 int dmp_load_motion_driver_firmware(void)
 505 {
 506     return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress);
 507 }
 508
 509 /**                                                                      // firmware code has been changed in order to contains the code for orientation 1 1 1, so this function can be avoided
 510  *  @brief      Push gyro and accel orientation to the DMP.
 511  *  The orientation is represented here as the output of
 512  *  @e inv_orientation_matrix_to_scalar.
 513  *  @param[in]  orient  Gyro and accel orientation in body frame.
 514  *  @return     0 if successful.
 515  */
 516 /*
 517 int dmp_set_orientation(unsigned short orient)                          // when called by oXs, orient contains 0X88 = 0b0 1000 1000 = 010 001 000
 518 {
 519     unsigned char gyro_regs[3], accel_regs[3];
 520     const unsigned char gyro_axes[3]  = {DINA4C, DINACD, DINA6C};  // 4C CD 6C
 521     const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C};  // 0C C9 2C
 522     const unsigned char gyro_sign[3]  = {DINA36, DINA56, DINA76};  // 36 56 76
 523     const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66};  // 26 46 66
 524
 525     gyro_regs[0] = gyro_axes[orient & 3];                          // with orient = 0x88 -> 4C
 526     gyro_regs[1] = gyro_axes[(orient >> 3) & 3];                   // with orient = 0x88 -> CD
 527     gyro_regs[2] = gyro_axes[(orient >> 6) & 3];                   // with orient = 0x88 -> 6C
 528     accel_regs[0] = accel_axes[orient & 3];                        // with orient = 0x88 -> 0C
 529     accel_regs[1] = accel_axes[(orient >> 3) & 3];                 // with orient = 0x88 -> C9
 530     accel_regs[2] = accel_axes[(orient >> 6) & 3];                 // with orient = 0x88 -> 2C
 531
 532     // Chip-to-body, axes only.
 533     if (mpu_write_mem(FCFG_1, 3, gyro_regs))                         // reg 1062 => bank 4, row 2, byte 6
 534         return -1;
 535     if (mpu_write_mem(FCFG_2, 3, accel_regs))                        // firmware is already loaded with 0C C9 2C = values for accel_reg for orient = 0x88
 536         return -1;
 537
 538     memcpy(gyro_regs, gyro_sign, 3);                                 // preload with normal values and add 1 if sign is inverted
 539     memcpy(accel_regs, accel_sign, 3);
 540     if (orient & 4) {
 541         gyro_regs[0] |= 1;
 542         accel_regs[0] |= 1;
 543     }
 544     if (orient & 0x20) {
 545         gyro_regs[1] |= 1;
 546         accel_regs[1] |= 1;
 547     }
 548     if (orient & 0x100) {
 549         gyro_regs[2] |= 1;
 550         accel_regs[2] |= 1;
 551     }
 552
 553     // Chip-to-body, sign only.
 554     if (mpu_write_mem(FCFG_3, 3, gyro_regs))
 555         return -1;
 556     if (mpu_write_mem(FCFG_7, 3, accel_regs))
 557         return -1;
 558 //    dmp.orient = orient;
 559     return 0;
 560 }
 561 */
 562
 563 /**
 564  *  @brief      Push gyro biases to the DMP.
 565  *  Because the gyro integration is handled in the DMP, any gyro biases
 566  *  calculated by the MPL should be pushed down to DMP memory to remove
 567  *  3-axis quaternion drift.
 568  *  \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will
 569  *  overwrite the biases written to this location once a new one is computed.
 570  *  @param[in]  bias    Gyro biases in q16.
 571  *  @return     0 if successful.
 572  */
 573  /*
 574 int dmp_set_gyro_bias(long *bias)
 575 {
 576     long gyro_bias_body[3];
 577     unsigned char regs[4];
 578
 579     gyro_bias_body[0] = bias[dmp.orient & 3];
 580     if (dmp.orient & 4)
 581         gyro_bias_body[0] *= -1;
 582     gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3];
 583     if (dmp.orient & 0x20)
 584         gyro_bias_body[1] *= -1;
 585     gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3];
 586     if (dmp.orient & 0x100)
 587         gyro_bias_body[2] *= -1;
 588
 589 #ifdef EMPL_NO_64BIT
 590     gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f);
 591     gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f);
 592     gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f);
 593 #else
 594     gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30);
 595     gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30);
 596     gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30);
 597 #endif
 598
 599     regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF);
 600     regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF);
 601     regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF);
 602     regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF);
 603     if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs))
 604         return -1;
 605
 606     regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF);
 607     regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF);
 608     regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF);
 609     regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF);
 610     if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs))
 611         return -1;
 612
 613     regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF);
 614     regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF);
 615     regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF);
 616     regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF);
 617     return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs);
 618 }
 619 */
 620
 621 /**
 622  *  @brief      Push accel biases to the DMP.
 623  *  These biases will be removed from the DMP 6-axis quaternion.
 624  *  @param[in]  bias    Accel biases in q16.
 625  *  @return     0 if successful.
 626  */
 627  /*
 628 int dmp_set_accel_bias(long *bias)
 629 {
 630     long accel_bias_body[3];
 631     unsigned char regs[12];
 632     long long accel_sf;
 633     unsigned short accel_sens;
 634
 635     mpu_get_accel_sens(&accel_sens);
 636     accel_sf = (long long)accel_sens << 15;
 637     __no_operation();
 638
 639     accel_bias_body[0] = bias[dmp.orient & 3];
 640     if (dmp.orient & 4)
 641         accel_bias_body[0] *= -1;
 642     accel_bias_body[1] = bias[(dmp.orient >> 3) & 3];
 643     if (dmp.orient & 0x20)
 644         accel_bias_body[1] *= -1;
 645     accel_bias_body[2] = bias[(dmp.orient >> 6) & 3];
 646     if (dmp.orient & 0x100)
 647         accel_bias_body[2] *= -1;
 648
 649 #ifdef EMPL_NO_64BIT
 650     accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f);
 651     accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f);
 652     accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f);
 653 #else
 654     accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30);
 655     accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30);
 656     accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30);
 657 #endif
 658
 659     regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF);
 660     regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF);
 661     regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF);
 662     regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF);
 663     regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF);
 664     regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF);
 665     regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);
 666     regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF);
 667     regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF);
 668     regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF);
 669     regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF);
 670     regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF);
 671     return mpu_write_mem(D_ACCEL_BIAS, 12, regs);
 672 }
 673 */
 674
 675 /**
 676  *  @brief      Set DMP output rate.
 677  *  Only used when DMP is on.
 678  *  @param[in]  rate    Desired fifo rate (Hz).
 679  *  @return     0 if successful.
 680  */
 681  /*
 682 int dmp_set_fifo_rate(unsigned short rate)
 683 {
 684 //    const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB, 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF};
 685 //    unsigned short div;
 686 //    unsigned char tmp[2];
 687
 688 //    if (rate > DMP_SAMPLE_RATE)                                // can be ommitted if we assume that fifo rate will be less or equal to DMP_SAMPLE_RATE
 689 //        return -1;
 690 //    div = DMP_SAMPLE_RATE / rate - 1;
 691 //    tmp[0] = (unsigned char)((div >> 8) & 0xFF);
 692 //    tmp[1] = (unsigned char)(div & 0xFF);
 693 //    if (mpu_write_mem(D_0_22, 2, tmp))                          // can be removed because the code has been changed from 0x05 to 0x03 in the memory (bank 2, row 1, byte 6)
 694 //        return -1;
 695 //    if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end))   // removed because the code has been changed directly in the memory (at bank 10, row 12, byte 1)
 696 //        return -1;
 697
 698     dmp.fifo_rate = rate;
 699     return 0;
 700 }
 701 */
 702
 703 /**                                                               // is not used by oXs (fifo rate is fixed to 50 hz
 704  *  @brief      Get DMP output rate.
 705  *  @param[out] rate    Current fifo rate (Hz).
 706  *  @return     0 if successful.
 707  */
 708 /*
 709 int dmp_get_fifo_rate(unsigned short *rate)
 710 {
 711     rate[0] = dmp.fifo_rate;
 712     return 0;
 713 }
 714 */
 715
 716 /**
 717  *  @brief      Enable DMP features.
 718  *  The following \#define's are used in the input mask:
 719  *  \n DMP_FEATURE_TAP
 720  *  \n DMP_FEATURE_ANDROID_ORIENT
 721  *  \n DMP_FEATURE_LP_QUAT
 722  *  \n DMP_FEATURE_6X_LP_QUAT
 723  *  \n DMP_FEATURE_GYRO_CAL
 724  *  \n DMP_FEATURE_SEND_RAW_ACCEL
 725  *  \n DMP_FEATURE_SEND_RAW_GYRO
 726  *  \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually
 727  *  exclusive.
 728  *  \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also
 729  *  mutually exclusive.
 730  *  @param[in]  mask    Mask of features to enable.
 731  *  @return     0 if successful.
 732  */
 733 int dmp_enable_feature(unsigned short mask)
 734 {
 735 //    unsigned char tmp[10];
 736
 737     /* TODO: All of these settings can probably be integrated into the default
 738      * DMP image.
 739      */
 740     /* Set integration scale factor. */                              // adress 104 depends on DMP sample rate = 200hz by default ; can be ommitted because firmware contains already those values
 741 //    tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF);
 742 //    tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF);
 743 //    tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF);
 744 //    tmp[3] = (unsigned char)(GYRO_SF & 0xFF);
 745 //    mpu_write_mem(D_0_104, 4, tmp);                    //  reg 104 = bank 0 , row 6 , byte 8
 746
 747     /* Send sensor data to the FIFO. */                             // adress 2727 has 7een modified to contain A3 + C0 C8 C2 C4 CC C6 (activate send gyro also otherwise reaction is very slow), other contain A3; so this code can be ommitted
 748 //    tmp[0] = 0xA3;
 749 //    if (mask & DMP_FEATURE_SEND_RAW_ACCEL) {
 750 //        tmp[1] = 0xC0;
 751 //        tmp[2] = 0xC8;
 752 //        tmp[3] = 0xC2;
 753 //    } else {
 754 //        tmp[1] = 0xA3;
 755 //        tmp[2] = 0xA3;
 756 //        tmp[3] = 0xA3;
 757 //    }
 758 //    if (mask & DMP_FEATURE_SEND_ANY_GYRO) {              // this is normally not used by oXs but it has to be activated otherwise it does not work
 759 //        tmp[4] = 0xC4;
 760 //        tmp[5] = 0xCC;
 761 //        tmp[6] = 0xC6;
 762 //    } else {
 763 //        tmp[4] = 0xA3;
 764 //        tmp[5] = 0xA3;
 765 //        tmp[6] = 0xA3;
 766 //    }
 767 //    tmp[7] = 0xA3;
 768 //    tmp[8] = 0xA3;
 769 //    tmp[9] = 0xA3;
 770 //    mpu_write_mem(CFG_15,10,tmp);                      // reg 2727 = bank 10 , row 10, byte 7
 771
 772     /* Send gesture data to the FIFO. */                             // not used by oXs
 773  //   if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
 774  //       tmp[0] = DINA20;
 775  //   else
 776  //       tmp[0] = 0xD8;
 777  //   mpu_write_mem(CFG_27,1,tmp);                                // reg 2742 = bank 10, row 11 , byte 6 ; can be removed because value D8 has been put in the firmware
 778
 779 //    if (mask & DMP_FEATURE_GYRO_CAL)              // saving : firmware contains already the code that enable this option, so this could be avoided
 780 //        dmp_enable_gyro_cal(1);
 781 //    else
 782 //        dmp_enable_gyro_cal(0);
 783
 784 //    if (mask & DMP_FEATURE_SEND_ANY_GYRO) {                        // in oXs we do not ask for gyro data, so this can be skipped
 785 //        if (mask & DMP_FEATURE_SEND_CAL_GYRO) {                    // those code are already in the code being preloaded
 786 //            tmp[0] = 0xB2;
 787 //            tmp[1] = 0x8B;
 788 //            tmp[2] = 0xB6;
 789 //            tmp[3] = 0x9B;
 790 //        } else {
 791 //            tmp[0] = DINAC0;
 792 //            tmp[1] = DINA80;
 793 //            tmp[2] = DINAC2;
 794 //            tmp[3] = DINA90;
 795 //        }
 796 //        mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp); // 2722 = bank 10, line 10 , byte 2
 797 //    }
 798 /*
 799     if (mask & DMP_FEATURE_TAP) {
 800         // Enable tap. //
 801         tmp[0] = 0xF8;
 802         mpu_write_mem(CFG_20, 1, tmp);
 803         dmp_set_tap_thresh(TAP_XYZ, 250);
 804         dmp_set_tap_axes(TAP_XYZ);
 805         dmp_set_tap_count(1);
 806         dmp_set_tap_time(100);
 807         dmp_set_tap_time_multi(500);
 808
 809         dmp_set_shake_reject_thresh(GYRO_SF, 200);
 810         dmp_set_shake_reject_time(40);
 811         dmp_set_shake_reject_timeout(10);
 812     } else {
 813         tmp[0] = 0xD8;
 814         mpu_write_mem(CFG_20, 1, tmp);
 815     }
 816 */
 817 //          tmp[0] = 0xD8;                   // code to disable DMP_FEATURE_TAP, Code can be avoided because already present in firmware
 818 //          mpu_write_mem(CFG_20, 1, tmp);
 819
 820 /*  // code to enable/disable DMP_FEATURE_ANDROID_ORIENT (replaced by always disable)
 821     if (mask & DMP_FEATURE_ANDROID_ORIENT) {
 822         tmp[0] = 0xD9;
 823     } else
 824         tmp[0] = 0xD8;
 825     mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);
 826 */
 827 //    tmp[0] = 0xD8;                                // disable DMP_FEATURE_ANDROID_ORIENT, Code can be avoided because already present in firmware
 828 //    mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);
 829
 830 //    if (mask & DMP_FEATURE_LP_QUAT)                               // saving : this option is not used and is already disabled in firmware
 831 //        dmp_enable_lp_quat(1);
 832 //    else
 833 //        dmp_enable_lp_quat(0);
 834
 835 //    if (mask & DMP_FEATURE_6X_LP_QUAT)             // firmware has been modified in order to activate this option (in reg 2718 bank 10, row 9 , byte 14 we now put 20 28 30 38
 836 //        dmp_enable_6x_lp_quat(1);
 837 //    else
 838 //        dmp_enable_6x_lp_quat(0);
 839
 840     /* Pedometer is always enabled. */                 //saving : activated feature are hardcoded in oXs ( send RawAccel, send 6LT_QUAT, automatic CalGyro)
 841 //    dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER;
 842 //    mpu_reset_fifo(); // better not to do it because the new version set already the bit DMP_EN; and normally FIFO is not yet activated.
 843
 844     dmp.packet_length = 22 ;                                         // fix the value because we use only  DMP_FEATURE_SEND_RAW_ACCEL +  DMP_FEATURE_6X_LP_QUAT
 845 //    dmp.packet_length = 0;                                           // saving : if oXs use always the same config, we could save some flash memory here and fix it directly to 28.
 846 //    if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
 847 //        dmp.packet_length += 6;
 848 //    if (mask & DMP_FEATURE_SEND_ANY_GYRO)
 849 //        dmp.packet_length += 6;
 850 //    if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))
 851 //        dmp.packet_length += 16;
 852 //    if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))      // this option is nver used by oXs
 853 //        dmp.packet_length += 4;
 854
 855     return 0;
 856 }
 857
 858 /**
 859  *  @brief      Get list of currently enabled DMP features.       // this function is not used by oXs
 860  *  @param[out] Mask of enabled features.
 861  *  @return     0 if successful.
 862  */
 863  /*
 864 int dmp_get_enabled_features(unsigned short *mask)
 865 {
 866     mask[0] = dmp.feature_mask;
 867     return 0;
 868 }
 869 */
 870
 871 /**
 872  *  @brief      Calibrate the gyro data in the DMP.                               // saving : this function could be reduced using only one of the 2 options
 873  *  After eight seconds of no motion, the DMP will compute gyro biases and
 874  *  subtract them from the quaternion output. If @e dmp_enable_feature is
 875  *  called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be
 876  *  subtracted from the gyro output.
 877  *  @param[in]  enable  1 to enable gyro calibration.
 878  *  @return     0 if successful.
 879  */
 880 /*
 881 int dmp_enable_gyro_cal(unsigned char enable)                                     // saving : could be avoided if firmware contains already the selected option
 882 {
 883     if (enable) {
 884         unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d};    // this is the option coded in the firmware
 885         return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
 886     } else {
 887         unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7};
 888         return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
 889     }
 890 }
 891 */
 892
 893 /**
 894  *  @brief      Generate 3-axis quaternions from the DMP.
 895  *  In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
 896  *  exclusive.
 897  *  @param[in]  enable  1 to enable 3-axis quaternion.
 898  *  @return     0 if successful.
 899  */
 900  /*
 901 int dmp_enable_lp_quat(unsigned char enable)
 902 {
 903     unsigned char regs[4];
 904     if (enable) {
 905         regs[0] = DINBC0;
 906         regs[1] = DINBC2;
 907         regs[2] = DINBC4;
 908         regs[3] = DINBC6;
 909     }
 910     else
 911         memset(regs, 0x8B, 4);                                                     // saving : strange that firmware is currently filled with A3
 912
 913     mpu_write_mem(CFG_LP_QUAT, 4, regs);                 // reg 2712 = bank 10 , row 9 , byte 8 // in oXs we do not use this lpquat, so we need 4*8B; it is so in firmware so this is disabled
 914
 915     return mpu_reset_fifo();
 916 }
 917 */
 918
 919 /**
 920  *  @brief       Generate 6-axis quaternions from the DMP.                          //saving : code are mutually exclusive, so keeping only what is used would save flash
 921  *  In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
 922  *  exclusive.
 923  *  @param[in]   enable  1 to enable 6-axis quaternion.
 924  *  @return      0 if successful.
 925  */
 926  /*
 927 int dmp_enable_6x_lp_quat(unsigned char enable)                                    // saving : used code could be put directly in firmware.
 928 {
 929     unsigned char regs[4];
 930     if (enable) {
 931         regs[0] = DINA20;
 932         regs[1] = DINA28;
 933         regs[2] = DINA30;
 934         regs[3] = DINA38;
 935     } else
 936         memset(regs, 0xA3, 4);                                                      // those are the code currently in firmware
 937
 938     mpu_write_mem(CFG_8, 4, regs);                                                  // reg 2718 = bank 10 , row 9 , byte 14
 939
 940     return mpu_reset_fifo();
 941 }
 942 */
 943
 944 /**
 945  *  @brief      Decode the four-byte gesture data and execute any callbacks.   // saving: this code is called by dmp_read_fifo; it has been commented because this option is not used.
 946  *  @param[in]  gesture Gesture data from DMP packet.
 947  *  @return     0 if successful.
 948  */
 949 /*
 950 static int decode_gesture(unsigned char *gesture)
 951 {
 952     unsigned char tap, android_orient;
 953
 954     android_orient = gesture[3] & 0xC0;
 955     tap = 0x3F & gesture[3];
 956
 957     if (gesture[1] & INT_SRC_TAP) {
 958         unsigned char direction, count;
 959         direction = tap >> 3;
 960         count = (tap % 8) + 1;
 961         if (dmp.tap_cb)
 962             dmp.tap_cb(direction, count);
 963     }
 964
 965     if (gesture[1] & INT_SRC_ANDROID_ORIENT) {
 966         if (dmp.android_orient_cb)
 967             dmp.android_orient_cb(android_orient >> 6);
 968     }
 969
 970     return 0;
 971 }
 972 */
 973
 974 /**
 975  *  @brief      Specify when a DMP interrupt should occur.                     // saving : only first option is used; so the code could be saved directly in the initial upload.
 976  *  A DMP interrupt can be configured to trigger on either of the two          // but it is already done in this way and this function is never called (so no saving)
 977  *  conditions below:
 978  *  \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate).
 979  *  \n b. A tap event has been detected.
 980  *  @param[in]  mode    DMP_INT_GESTURE or DMP_INT_CONTINUOUS.
 981  *  @return     0 if successful.
 982  */
 983 /*
 984 int dmp_set_interrupt_mode(unsigned char mode)
 985 {
 986     const unsigned char regs_continuous[11] =
 987         {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};    // saving : this option is already define as default in the code; so this function does not have to be called.
 988     const unsigned char regs_gesture[11] =
 989         {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};
 990
 991     switch (mode) {
 992     case DMP_INT_CONTINUOUS:
 993         return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, (unsigned char*)regs_continuous);  // address= 2690 = 10 8 2 (bank, line, byte)
 994     case DMP_INT_GESTURE:
 995         return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, (unsigned char*)regs_gesture);
 996     default:
 997         return -1;
 998     }
 999 }
1000 */
1001
1002 /**
1003  *  @brief      Get one packet from the FIFO.
1004  *  If @e sensors does not contain a particular sensor, disregard the data
1005  *  returned to that pointer.
1006  *  \n @e sensors can contain a combination of the following flags:
1007  *  \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
1008  *  \n INV_XYZ_GYRO
1009  *  \n INV_XYZ_ACCEL
1010  *  \n INV_WXYZ_QUAT
1011  *  \n If the FIFO has no new data, @e sensors will be zero.
1012  *  \n If the FIFO is disabled, @e sensors will be zero and this function will
1013  *  return a non-zero error code.
1014  *  @param[out] gyro        Gyro data in hardware units.
1015  *  @param[out] accel       Accel data in hardware units.
1016  *  @param[out] quat        3-axis quaternion data in hardware units.
1017  *  @param[out] timestamp   Timestamp in milliseconds.
1018  *  @param[out] sensors     Mask of sensors read from FIFO.
1019  *  @param[out] more        Number of remaining packets.
1020   *  @return     0 if successful.  Negative if error:  -1:  DMP Not On; -2:  I2C read error; -3:  Fifo Overflow -4: No Sensors -5: No more data available  -6:  Quaternion out of range (I2C corruption?)
1021  */
1022 int dmp_read_fifo(short *gyro, short *accel, long *quat,  short *sensors, unsigned char *more)
1023 {
1024     unsigned char fifo_data[MAX_PACKET_LENGTH];
1025     unsigned char ii = 0;
1026
1027     /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
1028      * cache this value and save some cycles.
1029      */
1030     sensors[0] = 0;
1031
1032     /* Get a packet. */
1033     int success = mpu_read_fifo_stream(dmp.packet_length, fifo_data, more);
1034           if ( success != 0 ) return success;
1035
1036     /* Parse DMP packet. */
1037 //    if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {     // saving : DMP_FEATURE_6X_LP_QUAT is always active
1038         quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
1039             ((long)fifo_data[2] << 8) | fifo_data[3];
1040         quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
1041             ((long)fifo_data[6] << 8) | fifo_data[7];
1042         quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
1043             ((long)fifo_data[10] << 8) | fifo_data[11];
1044         quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
1045             ((long)fifo_data[14] << 8) | fifo_data[15];
1046         ii += 16;
1047 #ifdef FIFO_CORRUPTION_CHECK
1048         /* We can detect a corrupted FIFO by monitoring the quaternion data and
1049          * ensuring that the magnitude is always normalized to one. This
1050          * shouldn't happen in normal operation, but if an I2C error occurs,
1051          * the FIFO reads might become misaligned.
1052          *
1053          * Let's start by scaling down the quaternion data to avoid long long
1054          * math.
1055          */
1056         long quat_q14[4], quat_mag_sq;
1057         quat_q14[0] = quat[0] >> 16;
1058         quat_q14[1] = quat[1] >> 16;
1059         quat_q14[2] = quat[2] >> 16;
1060         quat_q14[3] = quat[3] >> 16;
1061         quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
1062             quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
1063         if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
1064             (quat_mag_sq > QUAT_MAG_SQ_MAX)) {
1065             /* Quaternion is outside of the acceptable threshold. */
1066             mpu_reset_fifo();
1067             sensors[0] = 0;
1068             return -4;
1069         }
1070
1071 #endif
1072         sensors[0] |= INV_WXYZ_QUAT;
1073 //    }
1074
1075 //    if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {                                     // saving : some checks can be ommitted if feature is always enabled and sensor could fe filled only once
1076         accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
1077         accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
1078         accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
1079         ii += 6;
1080         sensors[0] |= INV_XYZ_ACCEL;
1081 //    }
1082 /*
1083     if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {                                     // saving : some checks can be ommitted if feature is always enabled
1084         gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
1085         gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
1086         gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
1087         ii += 6;
1088         sensors[0] |= INV_XYZ_GYRO;
1089     }
1090 */
1091     /* Gesture data is at the end of the DMP packet. Parse it and call
1092      * the gesture callbacks (if registered).
1093      */                                                                        // is not used by oXs
1094     //if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) decode_gesture(fifo_data + ii);
1095
1096 //    get_ms(timestamp); // set as comment to save flash memory
1097     return 0;
1098 }
1099
1100 /**
1101  *  @brief      Register a function to be executed on a tap event.             // this function is not used by oXs
1102  *  The tap direction is represented by one of the following:
1103  *  \n TAP_X_UP
1104  *  \n TAP_X_DOWN
1105  *  \n TAP_Y_UP
1106  *  \n TAP_Y_DOWN
1107  *  \n TAP_Z_UP
1108  *  \n TAP_Z_DOWN
1109  *  @param[in]  func    Callback function.
1110  *  @return     0 if successful.
1111  */
1112 /*
1113 int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char))
1114 {
1115     dmp.tap_cb = func;
1116     return 0;
1117 }
1118 */
1119 /**
1120  *  @brief      Register a function to be executed on a android orientation event.
1121  *  @param[in]  func    Callback function.
1122  *  @return     0 if successful.
1123  */
1124 /*
1125 int dmp_register_android_orient_cb(void (*func)(unsigned char))               // this function is not used by oXs
1126 {
1127     dmp.android_orient_cb = func;
1128     return 0;
1129 }
1130 */
1131 /**
1132  *  @}
1133  */
1134