Merge branch '防撞' of http://47.108.70.204:60062/r/XRange_Tag into 防撞

yincheng.zhong

2023-03-29 0e91bca2aaebc14a6ea8ef97433d4a7953876ffa

 Drivers/ExternlDrivers/lis3dh_driver.c

@@ -48,6 +48,120 @@
//   }   
//   
//}




#define ARM_BIT_8               0

#if ARM_BIT_8
//如下数据类型是在8位机上定义的，在其它平台（比如32位）可能存在差别，需要根据实际情况修改 。 
typedef unsigned char    u8_m;                   /* 无符号8位整型变量*/
typedef signed   char    s8_m;                   /* 有符号8位整型变量*/
typedef unsigned int     u16_m;                  /* 无符号16位整型变量*/
typedef signed   int     s16_m;                  /* 有符号16位整型变量*/
typedef unsigned long    u32_m;                  /* 无符号32位整型变量*/
typedef signed   long    s32_m;                  /* 有符号32位整型变量*/
typedef float            fp32_m;                 /* 单精度浮点数（32位长度）*/
typedef double           fp64_m;                 /* 双精度浮点数（64位长度）*/
#else
//如下数据类型是在32位机上定义的，在其它平台（比如8位）可能存在差别，需要根据实际情况修改 。 
typedef unsigned char    u8_m;                   /* 无符号8位整型变量*/
typedef signed   char    s8_m;                   /* 有符号8位整型变量*/
typedef unsigned short   u16_m;                  /* 无符号16位整型变量*/
typedef signed   short   s16_m;                  /* 有符号16位整型变量*/
typedef unsigned int     u32_m;                  /* 无符号32位整型变量*/
typedef signed   int     s32_m;                  /* 有符号32位整型变量*/
typedef float            fp32_m;                 /* 单精度浮点数（32位长度）*/
typedef double           fp64_m;                 /* 双精度浮点数（64位长度）*/
#endif

/*******************************************************************************
Macro definitions - Register define for Gsensor
********************************************************************************/
#define REG_SPI_CONFIG              0x00
#define REG_CHIP_ID                 0x01
#define REG_ACC_X_LSB               0x02
#define REG_ACC_X_MSB               0x03
#define REG_ACC_Y_LSB               0x04
#define REG_ACC_Y_MSB               0x05
#define REG_ACC_Z_LSB               0x06
#define REG_ACC_Z_MSB               0x07
#define REG_FIFO_STATUS             0x08
#define REG_MOTION_FLAG             0x09
#define REG_NEWDATA_FLAG            0x0A
#define REG_TAP_ACTIVE_STATUS       0x0B
#define REG_ORIENT_STATUS           0x0C
#define REG_STEPS_MSB               0x0D
#define REG_STEPS_LSB               0x0E
#define REG_RESOLUTION_RANGE        0x0F
#define REG_ODR_AXIS                0x10
#define REG_MODE_BW                 0x11
#define REG_SWAP_POLARITY           0x12
#define REG_FIFO_CTRL               0x14
#define REG_INT_SET0                0x15
#define REG_INT_SET1                0x16
#define REG_INT_SET2                0x17
#define REG_INT_MAP1                0x19
#define REG_INT_MAP2                0x1a
#define REG_INT_MAP3                0x1b
#define REG_INT_CONFIG              0x20
#define REG_INT_LATCH               0x21
#define REG_FREEFALL_DUR            0x22
#define REG_FREEFALL_THS            0x23
#define REG_FREEFALL_HYST           0x24
#define REG_ACTIVE_DUR              0x27
#define REG_ACTIVE_THS              0x28
#define REG_TAP_DUR                 0x2A
#define REG_TAP_THS                 0x2B
#define REG_ORIENT_HYST             0x2C
#define REG_Z_BLOCK                 0x2D
#define REG_RESET_STEP            0x2E
#define REG_STEP_CONGIF1         0x2F
#define REG_STEP_CONGIF2         0x30
#define REG_STEP_CONGIF3         0x31
#define REG_STEP_CONGIF4         0x32
#define REG_STEP_FILTER             0x33
#define   REG_SM_THRESHOLD            0x34

#define DIFF_CALIBRATION 1


u8_m i2c_addr = 0x27;

s16_m offset_x=0, offset_y=0, offset_z=0;

u16_m f_step=0;


s8_m mir3da_register_read(u8_m ReadAddr, u8_m *data_m, u8_m Len)
{
		 				 
   mir3da_ReadOneByte(ReadAddr);
	
}

