TKK_E32221292/Firmware ESP32/main/speed_sensor.c

#include "speed_sensor.h"

#include <stdbool.h>
#include <stddef.h>
#include <string.h>

#include "esp_attr.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"

static const char *TAG = "SPEED_SENSOR";

static gpio_num_t s_pin = GPIO_NUM_NC;
static portMUX_TYPE s_lock = portMUX_INITIALIZER_UNLOCKED;

static volatile uint32_t s_pulse_total = 0;
static volatile int64_t s_last_valid_pulse_us = 0;
static volatile uint32_t s_dt_history_us[5] = {0};
static volatile uint8_t s_dt_history_count = 0;
static volatile uint8_t s_dt_history_index = 0;

static const float WHEEL_CIRCUMFERENCE_M = 0.229336f;
static const float DISTANCE_PER_PULSE_KM = 0.000114668f;
static const int64_t PULSE_MIN_VALID_US = 15000;      // reject implausible pulses (> ~27.5 km/h with 7.3 cm wheel)
static const int64_t SPEED_ZERO_TIMEOUT_US = 2200000; // 2.2s timeout for slow manual rotation

static inline void clear_history_unsafe(void)
{
    memset((void *)s_dt_history_us, 0, sizeof(s_dt_history_us));
    s_dt_history_count = 0;
    s_dt_history_index = 0;
    s_last_valid_pulse_us = 0;
}

static uint32_t median_dt_us_unsafe(void)
{
    if (s_dt_history_count == 0) {
        return 0;
    }

    uint32_t temp[5] = {0};
    for (uint8_t i = 0; i < s_dt_history_count; i++) {
        temp[i] = s_dt_history_us[i];
    }

    for (uint8_t i = 1; i < s_dt_history_count; i++) {
        uint32_t key = temp[i];
        int8_t j = (int8_t)i - 1;
        while (j >= 0 && temp[j] > key) {
            temp[j + 1] = temp[j];
            j--;
        }
        temp[j + 1] = key;
    }

    return temp[s_dt_history_count / 2];
}

static void IRAM_ATTR speed_sensor_isr_handler(void *arg)
{
    (void)arg;
    const int64_t now_us = esp_timer_get_time();

    portENTER_CRITICAL_ISR(&s_lock);

    if (s_last_valid_pulse_us != 0) {
        const int64_t delta_us = now_us - s_last_valid_pulse_us;
        if (delta_us <= 0 || delta_us < PULSE_MIN_VALID_US) {
            portEXIT_CRITICAL_ISR(&s_lock);
            return;
        }

        if (s_dt_history_count >= 2) {
            const uint32_t recent_median_us = median_dt_us_unsafe();
            if (recent_median_us > 0) {
                const uint32_t lower_bound_us = (recent_median_us * 50U) / 100U;
                if ((uint32_t)delta_us < lower_bound_us) {
                    portEXIT_CRITICAL_ISR(&s_lock);
                    return;
                }
            }
        }

        s_dt_history_us[s_dt_history_index] = (uint32_t)delta_us;
        s_dt_history_index = (uint8_t)((s_dt_history_index + 1U) % 5U);
        if (s_dt_history_count < 5U) {
            s_dt_history_count++;
        }
    }

    s_last_valid_pulse_us = now_us;
    s_pulse_total++;
    portEXIT_CRITICAL_ISR(&s_lock);
}

esp_err_t speed_sensor_init(gpio_num_t pin)
{
    s_pin = pin;

    portENTER_CRITICAL(&s_lock);
    s_pulse_total = 0;
    clear_history_unsafe();
    portEXIT_CRITICAL(&s_lock);

    const bool internal_pull_supported = !(pin == GPIO_NUM_34 || pin == GPIO_NUM_35 || pin == GPIO_NUM_36 || pin == GPIO_NUM_39);

    const gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << pin),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = internal_pull_supported ? GPIO_PULLUP_ENABLE : GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_NEGEDGE,
    };

    esp_err_t err = gpio_config(&io_conf);
    if (err != ESP_OK) {
        return err;
    }

    err = gpio_install_isr_service(0);
    if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
        return err;
    }

    err = gpio_isr_handler_add(pin, speed_sensor_isr_handler, NULL);
    if (err != ESP_OK) {
        return err;
    }

    ESP_LOGI(TAG, "Speed sensor ready on GPIO %d (internal pull-up: %s)", pin, internal_pull_supported ? "on" : "off");
    return ESP_OK;
}

uint32_t speed_sensor_get_pulse_total(void)
{
    portENTER_CRITICAL(&s_lock);
    const uint32_t total = s_pulse_total;
    portEXIT_CRITICAL(&s_lock);
    return total;
}

uint32_t speed_sensor_get_last_dt_ms(void)
{
    uint32_t median_us = 0;
    portENTER_CRITICAL(&s_lock);
    median_us = median_dt_us_unsafe();
    portEXIT_CRITICAL(&s_lock);
    return median_us / 1000U;
}

void speed_sensor_update_timeout(void)
{
    portENTER_CRITICAL(&s_lock);
    const int64_t last_pulse_us = s_last_valid_pulse_us;
    if (last_pulse_us != 0) {
        const int64_t now_us = esp_timer_get_time();
        if ((now_us - last_pulse_us) > SPEED_ZERO_TIMEOUT_US) {
            clear_history_unsafe();
        }
    }
    portEXIT_CRITICAL(&s_lock);
}

float speed_sensor_get_speed_kmh(void)
{
    uint32_t median_dt_us = 0;
    uint8_t history_count = 0;
    int64_t last_pulse_us = 0;

    portENTER_CRITICAL(&s_lock);
    history_count = s_dt_history_count;
    median_dt_us = median_dt_us_unsafe();
    last_pulse_us = s_last_valid_pulse_us;
    portEXIT_CRITICAL(&s_lock);

    if (last_pulse_us == 0 || median_dt_us == 0 || history_count < 2) {
        return 0.0f;
    }

    const int64_t now_us = esp_timer_get_time();
    if ((now_us - last_pulse_us) > SPEED_ZERO_TIMEOUT_US) {
        return 0.0f;
    }

    const float dt_s = (float)median_dt_us / 1000000.0f;
    if (dt_s <= 0.0f) {
        return 0.0f;
    }

    const float distance_per_pulse_m = WHEEL_CIRCUMFERENCE_M / 2.0f;
    const float speed_kmh = (distance_per_pulse_m / dt_s) * 3.6f;
    return speed_kmh < 0.05f ? 0.0f : speed_kmh;
}

float speed_sensor_get_distance_km(void)
{
    portENTER_CRITICAL(&s_lock);
    const uint32_t total = s_pulse_total;
    portEXIT_CRITICAL(&s_lock);
    return (float)total * DISTANCE_PER_PULSE_KM;
}