1 /*
2 * Copyright (c) 2024 TDK Invensense
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "icm42670.h"
8 #include "imu/inv_imu_apex.h"
9
icm42670_apex_enable(inv_imu_device_t * s)10 int icm42670_apex_enable(inv_imu_device_t *s)
11 {
12 int err = 0;
13 inv_imu_apex_parameters_t apex_inputs;
14 inv_imu_interrupt_parameter_t config_int = {(inv_imu_interrupt_value)0};
15
16 /* Disabling FIFO to avoid extra power consumption due to ALP config */
17 err |= inv_imu_configure_fifo(s, INV_IMU_FIFO_DISABLED);
18
19 /* Enable Pedometer, Tilt and SMD interrupts */
20 config_int.INV_STEP_DET = INV_IMU_ENABLE;
21 config_int.INV_STEP_CNT_OVFL = INV_IMU_ENABLE;
22 config_int.INV_TILT_DET = INV_IMU_ENABLE;
23 config_int.INV_SMD = INV_IMU_ENABLE;
24 err |= inv_imu_set_config_int1(s, &config_int);
25
26 /* Enable accelerometer to feed the APEX Pedometer algorithm */
27 err |= inv_imu_set_accel_frequency(s, ACCEL_CONFIG0_ODR_50_HZ);
28
29 /* Set 2x averaging, in order to minimize power consumption (16x by default) */
30 err |= inv_imu_set_accel_lp_avg(s, ACCEL_CONFIG1_ACCEL_FILT_AVG_2);
31 err |= inv_imu_enable_accel_low_power_mode(s);
32
33 /* Get the default parameters for the APEX features */
34 err |= inv_imu_apex_init_parameters_struct(s, &apex_inputs);
35
36 /*
37 * Configure the power mode Normal mode.
38 * Avalaible mode : Low Power mode (WoM+Pedometer),
39 * configure the WoM to wake-up the DMP once it goes in power save mode
40 */
41 apex_inputs.power_save = APEX_CONFIG0_DMP_POWER_SAVE_DIS;
42 err |= inv_imu_apex_configure_parameters(s, &apex_inputs);
43
44 /* Configure sampling frequency to 50Hz */
45 err |= inv_imu_apex_set_frequency(s, APEX_CONFIG1_DMP_ODR_50Hz);
46
47 return err;
48 }
49
icm42670_apex_fetch_from_dmp(const struct device * dev)50 int icm42670_apex_fetch_from_dmp(const struct device *dev)
51 {
52 struct icm42670_data *data = dev->data;
53 int rc = 0;
54 uint8_t int_status2, int_status3;
55
56 /* Read APEX interrupt status */
57 rc |= inv_imu_read_reg(&data->driver, INT_STATUS2, 1, &int_status2);
58 rc |= inv_imu_read_reg(&data->driver, INT_STATUS3, 1, &int_status3);
59
60 /* Test Pedometer interrupt */
61 if (int_status3 & (INT_STATUS3_STEP_DET_INT_MASK)) {
62 inv_imu_apex_step_activity_t apex_pedometer;
63 uint8_t step_cnt_ovflw = 0;
64
65 if (int_status3 & INT_STATUS3_STEP_CNT_OVF_INT_MASK) {
66 step_cnt_ovflw = 1;
67 }
68
69 rc |= inv_imu_apex_get_data_activity(&data->driver, &apex_pedometer);
70
71 if (data->pedometer_cnt !=
72 apex_pedometer.step_cnt + step_cnt_ovflw * (uint64_t)UINT16_MAX) {
73 data->pedometer_cnt =
74 apex_pedometer.step_cnt + step_cnt_ovflw * (uint64_t)UINT16_MAX;
75 data->pedometer_activity = apex_pedometer.activity_class;
76 data->pedometer_cadence = apex_pedometer.step_cadence;
77 } else {
78 /* Pedometer data processing */
79 rc = 1;
80 }
81 }
82 /* Test Tilt interrupt */
83 if (int_status3 & (INT_STATUS3_TILT_DET_INT_MASK)) {
84 data->apex_status = ICM42670_APEX_STATUS_MASK_TILT;
85 }
86 /* Test SMD interrupt */
87 if ((int_status2 & (INT_STATUS2_SMD_INT_MASK)) || (rc != 0)) {
88 data->apex_status = ICM42670_APEX_STATUS_MASK_SMD;
89 }
90 /* Test WOM interrupts */
91 if (int_status2 & (INT_STATUS2_WOM_X_INT_MASK | INT_STATUS2_WOM_Y_INT_MASK |
92 INT_STATUS2_WOM_Z_INT_MASK)) {
93 data->apex_status = 0;
94 if (int_status2 & INT_STATUS2_WOM_X_INT_MASK) {
95 data->apex_status |= ICM42670_APEX_STATUS_MASK_WOM_X;
96 }
97 if (int_status2 & INT_STATUS2_WOM_Y_INT_MASK) {
98 data->apex_status |= ICM42670_APEX_STATUS_MASK_WOM_Y;
99 }
100 if (int_status2 & INT_STATUS2_WOM_Z_INT_MASK) {
101 data->apex_status |= ICM42670_APEX_STATUS_MASK_WOM_Z;
102 }
103 }
104 return rc;
105 }
106
icm42670_apex_pedometer_cadence_convert(struct sensor_value * val,uint8_t raw_val,uint8_t dmp_odr_hz)107 void icm42670_apex_pedometer_cadence_convert(struct sensor_value *val, uint8_t raw_val,
108 uint8_t dmp_odr_hz)
109 {
110 int64_t conv_val;
111
112 /* Converting u6.2 */
113 conv_val = (int64_t)(dmp_odr_hz << 2) * 1000000 / (raw_val + (raw_val & 0x03));
114 val->val1 = conv_val / 1000000;
115 val->val2 = conv_val % 1000000;
116 }
117
icm42670_apex_enable_pedometer(const struct device * dev,inv_imu_device_t * s)118 int icm42670_apex_enable_pedometer(const struct device *dev, inv_imu_device_t *s)
119 {
120 struct icm42670_data *data = dev->data;
121
122 data->dmp_odr_hz = 50;
123 /* Enable the pedometer */
124 return inv_imu_apex_enable_pedometer(s);
125 }
126
icm42670_apex_enable_tilt(inv_imu_device_t * s)127 int icm42670_apex_enable_tilt(inv_imu_device_t *s)
128 {
129 /* Enable Tilt */
130 return inv_imu_apex_enable_tilt(s);
131 }
132
icm42670_apex_enable_smd(inv_imu_device_t * s)133 int icm42670_apex_enable_smd(inv_imu_device_t *s)
134 {
135 int rc = 0;
136
137 /* Enable SMD (and Pedometer as SMD uses it) */
138 rc |= inv_imu_apex_enable_pedometer(s);
139 rc |= inv_imu_apex_enable_smd(s);
140
141 return rc;
142 }
143
icm42670_apex_enable_wom(inv_imu_device_t * s)144 int icm42670_apex_enable_wom(inv_imu_device_t *s)
145 {
146 int rc = 0;
147 inv_imu_interrupt_parameter_t config_int = {(inv_imu_interrupt_value)0};
148
149 /*
150 * Optimize power consumption:
151 * - Disable FIFO usage.
152 * - Disable data ready interrupt and enable WOM interrupts.
153 * - Set 2X averaging.
154 * - Use Low-Power mode at low frequency.
155 */
156 rc |= inv_imu_configure_fifo(s, INV_IMU_FIFO_DISABLED);
157
158 config_int.INV_WOM_X = INV_IMU_ENABLE;
159 config_int.INV_WOM_Y = INV_IMU_ENABLE;
160 config_int.INV_WOM_Z = INV_IMU_ENABLE;
161 rc |= inv_imu_set_config_int1(s, &config_int);
162
163 rc |= inv_imu_set_accel_lp_avg(s, ACCEL_CONFIG1_ACCEL_FILT_AVG_2);
164 rc |= inv_imu_set_accel_frequency(s, ACCEL_CONFIG0_ODR_12_5_HZ);
165 rc |= inv_imu_enable_accel_low_power_mode(s);
166
167 /*
168 * Configure WOM thresholds for each axis to 195 mg (Resolution 1g/256)
169 * WOM threshold = 50 * 1000 / 256 = 195 mg
170 * and enable WOM
171 */
172 rc |= inv_imu_configure_wom(s, 50, 50, 50, WOM_CONFIG_WOM_INT_MODE_ORED,
173 WOM_CONFIG_WOM_INT_DUR_1_SMPL);
174 rc |= inv_imu_enable_wom(s);
175
176 return rc;
177 }
178
