URL
https://opencores.org/ocsvn/openrisc/openrisc/trunk
Subversion Repositories openrisc
[/] [openrisc/] [trunk/] [rtos/] [freertos-6.1.1/] [Demo/] [CORTEX_STM32F103_Primer_GCC/] [ST_Code/] [mems.c] - Rev 589
Go to most recent revision | Compare with Previous | Blame | View Log
/********************* (C) COPYRIGHT 2007 RAISONANCE S.A.S. *******************/ /** * * @file mems.c * @brief Mems Initialization and management * @author FL * @date 07/2007 * @version 1.1 * @date 10/2007 * @version 1.5 various corrections reported by Ron Miller * **/ /******************************************************************************/ /* Includes ------------------------------------------------------------------*/ #include "circle.h" /// @cond Internal /* Private define ------------------------------------------------------------*/ #define RDOUTXL 0xE8 /*!< Multiple Read from OUTXL */ #define WRCTRL_REG1 0x20 /*!< Single Write CTRL_REG */ #define RDCTRL_REG1 0xA0 /*!< Single Read CTRL_REG */ #define RDID 0x8F /*!< Single Read WHO_AM_I */ #define LOW 0x00 /*!< ChipSelect line low */ #define HIGH 0x01 /*!< ChipSelect line high */ #define DUMMY_BYTE 0xA5 #define MEMS_DIVIDER 1 #define MEMS_TESTING_DIVIDER 101 #define MARGIN 500 #define DELAY_REACT 20 #define MIN_REACT 15 #define DIV_REACT 10 #define GRAD_SHOCK 200000 /* Private variables ---------------------------------------------------------*/ tMEMS_Info MEMS_Info = {0}; // structure definition in circle.h int TestingActive = 0; int StartingFromResetOrShockCounter = 1000; int TimeCounterForDoubleClick = 0; int TimeLastShock = 0; static int divider = 0; static Rotate_H12_V_Match_TypeDef previous_Screen_Orientation; u32 Gradient2; //Filtering unsigned N_filtering = 0; //Gradient s16 GradX = 0; s16 GradY = 0; s16 GradZ = 0; // Pointer move: // each coordinate (X, Y and Z) is described by 3 variables where suffix means: // f = flag to indicate that a move has been done. Cleared by the Ptr Manager when acknowledged. // i = amplitude of the move (Grad / 10) // t = delay to accept the counter reaction int fMovePtrX; int iMovePtrX; int tMovePtrX; int fMovePtrY; int iMovePtrY; int tMovePtrY; int fMovePtrZ; int iMovePtrZ; int tMovePtrZ; s16 XInit = 0; s16 YInit = 0; s16 ZInit = 0; /* Private function prototypes -----------------------------------------------*/ static void MEMS_ChipSelect( u8 State ); static u8 MEMS_SendByte( u8 byte ); static void MEMS_WriteEnable( void ); static u32 MEMS_ReadOutXY( void ); static void MEMS_WakeUp( void ); /* Private functions ---------------------------------------------------------*/ /******************************************************************************* * * MEMS_WakeUp * *******************************************************************************/ /** * Wake Up Mems. * **/ /******************************************************************************/ static void MEMS_WakeUp( void ) { u8 reg_val; /* read RDCTRL_REG1 */ /* Chip Select low */ MEMS_ChipSelect( LOW ); /* Send "RDCTRL_REG1" instruction */ MEMS_SendByte( RDCTRL_REG1 ); reg_val = MEMS_SendByte( DUMMY_BYTE ); /* Chip Select high */ MEMS_ChipSelect( HIGH ); /* SET P0:P1 to '11' */ /* 0xC0 to wake up and 0x30 for full speed frequency (640 Hz). */ reg_val = reg_val | 0xC0 | 0x30; /* Chip Select low */ MEMS_ChipSelect( LOW ); /* Send "WRCTRL_REG1" instruction */ MEMS_SendByte( WRCTRL_REG1 ); MEMS_SendByte( reg_val ); /* Chip Select high */ MEMS_ChipSelect( HIGH ); } /******************************************************************************* * * MEMS_ReadOutXY * *******************************************************************************/ /** * Reads X and Y Out. * * @return An unsigned 32 bit word with the highest 16 bits containing the Y * and the lowest 16 bits the X. * **/ /******************************************************************************/ static u32 MEMS_ReadOutXY( void ) { u8 OutXL; u8 OutXH; u8 OutYL; u8 OutYH; u8 OutZL; u8 OutZH; /* Chip Select low */ MEMS_ChipSelect( LOW ); /* Send "RDOUTXL" instruction */ MEMS_SendByte( RDOUTXL ); /* Read a byte */ OutXL = MEMS_SendByte( DUMMY_BYTE ); /* Read a byte */ OutXH = MEMS_SendByte( DUMMY_BYTE ); /* Read a byte */ OutYL = MEMS_SendByte( DUMMY_BYTE ); /* Read a byte */ OutYH = MEMS_SendByte( DUMMY_BYTE ); /* Read a byte */ OutZL = MEMS_SendByte( DUMMY_BYTE ); /* Read a byte */ OutZH = MEMS_SendByte( DUMMY_BYTE ); MEMS_Info.OutX = OutXL + ( OutXH << 8 ); MEMS_Info.OutY = OutYL + ( OutYH << 8 ); MEMS_Info.OutZ = OutZL + ( OutZH << 8 ); /* Chip Select high */ MEMS_ChipSelect( HIGH ); MEMS_Info.OutX_F4 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F4 >> 2 ) ); // Filter on 4 values. MEMS_Info.OutY_F4 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F4 >> 2 ) ); // Filter on 4 values. MEMS_Info.OutZ_F4 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F4 >> 2 ) ); // Filter on 4 values. MEMS_Info.OutX_F16 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F16 >> 4 ) ); // Filter on 16 values. MEMS_Info.OutY_F16 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F16 >> 4 ) ); // Filter on 16 values. MEMS_Info.OutZ_F16 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F16 >> 4 ) ); // Filter on 16 values. MEMS_Info.OutX_F64 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F64 >> 6 ) ); // Filter on 64 values. MEMS_Info.OutY_F64 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F64 >> 6 ) ); // Filter on 64 values. MEMS_Info.OutZ_F64 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F64 >> 6 ) ); // Filter on 64 values. MEMS_Info.OutX_F256 += ( MEMS_Info.OutX - ( MEMS_Info.OutX_F256 >> 8) ); // Filter on 256 values. MEMS_Info.OutY_F256 += ( MEMS_Info.OutY - ( MEMS_Info.OutY_F256 >> 8) ); // Filter on 256 values. MEMS_Info.OutZ_F256 += ( MEMS_Info.OutZ - ( MEMS_Info.OutZ_F256 >> 8) ); // Filter on 256 values. if( N_filtering < 256 ) { // Just to validate the calculated average values. N_filtering++; } return ( MEMS_Info.OutX + ( MEMS_Info.OutY << 16 ) ); } /******************************************************************************* * * MEMS_ChipSelect * *******************************************************************************/ /** * Selects or deselects the MEMS device. * * @param[in] State Level to be applied on ChipSelect pin. * **/ /******************************************************************************/ static void MEMS_ChipSelect( u8 State ) { /* Set High or low the chip select line on PA.4 pin */ GPIO_WriteBit( GPIOD, GPIO_Pin_2, (BitAction)State ); } /******************************************************************************* * * MEMS_SendByte * *******************************************************************************/ /** * Sends a byte through the SPI interface and return the byte received from * the SPI bus. * * @param[in] byte The byte to send to the SPI interface. * * @return The byte returned by the SPI bus. * **/ /******************************************************************************/ static u8 MEMS_SendByte( u8 byte ) { /* Loop while DR register in not emplty */ while( SPI_I2S_GetFlagStatus( SPI2, SPI_I2S_FLAG_TXE ) == RESET ); /* Send byte through the SPI2 peripheral */ SPI_I2S_SendData( SPI2, byte ); /* Wait to receive a byte */ while( SPI_I2S_GetFlagStatus( SPI2, SPI_I2S_FLAG_RXNE ) == RESET ); /* Return the byte read from the SPI bus */ return SPI_I2S_ReceiveData( SPI2 ); } /* Public functions for CircleOS ---------------------------------------------*/ /******************************************************************************* * * MEMS_Init * *******************************************************************************/ /** * * Initializes the peripherals used by the SPI MEMS driver. * * @attention This function must <b>NOT</b> be called by the user. * **/ /******************************************************************************/ void MEMS_Init(void) { SPI_InitTypeDef SPI_InitStructure; GPIO_InitTypeDef GPIO_InitStructure; /* Configure PC6 and PC7 as Output push-pull For MEMS*/ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init( GPIOC, &GPIO_InitStructure ); /* Enable SPI2 and GPIOA clocks */ RCC_APB1PeriphClockCmd( RCC_APB1Periph_SPI2, ENABLE ); RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE ); RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOD, ENABLE ); /* Configure SPI2 pins: SCK, MISO and MOSI */ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_Init( GPIOB, &GPIO_InitStructure ); /* Configure PD2 as Output push-pull, used as MEMS Chip select */ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init( GPIOD, &GPIO_InitStructure ); /* SPI2 configuration */ SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; SPI_InitStructure.SPI_Mode = SPI_Mode_Master; SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; SPI_InitStructure.SPI_CPOL = SPI_CPOL_High; SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge; SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256; SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; SPI_InitStructure.SPI_CRCPolynomial = 7; SPI_Init( SPI2, &SPI_InitStructure ); /* Enable SPI2 */ SPI_Cmd( SPI2, ENABLE ); if( MEMS_ReadID() != 0x3A ) { int i; // Try to resynchronize for( i = 0 ; i < 17 ; i++ ) { /* Configure SPI2 pins: SCK, MISO and MOSI */ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_15; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init( GPIOB, &GPIO_InitStructure ); GPIO_WriteBit( GPIOB, GPIO_Pin_15, HIGH ); MEMS_ChipSelect( LOW ); GPIO_WriteBit( GPIOB, GPIO_Pin_13, LOW ); GPIO_WriteBit( GPIOB, GPIO_Pin_13, HIGH ); MEMS_ChipSelect( HIGH ); /* Configure again PB. SCK as SPI2 pin */ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_15; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_Init( GPIOB, &GPIO_InitStructure ); if ( MEMS_ReadID() == 0x3A ) { break; } } if( i == 17 ) { DRAW_DisplayString( 1, 50, "Test MEM ID Failed", 17 ); } } /* Read for the first time */ N_filtering = 0; MEMS_ReadOutXY(); MEMS_Info.OutX_F4 = MEMS_Info.OutX_F16 = MEMS_Info.OutX_F64 = MEMS_Info.OutX_F256 = MEMS_Info.OutX; MEMS_Info.OutY_F4 = MEMS_Info.OutY_F16 = MEMS_Info.OutY_F64 = MEMS_Info.OutY_F256 = MEMS_Info.OutY; MEMS_Info.OutZ_F4 = MEMS_Info.OutZ_F16 = MEMS_Info.OutZ_F64 = MEMS_Info.OutZ_F256 = MEMS_Info.OutZ; /* Init X and Y*/ MEMS_GetPosition( &XInit, &YInit ); /* Wake Up Mems*/ MEMS_WakeUp(); } /******************************************************************************* * * MEMS_Handler * *******************************************************************************/ /** * * Called by the CircleOS scheduler to manage the MEMS. The Circle beeps if the * MEMS is shocked. * * @attention This function must <b>NOT</b> be called by the user. * **/ /******************************************************************************/ void MEMS_Handler( void ) { char buffer [20]; int i; int ofs_disp = 0; if( StartingFromResetOrShockCounter ) { StartingFromResetOrShockCounter--; } TimeCounterForDoubleClick++; MEMS_ReadOutXY(); // Evaluate gradients GradX = ( MEMS_Info.OutX_F4 >> 2 ) - MEMS_Info.OutX; GradY = ( MEMS_Info.OutY_F4 >> 2 ) - MEMS_Info.OutY; GradZ = ( MEMS_Info.OutZ_F4 >> 2 ) - MEMS_Info.OutZ; // Decide whether a direction is selected if( tMovePtrX == 0 ) { if( ( GradX > MIN_REACT ) || ( GradX < -MIN_REACT ) ) { iMovePtrX = GradX / DIV_REACT; tMovePtrX = DELAY_REACT; fMovePtrX = 1; } } else { tMovePtrX--; } if( tMovePtrY == 0 ) { if( ( GradY > MIN_REACT ) || ( GradY < -MIN_REACT ) ) { iMovePtrY = GradY / DIV_REACT; //FL071012 rrm fix tMovePtrY = DELAY_REACT; fMovePtrY = 1; } } else { tMovePtrY--; } if( tMovePtrZ==0 ) { if( ( GradZ > MIN_REACT ) || ( GradY < -MIN_REACT ) ) { iMovePtrZ = GradZ / DIV_REACT; tMovePtrZ = DELAY_REACT; fMovePtrZ = 1; } } else { tMovePtrZ--; } Gradient2 = (s32)GradX * (s32)GradX + (s32)GradY * (s32)GradY + (s32)GradZ * (s32)GradZ; // MEMS is shocked, let's beep! if( ( Gradient2 > GRAD_SHOCK ) && ( BUZZER_GetMode() == BUZZER_OFF ) && ( StartingFromResetOrShockCounter == 0 ) ) { MEMS_Info.Shocked++; /*FL071007 = 1; Suggested by Bob Seabrook: a further posiblity is to increment Shocked rather than just setting it So it can still be tested for non zero as before but one can get more info from the int without extra cost. */ #define DELAY_BETWEEN_TWO_SHOCK 20 #define MAX_DELAY_FOR_DOUBLECLICK 150 StartingFromResetOrShockCounter = DELAY_BETWEEN_TWO_SHOCK; //< filter: short delay before detecting the next shock if ( (TimeCounterForDoubleClick - TimeLastShock) < MAX_DELAY_FOR_DOUBLECLICK ) { MEMS_Info.DoubleClick++; TimeLastShock = 0; } else { TimeLastShock = TimeCounterForDoubleClick; } BUZZER_SetMode( BUZZER_SHORTBEEP ); } } /******************************************************************************* * * MEMS_ReadID * *******************************************************************************/ /** * Reads SPI chip identification. * * @return The SPI chip identification. * **/ /******************************************************************************/ u8 MEMS_ReadID( void ) { u8 Temp = 0; /* Chip Select low */ MEMS_ChipSelect( LOW ); /* Send "RDID" instruction */ MEMS_SendByte( RDID ); /* Read a byte from the MEMS */ Temp = MEMS_SendByte( DUMMY_BYTE ); /* Chip Select low */ MEMS_ChipSelect( HIGH ); return Temp; } /// @endcond /* Public functions ----------------------------------------------------------*/ /******************************************************************************* * * MEMS_GetPosition * *******************************************************************************/ /** * * Returns the current (relative) position of the Primer. * Only X-Y axis are considered here. * * @param[out] pX Current horizontal coordinate. * @param[out] pY Current vertical coordinate. * * @warning The (0x0) point in on the low left corner. * @note For absolute position information use MEMS_GetInfo() * **/ /******************************************************************************/ void MEMS_GetPosition( s16* pX, s16* pY ) { *pX = MEMS_Info.OutX - XInit; *pY = MEMS_Info.OutY - YInit; } /******************************************************************************* * * MEMS_GetRotation * *******************************************************************************/ /** * * Returns current screen orientation. * * @param[out] pH12 Current screen orientation. * **/ /******************************************************************************/ void MEMS_GetRotation( Rotate_H12_V_Match_TypeDef* pH12 ) { s16 sX = MEMS_Info.OutX; s16 sY = MEMS_Info.OutY; if( ( ( sX <= -MARGIN ) && ( sY <= 0 ) && (sX<=sY ) ) || ( ( sX <=- MARGIN ) && ( sY > 0) && (sX <= (-sY ) ) ) ) { // 1st case: x<0, |x|>y => H12 = V9 *pH12 = V9; } else if( ( ( sY <= -MARGIN ) && ( sX <= 0 ) && ( sY <= sX ) ) || ( ( sY <= -MARGIN ) && ( sX > 0 ) && ( sY <= (-sX ) ) ) ) { // 2nd case: y<0, |y|>x => H12 = V12 *pH12 = V12; } else if( ( ( sX >= MARGIN ) && ( sY <= 0 ) && ( sX >= (-sY) ) ) || ( ( sX >= MARGIN ) && ( sY > 0 ) && ( sX >= sY ) ) ) { // 3rd case: x>0, |x|>y => H12=V3 *pH12 = V3; } else if( ( ( sY >= MARGIN ) && ( sX <= 0 ) && ( sY >= (-sX ) ) ) || ( ( sY >= MARGIN ) && ( sX > 0 ) && ( sY >= sX ) ) ) { // 4th case: y>0, |y|>x => H12=V6 *pH12 = V6; } } /******************************************************************************* * * MEMS_SetNeutral * *******************************************************************************/ /** * * Set current position as "neutral position". * **/ /******************************************************************************/ void MEMS_SetNeutral( void ) { // Set Neutral position. MEMS_GetPosition( &XInit, &YInit ); } /******************************************************************************* * * MEMS_GetInfo * *******************************************************************************/ /** * * Return the current MEMS information (state, absolute position...). * * @return a pointer to tMEMS_Info * **/ /******************************************************************************/ tMEMS_Info* MEMS_GetInfo( void ) { return &MEMS_Info; }
Go to most recent revision | Compare with Previous | Blame | View Log