/*************************************************************************************************
**Filename:	lsm6d.c
**Version :			
**Author  :				
**Date    :	 				
**Description:	
**
*************************************************************************************************/
#include "lsm6d.h"
#include "gpio.h"
#include "i2c.h"
#include "delay.h"
#include "mcu_bsp.h"
#include "DebugLog.h"
/*********************************************************************
 * Global Variables (declared in header file with 'extern')
 */
stmdev_ctx_t dev_ctx; 
tFilter_Type tFilter;
float Angle;
PAS_SENSOR_t PAS;
/*********************************************************************
 * Static Variables 
 */
static int16_t data_raw_acc[3];
static int16_t data_raw_ang[3];
static int16_t data_acc_offset[3];
static int16_t data_ang_offset[3];

/*********************************************************************
 * Static Functions ('static')
 */
static void platform0_delay(uint16_t ms);
static int32_t platform0_write(uint8_t I2C_addr, uint8_t addr, uint8_t* value, uint16_t len);
static int32_t platform0_read(uint8_t I2C_addr,uint8_t regAddress, uint8_t * data, uint16_t len);
static void Filter_Complement(float angle_m_cf,float gyro_m_cf);
static void Cadence_Stop_Judge(int16_t theta);

/*************************************************************************************************
 * Function Name:
 * Description  : lsm6dso 初始化
 * Arguments    : 
 * Return Value : 
*************************************************************************************************/
void Lsm6dso_Init(void)
{
  uint8_t temp = 0;
    
  dev_ctx.write_reg = platform0_write;  
  dev_ctx.read_reg = platform0_read;
  dev_ctx.mdelay = platform0_delay;
  dev_ctx.master_addr = LSM6DSO_ADDRESS;
  
  PEN_ENABLE(); //使能lsm6d供电
  
  platform0_delay(100);
   
  //获取设置Id
  lsm6dso_device_id_get(&dev_ctx, &temp);
  
  if(temp == LSM6DSO_ID)
  {
    PAS.exist = 1;
  }
  else
  {
    PAS.exist = 0;
  }
  
  //软件复位, 寄存器到默认配值
  lsm6dso_reset_set(&dev_ctx, PROPERTY_ENABLE);
  
  do{
     lsm6dso_reset_get(&dev_ctx, &temp);
  }while(temp);
  
  //禁止I3C接口
  lsm6dso_i3c_disable_set(&dev_ctx, LSM6DSO_I3C_DISABLE);
  
  //使能BDU
  lsm6dso_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
  
  //设置数据输出速率
  lsm6dso_gy_data_rate_set(&dev_ctx, LSM6DSO_GY_ODR_12Hz5);
  
  //设置满量程范围
  lsm6dso_gy_full_scale_set(&dev_ctx, LSM6DSO_1000dps);
    
  //使能角速度计的滤波功能
  lsm6dso_gy_lp1_bandwidth_set(&dev_ctx,LSM6DSO_MEDIUM);
  lsm6dso_gy_filter_lp1_set(&dev_ctx,PROPERTY_ENABLE);
}

/*************************************************************************************************
 * Function Name:
* Description  : 角度计算 注意:10ms中断计算一次
 * Arguments    : 
 * Return Value : 
*************************************************************************************************/
void Lsm6dso_RPM_Cal(void)
{
  uint8_t reg;
  static float gyro_z;
  
  lsm6dso_gy_flag_data_ready_get(&dev_ctx, &reg);
  if(reg)
  {
    lsm6dso_angular_rate_raw_get(&dev_ctx, data_raw_ang);
    gyro_z = data_raw_ang[2];
  }
 
  gyro_z = (gyro_z - data_ang_offset[2]) + 32768;
  gyro_z = (32768 - gyro_z)/32.8;  //得到的角速度 单位dps(degrees per second)
    
  Angle = Angle + gyro_z*0.1f*182.04;
  PAS.angle = Angle;
  
  Cadence_Stop_Judge(PAS.angle);

  if(PAS.actFlag == 1)
  {
    PAS.RPM = (gyro_z*60.0f/360);
  }
  else
  {
    PAS.RPM = 0;
  }
}

/*************************************************************************************************
 * Function Name:
 * Description  : lsm6dso 失能
 * Arguments    : 
 * Return Value : 
*************************************************************************************************/
void Lsm6dso_Disable(void)
{
  lsm6dso_reset_set(&dev_ctx, PROPERTY_ENABLE);
}

/*************************************************************************************************
 * Function Name:
 * Description  : 寄存器写
 * Arguments    : 
 * Return Value : 
*************************************************************************************************/
static void Cadence_Stop_Judge(int16_t theta)
{
  static uint8_t cntTimes = 0;
  static int16_t Last_theta = 0,temp;
  static uint8_t ActFlag = 0;
  
  cntTimes++;
  
  if(cntTimes >= 6)
  {
    cntTimes = 0;
    
    temp = theta - Last_theta;
    
    if(ABS(temp) > 3200) 
    {
      PAS.actFlag = 1;
    }
    else
    {
      PAS.actFlag = 0;
    }
    
    Last_theta =  theta;   
  }
} 
/*************************************************************************************************
 * Function Name:
 * Description  : 寄存器写
 * Arguments    : 
 * Return Value : 
*************************************************************************************************/
static int32_t platform0_write(uint8_t I2C_addr, uint8_t addr, uint8_t* value, uint16_t len)
{	
  ErrorStatus bret = SUCCESS;

	bret = i2c_0_write(I2C_addr, 1, addr, value, len);
    
  return (int32_t)(!bret);
}
/*************************************************************************************************
 * Function Name:
 * Description  : 寄存器读
 * Arguments    : 
 * Return Value : 
*************************************************************************************************/
static int32_t platform0_read(uint8_t I2C_addr,uint8_t regAddress, uint8_t * data, uint16_t len)
{
	ErrorStatus bret = SUCCESS;
  
	bret = i2c_0_read(I2C_addr, 1, regAddress, data, len);
  
	return (int32_t)(!bret);
}
/*************************************************************************************************
 * Function Name:
 * Description  : 延时
 * Arguments    : 
 * Return Value : 
*************************************************************************************************/
static void platform0_delay(uint16_t ms)
{
  delay_ms(ms);
}
/*************************************************************************************************/