123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462 |
- /***************************************************************************************************
- Motion.c
- ****************************************************************************************************/
- #define THIS_FILE_ID 19
- #include "includes.h"
- #define LSM303D_WHO_AM_I_VAL 0x49
- #define LSM6DS3_WHO_AM_I_VAL 0x69
- MOTION_SENSOR_TYPE MotionSensorType;
- SUT_MOTION sutMotion;
- /**********************************************************************
- DelayNs
- ***********************************************************************/
- void DelayNs(unsigned short ns)
- {
- while(ns--);
- }
- /**********************************************************************
- DelayUs
- SystemCoreClock=72 Mhz t=70
- SystemCoreClock=60 Mhz t=60
- ***********************************************************************/
- //void DelayUs(unsigned short us)
- //{
- // int t;
- // while(us--){
- // t=60;
- // while(t--);
- // }
- //}
- //void DelayMs(unsigned short ms)
- //{
- // while(ms--){
- // DelayUs(1000);
- // IWDG_ReloadCounter();
- // }
- //}
- static int32_t platform_write(void *handle, uint8_t Reg, uint8_t *Bufp,
- uint16_t len)
- {
- unsigned char data;
- unsigned short i,j;
- data=Reg;
-
- if(handle==(void *)LSM303AGR_I2C_ADD_XL){
- MOTION_CS_XL_LOW;
- }else MOTION_CS_MAG_LOW;
- DELAY_NOP;
- for(i=0;i<8;i++)
- {
- MOTION_CLK_LOW;
- DELAY_NOP;
- DELAY_NOP;
- if((data<<i)&0x80) MOTION_SDA_HIGH;
- else MOTION_SDA_LOW;
- DELAY_NOP;
- MOTION_CLK_HIGH;
- DELAY_NOP;
- }
- for(j=0;j<len;j++){
- data=Bufp[j];
- for(i=0;i<8;i++)
- {
- MOTION_CLK_LOW;
- DELAY_NOP;
- if((data<<i)&0x80) MOTION_SDA_HIGH;
- else MOTION_SDA_LOW;
- DELAY_NOP;
- MOTION_CLK_HIGH;
- DELAY_NOP;
- }
- }
-
- if(handle==(void *)LSM303AGR_I2C_ADD_XL)MOTION_CS_XL_HIGH;
- else MOTION_CS_MAG_HIGH;
- DELAY_NOP;
- return 0;
- }
- static int32_t platform_read(void *handle, uint8_t Reg, uint8_t *Bufp,
- uint16_t len)
- {
- unsigned char data;
- unsigned short dat=0,i,j;
- data=0x80|Reg;
- if(handle==(void *)LSM303AGR_I2C_ADD_XL){
- MOTION_CS_XL_LOW;
- if(len>1)data|=0x40;//地址自增
- }else MOTION_CS_MAG_LOW;
-
- DELAY_NOP;
-
- //发地址
- for(i=0;i<8;i++)
- {
- DELAY_NOP;
- MOTION_CLK_LOW;
- DELAY_NOP;
- if((data<<i)&0x80) MOTION_SDA_HIGH;
- else MOTION_SDA_LOW;
- DELAY_NOP;
- MOTION_CLK_HIGH;
- DELAY_NOP;
- }
- //读数据
- for(j=0;j<len;j++){
- for(i=0;i<8;i++)
- {
- data<<=1;
- DELAY_NOP;
- MOTION_CLK_LOW;
- DELAY_NOP;
- MOTION_CLK_HIGH;
- DELAY_NOP;
- if(MOTION_SDA_READ) data|=0x01;
- }
- Bufp[j]=data;
- }
- if(handle==(void *)LSM303AGR_I2C_ADD_XL)MOTION_CS_XL_HIGH;
- else MOTION_CS_MAG_HIGH;
-
- DELAY_NOP;
- return 0;
- }
- /**********************************************************************************************
- *
- ***********************************************************************************************/
- void SPI_LSM_Init()
- {
-
- }
- void Motion_PortInit()
- {
- //SPI 初始化
- GPIO_InitTypeDef GPIO_InitStructure;
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA,ENABLE);
- //IP模拟SPI三线式 SCK--PB13 SDA(MISO/MOSI)--PB15
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 ;
- GPIO_InitStructure.GPIO_Speed =GPIO_Speed_10MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15 ;
- GPIO_InitStructure.GPIO_Speed =GPIO_Speed_10MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- //片选IO设置
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2,ENABLE);
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA,ENABLE);
-
- GPIO_InitStructure.GPIO_Pin = MOTION_CS_XL_PIN|MOTION_CS_MAG_PIN;
- GPIO_InitStructure.GPIO_Speed =GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(MOTION_CS_XL_PORT, &GPIO_InitStructure);
- MOTION_CS_XL_HIGH;
- MOTION_CS_MAG_HIGH;
- }
- //lsm303agr_ctx_t dev_ctx_xl;
- //lsm303agr_ctx_t dev_ctx_mg;
- static uint8_t whoamI, rst;
- int MotionInit(void)
- {
- unsigned char whoAmI = 0,r,a;
- status_t response; // mems error variable
- Motion_PortInit();
- os_dly_wait(10);
- //配置为SPI模式
- a=1;
- LSM303C_ACC_WriteReg(0,0x23,&a,1);
- a=4;
- LSM303C_MAG_WriteReg(0,0x22,&a,1);
- whoAmI=0;
- //读WHO AM I
- LSM303C_MAG_R_WHO_AM_I_(0, &whoAmI);
- if(whoAmI !=MAG_WHO_AM_I_VALUE){
- SlwTrace(DEBUG,"Lsm303C Init Failed.\r\n");
- return 0;
- }
- whoAmI=0;
- LSM303C_ACC_ReadReg(0, LSM303C_ACC_WHO_AM_I_REG, &whoAmI, 1);
- if(whoAmI !=ACC_WHO_AM_I_VALUE){
- SlwTrace(DEBUG,"Lsm303C Init Failed.\r\n");
- return 0;
- }
-
- //Initialize Magnetometer
- //set ODR (turn ON device)
- response = LSM303C_MAG_W_OutputDataRate(0, LSM303C_MAG_DO_10Hz);
- if(response==MEMS_ERROR) while(1); //manage here comunication error
- //set Fullscale
- response = LSM303C_MAG_W_FullScale(0, LSM303C_MAG_FS_16Ga);
- if(response==MEMS_ERROR) while(1); //manage here comunication error
-
- //set Block Data Update
- response = LSM303C_MAG_W_BlockDataUpdate(0, LSM303C_MAG_BDU_ENABLE);
- if(response==MEMS_ERROR) while(1); //manage here comunication error
-
- //set XY Axis Operation Mode
- response = LSM303C_MAG_W_OperatingModeXY(0, LSM303C_MAG_OM_HIGH);
- if(response==MEMS_ERROR) while(1) //manage here comunication error
-
- //set Z Axis Operation Mode
- response = LSM303C_MAG_W_OperatingModeZ(0, LSM303C_MAG_OMZ_HIGH);
- if(response==MEMS_ERROR) while(1); //manage here comunication error
-
- //set Continuous Mode
- response = LSM303C_MAG_W_SystemOperatingMode(0, LSM303C_MAG_MD_CONTINUOUS);
- if(response==MEMS_ERROR) while(1); //manage here comunication error
-
- //this flag is used for reading data only when there are new data
-
- //Initialize Accelerometer
- //set Fullscale
- response = LSM303C_ACC_SetFullScale(0, LSM303C_ACC_FS_2g);
- if(response==MEMS_ERROR) while(1); //manage here comunication error
-
- //set Block Data Update
- response = LSM303C_ACC_BlockDataUpdate(0, LSM303C_ACC_BDU_ENABLE);
- if(response==MEMS_ERROR) while(1); //manage here comunication error
-
- //Enable Axis
- response = LSM303C_ACC_EnableAxis(0, LSM303C_ACC_X_ENABLE|LSM303C_ACC_Y_ENABLE|LSM303C_ACC_Z_ENABLE);
- if(response==MEMS_ERROR) while(1); //manage here comunication error
-
- //set ODR (turn ON device)
- response = LSM303C_ACC_SetODR(0, LSM303C_ACC_ODR_10_Hz);
- if(response==MEMS_ERROR) while(1); //manage here comunication error
-
- SlwTrace(DEBUG,"Lsm303C Init OK.\r\n");
- return 1;
- }
- int MotionReadAccel(short *pX,short *pY,short *pZ)
- {
- u8_t flag_LSM303C_ACC_STATUS_FLAGS;
- Type3Axis16bit_U data; // accelerometer row data
- status_t response; // mems error variable
- response = LSM303C_ACC_Status_Flags(0, &flag_LSM303C_ACC_STATUS_FLAGS);
- if(response==MEMS_ERROR)return 0;
- else if (flag_LSM303C_ACC_STATUS_FLAGS&LSM303C_ACC_ZYX_NEW_DATA_AVAILABLE){
- //read only if new data are available from accelerometer
- response = LSM303C_ACC_GetAccRaw(0, data.u8bit);
- *pX=data.i16bit[0];
- *pY=data.i16bit[1];
- *pZ=data.i16bit[2];
- }
- return 1;
- }
- int MotionReadMagne(short *pX,short *pY,short *pZ)
- {
- LSM303C_MAG_ZYXDA_t flag_LSM303C_MAG_XYZDA;
- Type3Axis16bit_U data; // accelerometer row data
- status_t response; // mems error variable
- response = LSM303C_MAG_R_NewXYZData(0, (LSM303C_MAG_ZYXDA_t*) &flag_LSM303C_MAG_XYZDA);
- if(response==MEMS_ERROR)return 0;
- else if ((LSM303C_MAG_ZYXDA_t) flag_LSM303C_MAG_XYZDA & LSM303C_MAG_ZYXDA_AVAILABLE){
- //read only if new data are available from accelerometer
- response = LSM303C_MAG_Get_Magnetic(0, data.u8bit);
- *pX=data.i16bit[0];
- *pY=data.i16bit[1];
- *pZ=data.i16bit[2];
- }
- return 1;
- }
- /*
- 循环采样,1秒一个点
- */
- void MotionLoop(void)
- {
- int i,i1,i2;
- char flag;
- short Ax,Ay,Az,Mx,My,Mz;
- int Axp,Ayp,Axm,Aym;
- static unsigned long lastTick=50;
- unsigned long curTick;
- static short Axx[10],Ayy[10],Azz[10],Mxx[10],Myy[10],Mzz[10];
- static unsigned char sucCt=0;
- //控制执行频率
- curTick=os_time_get();
- if(curTick>lastTick)lastTick=curTick+50;
- else return;
- flag=0;
- if(MotionReadAccel(&Ax,&Ay,&Az)){
- flag=1;
- }
- if(MotionReadMagne(&Mx,&My,&Mz)){
- flag=1;
- }
- if(flag){
- if(++sucCt>9)sucCt=0;
- //先存起来
- Axx[sucCt]=Ax;
- Ayy[sucCt]=Ay;
- Azz[sucCt]=Az;
- Mxx[sucCt]=Mx;
- Myy[sucCt]=My;
- Mzz[sucCt]=Mz;
- //再处理
- if(sucCt==9){//计算处理
- //求平均
- Axp=0;Ayp=0;
- for(i=0;i<10;i++){
- Axp+=Axx[i];
- Ayp+=Ayy[i];
- }
- Axp/=10;
- Ayp/=10;
- Axm=0;Aym=0;
- //找到最大偏差点
- for(i=0;i<10;i++){
- Ax=Axx[i]-Axp;
- Ay=Ayy[i]-Ayp;
- if(Ax<0)Ax*=-1;
- if(Ay<0)Ay*=-1;
- if(Ax>Axm){
- Axm=Ax;
- i1=i;
- }
- if(Ay>Aym){
- Aym=Ay;
- i2=i;
- }
- }
- tsk_lock();
- sutMotion.Flag=0;
- //判断Ax还是Ay偏差最大
- if(Axm>Aym){
- //Ax偏差最大,取Ax
- sutMotion.Ax=Axx[i1];
- sutMotion.Ay=Ayy[i1];
- sutMotion.Az=Azz[i1];
- sutMotion.Mx=Mxx[i1];
- sutMotion.My=Myy[i1];
- sutMotion.Mz=Mzz[i1];
- if(Axm>(Axp+ACC_THRESHOLD))sutMotion.Flag=1;
- }else{
- //Ay偏差最大,取Ay
- sutMotion.Ax=Axx[i2];
- sutMotion.Ay=Ayy[i2];
- sutMotion.Az=Azz[i2];
- sutMotion.Mx=Mxx[i2];
- sutMotion.My=Myy[i2];
- sutMotion.Mz=Mzz[i2];
- if(Aym>(Ayp+ACC_THRESHOLD))sutMotion.Flag=2;
- }
- tsk_unlock();
- //打印
- #if 0
- float fAx,fAy,fAz,fMx,fMy,fMz;
- fAx=sutMotion.Ax*SENSITIVITY_ACC;
- fAy=sutMotion.Ay*SENSITIVITY_ACC;
- fAz=sutMotion.Az*SENSITIVITY_ACC;
- fMx=sutMotion.Mx*SENSITIVITY_MAG;
- fMy=sutMotion.My*SENSITIVITY_MAG;
- fMz=sutMotion.Mz*SENSITIVITY_MAG;
- printf("\r\n");
- printf("Ax=%10.3f\r\n",fAx);
- printf("Ay=%10.3f\r\n",fAy);
- printf("Az=%10.3f\r\n",fAz);
- printf("Mx=%10.3f\r\n",fMx);
- printf("My=%10.3f\r\n",fMy);
- printf("Mz=%10.3f\r\n",fMz);
- printf("----Flag=%d---\r\n",sutMotion.Flag);
- #endif
- }
- }else{
- SlwTrace(DEBUG,"Motion Test Error!\r\n");
- sucCt=0;
- tsk_lock();
- sutMotion.Ax=0;
- sutMotion.Ay=0;
- sutMotion.Az=0;
- sutMotion.Mx=0;
- sutMotion.My=0;
- sutMotion.Mz=0;
- sutMotion.Flag=0;
- tsk_unlock();
- }
- }
- /***************************************************
- *每20ms调用一次
- ****************************************************/
- void MotionTest(void)
- {
- static unsigned char ct=0;
- int Ax,Ay,Az,Mx,My,Mz;
- float mgData_X, mgData_Y,mgData_Z; //data in mg
- float gaussData_X, gaussData_Y, gaussData_Z; //data in Gauss
-
- u8_t flag_LSM303C_ACC_STATUS_FLAGS;
- LSM303C_MAG_ZYXDA_t flag_LSM303C_MAG_XYZDA;
- Type3Axis16bit_U data; // accelerometer row data
- status_t response; // mems error variable
-
- if(++ct>100)ct=0;
- else return;
- //get Acceleration Raw data
- response = LSM303C_MAG_R_NewXYZData(0, (LSM303C_MAG_ZYXDA_t*) &flag_LSM303C_MAG_XYZDA);
- if(response==MEMS_ERROR) SlwTrace(DEBUG,"ERROR111"); //manage here comunication error
- //read only if new data are available from accelerometer
- else if (flag_LSM303C_ACC_STATUS_FLAGS&LSM303C_ACC_ZYX_NEW_DATA_AVAILABLE)
- {
- response = LSM303C_ACC_GetAccRaw(0, data.u8bit);
- //convert from LSB to mg
- mgData_X=data.i16bit[0]*SENSITIVITY_ACC;
- mgData_Y=data.i16bit[1]*SENSITIVITY_ACC;
- mgData_Z=data.i16bit[2]*SENSITIVITY_ACC;
- Ax=data.i16bit[0];
- Ax=data.i16bit[1];
- Ax=data.i16bit[2];
- }
- //get Magnetic Raw data
- response = LSM303C_MAG_R_NewXYZData(0, (LSM303C_MAG_ZYXDA_t*) &flag_LSM303C_MAG_XYZDA);
- if(response==MEMS_ERROR) SlwTrace(DEBUG,"ERROR222"); //manage here comunication error
- //read only if new data are available
- else if ((LSM303C_MAG_ZYXDA_t) flag_LSM303C_MAG_XYZDA & LSM303C_MAG_ZYXDA_AVAILABLE)
- {
- response = LSM303C_MAG_Get_Magnetic(0, data.u8bit);
- //convert from LSB to Gauss
- gaussData_X=data.i16bit[0]*SENSITIVITY_MAG;
- gaussData_Y=data.i16bit[1]*SENSITIVITY_MAG;
- gaussData_Z=data.i16bit[2]*SENSITIVITY_MAG;
- Mx=data.i16bit[0];
- My=data.i16bit[1];
- Mz=data.i16bit[2];
- }
- printf("\r\n");
- printf("Ax=%10.3f\r\n",mgData_X);
- printf("Ay=%10.3f\r\n",mgData_Y);
- printf("Az=%10.3f\r\n",mgData_Z);
- printf("Mx=%10.3f\r\n",gaussData_X);
- printf("My=%10.3f\r\n",gaussData_Y);
- printf("Mz=%10.3f\r\n",gaussData_Z);
- printf("----\r\n");
-
- }
|