• Main Page
  • Related Pages
  • Data Structures
  • Files
  • File List
  • Globals

mems_spe.c

Go to the documentation of this file.
00001 /****************** COPYRIGHT (C) 2007-2010 RAISONANCE S.A.S. *****************/
00017 /******************************************************************************/
00018 
00019 /* Includes ------------------------------------------------------------------*/
00020 #include "circle.h"
00021 
00023 
00024 /* Private define ------------------------------------------------------------*/
00025 #define RDOUTXL               0xE8  
00026 #define WRCTRL_REG1           0x20  
00027 #define RDCTRL_REG1           0xA0  
00028 #define RDID                  0x8F  
00029 #define DUMMY_BYTE            0xA5
00030 #define MEMS_TESTING_DIVIDER  101
00031 #define MARGIN                500
00032 #define DELAY_REACT           20
00033 #define MIN_REACT             15
00034 #define DIV_REACT             10
00035 #define GRAD_SHOCK            350000
00036 #define HIGH                  0x01
00037 #define LOW                   0x00
00038 #define ACTIVE                0x00  
00039 #define STDBY                 0x01  
00042 /* Private variables ---------------------------------------------------------*/
00043 tMEMS_Info                          MEMS_Info                        = {0};   // structure definition in circle.h
00044 s32                                 TestingActive                    = 0;
00045 s32                                 StartingFromResetOrShockCounter  = 1000;
00046 s32                                 TimeCounterForDoubleClick        = 0;
00047 s32                                 TimeLastShock                    = 0;
00048 u32                                 Gradient2;
00049 
00050 // Gradient
00051 s32                                 GradX                            = 0;
00052 s32                                 GradY                            = 0;
00053 s32                                 GradZ                            = 0;
00054 
00055 coord_t XInit = 0;
00056 coord_t YInit = 0;
00057 coord_t ZInit = 0;
00058 
00059 /* Private function prototypes -----------------------------------------------*/
00060 void MEMS_ChipSelect( u8 State );
00061 u8   MEMS_SendByte( u8 byte );
00062 u32  MEMS_ReadOutXY( void );
00063 void MEMS_WakeUp( void );
00064 
00065 /* Private functions ---------------------------------------------------------*/
00066 
00067 /*******************************************************************************
00068 *
00069 *                                MEMS_WakeUp
00070 *
00071 *******************************************************************************/
00076 /******************************************************************************/
00077 void MEMS_WakeUp( void )
00078 {
00079     u8 reg_val;
00080 
00081     /* read RDCTRL_REG1 */
00082 
00083     /* Chip Select low */
00084     MEMS_ChipSelect( ACTIVE );
00085 
00086     /* Send "RDCTRL_REG1" instruction */
00087     MEMS_SendByte( RDCTRL_REG1 );
00088 
00089     reg_val = MEMS_SendByte( DUMMY_BYTE );
00090 
00091     /* Chip Select high */
00092     MEMS_ChipSelect( STDBY );
00093 
00094     /* SET P0:P1 to '11' */
00095     /* 0xC0 to wake up and 0x30 for full speed frequency (640 Hz). */
00096     reg_val = reg_val | 0xC0 | 0x30;
00097 
00098     /* Chip Select low */
00099     MEMS_ChipSelect( ACTIVE );
00100 
00101     /* Send "WRCTRL_REG1" instruction */
00102     MEMS_SendByte( WRCTRL_REG1 );
00103     MEMS_SendByte( reg_val );
00104 
00105     /* Chip Select high */
00106     MEMS_ChipSelect( STDBY );
00107 }
00108 
00109 /*******************************************************************************
00110 *
00111 *                                MEMS_ReadOutXY
00112 *
00113 *******************************************************************************/
00121 /******************************************************************************/
00122 u32 MEMS_ReadOutXY( void )
00123 {
00124 
00125     u8 OutXL;
00126     u8 OutXH;
00127     u8 OutYL;
00128     u8 OutYH;
00129     u8 OutZL;
00130     u8 OutZH;
00131 
00132     /* Chip Select low */
00133     MEMS_ChipSelect( ACTIVE );
00134 
00135     /* Send "RDOUTXL" instruction */
00136     MEMS_SendByte( RDOUTXL );
00137 
00138     /* Read a byte */
00139     OutXL = MEMS_SendByte( DUMMY_BYTE );
00140 
00141     /* Read a byte */
00142     OutXH = MEMS_SendByte( DUMMY_BYTE );
00143 
00144     /* Read a byte */
00145     OutYL = MEMS_SendByte( DUMMY_BYTE );
00146 
00147     /* Read a byte */
00148     OutYH = MEMS_SendByte( DUMMY_BYTE );
00149 
00150     /* Read a byte */
00151     OutZL = MEMS_SendByte( DUMMY_BYTE );
00152 
00153     /* Read a byte */
00154     OutZH = MEMS_SendByte( DUMMY_BYTE );
00155 
00156     MEMS_Info.OutX =  OutXL + ( OutXH << 8 );
00157     MEMS_Info.OutY =  OutYL + ( OutYH << 8 );
00158     MEMS_Info.OutZ =  OutZL + ( OutZH << 8 );
00159 
00160     /* Chip Select high */
00161     MEMS_ChipSelect( STDBY );
00162 
00163     MEMS_Info.OutX_F4 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F4 >> 2 ) ); // Filter on 4 values.
00164     MEMS_Info.OutY_F4 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F4 >> 2 ) ); // Filter on 4 values.
00165     MEMS_Info.OutZ_F4 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F4 >> 2 ) ); // Filter on 4 values.
00166 
00167     MEMS_Info.OutX_F16 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F16 >> 4 ) ); // Filter on 16 values.
00168     MEMS_Info.OutY_F16 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F16 >> 4 ) ); // Filter on 16 values.
00169     MEMS_Info.OutZ_F16 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F16 >> 4 ) ); // Filter on 16 values.
00170 
00171     MEMS_Info.OutX_F64 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F64 >> 6 ) ); // Filter on 64 values.
00172     MEMS_Info.OutY_F64 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F64 >> 6 ) ); // Filter on 64 values.
00173     MEMS_Info.OutZ_F64 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F64 >> 6 ) ); // Filter on 64 values.
00174 
00175     MEMS_Info.OutX_F256 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F256 >> 8 ) ); // Filter on 256 values.
00176     MEMS_Info.OutY_F256 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F256 >> 8 ) ); // Filter on 256 values.
00177     MEMS_Info.OutZ_F256 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F256 >> 8 ) ); // Filter on 256 values.
00178 
00179     return ( MEMS_Info.OutX + ( MEMS_Info.OutY << 16 ) );
00180 }
00181 
00182 /*******************************************************************************
00183 *
00184 *                                MEMS_ChipSelect
00185 *
00186 *******************************************************************************/
00193 /******************************************************************************/
00194 void MEMS_ChipSelect( u8 State )
00195 {
00196     /* Set High or low the chip select line */
00197     GPIO_WriteBit( GPIOX_MEMS_CS, GPIO_PIN_MEMS_CS, ( BitAction )State );
00198 }
00199 
00200 /*******************************************************************************
00201 *
00202 *                                MEMS_SendByte
00203 *
00204 *******************************************************************************/
00214 /******************************************************************************/
00215 u8 MEMS_SendByte( u8 byte )
00216 {
00217     /* Loop while DR register in not empty */
00218     while ( SPI_I2S_GetFlagStatus( SPIX_MEMS, SPI_I2S_FLAG_TXE ) == RESET );
00219 
00220     /* Send byte through the SPIx peripheral */
00221     SPI_I2S_SendData( SPIX_MEMS, byte );
00222 
00223     /* Wait to receive a byte */
00224     while ( SPI_I2S_GetFlagStatus( SPIX_MEMS, SPI_I2S_FLAG_RXNE ) == RESET );
00225 
00226     /* Return the byte read from the SPI bus */
00227     return SPI_I2S_ReceiveData( SPIX_MEMS );
00228 }
00229 
00230 /* Public functions for CircleOS ---------------------------------------------*/
00231 
00232 /*******************************************************************************
00233 *
00234 *                                MEMS_Init
00235 *
00236 *******************************************************************************/
00244 /******************************************************************************/
00245 NODEBUG2 void MEMS_Init( void )
00246 {
00247     SPI_InitTypeDef  SPI_InitStructure;
00248     GPIO_InitTypeDef GPIO_InitStructure;
00249 
00250     /* Enable SPIx, GPIOX_MEMS_CTRL and GPIOX_MEMS_CS clocks */
00251     RCC_APBxPERIPH_MEMS_CLOCK_CMD( RCC_APBxPERIPH_SPIX_MEMS,  ENABLE );
00252     RCC_PERIPH_GPIO_CLOCK_CMD( RCC_APBxPERIPH_GPIOX_MEMS_CTRL, ENABLE );
00253     RCC_PERIPH_GPIO_CLOCK_CMD( RCC_APBxPERIPH_GPIOX_MEMS_CS, ENABLE );
00254 
00255 #ifdef STM32L1XX_MD
00256 
00257     /* Configure MSD_SPI pins: SCK, MISO and MOSI */
00258     GPIO_InitStructure.GPIO_Pin = GPIO_PIN_MEMS_SCK | GPIO_PIN_MEMS_MISO | GPIO_PIN_MEMS_MOSI;
00259     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
00260     GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
00261     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
00262     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_40MHz;
00263     GPIO_Init( GPIOX_MEMS_CTRL, &GPIO_InitStructure );
00264 
00265     /* Connect PXx to SD_SPI_SCK, SD_SPI_MISO and  SD_SPI_MOSI*/
00266     GPIO_PinAFConfig(GPIOX_MEMS_CTRL, MEMS_SPI_SCK_SOURCE, MEMS_SPI_SCK_AF);
00267     GPIO_PinAFConfig(GPIOX_MEMS_CTRL, MEMS_SPI_MISO_SOURCE, MEMS_SPI_MISO_AF); 
00268     GPIO_PinAFConfig(GPIOX_MEMS_CTRL, MEMS_SPI_MOSI_SOURCE, MEMS_SPI_MOSI_AF);  
00269   
00270 #else
00271 
00272     /* Configure SPI pins: SCK and MOSI */
00273     GPIO_InitStructure.GPIO_Pin   = GPIO_PIN_MEMS_SCK | GPIO_PIN_MEMS_MOSI;
00274     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
00275     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF_PP;
00276     GPIO_Init( GPIOX_MEMS_CTRL, &GPIO_InitStructure );
00277 
00278     /* Configure SPI pins: MISO */
00279     GPIO_InitStructure.GPIO_Pin   = GPIO_PIN_MEMS_MISO;
00280     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
00281     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_IN_FLOATING;
00282     GPIO_Init( GPIOX_MEMS_CTRL, &GPIO_InitStructure );
00283     
00284 #endif
00285 
00286     /* Configure Pxx as Output push-pull, used as MEMS Chip select */
00287     GPIO_InitStructure.GPIO_Pin   = GPIO_PIN_MEMS_CS;
00288 #ifdef STM32L1XX_MD
00289     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
00290     GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
00291     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
00292     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_40MHz;
00293 #else
00294     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
00295     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_Out_PP;
00296 #endif
00297     GPIO_Init( GPIOX_MEMS_CS, &GPIO_InitStructure );
00298 
00299     /* SPI configuration */
00300     SPI_InitStructure.SPI_Direction           = SPI_Direction_2Lines_FullDuplex;
00301     SPI_InitStructure.SPI_Mode                = SPI_Mode_Master;
00302     SPI_InitStructure.SPI_DataSize            = SPI_DataSize_8b;
00303     SPI_InitStructure.SPI_CPOL                = SPI_CPOL_High;
00304     SPI_InitStructure.SPI_CPHA                = SPI_CPHA_2Edge;
00305     SPI_InitStructure.SPI_NSS                 = SPI_NSS_Soft;
00306     SPI_InitStructure.SPI_BaudRatePrescaler   = SPI_BaudRatePrescaler_16;
00307     SPI_InitStructure.SPI_FirstBit            = SPI_FirstBit_MSB;
00308     SPI_InitStructure.SPI_CRCPolynomial       = 7;
00309 
00310     SPI_Init( SPIX_MEMS, &SPI_InitStructure );
00311 
00312     /* Enable SPI  */
00313     SPI_Cmd( SPIX_MEMS, ENABLE );
00314 
00315     if ( MEMS_ReadID() != 0x3A )
00316     {
00317         s32 i;
00318 
00319         // Try to resynchronize
00320         for ( i = 0 ; i < 17 ; i++ )
00321         {
00322             /* Configure SPI2 pins: SCK, MISO and MOSI */
00323             GPIO_InitStructure.GPIO_Pin   = GPIO_PIN_MEMS_SCK | GPIO_PIN_MEMS_MOSI;
00324         #ifndef STM32L1XX_MD
00325             GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
00326             GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_Out_PP;
00327             GPIO_Init( GPIOX_MEMS_CTRL, &GPIO_InitStructure );
00328         #else
00329             GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
00330             GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
00331             GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
00332             GPIO_InitStructure.GPIO_Speed = GPIO_Speed_40MHz;
00333         #endif
00334             GPIO_WriteBit( GPIOX_MEMS_CTRL, GPIO_PIN_MEMS_MOSI, HIGH );
00335             MEMS_ChipSelect( ACTIVE );
00336 
00337             GPIO_WriteBit( GPIOX_MEMS_CTRL, GPIO_PIN_MEMS_SCK, LOW );
00338             GPIO_WriteBit( GPIOX_MEMS_CTRL, GPIO_PIN_MEMS_SCK, HIGH );
00339             MEMS_ChipSelect( HIGH );
00340 
00341             /* Configure again PB. SCK as SPI2 pin */
00342             GPIO_InitStructure.GPIO_Pin   = GPIO_PIN_MEMS_SCK | GPIO_PIN_MEMS_MOSI;
00343          #ifndef STM32L1XX_MD
00344             GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
00345             GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF_PP;
00346             GPIO_Init( GPIOX_MEMS_CTRL, &GPIO_InitStructure );
00347          #else
00348             GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
00349             GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
00350             GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
00351             GPIO_InitStructure.GPIO_Speed = GPIO_Speed_40MHz;
00352          #endif
00353          
00354             if ( MEMS_ReadID() == 0x3A )
00355             {
00356                 break;
00357             }
00358         }
00359 
00360         if ( i == 17 )
00361         {
00362             DRAW_DisplayString( 1, 50, "Test MEM ID Failed", 18 );
00363             fIsStandAlone = 1;
00364         }
00365     }
00366 
00367     MEMS_ReadOutXY();
00368 
00369     MEMS_Info.OutX_F4 = MEMS_Info.OutX_F16 = MEMS_Info.OutX_F64 = MEMS_Info.OutX_F256 = MEMS_Info.OutX;
00370     MEMS_Info.OutY_F4 = MEMS_Info.OutY_F16 = MEMS_Info.OutY_F64 = MEMS_Info.OutY_F256 = MEMS_Info.OutY;
00371     MEMS_Info.OutZ_F4 = MEMS_Info.OutZ_F16 = MEMS_Info.OutZ_F64 = MEMS_Info.OutZ_F256 = MEMS_Info.OutZ;
00372 
00373     /* Init X and Y*/
00374     MEMS_GetPosition( &XInit, &YInit );
00375 
00376     /* Wake Up Mems*/
00377     MEMS_WakeUp();
00378 }
00379 
00380 /*******************************************************************************
00381 *
00382 *                                MEMS_Handler
00383 *
00384 *******************************************************************************/
00393 /******************************************************************************/
00394 IRQ void MEMS_Handler( void )
00395 {
00396     static s32 delta_time_ms = 0;
00397 
00398     if ( StartingFromResetOrShockCounter )
00399     {
00400         StartingFromResetOrShockCounter--;
00401     }
00402     TimeCounterForDoubleClick++;
00403 
00404     MEMS_ReadOutXY();
00405 
00406     // Evaluate gradients
00407     GradX = ( MEMS_Info.OutX_F16 >> 4 ) - MEMS_Info.OutX;
00408     GradY = ( MEMS_Info.OutY_F16 >> 4 ) - MEMS_Info.OutY;
00409     GradZ = ( MEMS_Info.OutZ_F16 >> 4 ) - MEMS_Info.OutZ;
00410 
00411     Gradient2 = ( s32 )GradX * ( s32 )GradX + ( s32 )GradY * ( s32 )GradY + ( s32 )GradZ * ( s32 )GradZ;
00412 
00413     Gradient2 *= freqTIM2[CurrentSpeed] ;
00414     Gradient2 /= freqTIM2[SPEED_MEDIUM] ;
00415 
00416 
00417     // MEMS is shocked, let's beep!
00418     if (( Gradient2 > GRAD_SHOCK ) && ( BUZZER_GetMode() == BUZZER_OFF ) && ( StartingFromResetOrShockCounter == 0 ) )
00419     {
00420         MEMS_Info.Shocked++;
00421         /*FL071007       = 1;
00422         Suggested by Bob Seabrook:  a further possiblity is to increment Shocked rather than just setting it
00423         So it can still be tested for non zero as before but one can  get more
00424         info from the s32 without extra cost. */
00425 
00426 #define DELAY_BETWEEN_TWO_SHOCK     200
00427 #define MAX_DELAY_FOR_DOUBLECLICK   800
00428 
00429         delta_time_ms = (( TimeCounterForDoubleClick - TimeLastShock ) * 1000 ) / freqTIM2[CurrentSpeed];
00430 
00431         if ( delta_time_ms > DELAY_BETWEEN_TWO_SHOCK )
00432         {
00433             if ( delta_time_ms < MAX_DELAY_FOR_DOUBLECLICK )
00434             {
00435                 MEMS_Info.DoubleClick++;
00436 //               TimeLastShock = TimeCounterForDoubleClick;    // YRT20090304
00437                 BUZZER_SetMode( BUZZER_SHORTBEEP );
00438                 StartingFromResetOrShockCounter = freqTIM2[CurrentSpeed]; //1s off
00439             }
00440             else
00441             {
00442 //                TimeLastShock = TimeCounterForDoubleClick;   // YRT20090304
00443                 delta_time_ms  = 0;
00444                 StartingFromResetOrShockCounter = 0;
00445             }
00446             TimeLastShock = TimeCounterForDoubleClick;         // YRT20090304
00447         }
00448         else
00449         {
00450             ;//TimeLastShock = TimeCounterForDoubleClick;
00451         }
00452     }
00453 }
00454 
00455 /*******************************************************************************
00456 *
00457 *                                MEMS_ReadID
00458 *
00459 *******************************************************************************/
00466 /******************************************************************************/
00467 u8 MEMS_ReadID( void )
00468 {
00469     u8 Temp = 0;
00470 
00471     /* Chip Select low */
00472     MEMS_ChipSelect( ACTIVE );
00473 
00474     /* Send "RDID" instruction */
00475     MEMS_SendByte( RDID );
00476 
00477     /* Read a byte from the MEMS */
00478     Temp = MEMS_SendByte( DUMMY_BYTE );
00479 
00480     /* Chip Select low */
00481     MEMS_ChipSelect( STDBY );
00482 
00483     return Temp;
00484 }
00485 
00487 
00488 /* Public functions ----------------------------------------------------------*/
00489 
00490 /*******************************************************************************
00491 *
00492 *                                MEMS_GetPosition
00493 *
00494 *******************************************************************************/
00507 /******************************************************************************/
00508 void MEMS_GetPosition( coord_t* pX, coord_t* pY )
00509 {
00510     *pX = MEMS_Info.OutX - XInit;
00511     *pY = MEMS_Info.OutY - YInit;
00512 }
00513 
00514 /*******************************************************************************
00515 *
00516 *                                MEMS_GetRotation
00517 *
00518 *******************************************************************************/
00526 /******************************************************************************/
00527 void MEMS_GetRotation( Rotate_H12_V_Match_TypeDef* pH12 )
00528 {
00529     s16 sX = MEMS_Info.OutX;
00530     s16 sY = MEMS_Info.OutY;
00531 
00532     if ((( sX <= -MARGIN ) && ( sY <= 0 ) && ( sX <= sY ) ) ||
00533             (( sX <= - MARGIN ) && ( sY > 0 ) && ( sX <= ( -sY ) ) ) )
00534     {
00535         // 1st case: x<0, |x|>y => H12 = V9
00536         *pH12 = V9;
00537     }
00538     else if ((( sY <= -MARGIN ) && ( sX <= 0 ) && ( sY <= sX ) ) ||
00539              (( sY <= -MARGIN ) && ( sX > 0 ) && ( sY <= ( -sX ) ) ) )
00540     {
00541         // 2nd case: y<0, |y|>x => H12 = V12
00542         *pH12 = V12;
00543     }
00544     else if ((( sX >= MARGIN ) && ( sY <= 0 ) && ( sX >= ( -sY ) ) ) ||
00545              (( sX >= MARGIN ) && ( sY > 0 ) && ( sX >= sY ) ) )
00546     {
00547         // 3rd case: x>0, |x|>y => H12=V3
00548         *pH12 = V3;
00549     }
00550     else if ((( sY >= MARGIN ) && ( sX <= 0 ) && ( sY >= ( -sX ) ) ) ||
00551              (( sY >= MARGIN ) && ( sX > 0 ) && ( sY >= sX ) ) )
00552     {
00553         // 4th case: y>0,  |y|>x => H12=V6
00554         *pH12 = V6;
00555     }
00556 }
00557 
00558 /*******************************************************************************
00559 *
00560 *                                MEMS_SetNeutral
00561 *
00562 *******************************************************************************/
00568 /******************************************************************************/
00569 void MEMS_SetNeutral( void )
00570 {
00571     // Set Neutral position.
00572     MEMS_GetPosition( &XInit, &YInit );
00573 }
00574 
00575 /*******************************************************************************
00576 *
00577 *                                MEMS_GetInfo
00578 *
00579 *******************************************************************************/
00587 /******************************************************************************/
00588 tMEMS_Info* MEMS_GetInfo( void )
00589 {
00590     return &MEMS_Info;
00591 }