Skip to content

Commit

Permalink
feat(sensors): debounce ec11 encoder.
Browse files Browse the repository at this point in the history
  • Loading branch information
root committed Nov 19, 2024
1 parent 2ded791 commit bdcac73
Show file tree
Hide file tree
Showing 5 changed files with 137 additions and 219 deletions.
1 change: 0 additions & 1 deletion app/module/drivers/sensor/ec11/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -6,4 +6,3 @@ zephyr_include_directories(.)
zephyr_library()

zephyr_library_sources(ec11.c)
zephyr_library_sources_ifdef(CONFIG_EC11_TRIGGER ec11_trigger.c)
168 changes: 117 additions & 51 deletions app/module/drivers/sensor/ec11/ec11.c
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,6 @@
#include <zephyr/drivers/gpio.h>
#include <zephyr/sys/util.h>
#include <zephyr/kernel.h>
#include <zephyr/drivers/sensor.h>
#include <zephyr/sys/__assert.h>
#include <zephyr/logging/log.h>

#include "ec11.h"
Expand All @@ -20,57 +18,42 @@

LOG_MODULE_REGISTER(EC11, CONFIG_SENSOR_LOG_LEVEL);

static int ec11_get_ab_state(const struct device *dev) {
const struct ec11_config *drv_cfg = dev->config;

return (gpio_pin_get_dt(&drv_cfg->a) << 1) | gpio_pin_get_dt(&drv_cfg->b);
}

