347 lines
9.7 KiB
C
347 lines
9.7 KiB
C
#include "gSensor/SC7A20.h"
|
||
#include "gSensor/gSensor_manage.h"
|
||
#include "app_config.h"
|
||
|
||
#include "gSensor/SC7A20.h"
|
||
#include "math.h"
|
||
|
||
|
||
#if TCFG_SC7A20_EN
|
||
|
||
u8 volatile sl_click_timer_en = 0;
|
||
|
||
|
||
#define SL_Sensor_Algo_Release_Enable 1
|
||
|
||
|
||
u8 Click_time = 0X00;
|
||
u8 Click_add_flag = 0X00;
|
||
|
||
|
||
u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf)
|
||
{
|
||
return _gravity_sensor_get_ndata(addr, reg, buf, len);
|
||
}
|
||
|
||
u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data)
|
||
{
|
||
gravity_sensor_command(addr, reg, data);
|
||
return 0;
|
||
}
|
||
|
||
char SC7A20_Check()
|
||
{
|
||
u8 reg_value = 0;
|
||
u8 i;
|
||
SL_MEMS_i2cRead(SC7A20_R_ADDR, SC7A20_WHO_AM_I, 1, ®_value);
|
||
if (reg_value == 0x11) {
|
||
return 0x01;
|
||
} else {
|
||
return 0x00;
|
||
}
|
||
}
|
||
|
||
u8 SC7A20_Config(void)
|
||
{
|
||
u8 Check_Flag = 0;
|
||
u8 write_num = 0, i = 0;
|
||
|
||
u8 ODR = 0x7f; //400HZ/6f 200HZ
|
||
u8 HP = 0x0c; //开启高通滤波
|
||
u8 click_int = 0x82; //将Click中断映射到INT1 OVERRUN
|
||
u8 range = 0x90; //4g量程
|
||
u8 fifo_en = 0x40; //使能fifo
|
||
u8 fifo_mode = 0x9d; //Stream模式，0-29(WTM30个)
|
||
u8 click_mode = 0x15; //单击3轴触发
|
||
|
||
Check_Flag = SC7A20_Check();
|
||
if (Check_Flag == 1) {
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_CTRL_REG1, ODR);
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_CTRL_REG2, HP);
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_CTRL_REG3, click_int);// click int1
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_CTRL_REG4, range);
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_CTRL_REG5, fifo_en);
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_FIFO_CTRL_REG, fifo_mode);
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_CLICK_CFG, click_mode);//单Z轴
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_CLICK_THS, SC7A20_CLICK_TH);//62.6mg(4g)*10
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_TIME_LIMIT, SC7A20_CLICK_WINDOWS);
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_TIME_LATENCY, SC7A20_CLICK_LATENCY);
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_CTRL_REG6, SC7A20_INT_LEVEL);
|
||
printf("check ok\n");
|
||
return 0;
|
||
} else {
|
||
printf("check fail\n");
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
|
||
#if SL_Sensor_Algo_Release_Enable==0
|
||
extern signed short SL_DEBUG_DATA[10][128];
|
||
extern u8 SL_DEBUG_DATA_LEN;
|
||
#endif
|
||
|
||
u8 sl_pp_num = 0;
|
||
|
||
u8 SL_Get_CLICK_PP_CNT(u8 fun_flag)
|
||
{
|
||
if (fun_flag == 0) {
|
||
sl_pp_num = 0;
|
||
}
|
||
return sl_pp_num;
|
||
}
|
||
|
||
unsigned int SL_Click_Sqrt(unsigned int sqrt_data)
|
||
{
|
||
unsigned int SL_SQRT_LOW, SL_SQRT_UP, SL_SQRT_MID;
|
||
u8 sl_sqrt_num = 0;
|
||
|
||
SL_SQRT_LOW = 0;
|
||
SL_SQRT_UP = sqrt_data;
|
||
SL_SQRT_MID = (SL_SQRT_UP + SL_SQRT_LOW) / 2;
|
||
while (sl_sqrt_num < 200) {
|
||
if (SL_SQRT_MID * SL_SQRT_MID > sqrt_data) {
|
||
SL_SQRT_UP = SL_SQRT_MID;
|
||
} else {
|
||
SL_SQRT_LOW = SL_SQRT_MID;
|
||
}
|
||
if (SL_SQRT_UP - SL_SQRT_LOW == 1) {
|
||
if (SL_SQRT_UP * SL_SQRT_UP - sqrt_data > sqrt_data - SL_SQRT_LOW * SL_SQRT_LOW) {
|
||
return SL_SQRT_LOW;
|
||
} else {
|
||
return SL_SQRT_UP;
|
||
}
|
||
}
|
||
SL_SQRT_MID = (SL_SQRT_UP + SL_SQRT_LOW) / 2;
|
||
sl_sqrt_num++;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
char SC7A20_Click_Read(int TH1, int TH2)
|
||
{
|
||
u8 i = 0, j = 0, k = 0;
|
||
u8 click_num = 0;
|
||
u8 fifo_len;
|
||
unsigned int sc7a20_data = 0;
|
||
unsigned int fifo_data_xyz[32];
|
||
u8 click_result = 0x00;
|
||
unsigned int click_sum = 0;
|
||
u8 data1[6];
|
||
signed char data[6];
|
||
|
||
g_printf("INT_HAPPEN!\r\n");
|
||
|
||
SL_MEMS_i2cRead(SC7A20_R_ADDR, SC7A20_SRC_REG, 1, &fifo_len);
|
||
if ((fifo_len & 0x40) == 0x40) {
|
||
fifo_len = 32;
|
||
} else {
|
||
fifo_len = fifo_len & 0x1f;
|
||
}
|
||
|
||
for (i = 0; i < fifo_len; i++) {
|
||
SL_MEMS_i2cRead(SC7A20_R_ADDR, 0xA8, 6, &data1[0]);
|
||
data[1] = (signed char)data1[1];
|
||
data[3] = (signed char)data1[3];
|
||
data[5] = (signed char)data1[5];
|
||
sc7a20_data = (data[1]) * (data[1]) + (data[3]) * (data[3]) + (data[5]) * (data[5]);
|
||
sc7a20_data = SL_Click_Sqrt(sc7a20_data);
|
||
fifo_data_xyz[i] = sc7a20_data;
|
||
|
||
#if SL_Sensor_Algo_Release_Enable==0
|
||
SL_DEBUG_DATA[0][i] = data[1];
|
||
SL_DEBUG_DATA[1][i] = data[3];
|
||
SL_DEBUG_DATA[2][i] = data[5];
|
||
SL_DEBUG_DATA[3][i] = fifo_data_xyz[i];
|
||
SL_DEBUG_DATA_LEN = fifo_len;
|
||
#endif
|
||
}
|
||
|
||
k = 0;
|
||
for (i = 1; i < fifo_len - 1; i++) {
|
||
if ((fifo_data_xyz[i + 1] > TH1) && (fifo_data_xyz[i - 1] < 30)) {
|
||
g_printf("in_th\r\n");
|
||
if (click_num == 0) {
|
||
click_sum = 0; //first peak
|
||
for (j = 0; j < i - 1; j++) {
|
||
if (fifo_data_xyz[j] > fifo_data_xyz[j + 1]) {
|
||
click_sum += fifo_data_xyz[j] - fifo_data_xyz[j + 1];
|
||
} else {
|
||
click_sum += fifo_data_xyz[j + 1] - fifo_data_xyz[j];
|
||
}
|
||
}
|
||
#if SL_Sensor_Algo_Release_Enable==0
|
||
g_printf("click_sum:%d!\r\n", click_sum);
|
||
#endif
|
||
if (click_sum > TH2) {
|
||
sl_pp_num++;
|
||
break;
|
||
}
|
||
k = i;
|
||
} else {
|
||
k = i; //sencond peak
|
||
}
|
||
}
|
||
|
||
if (k != 0) {
|
||
if (fifo_data_xyz[i - 1] - fifo_data_xyz[i + 1] > TH1 - 10) {
|
||
if (i - k < 5) {
|
||
click_num = 1;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (click_num == 1) {
|
||
click_result = 1;
|
||
} else {
|
||
click_result = 0;
|
||
}
|
||
|
||
return click_result;
|
||
}
|
||
|
||
//GPIO?????,??INT2
|
||
//Service function in Int Function
|
||
//u8 sl_click_timer_en =0;
|
||
u8 sl_click_status = 0;
|
||
unsigned short click_timer_cnt = 0;
|
||
unsigned short click_timer_total_cnt = 0;
|
||
u8 click_click_final_cnt = 0;
|
||
|
||
char SC7A20_Click_Alog(void)
|
||
{
|
||
u8 click_status = 0;
|
||
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_CTRL_REG3, 0x00);
|
||
click_status = SC7A20_Click_Read(SC7A20_TH1, SC7A20_TH2); //40,50
|
||
|
||
g_printf("click_status = 0x%x\n", click_status);
|
||
if (click_status == 1) {
|
||
if (sl_click_timer_en == 0) {
|
||
//set click timer flag
|
||
sl_click_timer_en = 1;
|
||
//clear click timer cnt value
|
||
click_timer_cnt = 0;
|
||
click_timer_total_cnt = 0;
|
||
click_click_final_cnt = 0;
|
||
}
|
||
sl_click_status = 1;
|
||
}
|
||
SL_MEMS_i2cWrite(SC7A20_W_ADDR, SC7A20_CTRL_REG3, 0x82);
|
||
return click_status;
|
||
}
|
||
|
||
//This fuction is execute in 50ms timer when the timer is opened
|
||
char SC7A20_click_status(void)
|
||
{
|
||
u8 click_e_cnt = 0;
|
||
if (sl_click_timer_en == 1) {
|
||
click_timer_cnt++;
|
||
if ((click_timer_cnt < click_pp_num) && (sl_click_status == 1)) {
|
||
g_printf("status:%d\r\n", sl_click_status);
|
||
g_printf("fin_num:%d\r\n", click_click_final_cnt);
|
||
sl_click_status = 0;
|
||
click_timer_total_cnt = click_timer_total_cnt + click_timer_cnt;
|
||
click_timer_cnt = 0;
|
||
click_click_final_cnt++;
|
||
}
|
||
|
||
click_e_cnt = SL_Get_CLICK_PP_CNT(1);
|
||
|
||
if ((((click_timer_cnt >= click_pp_num) || (click_timer_total_cnt >= click_max_num)) && (click_e_cnt < 1)) ||
|
||
((click_timer_cnt >= click_pp_num) && (click_e_cnt > 0)))
|
||
// if((click_timer_cnt>=click_pp_num)||(click_timer_total_cnt>=click_max_num))
|
||
{
|
||
//clear click timer flag
|
||
sl_click_timer_en = 0;
|
||
//clear click timer cnt value
|
||
click_timer_cnt = 0;
|
||
click_timer_total_cnt = 0;
|
||
g_printf("fin_num:%d\r\n", click_click_final_cnt);
|
||
if (click_e_cnt > 0) {
|
||
click_e_cnt = SL_Get_CLICK_PP_CNT(0);
|
||
return 0;
|
||
} else {
|
||
return click_click_final_cnt;
|
||
}
|
||
}
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
void SC7A20_int_io_detect(u8 int_io_status)
|
||
{
|
||
static u8 int_io_status_old;
|
||
/* u8 int_io_status = 0; */
|
||
/* int_io_status = gpio_read(INT_IO); */
|
||
if ((int_io_status) && (int_io_status_old == 0)) {
|
||
log_e("sc7a20_int_io_detect\n");
|
||
SC7A20_Click_Alog();
|
||
} else {
|
||
}
|
||
int_io_status_old = int_io_status;
|
||
}
|
||
|
||
void SC7A20_read_data(axis_info_t *sl_accel)
|
||
{
|
||
u8 data[6];
|
||
u8 fifo_len = 0;
|
||
SL_MEMS_i2cRead(SC7A20_R_ADDR, SC7A20_SRC_REG, 1, &fifo_len);
|
||
if ((fifo_len & 0x40) == 0x40) {
|
||
fifo_len = 32;
|
||
} else {
|
||
fifo_len = fifo_len & 0x1f;
|
||
}
|
||
for (u8 i = 0; i < fifo_len; i++) {
|
||
SL_MEMS_i2cRead(SC7A20_R_ADDR, 0xA8, 6, data);
|
||
sl_accel[i].x = (short)((data[1] << 8) | data[0]);
|
||
sl_accel[i].x = sl_accel[i].x >> 4;
|
||
sl_accel[i].y = (short)((data[3] << 8) | data[2]);
|
||
sl_accel[i].y = sl_accel[i].y >> 4;
|
||
sl_accel[i].z = (short)((data[5] << 8) | data[4]);
|
||
sl_accel[i].z = sl_accel[i].z >> 4;
|
||
/* printf("group:%2d,sl_accel_x:%5d, sl_accel_y:%5d, sl_accel_z:%5d\n", i, sl_accel[i].x, sl_accel[i].y, sl_accel[i].z); */
|
||
}
|
||
}
|
||
void SC7A20_ctl(u8 cmd, void *arg)
|
||
{
|
||
switch (cmd) {
|
||
case GSENSOR_RESET_INT:
|
||
break;
|
||
case GSENSOR_RESUME_INT:
|
||
break;
|
||
case GSENSOR_INT_DET:
|
||
SC7A20_int_io_detect(*(u8 *)arg);
|
||
break;
|
||
case READ_GSENSOR_DATA:
|
||
SC7A20_read_data((axis_info_t *)arg);
|
||
break;
|
||
default:
|
||
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
static u8 sc7a20_idle_query(void)
|
||
{
|
||
return !sl_click_timer_en;
|
||
}
|
||
|
||
REGISTER_GRAVITY_SENSOR(gSensor) = {
|
||
.logo = "sc7a20",
|
||
.gravity_sensor_init = SC7A20_Config,
|
||
.gravity_sensor_check = SC7A20_click_status,
|
||
.gravity_sensor_ctl = SC7A20_ctl,
|
||
};
|
||
|
||
REGISTER_LP_TARGET(sc7a20_lp_target) = {
|
||
.name = "sc7a20",
|
||
.is_idle = sc7a20_idle_query,
|
||
};
|
||
|
||
|
||
#endif
|
||
|