s8_m mir3da_register_write(u8_m ReadAddr, u8_m data_m)
{
	
   mir3da_WriteOneByte(ReadAddr,data_m);
	
}

s8_m mir3da_register_mask_write(u8_m ReadAddr, u8_m mask, u8_m data)
{
    int res=0;
    unsigned char tmp_data=0;
	
    res = mir3da_register_read(ReadAddr, &tmp_data, 1);
    tmp_data &= ~mask; 
    tmp_data |= data & mask;
    res |= mir3da_register_write(ReadAddr, tmp_data);
  
    return 0;
}


void delay_us(uint32_t nTimer)  
{  
    uint32_t i=0;  
@@ -68,6 +182,7 @@
{                    
   GPIO_InitTypeDef GPIO_InitStructure= {0};
 __HAL_RCC_GPIOA_CLK_ENABLE();
	
   GPIO_InitStructure.Pin = GPIO_PIN_2|GPIO_PIN_3;
   GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_OD ;   //推挽输出
   GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_HIGH;
@@ -75,13 +190,81 @@
   HAL_GPIO_WritePin(GPIOA,GPIO_PIN_2|GPIO_PIN_3,GPIO_PIN_SET);    //PB6,PB7 输出高
}

uint8_t lisid;
static uint8_t lisidtemp;

void Accelerometer_Init()    
{
      lisid = mir3da_ReadOneByte(REG_CHIP_ID); 
		
      if(lisid == 0x13)
      {
         mir3da_init();
      }
      else
      {
         LIS3DH_GetWHO_AM_I(&lisidtemp);  
         lisid=(uint16_t)lisidtemp;
			
         if(lisid == 0x33)
         {
            LIS3DH_Data_Init();
         }
         else
         {
            g_com_map[IMU_ENABLE]=0;
         }
      }
}

void mir3da_init()	
{
   u8_m data_m = 0;
   int i;
   int threshold = 0;
	
	
   //softreset
   mir3da_register_write(REG_SPI_CONFIG, 0x24);
   delay_us(20000);

   mir3da_register_write(REG_RESOLUTION_RANGE, 0x00);   //0x0F; +/-2G,14bit
   mir3da_register_write(REG_ODR_AXIS, 0x05);         //0x10; ODR=31.25hz(step counter minimum ODR)
   mir3da_register_write(REG_MODE_BW, 0x04);         //0x11; suspend mode, osr=1x, bw=1/10odr, autosleep disable

   //load_offset_from_filesystem(offset_x, offset_y, offset_z);   //pseudo-code
	
   //private register, just do it.
   mir3da_register_write(0x7F, 0x83);
   mir3da_register_write(0x7F, 0x69);
   mir3da_register_write(0x7F, 0xBD);

   //don't pull up sd0 pin
   if(i2c_addr == 0x26){
      mir3da_register_mask_write(0x8C, 0x40, 0x00);
   }

   mir3da_set_active_interrupt_enable(1);
	
   g_com_map[IMU_THRES]=0x14;
//   LIS3DH_WriteReg(LIS3DH_INT1_THS, (uint8_t)g_com_map[IMU_THRES]);     /* INT1_THS(32h): ?????? 0x10: 16*2(FS)  0x20: 32*16(FS) */
   mir3da_register_write(REG_ACTIVE_THS,(uint8_t)g_com_map[IMU_THRES]);
	
   delay_us(10); 
	
   mir3da_set_enable(1);
	
   mir3da_set_step_counter_open(1);

}


void LIS3DH_Data_Init()   
{
   //uint8_t buffer[26]; 
   
   uint8_t lisid,response;
    uint8_t lisidtemp;
   uint8_t response;
//  uint8_t lisidtemp;
   
   LIS3DH_WriteReg(LIS3DH_CTRL_REG1, 0x37);   /* CTRL_REG1(20h): ??sensor,???????? ODR 25HZ */
   LIS3DH_WriteReg(LIS3DH_CTRL_REG2, 0x03);   /* CTRL_REG2(21h): IA1?IA2 ?????? bc */
@@ -89,15 +272,19 @@
   LIS3DH_WriteReg(LIS3DH_CTRL_REG3, 0x40);   /* CTRL_REG3(22h): 0x80 ???????INT_1 INT_2 */
   LIS3DH_WriteReg(LIS3DH_CTRL_REG4, 0x08);  /* CTRL_REG4(23h): ???,????,???+/-2G,?????? */
   LIS3DH_WriteReg(LIS3DH_INT1_CFG, 0xaa);  /* INT1_CFG(30h): ??,6D X/Y/Z???????? */
   g_com_map[IMU_THRES]=2;
   g_com_map[IMU_THRES]=1;
   LIS3DH_WriteReg(LIS3DH_INT1_THS, (uint8_t)g_com_map[IMU_THRES]);     /* INT1_THS(32h): ?????? 0x10: 16*2(FS)  0x20: 32*16(FS) */
 LIS3DH_WriteReg(LIS3DH_INT1_DURATION, 0x01);     /* INT1_DURATION(33h): 1LSB=1/ODR  ??ODR=100HZ  ??1LSB=10ms ???? 1s,??100->0x64 */
   /* Start sensor */
   LIS3DH_WriteReg(0x20, 0x5f);  /* CTRL_REG1(20h): Start sensor at ODR 100Hz Low-power mode */
	
	
   // Read ID
   LIS3DH_GetWHO_AM_I(&lisidtemp);  
//   lisid=(uint16_t)lisidtemp;
//   printf("LIS3DH ID: %x \r\n",lisid);


	
   //get Acceleration Raw data  
//   response=LIS3DH_GetAccAxesRaw(&data);
   //print data values
@@ -118,8 +305,8 @@
{
   //uint8_t buffer[26]; 
   
   uint8_t lisid,response;
    uint8_t lisidtemp;
   uint8_t response;
  uint8_t lisidtemp;
   //set ODR (turn ON device)
   LIS3DH_SetODR(LIS3DH_ODR_10Hz);
   //set PowerMode 
@@ -153,15 +340,99 @@
//   }

}


//enable/disable the chip
void mir3da_set_enable(int enable)     //开启使能
{ 
   if(enable)
      mir3da_register_mask_write(REG_MODE_BW, 0x80, 0x00);   //power on
   else	
    mir3da_register_mask_write(REG_MODE_BW, 0x80, 0x80);
	
   delay_us(100000);
}

//Read three axis data, 2g range, counting 12bit, 1G=1000mg=1024lsb
void mir3da_read_raw_data(short *x, short *y, short *z)
{
    u8_m tmp_data[6] = {0};
   
   mir3da_register_read(REG_ACC_X_LSB, tmp_data, 6);

    *x = ((short)(tmp_data[1] << 8 | tmp_data[0]))>> 4;
    *y = ((short)(tmp_data[3] << 8 | tmp_data[2]))>> 4;
    *z = ((short)(tmp_data[5] << 8 | tmp_data[4]))>> 4;	
}

void mir3da_read_data(short *x, short *y, short *z)
{
    u8_m tmp_data[6] = {0};
   
   mir3da_register_read(REG_ACC_X_LSB, tmp_data, 6);

    *x = ((short)(tmp_data[1] << 8 | tmp_data[0])) >> 4 - offset_x;
    *y = ((short)(tmp_data[3] << 8 | tmp_data[2])) >> 4 - offset_y;
    *z = ((short)(tmp_data[5] << 8 | tmp_data[4])) >> 4 - offset_z;
}

//open active interrupt
void mir3da_set_active_interrupt_enable(int enable)
{
    if(enable){
      mir3da_register_write(REG_ACTIVE_DUR, 0x00);  //持续时间
//      mir3da_register_write(REG_ACTIVE_THS, 0x3);   //阈值
      mir3da_register_write(REG_INT_MAP1, 0x04 );
      mir3da_register_write(REG_INT_SET1, 0x87);		
   }
   else{
        mir3da_register_write(REG_INT_SET1,0x00 );
        mir3da_register_write(REG_INT_MAP1,0x00 );		
   }
}

void mir3da_set_step_counter_open(int enable)     //开启步数计数
{
   if(enable){
            mir3da_register_write(REG_STEP_CONGIF1, 0x01);
        mir3da_register_write(REG_STEP_CONGIF2, 0x62);
        mir3da_register_write(REG_STEP_CONGIF3, 0x46);
        mir3da_register_write(REG_STEP_CONGIF4, 0x32);
        mir3da_register_write(REG_STEP_FILTER, 0xa2);	
   }
   else{
        mir3da_register_write(REG_STEP_FILTER, 0x22);
   }
}

void mir3da_reset_step_counter(void)
{
   mir3da_register_mask_write(REG_RESET_STEP, 0x80, 0x80);	
}


u16_m mir3da_get_step()    //获得运动步数
{
   u8_m tmp_data[2] = {0};

   f_step = mir3da_ReadLenByte(REG_STEPS_MSB, 2);
	
   return f_step;
	
}

//获取数据

float  acc_g;

float drv_lis2dh12_get_angle(void)
{
   float acc_x, acc_y, acc_z, acc_g;
   float acc_x, acc_y, acc_z,acc_g;
   float angle_x, angle_y, angle_z, angle_xyz;
   int8_t data[6];
   uint8_t i;
   uint8_t lisid,response;
    uint8_t lisidtemp;
   uint8_t i; 
   uint8_t response;
  uint8_t lisidtemp;
   //set ODR (turn ON device)
   LIS3DH_SetODR(LIS3DH_ODR_100Hz);
   //set PowerMode 
@@ -172,7 +443,7 @@
   LIS3DH_SetAxis(LIS3DH_X_ENABLE | LIS3DH_Y_ENABLE | LIS3DH_Z_ENABLE);
   // Read ID
   LIS3DH_GetWHO_AM_I(&lisidtemp);
   lisid=(uint16_t)lisidtemp;
//   lisid=(uint16_t)lisidtemp;
//   printf("LIS3DH ID: %x \r\n",lisid);
   //get Acceleration Raw data  
   response=LIS3DH_GetAccAxesRaw(&lis2dhdata);
@@ -393,6 +664,75 @@
   return temp;                                        
}

//在mir3da里面的指定地址开始读一字节数据
//ReadAddr   :读出的地址 
//返回值     :读出的数据
uint8_t mir3da_ReadOneByte(uint16_t ReadAddr)
{
   uint8_t temp=0;		  	    																 
   IIC2_Start();  
   IIC2_Send_Byte(0X4e);        //发送写命令
   IIC2_Wait_Ack();       
   IIC2_Send_Byte(ReadAddr);   //发送读地址
   IIC2_Wait_Ack();	    
   IIC2_Start();  	 	   
   IIC2_Send_Byte(0X4f);       //发送读命令，进入接收模式		   
   IIC2_Wait_Ack();	 
   temp=IIC2_Read_Byte(0);		   
   IIC2_Stop();                        //产生一个停止条件	     
   return temp;
}
//在mir3da指定地址写入一个数据
//WriteAddr  :写入数据的目的地址    
//DataToWrite:要写入的数据
void mir3da_WriteOneByte(uint16_t WriteAddr,uint8_t DataToWrite)
{				   	  	    																 
   IIC2_Start();  
   IIC2_Send_Byte(0X4e);       //发送写命令
   IIC2_Wait_Ack();
   IIC2_Send_Byte(WriteAddr);               //发送地址
   IIC2_Wait_Ack();	   	 										  		   
   IIC2_Send_Byte(DataToWrite);     //发送字节							   
   IIC2_Wait_Ack();  		    	   
   IIC2_Stop();//产生一个停止条件 
   delay_us(10);	
			
}
//在AT24CXX里面的指定地址开始写入长度为Len的数据
//该函数用于写入16bit或者32bit的数据.
//WriteAddr  :开始写入的地址  
//DataToWrite:数据数组首地址
//Len        :要写入数据的长度2,4
void mir3da_WriteLenByte(uint16_t WriteAddr,uint32_t DataToWrite,uint8_t Len)
{  	
   uint8_t t;
   for(t=0;t<Len;t++)
   {
      mir3da_WriteOneByte(WriteAddr+t,(DataToWrite>>(8*t))&0xff);
   }												    
}

//在AT24CXX里面的指定地址开始读出长度为Len的数据
//该函数用于读出16bit或者32bit的数据.
//ReadAddr   :开始读出的地址 
//返回值     :数据
//Len        :要读出数据的长度2,4
uint32_t bu=0;
uint32_t mir3da_ReadLenByte(uint16_t ReadAddr,int Len)
{  	
   uint8_t t;
   u8_m data[Len];  
  memset(data, 0, Len);  
   for(t=0;t<Len;t++)
   { 	
         data[t] = mir3da_ReadOneByte(ReadAddr+t);
   }
   bu = (data[0] << 8 | data[1])/2;
	
   return bu;												    
}


/*******************************************************************************
* Function Name      : LIS3DH_ReadReg
* Description      : Generic Reading function. It must be fullfilled with either