355 lines
13 KiB
C
355 lines
13 KiB
C
/*
|
|
*
|
|
* Copyright (c) [2018] by InvenSense, Inc.
|
|
*
|
|
* Permission to use, copy, modify, and/or distribute this software for any
|
|
* purpose with or without fee is hereby granted.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
|
|
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
|
|
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
|
|
* SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
|
|
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
|
|
* OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
|
|
* CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
|
|
*
|
|
*/
|
|
|
|
#include "imu/inv_imu_apex.h"
|
|
#include "imu/inv_imu_extfunc.h"
|
|
|
|
int inv_imu_apex_enable_ff(inv_imu_device_t *s)
|
|
{
|
|
int status = 0;
|
|
uint8_t value;
|
|
|
|
status |= inv_imu_start_dmp(s);
|
|
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
|
value &= ~APEX_CONFIG1_FF_ENABLE_MASK;
|
|
value |= (uint8_t)APEX_CONFIG1_FF_ENABLE_EN;
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_disable_ff(inv_imu_device_t *s)
|
|
{
|
|
int status = 0;
|
|
uint8_t value;
|
|
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
|
value &= ~APEX_CONFIG1_FF_ENABLE_MASK;
|
|
value |= (uint8_t)APEX_CONFIG1_FF_ENABLE_DIS;
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_enable_smd(inv_imu_device_t *s)
|
|
{
|
|
int status = 0;
|
|
uint8_t value;
|
|
|
|
status |= inv_imu_start_dmp(s);
|
|
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
|
value &= ~APEX_CONFIG1_SMD_ENABLE_MASK;
|
|
value |= (uint8_t)APEX_CONFIG1_SMD_ENABLE_EN;
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_disable_smd(inv_imu_device_t *s)
|
|
{
|
|
int status = 0;
|
|
uint8_t value;
|
|
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
|
value &= ~APEX_CONFIG1_SMD_ENABLE_MASK;
|
|
value |= (uint8_t)APEX_CONFIG1_SMD_ENABLE_DIS;
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_init_parameters_struct(inv_imu_device_t *s, inv_imu_apex_parameters_t *apex_inputs)
|
|
{
|
|
int status = 0;
|
|
(void)s;
|
|
|
|
/* Default parameters at POR */
|
|
apex_inputs->pedo_amp_th = APEX_CONFIG3_PEDO_AMP_TH_62_MG;
|
|
apex_inputs->pedo_step_cnt_th = 0x5;
|
|
apex_inputs->pedo_step_det_th = 0x2;
|
|
apex_inputs->pedo_sb_timer_th = APEX_CONFIG4_PEDO_SB_TIMER_TH_150_SAMPLES;
|
|
apex_inputs->pedo_hi_enrgy_th = APEX_CONFIG4_PEDO_HI_ENRGY_TH_104_MG;
|
|
apex_inputs->tilt_wait_time = APEX_CONFIG5_TILT_WAIT_TIME_4_S;
|
|
apex_inputs->power_save_time = APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_8_S;
|
|
apex_inputs->power_save = APEX_CONFIG0_DMP_POWER_SAVE_EN;
|
|
apex_inputs->sensitivity_mode = APEX_CONFIG9_SENSITIVITY_MODE_NORMAL;
|
|
apex_inputs->low_energy_amp_th = APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_80_MG;
|
|
apex_inputs->smd_sensitivity = APEX_CONFIG9_SMD_SENSITIVITY_0;
|
|
apex_inputs->ff_debounce_duration = APEX_CONFIG9_FF_DEBOUNCE_DURATION_2000_MS;
|
|
apex_inputs->ff_max_duration_cm = APEX_CONFIG12_FF_MAX_DURATION_204_CM;
|
|
apex_inputs->ff_min_duration_cm = APEX_CONFIG12_FF_MIN_DURATION_10_CM;
|
|
apex_inputs->lowg_peak_th = APEX_CONFIG10_LOWG_PEAK_TH_563_MG;
|
|
apex_inputs->lowg_peak_hyst = APEX_CONFIG5_LOWG_PEAK_TH_HYST_156_MG;
|
|
apex_inputs->lowg_samples_th = APEX_CONFIG10_LOWG_TIME_TH_1_SAMPLE;
|
|
apex_inputs->highg_peak_th = APEX_CONFIG11_HIGHG_PEAK_TH_2500_MG;
|
|
apex_inputs->highg_peak_hyst = APEX_CONFIG5_HIGHG_PEAK_TH_HYST_156_MG;
|
|
apex_inputs->highg_samples_th = APEX_CONFIG11_HIGHG_TIME_TH_1_SAMPLE;
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_configure_parameters(inv_imu_device_t * s,
|
|
const inv_imu_apex_parameters_t *apex_inputs)
|
|
{
|
|
int status = 0;
|
|
uint8_t data;
|
|
uint8_t apexConfig[7];
|
|
APEX_CONFIG1_PED_ENABLE_t pedo_state;
|
|
APEX_CONFIG1_TILT_ENABLE_t tilt_state;
|
|
APEX_CONFIG1_FF_ENABLE_t ff_state;
|
|
APEX_CONFIG1_SMD_ENABLE_t smd_state;
|
|
|
|
/* DMP cannot be configured if it is running, hence make sure all APEX algorithms are off */
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &data);
|
|
pedo_state = (APEX_CONFIG1_PED_ENABLE_t)(data & APEX_CONFIG1_PED_ENABLE_MASK);
|
|
tilt_state = (APEX_CONFIG1_TILT_ENABLE_t)(data & APEX_CONFIG1_TILT_ENABLE_MASK);
|
|
ff_state = (APEX_CONFIG1_FF_ENABLE_t)(data & APEX_CONFIG1_FF_ENABLE_MASK);
|
|
smd_state = (APEX_CONFIG1_SMD_ENABLE_t)(data & APEX_CONFIG1_SMD_ENABLE_MASK);
|
|
if (pedo_state == APEX_CONFIG1_PED_ENABLE_EN)
|
|
return INV_ERROR;
|
|
if (tilt_state == APEX_CONFIG1_TILT_ENABLE_EN)
|
|
return INV_ERROR;
|
|
if (ff_state == APEX_CONFIG1_FF_ENABLE_EN)
|
|
return INV_ERROR;
|
|
if (smd_state == APEX_CONFIG1_SMD_ENABLE_EN)
|
|
return INV_ERROR;
|
|
|
|
status |= inv_imu_switch_on_mclk(s);
|
|
|
|
/* Power Save mode and low energy amplitude threshold (for Pedometer in Slow Walk mode) */
|
|
/* APEX_CONFIG2_MREG1 */
|
|
apexConfig[0] = (uint8_t)apex_inputs->power_save_time | (uint8_t)apex_inputs->low_energy_amp_th;
|
|
|
|
/* Pedometer parameters */
|
|
/* APEX_CONFIG3_MREG1 */
|
|
apexConfig[1] = (uint8_t)apex_inputs->pedo_amp_th |
|
|
(apex_inputs->pedo_step_cnt_th & APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK);
|
|
|
|
/* APEX_CONFIG4_MREG1 */
|
|
apexConfig[2] = ((apex_inputs->pedo_step_det_th << APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS) &
|
|
APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK) |
|
|
(uint8_t)apex_inputs->pedo_sb_timer_th | (uint8_t)apex_inputs->pedo_hi_enrgy_th;
|
|
|
|
/* Tilt, Lowg and highg parameters */
|
|
/* APEX_CONFIG5_MREG1 */
|
|
apexConfig[3] = (uint8_t)apex_inputs->tilt_wait_time | (uint8_t)apex_inputs->lowg_peak_hyst |
|
|
(uint8_t)apex_inputs->highg_peak_hyst;
|
|
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG2_MREG1, 4, &apexConfig[0]);
|
|
|
|
/* APEX_CONFIG0 */
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &apexConfig[0]);
|
|
apexConfig[0] &= ~APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK;
|
|
apexConfig[0] |= apex_inputs->power_save;
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG0, 1, &apexConfig[0]);
|
|
|
|
/* free fall parameter, SMD parameter and parameters for Pedometer in Slow Walk mode */
|
|
/* APEX_CONFIG9_MREG1 */
|
|
apexConfig[0] = (uint8_t)apex_inputs->ff_debounce_duration |
|
|
(uint8_t)apex_inputs->smd_sensitivity | (uint8_t)apex_inputs->sensitivity_mode;
|
|
|
|
/* Lowg and highg parameters and free fall parameters */
|
|
/* APEX_CONFIG10_MREG1 */
|
|
apexConfig[1] = (uint8_t)apex_inputs->lowg_peak_th | (uint8_t)apex_inputs->lowg_samples_th;
|
|
|
|
/* APEX_CONFIG11_MREG1 */
|
|
apexConfig[2] = (uint8_t)apex_inputs->highg_peak_th | (uint8_t)apex_inputs->highg_samples_th;
|
|
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG9_MREG1, 3, &apexConfig[0]);
|
|
|
|
/* APEX_CONFIG12_MREG1 */
|
|
apexConfig[0] =
|
|
(uint8_t)apex_inputs->ff_max_duration_cm | (uint8_t)apex_inputs->ff_min_duration_cm;
|
|
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG12_MREG1, 1, &apexConfig[0]);
|
|
|
|
status |= inv_imu_switch_off_mclk(s);
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_get_parameters(inv_imu_device_t *s, inv_imu_apex_parameters_t *apex_params)
|
|
{
|
|
int status = 0;
|
|
uint8_t data[7];
|
|
uint8_t value;
|
|
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &value);
|
|
apex_params->power_save =
|
|
(APEX_CONFIG0_DMP_POWER_SAVE_t)(value & APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK);
|
|
|
|
/* Access continuous config registers (CONFIG2-CONFIG11) */
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG2_MREG1, sizeof(data), &data[0]);
|
|
|
|
/* Get params from apex_config2 : dmp_power_save_time and low_energy_amp_th */
|
|
apex_params->power_save_time =
|
|
(APEX_CONFIG2_DMP_POWER_SAVE_TIME_t)(data[0] & APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_MASK);
|
|
apex_params->low_energy_amp_th =
|
|
(APEX_CONFIG2_LOW_ENERGY_AMP_TH_t)(data[0] & APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_MASK);
|
|
|
|
/* Get params from apex_config3 : pedo_amp_th and pedo_step_cnt_th */
|
|
apex_params->pedo_amp_th =
|
|
(APEX_CONFIG3_PEDO_AMP_TH_t)(data[1] & APEX_CONFIG3_PED_AMP_TH_SEL_MASK);
|
|
apex_params->pedo_step_cnt_th =
|
|
(data[1] & APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK) >> APEX_CONFIG3_PED_STEP_CNT_TH_SEL_POS;
|
|
|
|
/* Get params from apex_config4 : pedo_step_det_th, pedo_sb_timer_th and pedo_hi_enrgy_th */
|
|
apex_params->pedo_step_det_th =
|
|
(data[2] & APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK) >> APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS;
|
|
apex_params->pedo_sb_timer_th =
|
|
(APEX_CONFIG4_PEDO_SB_TIMER_TH_t)(data[2] & APEX_CONFIG4_PED_SB_TIMER_TH_SEL_MASK);
|
|
apex_params->pedo_hi_enrgy_th =
|
|
(APEX_CONFIG4_PEDO_HI_ENRGY_TH_t)(data[2] & APEX_CONFIG4_PED_HI_EN_TH_SEL_MASK);
|
|
|
|
/* Get params from apex_config5 : tilt_wait_time, lowg_peak_hyst and highg_peak_hyst */
|
|
apex_params->tilt_wait_time =
|
|
(APEX_CONFIG5_TILT_WAIT_TIME_t)(data[3] & APEX_CONFIG5_TILT_WAIT_TIME_SEL_MASK);
|
|
apex_params->lowg_peak_hyst =
|
|
(APEX_CONFIG5_LOWG_PEAK_TH_HYST_t)(data[3] & APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_MASK);
|
|
apex_params->highg_peak_hyst =
|
|
(APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t)(data[3] & APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_MASK);
|
|
|
|
/* Get params from apex_config9 : ff_debounce_duration, smd_sensitivity and sensitivity_mode */
|
|
apex_params->ff_debounce_duration =
|
|
(APEX_CONFIG9_FF_DEBOUNCE_DURATION_t)(data[4] & APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_MASK);
|
|
apex_params->smd_sensitivity =
|
|
(APEX_CONFIG9_SMD_SENSITIVITY_t)(data[4] & APEX_CONFIG9_SMD_SENSITIVITY_SEL_MASK);
|
|
apex_params->sensitivity_mode =
|
|
(APEX_CONFIG9_SENSITIVITY_MODE_t)(data[4] & APEX_CONFIG9_SENSITIVITY_MODE_MASK);
|
|
|
|
/* Get params from apex_config10 : lowg_peak_th and lowg_samples_th */
|
|
apex_params->lowg_peak_th =
|
|
(APEX_CONFIG10_LOWG_PEAK_TH_t)(data[5] & APEX_CONFIG10_LOWG_PEAK_TH_SEL_MASK);
|
|
apex_params->lowg_samples_th =
|
|
(APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t)(data[5] & APEX_CONFIG10_LOWG_TIME_TH_SEL_MASK);
|
|
|
|
/* Get params from apex_config11 : highg_peak_th and highg_samples_th */
|
|
apex_params->highg_peak_th =
|
|
(APEX_CONFIG11_HIGHG_PEAK_TH_t)(data[6] & APEX_CONFIG11_HIGHG_PEAK_TH_SEL_MASK);
|
|
apex_params->highg_samples_th =
|
|
(APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t)(data[6] & APEX_CONFIG11_HIGHG_TIME_TH_SEL_MASK);
|
|
|
|
/* Access apex reg 12 */
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG12_MREG1, 1, &data[0]);
|
|
|
|
/* Get params from apex_config12 : ff_max_duration_cm and ff_min_duration_cm */
|
|
apex_params->ff_max_duration_cm =
|
|
(APEX_CONFIG12_FF_MAX_DURATION_t)(data[0] & APEX_CONFIG12_FF_MAX_DURATION_SEL_MASK);
|
|
apex_params->ff_min_duration_cm =
|
|
(APEX_CONFIG12_FF_MIN_DURATION_t)(data[0] & APEX_CONFIG12_FF_MIN_DURATION_SEL_MASK);
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_set_frequency(inv_imu_device_t *s, const APEX_CONFIG1_DMP_ODR_t frequency)
|
|
{
|
|
uint8_t value;
|
|
int status = 0;
|
|
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
|
|
|
value &= ~APEX_CONFIG1_DMP_ODR_MASK;
|
|
value |= frequency;
|
|
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_enable_pedometer(inv_imu_device_t *s)
|
|
{
|
|
int status = 0;
|
|
uint8_t value;
|
|
|
|
status |= inv_imu_start_dmp(s);
|
|
|
|
/* Enable Pedometer */
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
|
value &= ~APEX_CONFIG1_PED_ENABLE_MASK;
|
|
value |= (uint8_t)APEX_CONFIG1_PED_ENABLE_EN;
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_disable_pedometer(inv_imu_device_t *s)
|
|
{
|
|
int status = 0;
|
|
uint8_t value;
|
|
|
|
/* Disable Pedometer */
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
|
value &= ~APEX_CONFIG1_PED_ENABLE_MASK;
|
|
value |= (uint8_t)APEX_CONFIG1_PED_ENABLE_DIS;
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_enable_tilt(inv_imu_device_t *s)
|
|
{
|
|
int status = 0;
|
|
uint8_t value;
|
|
|
|
status |= inv_imu_start_dmp(s);
|
|
|
|
/* Enable Tilt */
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
|
value &= ~APEX_CONFIG1_TILT_ENABLE_MASK;
|
|
value |= (uint8_t)APEX_CONFIG1_TILT_ENABLE_EN;
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_disable_tilt(inv_imu_device_t *s)
|
|
{
|
|
int status = 0;
|
|
uint8_t value;
|
|
|
|
/* Disable Tilt */
|
|
status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
|
|
value &= ~APEX_CONFIG1_TILT_ENABLE_MASK;
|
|
value |= (uint8_t)APEX_CONFIG1_TILT_ENABLE_DIS;
|
|
status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_get_data_activity(inv_imu_device_t *s, inv_imu_apex_step_activity_t *apex_activity)
|
|
{
|
|
uint8_t data[4];
|
|
int status = inv_imu_read_reg(s, APEX_DATA0, 4, data);
|
|
|
|
apex_activity->step_cnt = (uint16_t)((data[1] << 8) | data[0]);
|
|
apex_activity->step_cadence = data[2];
|
|
apex_activity->activity_class = data[3] & APEX_DATA3_ACTIVITY_CLASS_MASK;
|
|
|
|
return status;
|
|
}
|
|
|
|
int inv_imu_apex_get_data_free_fall(inv_imu_device_t *s, uint16_t *freefall_duration)
|
|
{
|
|
uint8_t data[2];
|
|
int status = inv_imu_read_reg(s, APEX_DATA4, 2, &data[0]);
|
|
|
|
*freefall_duration = (uint16_t)((data[1] << 8) | data[0]);
|
|
|
|
return status;
|
|
}
|