239 lines
8.1 KiB
C
239 lines
8.1 KiB
C
#include "sensors.h"
|
||
#include "board_config.h"
|
||
#include "esp_log.h"
|
||
#include "esp_task_wdt.h"
|
||
#include "driver/gpio.h"
|
||
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/task.h"
|
||
#include "freertos/queue.h"
|
||
#include "i2c.h"
|
||
#include "storage.h"
|
||
#include <sys/param.h>
|
||
|
||
// make the compiler shut up about casting an int to a void
|
||
#define INT2VOIDP(i) (void*)(uintptr_t)(i)
|
||
|
||
|
||
static const char* TAG = "SENS";
|
||
|
||
#ifdef BOARD_V5
|
||
// V5 physical connectors:
|
||
// J1 = IO27 → SAFETY
|
||
// J2 = IO14 → JACK
|
||
// J3 = IO23 → n/c (AUX) (J3 unreliable on the V5 board, moved DRIVE off)
|
||
// J4 = IO19 → DRIVE
|
||
// Array order matches sensor_t: SAFETY, DRIVE, JACK, AUX2
|
||
uint8_t sensor_pins[N_SENSORS] = {GPIO_NUM_27, GPIO_NUM_19, GPIO_NUM_14, GPIO_NUM_23};
|
||
#else // BOARD_V4
|
||
uint8_t sensor_pins[N_SENSORS] = {GPIO_NUM_27, GPIO_NUM_14, GPIO_NUM_16, GPIO_NUM_19};
|
||
#endif
|
||
|
||
volatile int16_t sensor_count[N_SENSORS] = {0};
|
||
/* Bumped directly in the ISR on every edge — does not require sensors_check()
|
||
* to run, so it works even while bring-up pauses the FSM task. */
|
||
volatile uint32_t sensor_isr_edge_count[N_SENSORS] = {0};
|
||
static volatile uint64_t sensor_last_isr_time[N_SENSORS] = {0};
|
||
static volatile bool sensor_stable_state[N_SENSORS] = {false};
|
||
static QueueHandle_t sensor_event_queue = NULL;
|
||
|
||
|
||
static volatile bool last_raw_state[N_SENSORS] = {false};
|
||
|
||
// Safety sensor debouncing
|
||
static volatile bool is_safe = true;
|
||
static volatile uint64_t safety_low_start_time = 0;
|
||
static volatile uint64_t safety_high_start_time = 0;
|
||
#define SAFETY_BREAK_DEBOUNCE_US get_param_value_t(PARAM_SAFETY_BREAK_US).u32
|
||
#define SAFETY_MAKE_DEBOUNCE_US get_param_value_t(PARAM_SAFETY_MAKE_US).u32
|
||
|
||
#define DEBOUNCE_TIME_US 2000 // 2 ms debounce (adjust per switch)
|
||
#define DEBOUNCE_TICKS pdMS_TO_TICKS(DEBOUNCE_TIME_MS)
|
||
|
||
typedef struct {
|
||
uint8_t sensor_id;
|
||
bool level;
|
||
} sensor_event_t;
|
||
|
||
// ISR: Minimal work – just record timestamp and forward to queue
|
||
static void IRAM_ATTR sensor_isr_handler(void* arg) {
|
||
uint32_t gpio_num = (uint32_t)arg;
|
||
uint8_t i;
|
||
for (i = 0; i < N_SENSORS; i++) {
|
||
if (sensor_pins[i] == gpio_num) break;
|
||
}
|
||
if (i == N_SENSORS) return;
|
||
|
||
uint64_t now = esp_timer_get_time();
|
||
sensor_last_isr_time[i] = now;
|
||
|
||
sensor_isr_edge_count[i]++;
|
||
|
||
sensor_event_t evt = {.sensor_id = i, .level = !gpio_get_level(gpio_num)};
|
||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||
xQueueSendFromISR(sensor_event_queue, &evt, &xHigherPriorityTaskWoken);
|
||
if (xHigherPriorityTaskWoken) portYIELD_FROM_ISR();
|
||
}
|
||
|
||
esp_err_t sensors_init() {
|
||
|
||
uint64_t pin_mask = 0;
|
||
for (uint8_t i = 0; i < N_SENSORS; i++) pin_mask |= (1ULL << sensor_pins[i]);
|
||
|
||
/* Belt-and-suspenders: force each sensor pin into digital-GPIO mode with
|
||
* pull-up explicitly applied. gpio_config()'s pull_up_en is known to be
|
||
* shadowed by RTC-subsystem settings on RTC-capable pins (IO27, 32, 33,
|
||
* 34–39). gpio_reset_pin() detaches any lingering RTC/peripheral mux,
|
||
* and the explicit gpio_set_pull_mode() call goes through the right
|
||
* path regardless of which sub-block owns the pin. */
|
||
for (uint8_t i = 0; i < N_SENSORS; i++) {
|
||
gpio_reset_pin(sensor_pins[i]);
|
||
}
|
||
|
||
gpio_config_t io_conf = {
|
||
.pin_bit_mask = pin_mask,
|
||
.mode = GPIO_MODE_INPUT,
|
||
.pull_up_en = GPIO_PULLUP_ENABLE,
|
||
.pull_down_en = GPIO_PULLDOWN_DISABLE,
|
||
.intr_type = GPIO_INTR_ANYEDGE,
|
||
};
|
||
ESP_ERROR_CHECK(gpio_config(&io_conf));
|
||
|
||
for (uint8_t i = 0; i < N_SENSORS; i++) {
|
||
ESP_ERROR_CHECK(gpio_set_pull_mode(sensor_pins[i], GPIO_PULLUP_ONLY));
|
||
}
|
||
|
||
sensor_event_queue = xQueueCreate(16, sizeof(sensor_event_t));
|
||
if (!sensor_event_queue) {
|
||
ESP_LOGE(TAG, "Failed to create sensor queue");
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
// Install ISR service
|
||
ESP_ERROR_CHECK(gpio_install_isr_service(0));
|
||
|
||
for (uint8_t i = 0; i < N_SENSORS; i++) {
|
||
ESP_ERROR_CHECK(gpio_isr_handler_add(sensor_pins[i], sensor_isr_handler, INT2VOIDP(sensor_pins[i])));
|
||
sensor_stable_state[i] = !gpio_get_level(sensor_pins[i]);
|
||
}
|
||
|
||
//static uint64_t last_processed_time[N_SENSORS] = {0};
|
||
|
||
// Initialize stable state
|
||
for (uint8_t i = 0; i < N_SENSORS; i++) {
|
||
bool level = !gpio_get_level(sensor_pins[i]);
|
||
sensor_stable_state[i] = level;
|
||
last_raw_state[i] = level;
|
||
//last_processed_time[i] = esp_timer_get_time();
|
||
}
|
||
|
||
// Initialize safety sensor
|
||
bool safety_current = !gpio_get_level(sensor_pins[SENSOR_SAFETY]);
|
||
if (safety_current) {
|
||
safety_low_start_time = esp_timer_get_time();
|
||
safety_high_start_time = 0;
|
||
} else {
|
||
safety_low_start_time = 0;
|
||
safety_high_start_time = esp_timer_get_time();
|
||
}
|
||
|
||
return ESP_OK;
|
||
}
|
||
|
||
void sensors_check() {
|
||
/* Drain ALL queued events with a non-blocking receive. Previously this
|
||
* was a single blocking receive with a 10 ms timeout, which (a) chewed up
|
||
* half the FSM tick window waiting on quiet sensors, and (b) consumed
|
||
* only one edge per tick — encoder bursts above 50 Hz overflowed the
|
||
* 16-entry queue and undercounted distance. Now each tick consumes the
|
||
* full queue contents and applies a per-sensor debounce. */
|
||
sensor_event_t evt;
|
||
static uint64_t last_counted_us[N_SENSORS] = {0};
|
||
while (xQueueReceive(sensor_event_queue, &evt, 0) == pdTRUE) {
|
||
uint8_t i = evt.sensor_id;
|
||
if (i >= N_SENSORS) continue;
|
||
|
||
/* Use the ISR-captured level (snapshot at edge time) rather than a
|
||
* fresh GPIO read here — by the time the FSM tick drains the queue,
|
||
* the line may have toggled again and a re-read would miss the
|
||
* transition we're processing. */
|
||
bool current_raw = evt.level;
|
||
|
||
/* Software debounce on non-safety sensors. The safety sensor has its
|
||
* own asymmetric debouncer below, so we don't double-count there. */
|
||
uint64_t now = esp_timer_get_time();
|
||
if (i != SENSOR_SAFETY) {
|
||
if (now - last_counted_us[i] < DEBOUNCE_TIME_US) continue;
|
||
}
|
||
last_counted_us[i] = now;
|
||
|
||
sensor_stable_state[i] = current_raw;
|
||
if (current_raw != last_raw_state[i]) {
|
||
sensor_count[i]++;
|
||
}
|
||
last_raw_state[i] = current_raw;
|
||
}
|
||
|
||
// Handle safety sensor debouncing with asymmetric timing
|
||
bool safety_current = !gpio_get_level(sensor_pins[SENSOR_SAFETY]);
|
||
uint64_t now = esp_timer_get_time();
|
||
|
||
if (safety_current) {
|
||
// Safety sensor is LOW (active)
|
||
if (safety_low_start_time == 0) {
|
||
// First time going low, start timing
|
||
safety_low_start_time = now;
|
||
safety_high_start_time = 0;
|
||
ESP_LOGI(TAG, "Safety sensor went LOW, starting make timer");
|
||
} else if (!is_safe && (now - safety_low_start_time >= SAFETY_MAKE_DEBOUNCE_US)) {
|
||
is_safe = true;
|
||
ESP_LOGW(TAG, "SAFETY MADE - Relays enabled");
|
||
}
|
||
} else {
|
||
// Safety sensor is HIGH (inactive)
|
||
if (safety_high_start_time == 0) {
|
||
// First time going high, start timing
|
||
safety_high_start_time = now;
|
||
safety_low_start_time = 0;
|
||
ESP_LOGI(TAG, "Safety sensor went HIGH, starting break timer");
|
||
} else if (is_safe && (now - safety_high_start_time >= SAFETY_BREAK_DEBOUNCE_US)) {
|
||
is_safe = false;
|
||
/* Kill all bridges but leave the sensor rail up — we still
|
||
* want to observe the safety input. */
|
||
i2c_relays_idle();
|
||
ESP_LOGI(TAG, "SAFETY BREAK - Relays disabled");
|
||
}
|
||
}
|
||
|
||
esp_task_wdt_reset();
|
||
}
|
||
|
||
int8_t pack_sensors() {
|
||
int8_t ret = 0;
|
||
for(uint8_t i=0; i<MIN(4, N_SENSORS); i++) {
|
||
if(sensor_stable_state[i]) ret |= 0x01<<i;
|
||
if(last_raw_state[i]) ret |= 0x01<<(i+4);
|
||
}
|
||
return ret;
|
||
}
|
||
|
||
// Public API
|
||
bool get_sensor(sensor_t i) {
|
||
return sensor_stable_state[i];
|
||
}
|
||
|
||
bool get_is_safe(void) {
|
||
return is_safe;
|
||
}
|
||
|
||
int16_t get_sensor_counter(sensor_t i) {
|
||
return sensor_count[i];
|
||
}
|
||
|
||
void set_sensor_counter(sensor_t i, int16_t to) {
|
||
sensor_count[i] = to;
|
||
}
|
||
|
||
uint32_t get_sensor_isr_edges(sensor_t i) {
|
||
if (i >= N_SENSORS) return 0;
|
||
return sensor_isr_edge_count[i];
|
||
} |