diff --git a/drivers/sensor/lis2dw12/Kconfig b/drivers/sensor/lis2dw12/Kconfig index 70de8a03d9d..ba00201d09d 100644 --- a/drivers/sensor/lis2dw12/Kconfig +++ b/drivers/sensor/lis2dw12/Kconfig @@ -59,4 +59,16 @@ config LIS2DW12_TAP endif # LIS2DW12_TRIGGER +config LIS2DW12_THRESHOLD + bool "Wakeup threshold trigger (via interrupt)" + help + Enable the wakeup threshold trigger feature. + The wake-up interrupt signal is generated if a certain number of + consecutive data exceed the configured threshold (config in DT). + The threshold is applied to both positive and negative data: for + a wake-up interrupt generation at least one of the three axes must + be bigger than the threshold. See ST AN5038 for more details about + the feature. + + endif # LIS2DW12 diff --git a/drivers/sensor/lis2dw12/lis2dw12.c b/drivers/sensor/lis2dw12/lis2dw12.c index 4b65d938962..fc0a914622b 100644 --- a/drivers/sensor/lis2dw12/lis2dw12.c +++ b/drivers/sensor/lis2dw12/lis2dw12.c @@ -11,6 +11,7 @@ #define DT_DRV_COMPAT st_lis2dw12 #include +#include #include #include #include @@ -157,11 +158,102 @@ static int lis2dw12_config(const struct device *dev, enum sensor_channel chan, return -ENOTSUP; } + +static inline int32_t sensor_ms2_to_mg(const struct sensor_value *ms2) +{ + int64_t nano_ms2 = (ms2->val1 * 1000000LL + ms2->val2) * 1000LL; + + if (nano_ms2 > 0) { + return (nano_ms2 + SENSOR_G / 2) / SENSOR_G; + } else { + return (nano_ms2 - SENSOR_G / 2) / SENSOR_G; + } +} + +#if CONFIG_LIS2DW12_THRESHOLD + +/* Converts a lis2dw12_fs_t range to its value in milli-g + * Range can be 2/4/8/16G + */ +#define FS_RANGE_TO_MG(fs_range) ((2U << fs_range) * 1000U) + +/* Converts a range in mg to the lsb value for the WK_THS register + * For the reg value: 1 LSB = 1/64 of FS + * Range can be 2/4/8/16G + */ +#define MG_TO_WK_THS_LSB(range_mg) (range_mg / 64) + +/* Calculates the WK_THS reg value + * from the threshold in mg and the lsb value in mg + * with correct integer rounding + */ +#define THRESHOLD_MG_TO_WK_THS_REG(thr_mg, lsb_mg) \ + ((thr_mg + (lsb_mg / 2)) / lsb_mg) + +static int lis2dw12_attr_set_thresh(const struct device *dev, + enum sensor_channel chan, + enum sensor_attribute attr, + const struct sensor_value *val) +{ + uint8_t reg; + size_t ret; + int lsb_mg; + const struct lis2dw12_device_config *cfg = dev->config; + stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx; + + LOG_DBG("%s on channel %d", __func__, chan); + + /* can only be set for all directions at once */ + if (chan != SENSOR_CHAN_ACCEL_XYZ) { + return -EINVAL; + } + + /* Configure wakeup threshold threshold. */ + lis2dw12_fs_t range; + int err = lis2dw12_full_scale_get(ctx, &range); + + if (err) { + return err; + } + + uint32_t thr_mg = abs(sensor_ms2_to_mg(val)); + + /* Check maximum value: depends on current FS value */ + if (thr_mg >= FS_RANGE_TO_MG(range)) { + return -EINVAL; + } + + /* The threshold is applied to both positive and negative data: + * for a wake-up interrupt generation at least one of the three axes must be + * bigger than the threshold. + */ + lsb_mg = MG_TO_WK_THS_LSB(FS_RANGE_TO_MG(range)); + reg = THRESHOLD_MG_TO_WK_THS_REG(thr_mg, lsb_mg); + + LOG_DBG("Threshold %d mg -> fs: %u mg -> reg = %d LSBs", + thr_mg, FS_RANGE_TO_MG(range), reg); + ret = 0; + + return lis2dw12_wkup_threshold_set(ctx, reg); +} +#endif + static int lis2dw12_attr_set(const struct device *dev, enum sensor_channel chan, enum sensor_attribute attr, const struct sensor_value *val) { +#if CONFIG_LIS2DW12_THRESHOLD + switch (attr) { + case SENSOR_ATTR_UPPER_THRESH: + case SENSOR_ATTR_LOWER_THRESH: + return lis2dw12_attr_set_thresh(dev, chan, attr, val); + default: + /* Do nothing */ + break; + } +#endif + switch (chan) { case SENSOR_CHAN_ACCEL_X: case SENSOR_CHAN_ACCEL_Y: diff --git a/drivers/sensor/lis2dw12/lis2dw12.h b/drivers/sensor/lis2dw12/lis2dw12.h index d3839999dea..74f6b2a55db 100644 --- a/drivers/sensor/lis2dw12/lis2dw12.h +++ b/drivers/sensor/lis2dw12/lis2dw12.h @@ -101,6 +101,9 @@ struct lis2dw12_data { sensor_trigger_handler_t tap_handler; sensor_trigger_handler_t double_tap_handler; #endif /* CONFIG_LIS2DW12_TAP */ +#ifdef CONFIG_LIS2DW12_THRESHOLD + sensor_trigger_handler_t threshold_handler; +#endif /* CONFIG_LIS2DW12_THRESHOLD */ #if defined(CONFIG_LIS2DW12_TRIGGER_OWN_THREAD) K_KERNEL_STACK_MEMBER(thread_stack, CONFIG_LIS2DW12_THREAD_STACK_SIZE); struct k_thread thread; diff --git a/drivers/sensor/lis2dw12/lis2dw12_trigger.c b/drivers/sensor/lis2dw12/lis2dw12_trigger.c index 9b2824e1be5..87b780cacbf 100644 --- a/drivers/sensor/lis2dw12/lis2dw12_trigger.c +++ b/drivers/sensor/lis2dw12/lis2dw12_trigger.c @@ -67,6 +67,21 @@ static int lis2dw12_enable_int(const struct device *dev, return lis2dw12_pin_int1_route_set(ctx, &int_route.ctrl4_int1_pad_ctrl); #endif /* CONFIG_LIS2DW12_TAP */ +#ifdef CONFIG_LIS2DW12_THRESHOLD + /** + * Trigger fires when channel reading transitions configured + * thresholds. The thresholds are configured via the @ref + * SENSOR_ATTR_LOWER_THRESH and @ref SENSOR_ATTR_UPPER_THRESH + * attributes. + */ + case SENSOR_TRIG_THRESHOLD: + LOG_DBG("Setting int1_wu: %d\n", enable); + lis2dw12_pin_int1_route_get(ctx, + &int_route.ctrl4_int1_pad_ctrl); + int_route.ctrl4_int1_pad_ctrl.int1_wu = enable; + return lis2dw12_pin_int1_route_set(ctx, + &int_route.ctrl4_int1_pad_ctrl); +#endif default: LOG_ERR("Unsupported trigger interrupt route %d", type); return -ENOTSUP; @@ -121,6 +136,14 @@ int lis2dw12_trigger_set(const struct device *dev, lis2dw12->double_tap_handler = handler; return lis2dw12_enable_int(dev, SENSOR_TRIG_DOUBLE_TAP, state); #endif /* CONFIG_LIS2DW12_TAP */ +#ifdef CONFIG_LIS2DW12_THRESHOLD + case SENSOR_TRIG_THRESHOLD: + { + LOG_DBG("Set trigger %d (handler: %p)\n", trig->type, handler); + lis2dw12->threshold_handler = handler; + return lis2dw12_enable_int(dev, SENSOR_TRIG_THRESHOLD, state); + } +#endif default: LOG_ERR("Unsupported sensor trigger"); return -ENOTSUP; @@ -179,6 +202,25 @@ static int lis2dw12_handle_double_tap_int(const struct device *dev) } #endif /* CONFIG_LIS2DW12_TAP */ +#ifdef CONFIG_LIS2DW12_THRESHOLD +static int lis2dw12_handle_wu_ia_int(const struct device *dev) +{ + struct lis2dw12_data *lis2dw12 = dev->data; + sensor_trigger_handler_t handler = lis2dw12->threshold_handler; + + struct sensor_trigger thresh_trig = { + .type = SENSOR_TRIG_THRESHOLD, + .chan = SENSOR_CHAN_ALL, + }; + + if (handler) { + handler(dev, &thresh_trig); + } + + return 0; +} +#endif + /** * lis2dw12_handle_interrupt - handle the drdy event * read data and call handler if registered any @@ -202,6 +244,11 @@ static void lis2dw12_handle_interrupt(const struct device *dev) lis2dw12_handle_double_tap_int(dev); } #endif /* CONFIG_LIS2DW12_TAP */ +#ifdef CONFIG_LIS2DW12_THRESHOLD + if (sources.all_int_src.wu_ia) { + lis2dw12_handle_wu_ia_int(dev); + } +#endif gpio_pin_interrupt_configure_dt(&cfg->gpio_int, GPIO_INT_EDGE_TO_ACTIVE);