static int ec11_sample_fetch(const struct device *dev, enum sensor_channel chan) {
struct ec11_data *drv_data = dev->data;
const struct ec11_config *drv_cfg = dev->config;
uint8_t val;
int8_t delta;

__ASSERT_NO_MSG(chan == SENSOR_CHAN_ALL || chan == SENSOR_CHAN_ROTATION);

val = ec11_get_ab_state(dev);

LOG_DBG("prev: %d, new: %d", drv_data->ab_state, val);

switch (val | (drv_data->ab_state << 2)) {
case 0b0010:
case 0b0100:
case 0b1101:
case 0b1011:
delta = -1;
break;
case 0b0001:
case 0b0111:
case 0b1110:
case 0b1000:
delta = 1;
break;
default:
delta = 0;
break;
static void ec11_apply_reading(struct ec11_data *drv_data, uint8_t a, uint8_t b) {
if (a == drv_data->prev_a && b == drv_data->prev_b) {
LOG_DBG("no state change");
return;
}

LOG_DBG("Delta: %d", delta);
bool bwd1 = (drv_data->prev_b != a);
bool bwd2 = (drv_data->prev_a == b);
int8_t delta = (bwd1 == bwd2) ? (bwd1 ? -1 : +1) : 0; // delta == 0 implies missing states
LOG_DBG("state %u%u -> %u%u delta:%d", drv_data->prev_a, drv_data->prev_b, a, b, delta);

drv_data->pulses += delta;
drv_data->ab_state = val;
drv_data->prev_a = a;
drv_data->prev_b = b;

// TODO: Temporary code for backwards compatibility to support
// the sensor channel rotation reporting *ticks* instead of delta of degrees.
// REMOVE ME
const struct ec11_config *drv_cfg = drv_data->dev->config;
if (drv_cfg->steps == 0) {
drv_data->ticks = drv_data->pulses / drv_cfg->resolution;
drv_data->delta = delta;
drv_data->pulses %= drv_cfg->resolution;
}

return 0;
#ifdef CONFIG_EC11_TRIGGER
// TODO: CONFIG_EC11_TRIGGER_OWN_THREAD, CONFIG_EC11_TRIGGER_GLOBAL_THREAD
// XXX: zmk_sensors_trigger_handler() already uses a work queue?
if (delta != 0 && drv_data->handler) {
drv_data->handler(drv_data->dev, drv_data->trigger);
}
#endif
}

static int ec11_sample_fetch(const struct device *dev, enum sensor_channel chan) {
return 0; // nothing to do; driven by interrupts & timer
}

static int ec11_channel_get(const struct device *dev, enum sensor_channel chan,
Expand Down Expand Up @@ -100,6 +83,16 @@ static int ec11_channel_get(const struct device *dev, enum sensor_channel chan,
return 0;
}

#ifdef CONFIG_EC11_TRIGGER
static int ec11_trigger_set(const struct device *dev, const struct sensor_trigger *trig,
sensor_trigger_handler_t handler) {
struct ec11_data *drv_data = dev->data;
drv_data->trigger = trig;
drv_data->handler = handler;
return 0;
}
#endif

static const struct sensor_driver_api ec11_driver_api = {
#ifdef CONFIG_EC11_TRIGGER
.trigger_set = ec11_trigger_set,
Expand All @@ -108,42 +101,113 @@ static const struct sensor_driver_api ec11_driver_api = {
.channel_get = ec11_channel_get,
};

static void ec11_period(struct k_timer *timer_id) {
struct ec11_data *drv_data = CONTAINER_OF(timer_id, struct ec11_data, debouncer);
const struct ec11_config *drv_cfg = drv_data->dev->config;

uint32_t samples_needed = drv_cfg->debounce_ms / drv_cfg->debounce_scan_period_ms;
samples_needed += !!(drv_cfg->debounce_ms % drv_cfg->debounce_scan_period_ms); // round up

// add a single reading to the moving window
drv_data->hist_a = (drv_data->hist_a << 1) | gpio_pin_get_dt(&drv_cfg->a);
drv_data->hist_b = (drv_data->hist_b << 1) | gpio_pin_get_dt(&drv_cfg->b);
if (drv_data->samples < samples_needed) {
drv_data->samples++;
}

// histogram from the window
uint32_t as = drv_data->hist_a;
uint32_t bs = drv_data->hist_b;
uint8_t cnts[4] = {0, 0, 0, 0};
for (uint8_t i = 0; i < drv_data->samples; i++) {
cnts[((as & 1) << 1) | (bs & 1)]++;
as >>= 1;
bs >>= 1;
}
LOG_DBG("histogram 00:%u 01:%u 10:%u 11:%u", cnts[0], cnts[1], cnts[2], cnts[3]);

// check if any state has reached the threshold
for (uint8_t ab = 0; ab < 4; ab++) {
if (cnts[ab] >= samples_needed / 2 + 1) { // more than half
ec11_apply_reading(drv_data, ab >> 1, ab & 1);
if (cnts[ab] == samples_needed) { // stable for a full window
LOG_DBG("timer stop");
drv_data->samples = 0;
drv_data->running = false;
k_timer_stop(&drv_data->debouncer);
}
break;
}
}
}

static void ec11_interrupt_cb_comon(struct ec11_data *drv_data) {
if (!drv_data->running) {
LOG_DBG("timer start");
drv_data->running = true;
const struct ec11_config *drv_cfg = drv_data->dev->config;
int32_t ms = drv_cfg->debounce_scan_period_ms;
k_timer_start(&drv_data->debouncer, K_MSEC(ms), K_MSEC(ms));
}
}

static void ec11_cb_a(const struct device *port, struct gpio_callback *cb, uint32_t pins) {
ec11_interrupt_cb_comon(CONTAINER_OF(cb, struct ec11_data, a_gpio_cb));
}

static void ec11_cb_b(const struct device *port, struct gpio_callback *cb, uint32_t pins) {
ec11_interrupt_cb_comon(CONTAINER_OF(cb, struct ec11_data, b_gpio_cb));
}

int ec11_init(const struct device *dev) {
struct ec11_data *drv_data = dev->data;
const struct ec11_config *drv_cfg = dev->config;

LOG_DBG("A: %s %d B: %s %d resolution %d", drv_cfg->a.port->name, drv_cfg->a.pin,
drv_cfg->b.port->name, drv_cfg->b.pin, drv_cfg->resolution);

if (!device_is_ready(drv_cfg->a.port)) {
LOG_ERR("A GPIO device is not ready");
return -EINVAL;
}

if (!device_is_ready(drv_cfg->b.port)) {
LOG_ERR("B GPIO device is not ready");
return -EINVAL;
}

if (gpio_pin_configure_dt(&drv_cfg->a, GPIO_INPUT)) {
LOG_DBG("Failed to configure A pin");
LOG_ERR("Failed to configure A pin");
return -EIO;
}

if (gpio_pin_configure_dt(&drv_cfg->b, GPIO_INPUT)) {
LOG_DBG("Failed to configure B pin");
LOG_ERR("Failed to configure B pin");
return -EIO;
}

#ifdef CONFIG_EC11_TRIGGER
if (ec11_init_interrupt(dev) < 0) {
LOG_DBG("Failed to initialize interrupt!");
struct ec11_data *drv_data = dev->data; // already zero-initialized
drv_data->dev = dev;
drv_data->prev_a = gpio_pin_get_dt(&drv_cfg->a);
drv_data->prev_b = gpio_pin_get_dt(&drv_cfg->b);

// enable interrupts
gpio_init_callback(&drv_data->a_gpio_cb, ec11_cb_a, BIT(drv_cfg->a.pin));
if (gpio_add_callback(drv_cfg->a.port, &drv_data->a_gpio_cb) < 0) {
LOG_ERR("Failed to set A callback!");
return -EIO;
}
gpio_init_callback(&drv_data->b_gpio_cb, ec11_cb_b, BIT(drv_cfg->b.pin));
if (gpio_add_callback(drv_cfg->b.port, &drv_data->b_gpio_cb) < 0) {
LOG_ERR("Failed to set B callback!");
return -EIO;
}
if (gpio_pin_interrupt_configure_dt(&drv_cfg->a, GPIO_INT_EDGE_BOTH)) {
LOG_ERR("Unable to set A pin GPIO interrupt");
return -EIO;
}
if (gpio_pin_interrupt_configure_dt(&drv_cfg->b, GPIO_INT_EDGE_BOTH)) {
LOG_ERR("Unable to set B pin GPIO interrupt");
return -EIO;
}
#endif

drv_data->ab_state = ec11_get_ab_state(dev);

k_timer_init(&drv_data->debouncer, ec11_period, NULL);
return 0;
}

Expand All @@ -154,6 +218,8 @@ int ec11_init(const struct device *dev) {
.b = GPIO_DT_SPEC_INST_GET(n, b_gpios), \
.resolution = DT_INST_PROP_OR(n, resolution, 1), \
.steps = DT_INST_PROP_OR(n, steps, 0), \
.debounce_ms = DT_INST_PROP_OR(n, debounce_ms, 5), \
.debounce_scan_period_ms = DT_INST_PROP_OR(n, debounce_scan_period_ms, 1), \
}; \
DEVICE_DT_INST_DEFINE(n, ec11_init, NULL, &ec11_data_##n, &ec11_cfg_##n, POST_KERNEL, \
CONFIG_SENSOR_INIT_PRIORITY, &ec11_driver_api);
Expand Down
33 changes: 12 additions & 21 deletions app/module/drivers/sensor/ec11/ec11.h
Original file line number Diff line number Diff line change
Expand Up @@ -6,47 +6,38 @@

#pragma once

#include <zephyr/device.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/sys/util.h>
#include <zephyr/drivers/sensor.h>

struct ec11_config {
const struct gpio_dt_spec a;
const struct gpio_dt_spec b;

const uint16_t steps;
const uint8_t resolution;
int32_t debounce_ms;
int32_t debounce_scan_period_ms;
};

struct ec11_data {
uint8_t ab_state;
int8_t pulses;
int8_t ticks;
int8_t delta;

#ifdef CONFIG_EC11_TRIGGER
struct gpio_callback a_gpio_cb;
struct gpio_callback b_gpio_cb;
const struct device *dev;

#ifdef CONFIG_EC11_TRIGGER
sensor_trigger_handler_t handler;
const struct sensor_trigger *trigger;

#if defined(CONFIG_EC11_TRIGGER_OWN_THREAD)
K_THREAD_STACK_MEMBER(thread_stack, CONFIG_EC11_THREAD_STACK_SIZE);
struct k_sem gpio_sem;
struct k_thread thread;
#elif defined(CONFIG_EC11_TRIGGER_GLOBAL_THREAD)
struct k_work work;
#endif

#endif /* CONFIG_EC11_TRIGGER */
};

#ifdef CONFIG_EC11_TRIGGER

int ec11_trigger_set(const struct device *dev, const struct sensor_trigger *trig,
sensor_trigger_handler_t handler);

int ec11_init_interrupt(const struct device *dev);
#endif
bool running; // timer running?
uint8_t prev_a; // previous state (debounced)
uint8_t prev_b;
uint8_t samples; // the window size
uint32_t hist_a; // the moving window
uint32_t hist_b;
struct k_timer debouncer;
};
Loading

0 comments on commit bdcac73

Please sign in to comment.