static int bmg160_read_gyro_xyz(struct bmg160_p *data, struct bmg160_v *gyro)
{
int ret = 0, i;
unsigned char temp[READ_DATA_LENTH];
for (i = 0; i < READ_DATA_LENTH; i++) {
ret += bmg160_i2c_read(data->client,
BMG160_RATE_X_LSB_VALUEX__REG + i, &temp[i]);
}
temp[0] = BMG160_GET_BITSLICE(temp[0], BMG160_RATE_X_LSB_VALUEX);
gyro->x = (short)((((short)((signed char)temp[1])) << 8) | (temp[0]));
temp[2] = BMG160_GET_BITSLICE(temp[2], BMG160_RATE_Y_LSB_VALUEY);
gyro->y = (short)((((short)((signed char)temp[3])) << 8) | (temp[2]));
temp[4] = BMG160_GET_BITSLICE(temp[4], BMG160_RATE_Z_LSB_VALUEZ);
gyro->z = (short)((((short)((signed char)temp[5])) << 8) | (temp[4]));
remap_sensor_data(gyro->v, data->chip_pos);
if (data->gyro_dps == BMG160_RANGE_250DPS) {
gyro->x = gyro->x >> 1;
gyro->y = gyro->y >> 1;
gyro->z = gyro->z >> 1;
} else if (data->gyro_dps == BMG160_RANGE_2000DPS) {
static int bmg160_get_bw(struct bmg160_p *data, unsigned char *bandwidth)
{
int ret;
unsigned char temp;
ret = bmg160_i2c_read(data->client, BMG160_BW_ADDR__REG, &temp);
*bandwidth = BMG160_GET_BITSLICE(temp, BMG160_BW_ADDR);
return ret;
}
static int bmg160_get_autosleepdur(struct bmg160_p *data,
unsigned char *duration)
{
int ret = 0;
unsigned char temp;
ret = bmg160_i2c_read(data->client,
BMG160_MODE_LPM2_ADDR_AUTOSLEEPDUR__REG, &temp);
*duration = BMG160_GET_BITSLICE(temp,
BMG160_MODE_LPM2_ADDR_AUTOSLEEPDUR);
return ret;
}
/*=========================================================================*/
void sns_dd_bmg160_interrupt_handler(
sns_ddf_handle_t dd_handle,
uint32_t gpio_num,
sns_ddf_time_t timestamp)
{
sns_ddf_status_e status;
sns_dd_bmg160_state_t *state = (sns_dd_bmg160_state_t *)dd_handle;
uint8_t int_stat1;
int in_irq;
uint8_t f_stat;
uint8_t f_count;
uint8_t f_overrun;
uint8_t int_stat_fifo = 0;
#ifdef BMG160_DEBUG
uint8_t int_stat_drdy = 0;
#endif // BMG160_DEBUG
uint8_t dri_mockup_wmi = 0;
uint8_t downsampling_factor = (state->odr_hw / state->odr_reported);
uint8_t si_buf_wr;
uint8_t tmp_r;
bool wmi_missing_possible = false;
BMG160_MSG_3_F(HIGH, "sns_dd_bmg160_interrupt_handler %d %d %d",
55555550, timestamp, gpio_num);
if (gpio_num != ((sns_dd_bmg160_state_t*)dd_handle)->gpio_num) {
return;
}
state->ts_irq = timestamp;
#if BMG160_CONFIG_DRI_MOCKUP_WMI
if ((state->en_fifo_int) && (1 == state->f_wml)) {
dri_mockup_wmi = 1;
}
#endif
if ((state->en_dri) || (dri_mockup_wmi)) {
//this is to reduce false drdy interrupts rate
int32_t lapse;
uint32_t false_irq = false;
lapse = ((int32_t)timestamp - (int32_t)state->ts_last_drdy);
BMG160_MSG_3(HIGH, "sns_dd_bmg160_interrupt_handler [anti_false_irq] <%d %d %d>",
lapse, state->itvl_drdy - lapse, state->itvl_drdy_tolerance);
if (lapse < state->itvl_drdy) {
if ((state->itvl_drdy - lapse) > state->itvl_drdy_tolerance) {
false_irq = true;
}
}
if (!false_irq) {
state->num_irq_drdy++;
if (state->sample_cnt_to_skip_odrc > 0) {
state->sample_cnt_to_skip_odrc--;
}
state->ts_last_drdy = timestamp;
} else {
BMG160_MSG_0(HIGH, "sns_dd_bmg160_interrupt_handler [anti_false_irq] false_irq detected");
}
if (0 == (state->num_irq_drdy % downsampling_factor)) {
if (dri_mockup_wmi) {
status = sns_ddf_smgr_notify_event(state->smgr_handle,
SNS_DDF_SENSOR_GYRO,
SNS_DDF_EVENT_FIFO_WM_INT);
}
sns_dd_bmg160_get_data_all(state);
state->f_frames_cache.samples[SDD_GYRO_X].sample = state->data_cache[SDD_GYRO_X];
state->f_frames_cache.samples[SDD_GYRO_Y].sample = state->data_cache[SDD_GYRO_Y];
state->f_frames_cache.samples[SDD_GYRO_Z].sample = state->data_cache[SDD_GYRO_Z];
if (0 >= state->sample_cnt_to_skip_odrc) {
state->f_frames_cache.samples[SDD_GYRO_X].status = SNS_DDF_SUCCESS;
state->f_frames_cache.samples[SDD_GYRO_Y].status = SNS_DDF_SUCCESS;
state->f_frames_cache.samples[SDD_GYRO_Z].status = SNS_DDF_SUCCESS;
state->f_frames_cache.status = SNS_DDF_SUCCESS;
} else {
state->f_frames_cache.samples[SDD_GYRO_X].status = SNS_DDF_EINVALID_DATA;
state->f_frames_cache.samples[SDD_GYRO_Y].status = SNS_DDF_EINVALID_DATA;
state->f_frames_cache.samples[SDD_GYRO_Z].status = SNS_DDF_EINVALID_DATA;
state->f_frames_cache.status = SNS_DDF_EINVALID_DATA;
BMG160_MSG_1(HIGH, "sns_dd_bmg160_interrupt_handler sample_cnt_to_skip_odrc: %d",
state->sample_cnt_to_skip_odrc);
}
state->f_frames_cache.sensor = SNS_DDF_SENSOR_GYRO;
/*! notice : the number samples value */
state->f_frames_cache.num_samples = SDD_GYRO_NUM_AXES;
sns_dd_bmg160_update_sample_ts(state, 1, 1 * downsampling_factor);
status = sns_ddf_smgr_notify_data(state->smgr_handle,
&state->f_frames_cache, 1);
if(state->f_frames_cache.status == SNS_DDF_SUCCESS) {
sns_dd_gyro_log_fifo(&state->f_frames_cache);
}
}
}
if (state->en_fifo_int && (!dri_mockup_wmi) && (state->f_wml > 0)) {
/* for FIFO, status1 does not need to be checked */
int_stat1 = 0;
if ((status = bmg160_get_interrupt_status_reg_1(&int_stat1)) != SNS_DDF_SUCCESS)
{
return;
}
#ifdef BMG160_DEBUG
int_stat_drdy = BMG160_GET_BITSLICE(int_stat1, BMG160_INT_STATUS1_DATA_INT);
#endif // BMG160_DEBUG
int_stat_fifo = BMG160_GET_BITSLICE(int_stat1, BMG160_INT_STATUS1_FIFO_INT);
f_stat = 0;
if ((status = bmg160_get_fifostatus_reg(&f_stat))
!= SNS_DDF_SUCCESS) {
return;
}
f_count = BMG160_GET_BITSLICE(f_stat,BMG160_FIFO_STATUS_FRAME_COUNTER);
f_overrun = BMG160_GET_BITSLICE(f_stat, BMG160_FIFO_STATUS_OVERRUN);
if (f_overrun) {
status = sns_ddf_smgr_notify_event(state->smgr_handle,
SNS_DDF_SENSOR_GYRO,
SNS_DDF_EVENT_FIFO_OVERFLOW);
BMG160_MSG_3_F(ERROR, "sns_dd_bmg160_interrupt_handler SNS_DDF_EVENT_FIFO_OVERFLOW %d %d %d",
55555551, 1, f_count);
sns_dd_bmg160_reset_fifo(state);
return;
}
#ifdef BMG160_DEBUG
BMG160_MSG_3_P(HIGH,
"sns_dd_bmg160_interrupt_handler <---1---> int_stat_fifo=%d int_stat_drdy=%d f_count=%d",
int_stat_fifo, int_stat_drdy, f_count);
#endif
in_irq = 1;
while (int_stat_fifo
|| (f_count >= state->f_wml)
) {
//this will solve the issue of potential timing inconsistancy that can happen during a boundary
//condition when interrupt has fired but fifo count register is locked from being updated.
if ((int_stat_fifo) && (f_count == (state->f_wml - 1))) {
f_count = state->f_wml;
#ifdef BMG160_DEBUG
BMG160_MSG_3_P(HIGH,
"sns_dd_bmg160_interrupt_handler <-----> int_stat_fifo= %d int_stat_drdy=%d f_count=%d",
int_stat_fifo, int_stat_drdy, f_count);
#endif
}
if (int_stat_fifo
|| (f_count >= state->f_wml)) {
status = sns_ddf_smgr_notify_event(state->smgr_handle,
SNS_DDF_SENSOR_GYRO,
SNS_DDF_EVENT_FIFO_WM_INT);
}
tmp_r = f_count % state->f_wml;
if (((tmp_r + 1) == state->f_wml) || (f_count >= (2 * state->f_wml))) {
wmi_missing_possible = true;
si_buf_wr = 0x00;
bmg160_sbus_write(state->port_handle,
BMG160_INT_MAP_1, &si_buf_wr, 1, NULL);
BMG160_MSG_3(ERROR, "bmg160_interrupt_handler wmi_missing_possible 1"
"timestamp: %d wml: %d f_count: %d",
timestamp, state->f_wml, f_count | (in_irq << 8));
}
f_count = f_count / state->f_wml * state->f_wml;
#if 0
if (f_count < 2 * state->f_wml) {
f_count = state->f_wml;
}
#endif
bmg160_fifo_data_read_out_frames(state, f_count, in_irq);
in_irq = 0;
if (state->f_frames_cache.num_samples > 0) {
status = sns_ddf_smgr_notify_data(state->smgr_handle,
&state->f_frames_cache, 1);
if(state->f_frames_cache.status == SNS_DDF_SUCCESS) {
sns_dd_gyro_log_fifo(&state->f_frames_cache);
}
}
//poll the status and frame counter again
int_stat1 = 0;
if ((status = bmg160_get_interrupt_status_reg_1(&int_stat1)) != SNS_DDF_SUCCESS)
{
return;
}
#ifdef BMG160_DEBUG
int_stat_drdy = BMG160_GET_BITSLICE(int_stat1, BMG160_INT_STATUS1_DATA_INT);
#endif // BMG160_DEBUG
int_stat_fifo = BMG160_GET_BITSLICE(int_stat1, BMG160_INT_STATUS1_FIFO_INT);
f_stat = 0;
if ((status = bmg160_get_fifostatus_reg(&f_stat))
!= SNS_DDF_SUCCESS) {
return;
}
f_count = BMG160_GET_BITSLICE(f_stat,BMG160_FIFO_STATUS_FRAME_COUNTER);
f_overrun = BMG160_GET_BITSLICE(f_stat, BMG160_FIFO_STATUS_OVERRUN);
#ifdef BMG160_DEBUG
BMG160_MSG_3_P(HIGH, "sns_dd_bmg160_interrupt_handler <---2---> int_stat_fifo= %d int_stat_drdy= %d f_count =%d",
int_stat_fifo, int_stat_drdy, f_count);
#endif //BMG160_DEBUG
}
}
if (wmi_missing_possible) {
bmg160_sbus_write(state->port_handle, BMG160_INT_MAP_1, &state->regv_int_map_1, 1, NULL);
}
return;
}
