git.sur5r.net Git - freertos/blob - Demo/CORTEX_STM32F103_Primer_GCC/ST_Code/mems.c

   1 /********************* (C) COPYRIGHT 2007 RAISONANCE S.A.S. *******************/\r
   2 /**\r
   3 *\r
   4 * @file     mems.c\r
   5 * @brief    Mems Initialization and management\r
   6 * @author   FL\r
   7 * @date     07/2007\r
   8 * @version  1.1\r
   9 * @date     10/2007\r
  10 * @version  1.5 various corrections reported by Ron Miller \r
  11 *\r
  12 **/\r
  13 /******************************************************************************/\r
  14 \r
  15 /* Includes ------------------------------------------------------------------*/\r
  16 #include "circle.h"\r
  17 \r
  18 /// @cond Internal\r
  19 \r
  20 /* Private define ------------------------------------------------------------*/\r
  21 #define RDOUTXL               0xE8  /*!< Multiple Read from OUTXL             */\r
  22 #define WRCTRL_REG1           0x20  /*!< Single Write CTRL_REG                */\r
  23 #define RDCTRL_REG1           0xA0  /*!< Single Read CTRL_REG                 */\r
  24 #define RDID                  0x8F  /*!< Single Read WHO_AM_I                 */\r
  25 #define LOW                   0x00  /*!< ChipSelect line low                  */\r
  26 #define HIGH                  0x01  /*!< ChipSelect line high                 */\r
  27 #define DUMMY_BYTE            0xA5\r
  28 #define MEMS_DIVIDER          1\r
  29 #define MEMS_TESTING_DIVIDER  101\r
  30 #define MARGIN                500\r
  31 #define DELAY_REACT           20\r
  32 #define MIN_REACT             15\r
  33 #define DIV_REACT             10\r
  34 #define GRAD_SHOCK            200000\r
  35 \r
  36 /* Private variables ---------------------------------------------------------*/\r
  37 tMEMS_Info                          MEMS_Info                        = {0};   // structure definition in circle.h\r
  38 int                                 TestingActive                    = 0;\r
  39 int                                 StartingFromResetOrShockCounter  = 1000;\r
  40 int                                 TimeCounterForDoubleClick        = 0;\r
  41 int                                 TimeLastShock                    = 0;\r
  42 static int                          divider                          = 0;\r
  43 static Rotate_H12_V_Match_TypeDef   previous_Screen_Orientation;\r
  44 u32                                 Gradient2;\r
  45 \r
  46 //Filtering\r
  47 unsigned                            N_filtering                      = 0;\r
  48 \r
  49 //Gradient\r
  50 s16                                 GradX                            = 0;\r
  51 s16                                 GradY                            = 0;\r
  52 s16                                 GradZ                            = 0;\r
  53 \r
  54 // Pointer move:\r
  55 // each coordinate (X, Y and Z) is described by 3 variables where suffix means:\r
  56 //  f = flag to indicate that a move has been done. Cleared by the Ptr Manager when acknowledged.\r
  57 //  i = amplitude of the move (Grad / 10)\r
  58 //  t = delay to accept the counter reaction\r
  59 int fMovePtrX; \r
  60 int iMovePtrX;\r
  61 int tMovePtrX;\r
  62 int fMovePtrY;\r
  63 int iMovePtrY;\r
  64 int tMovePtrY;\r
  65 int fMovePtrZ;\r
  66 int iMovePtrZ;\r
  67 int tMovePtrZ;\r
  68 \r
  69 s16 XInit      = 0;\r
  70 s16 YInit      = 0;\r
  71 s16 ZInit      = 0;\r
  72 \r
  73 /* Private function prototypes -----------------------------------------------*/\r
  74 static void MEMS_ChipSelect( u8 State );\r
  75 static u8 MEMS_SendByte( u8 byte );\r
  76 static void MEMS_WriteEnable( void );\r
  77 static u32 MEMS_ReadOutXY( void );\r
  78 static void MEMS_WakeUp( void );\r
  79 \r
  80 /* Private functions ---------------------------------------------------------*/\r
  81 \r
  82 /*******************************************************************************\r
  83 *\r
  84 *                                MEMS_WakeUp\r
  85 *\r
  86 *******************************************************************************/\r
  87 /**\r
  88 *  Wake Up Mems.\r
  89 *\r
  90 **/\r
  91 /******************************************************************************/\r
  92 static void MEMS_WakeUp( void )\r
  93    {\r
  94    u8 reg_val;\r
  95 \r
  96    /* read RDCTRL_REG1 */\r
  97 \r
  98    /* Chip Select low */\r
  99    MEMS_ChipSelect( LOW );\r
 100 \r
 101    /* Send "RDCTRL_REG1" instruction */\r
 102    MEMS_SendByte( RDCTRL_REG1 );\r
 103 \r
 104    reg_val = MEMS_SendByte( DUMMY_BYTE );\r
 105 \r
 106    /* Chip Select high */\r
 107    MEMS_ChipSelect( HIGH );\r
 108 \r
 109    /* SET P0:P1 to '11' */\r
 110    /* 0xC0 to wake up and 0x30 for full speed frequency (640 Hz). */\r
 111    reg_val = reg_val | 0xC0 | 0x30;\r
 112 \r
 113    /* Chip Select low */\r
 114    MEMS_ChipSelect( LOW );\r
 115 \r
 116    /* Send "WRCTRL_REG1" instruction */\r
 117    MEMS_SendByte( WRCTRL_REG1 );\r
 118    MEMS_SendByte( reg_val );\r
 119 \r
 120    /* Chip Select high */\r
 121    MEMS_ChipSelect( HIGH );\r
 122    }\r
 123 \r
 124 /*******************************************************************************\r
 125 *\r
 126 *                                MEMS_ReadOutXY\r
 127 *\r
 128 *******************************************************************************/\r
 129 /**\r
 130 *  Reads X and Y Out.\r
 131 *\r
 132 *  @return An unsigned 32 bit word with the highest 16 bits containing the Y\r
 133 *          and the lowest 16 bits the X.\r
 134 *\r
 135 **/\r
 136 /******************************************************************************/\r
 137 static u32 MEMS_ReadOutXY( void )\r
 138    {\r
 139    u8 OutXL;\r
 140    u8 OutXH;\r
 141    u8 OutYL;\r
 142    u8 OutYH;\r
 143    u8 OutZL;\r
 144    u8 OutZH;\r
 145 \r
 146    /* Chip Select low */\r
 147    MEMS_ChipSelect( LOW );\r
 148 \r
 149    /* Send "RDOUTXL" instruction */\r
 150    MEMS_SendByte( RDOUTXL );\r
 151 \r
 152    /* Read a byte */\r
 153    OutXL = MEMS_SendByte( DUMMY_BYTE );\r
 154 \r
 155    /* Read a byte */\r
 156    OutXH = MEMS_SendByte( DUMMY_BYTE );\r
 157 \r
 158    /* Read a byte */\r
 159    OutYL = MEMS_SendByte( DUMMY_BYTE );\r
 160 \r
 161    /* Read a byte */\r
 162    OutYH = MEMS_SendByte( DUMMY_BYTE );\r
 163 \r
 164    /* Read a byte */\r
 165    OutZL = MEMS_SendByte( DUMMY_BYTE );\r
 166 \r
 167    /* Read a byte */\r
 168    OutZH = MEMS_SendByte( DUMMY_BYTE );\r
 169 \r
 170    MEMS_Info.OutX =  OutXL + ( OutXH << 8 );\r
 171    MEMS_Info.OutY =  OutYL + ( OutYH << 8 );\r
 172    MEMS_Info.OutZ =  OutZL + ( OutZH << 8 );\r
 173 \r
 174    /* Chip Select high */\r
 175    MEMS_ChipSelect( HIGH );\r
 176 \r
 177    MEMS_Info.OutX_F4 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F4 >> 2 ) ); // Filter on 4 values.\r
 178    MEMS_Info.OutY_F4 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F4 >> 2 ) ); // Filter on 4 values.\r
 179    MEMS_Info.OutZ_F4 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F4 >> 2 ) ); // Filter on 4 values.\r
 180 \r
 181    MEMS_Info.OutX_F16 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F16 >> 4 ) ); // Filter on 16 values.\r
 182    MEMS_Info.OutY_F16 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F16 >> 4 ) ); // Filter on 16 values.\r
 183    MEMS_Info.OutZ_F16 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F16 >> 4 ) ); // Filter on 16 values.\r
 184 \r
 185    MEMS_Info.OutX_F64 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F64 >> 6 ) ); // Filter on 64 values.\r
 186    MEMS_Info.OutY_F64 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F64 >> 6 ) ); // Filter on 64 values.\r
 187    MEMS_Info.OutZ_F64 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F64 >> 6 ) ); // Filter on 64 values.\r
 188 \r
 189    MEMS_Info.OutX_F256 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F256 >> 8) ); // Filter on 256 values.\r
 190    MEMS_Info.OutY_F256 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F256 >> 8) ); // Filter on 256 values.\r
 191    MEMS_Info.OutZ_F256 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F256 >> 8) ); // Filter on 256 values.\r
 192 \r
 193    if( N_filtering < 256 )\r
 194       {\r
 195       // Just to validate the calculated average values.\r
 196       N_filtering++; \r
 197       }\r
 198 \r
 199    return ( MEMS_Info.OutX + ( MEMS_Info.OutY << 16 ) );\r
 200    }\r
 201 \r
 202 /*******************************************************************************\r
 203 *\r
 204 *                                MEMS_ChipSelect\r
 205 *\r
 206 *******************************************************************************/\r
 207 /**\r
 208 *  Selects or deselects the MEMS device.\r
 209 *\r
 210 *  @param[in]  State Level to be applied on ChipSelect pin.\r
 211 *\r
 212 **/\r
 213 /******************************************************************************/\r
 214 static void MEMS_ChipSelect( u8 State )\r
 215    {\r
 216    /* Set High or low the chip select line on PA.4 pin */\r
 217    GPIO_WriteBit( GPIOD, GPIO_Pin_2, (BitAction)State );\r
 218    }\r
 219 \r
 220 /*******************************************************************************\r
 221 *\r
 222 *                                MEMS_SendByte\r
 223 *\r
 224 *******************************************************************************/\r
 225 /**\r
 226 *  Sends a byte through the SPI interface and return the byte received from \r
 227 *  the SPI bus.\r
 228 *\r
 229 *  @param[in]  byte The byte to send to the SPI interface.                  \r
 230 *\r
 231 *  @return The byte returned by the SPI bus.\r
 232 *\r
 233 **/\r
 234 /******************************************************************************/\r
 235 static u8 MEMS_SendByte( u8 byte )\r
 236    {\r
 237    /* Loop while DR register in not emplty */\r
 238    while( SPI_I2S_GetFlagStatus( SPI2, SPI_I2S_FLAG_TXE ) == RESET );\r
 239 \r
 240    /* Send byte through the SPI2 peripheral */\r
 241    SPI_I2S_SendData( SPI2, byte );\r
 242 \r
 243    /* Wait to receive a byte */\r
 244    while( SPI_I2S_GetFlagStatus( SPI2, SPI_I2S_FLAG_RXNE ) == RESET );\r
 245 \r
 246    /* Return the byte read from the SPI bus */\r
 247    return SPI_I2S_ReceiveData( SPI2 );\r
 248    }\r
 249 \r
 250 /* Public functions for CircleOS ---------------------------------------------*/\r
 251 \r
 252 /*******************************************************************************\r
 253 *\r
 254 *                                MEMS_Init\r
 255 *\r
 256 *******************************************************************************/\r
 257 /**\r
 258 *\r
 259 *  Initializes the peripherals used by the SPI MEMS driver.\r
 260 *\r
 261 *  @attention  This function must <b>NOT</b> be called by the user.\r
 262 *\r
 263 **/\r
 264 /******************************************************************************/\r
 265 void MEMS_Init(void)\r
 266 {\r
 267    SPI_InitTypeDef  SPI_InitStructure;\r
 268    GPIO_InitTypeDef GPIO_InitStructure;\r
 269 \r
 270    /* Configure PC6 and PC7 as Output push-pull For MEMS*/\r
 271    GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_6 | GPIO_Pin_7;\r
 272    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;\r
 273    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_Out_PP;\r
 274 \r
 275    GPIO_Init( GPIOC, &GPIO_InitStructure );\r
 276 \r
 277    /* Enable SPI2 and GPIOA clocks */\r
 278    RCC_APB1PeriphClockCmd( RCC_APB1Periph_SPI2,  ENABLE );\r
 279    RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE );\r
 280    RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOD, ENABLE );\r
 281 \r
 282    /* Configure SPI2 pins: SCK, MISO and MOSI */\r
 283    GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;\r
 284    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;\r
 285    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF_PP;\r
 286 \r
 287    GPIO_Init( GPIOB, &GPIO_InitStructure );\r
 288 \r
 289    /* Configure PD2 as Output push-pull, used as MEMS Chip select */\r
 290    GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_2;\r
 291    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;\r
 292    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_Out_PP;\r
 293 \r
 294    GPIO_Init( GPIOD, &GPIO_InitStructure );\r
 295 \r
 296    /* SPI2 configuration */\r
 297    SPI_InitStructure.SPI_Direction           = SPI_Direction_2Lines_FullDuplex;\r
 298    SPI_InitStructure.SPI_Mode                = SPI_Mode_Master;\r
 299    SPI_InitStructure.SPI_DataSize            = SPI_DataSize_8b;\r
 300    SPI_InitStructure.SPI_CPOL                = SPI_CPOL_High;\r
 301    SPI_InitStructure.SPI_CPHA                = SPI_CPHA_2Edge;\r
 302    SPI_InitStructure.SPI_NSS                 = SPI_NSS_Soft;\r
 303    SPI_InitStructure.SPI_BaudRatePrescaler   = SPI_BaudRatePrescaler_256;\r
 304    SPI_InitStructure.SPI_FirstBit            = SPI_FirstBit_MSB;\r
 305    SPI_InitStructure.SPI_CRCPolynomial       = 7;\r
 306 \r
 307    SPI_Init( SPI2, &SPI_InitStructure );\r
 308 \r
 309    /* Enable SPI2  */\r
 310    SPI_Cmd( SPI2, ENABLE );\r
 311 \r
 312    if( MEMS_ReadID() != 0x3A )\r
 313       {\r
 314       int i;\r
 315 \r
 316       // Try to resynchronize\r
 317       for( i = 0 ; i < 17 ; i++ )\r
 318          {\r
 319          /* Configure SPI2 pins: SCK, MISO and MOSI */\r
 320          GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_13 | GPIO_Pin_15;\r
 321          GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;\r
 322          GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_Out_PP;\r
 323 \r
 324          GPIO_Init( GPIOB, &GPIO_InitStructure );\r
 325          GPIO_WriteBit( GPIOB, GPIO_Pin_15, HIGH );\r
 326          MEMS_ChipSelect( LOW );\r
 327 \r
 328          GPIO_WriteBit( GPIOB, GPIO_Pin_13, LOW );\r
 329          GPIO_WriteBit( GPIOB, GPIO_Pin_13, HIGH );\r
 330          MEMS_ChipSelect( HIGH );\r
 331 \r
 332          /* Configure again PB. SCK as SPI2 pin */\r
 333          GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_13 | GPIO_Pin_15;\r
 334          GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;\r
 335          GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF_PP;\r
 336 \r
 337          GPIO_Init( GPIOB, &GPIO_InitStructure );\r
 338          if ( MEMS_ReadID() == 0x3A )\r
 339             {\r
 340             break;\r
 341             }\r
 342          }\r
 343 \r
 344       if( i == 17 )\r
 345          {\r
 346          DRAW_DisplayString( 1, 50, "Test MEM ID Failed", 17 );\r
 347          }\r
 348       }\r
 349 \r
 350    /* Read for the first time */\r
 351    N_filtering = 0;\r
 352 \r
 353    MEMS_ReadOutXY();\r
 354 \r
 355    MEMS_Info.OutX_F4 = MEMS_Info.OutX_F16 = MEMS_Info.OutX_F64 = MEMS_Info.OutX_F256 = MEMS_Info.OutX;\r
 356    MEMS_Info.OutY_F4 = MEMS_Info.OutY_F16 = MEMS_Info.OutY_F64 = MEMS_Info.OutY_F256 = MEMS_Info.OutY;\r
 357    MEMS_Info.OutZ_F4 = MEMS_Info.OutZ_F16 = MEMS_Info.OutZ_F64 = MEMS_Info.OutZ_F256 = MEMS_Info.OutZ;\r
 358 \r
 359    /* Init X and Y*/\r
 360    MEMS_GetPosition( &XInit, &YInit );\r
 361 \r
 362    /* Wake Up Mems*/\r
 363    MEMS_WakeUp();\r
 364 }\r
 365 \r
 366 /*******************************************************************************\r
 367 *\r
 368 *                                MEMS_Handler\r
 369 *\r
 370 *******************************************************************************/\r
 371 /**\r
 372 *\r
 373 *  Called by the CircleOS scheduler to manage the MEMS. The Circle beeps if the\r
 374 *  MEMS is shocked.\r
 375 *\r
 376 *  @attention  This function must <b>NOT</b> be called by the user.\r
 377 *\r
 378 **/\r
 379 /******************************************************************************/\r
 380 void MEMS_Handler( void )\r
 381    {\r
 382    char buffer [20];\r
 383    int  i;\r
 384    int  ofs_disp = 0;\r
 385 \r
 386    if( StartingFromResetOrShockCounter )\r
 387       {\r
 388       StartingFromResetOrShockCounter--;\r
 389       }\r
 390    TimeCounterForDoubleClick++;\r
 391 \r
 392    MEMS_ReadOutXY();\r
 393 \r
 394    // Evaluate gradients\r
 395    GradX = ( MEMS_Info.OutX_F4 >> 2 ) - MEMS_Info.OutX;\r
 396    GradY = ( MEMS_Info.OutY_F4 >> 2 ) - MEMS_Info.OutY;\r
 397    GradZ = ( MEMS_Info.OutZ_F4 >> 2 ) - MEMS_Info.OutZ;\r
 398 \r
 399    // Decide whether a direction is selected\r
 400    if( tMovePtrX == 0 )\r
 401       {\r
 402       if( ( GradX > MIN_REACT ) || ( GradX < -MIN_REACT ) )\r
 403          {\r
 404          iMovePtrX = GradX / DIV_REACT; \r
 405          tMovePtrX = DELAY_REACT;\r
 406          fMovePtrX = 1;\r
 407          }\r
 408       }\r
 409    else\r
 410       {\r
 411       tMovePtrX--;\r
 412       }\r
 413 \r
 414    if( tMovePtrY == 0 )\r
 415       {\r
 416       if( ( GradY > MIN_REACT ) || ( GradY < -MIN_REACT ) )\r
 417          {\r
 418          iMovePtrY = GradY / DIV_REACT;   //FL071012 rrm fix\r
 419          tMovePtrY = DELAY_REACT;\r
 420          fMovePtrY = 1;\r
 421          }\r
 422       }\r
 423    else\r
 424       {\r
 425       tMovePtrY--;\r
 426       }\r
 427 \r
 428    if( tMovePtrZ==0 )\r
 429       {\r
 430       if( ( GradZ > MIN_REACT ) || ( GradY < -MIN_REACT ) )\r
 431          {\r
 432          iMovePtrZ = GradZ / DIV_REACT;\r
 433          tMovePtrZ = DELAY_REACT;\r
 434          fMovePtrZ = 1;\r
 435          }\r
 436       }\r
 437    else\r
 438       {\r
 439       tMovePtrZ--;\r
 440       }\r
 441 \r
 442    Gradient2 = (s32)GradX * (s32)GradX + (s32)GradY * (s32)GradY + (s32)GradZ * (s32)GradZ;\r
 443 \r
 444    // MEMS is shocked, let's beep!\r
 445    if( ( Gradient2 > GRAD_SHOCK ) && ( BUZZER_GetMode() == BUZZER_OFF ) && ( StartingFromResetOrShockCounter == 0 ) )\r
 446       {\r
 447       MEMS_Info.Shocked++;\r
 448 /*FL071007       = 1;\r
 449       Suggested by Bob Seabrook:  a further posiblity is to increment Shocked rather than just setting it\r
 450       So it can still be tested for non zero as before but one can  get more\r
 451       info from the int without extra cost. */\r
 452 \r
 453 #define DELAY_BETWEEN_TWO_SHOCK      20\r
 454 #define MAX_DELAY_FOR_DOUBLECLICK    150\r
 455       StartingFromResetOrShockCounter  = DELAY_BETWEEN_TWO_SHOCK; //< filter: short delay before detecting the next shock\r
 456       if ( (TimeCounterForDoubleClick - TimeLastShock) < MAX_DELAY_FOR_DOUBLECLICK )\r
 457          {\r
 458          MEMS_Info.DoubleClick++;\r
 459          TimeLastShock = 0;\r
 460          }\r
 461       else\r
 462          {\r
 463          TimeLastShock = TimeCounterForDoubleClick;\r
 464          }    \r
 465       BUZZER_SetMode( BUZZER_SHORTBEEP );\r
 466       }\r
 467    }\r
 468 \r
 469 /*******************************************************************************\r
 470 *\r
 471 *                                MEMS_ReadID\r
 472 *\r
 473 *******************************************************************************/\r
 474 /**\r
 475 *  Reads SPI chip identification.\r
 476 *\r
 477 *  @return The SPI chip identification.\r
 478 *\r
 479 **/\r
 480 /******************************************************************************/\r
 481 u8 MEMS_ReadID( void )\r
 482    {\r
 483    u8 Temp = 0;\r
 484 \r
 485    /* Chip Select low */\r
 486    MEMS_ChipSelect( LOW );\r
 487 \r
 488    /* Send "RDID" instruction */\r
 489    MEMS_SendByte( RDID );\r
 490 \r
 491    /* Read a byte from the MEMS */\r
 492    Temp = MEMS_SendByte( DUMMY_BYTE );\r
 493 \r
 494    /* Chip Select low */\r
 495    MEMS_ChipSelect( HIGH );\r
 496 \r
 497    return Temp;\r
 498    }\r
 499 \r
 500 /// @endcond\r
 501 \r
 502 /* Public functions ----------------------------------------------------------*/\r
 503 \r
 504 /*******************************************************************************\r
 505 *\r
 506 *                                MEMS_GetPosition\r
 507 *\r
 508 *******************************************************************************/\r
 509 /**\r
 510 *\r
 511 *  Returns the current (relative) position of the Primer.\r
 512 *  Only X-Y axis are considered here. \r
 513 *\r
 514 *  @param[out] pX    Current horizontal coordinate.\r
 515 *  @param[out] pY    Current vertical coordinate.\r
 516 *\r
 517 *  @warning    The (0x0) point in on the low left corner.\r
 518 *  @note       For absolute position information use MEMS_GetInfo()\r
 519 *\r
 520 **/\r
 521 /******************************************************************************/\r
 522 void MEMS_GetPosition( s16* pX, s16* pY )\r
 523    {\r
 524    *pX = MEMS_Info.OutX - XInit;\r
 525    *pY = MEMS_Info.OutY - YInit;\r
 526    }\r
 527 \r
 528 /*******************************************************************************\r
 529 *\r
 530 *                                MEMS_GetRotation\r
 531 *\r
 532 *******************************************************************************/\r
 533 /**\r
 534 *\r
 535 *  Returns current screen orientation.\r
 536 *\r
 537 *  @param[out]  pH12 Current screen orientation.\r
 538 *\r
 539 **/\r
 540 /******************************************************************************/\r
 541 void MEMS_GetRotation( Rotate_H12_V_Match_TypeDef* pH12 )\r
 542    {\r
 543    s16 sX = MEMS_Info.OutX;\r
 544    s16 sY = MEMS_Info.OutY;\r
 545 \r
 546    if( ( ( sX <= -MARGIN ) && ( sY <= 0 ) && (sX<=sY ) ) || \r
 547        ( ( sX <=- MARGIN ) && ( sY > 0) && (sX <= (-sY ) ) ) )\r
 548       {\r
 549       // 1st case: x<0, |x|>y => H12 = V9\r
 550       *pH12 = V9;\r
 551       }\r
 552    else if( ( ( sY <= -MARGIN ) && ( sX <= 0 ) && ( sY <= sX ) ) ||\r
 553             ( ( sY <= -MARGIN ) && ( sX > 0 ) && ( sY <= (-sX ) ) ) ) \r
 554       {\r
 555       // 2nd case: y<0, |y|>x => H12 = V12\r
 556       *pH12 = V12;\r
 557       }\r
 558    else if( ( ( sX >= MARGIN ) && ( sY <= 0 ) && ( sX >= (-sY) ) ) || \r
 559             ( ( sX >= MARGIN ) && ( sY > 0 ) && ( sX >= sY ) ) )\r
 560       {\r
 561       // 3rd case: x>0, |x|>y => H12=V3\r
 562       *pH12 = V3;\r
 563       }\r
 564    else if( ( ( sY >= MARGIN ) && ( sX <= 0 ) && ( sY >= (-sX ) ) ) ||\r
 565             ( ( sY >= MARGIN ) && ( sX > 0 ) && ( sY >= sX ) ) )\r
 566       {\r
 567       // 4th case: y>0,  |y|>x => H12=V6\r
 568       *pH12 = V6;\r
 569       }\r
 570    }\r
 571 \r
 572 /*******************************************************************************\r
 573 *\r
 574 *                                MEMS_SetNeutral\r
 575 *\r
 576 *******************************************************************************/\r
 577 /**\r
 578 *\r
 579 *  Set current position as "neutral position".\r
 580 *\r
 581 **/\r
 582 /******************************************************************************/\r
 583 void MEMS_SetNeutral( void )\r
 584    {\r
 585    // Set Neutral position.\r
 586    MEMS_GetPosition( &XInit, &YInit );\r
 587    }\r
 588 \r
 589 /*******************************************************************************\r
 590 *\r
 591 *                                MEMS_GetInfo\r
 592 *\r
 593 *******************************************************************************/\r
 594 /**\r
 595 *\r
 596 *  Return the current MEMS information (state, absolute position...).\r
 597 *\r
 598 *  @return  a pointer to tMEMS_Info\r
 599 *\r
 600 **/\r
 601 /******************************************************************************/\r
 602 tMEMS_Info* MEMS_GetInfo( void )\r
 603    {\r
 604    return &MEMS_Info;\r
 605    }\r