dominic/pestdetector
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
pestdetector/sw/firmware/main/main_isr.c
Dominic Höglinger 78102e2f02 Initial release of the pest detector project 
This repository contains an attempt to detect wood pests
using three piezo sensors.
Each sensor records the time of flight of a noise source in their enclosing
area and calculates the coordinate as best it can.

This utilizes an ESP32's motor controller peripheral for exact timestamping
of three sensor events, which is then processed by a Python script.
The Python script takes the timestamps of an event,
and a photo of the sensor arrangement and places a marker where the noise
originates.
2025-05-20 20:02:50 +02:00

478 lines
13 KiB
C
Raw Permalink Blame History

#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "driver/periph_ctrl.h"
#include "driver/timer.h"
#include "driver/ledc.h"
#include "freertos/semphr.h"
#include <string.h>
#include "esp_system.h"
#include "esp_log.h"
#include "esp_console.h"
#include "argtable3/argtable3.h"
#include "nvs.h"
#include "nvs_flash.h"
/**
* Brief:
* This test code shows how to configure multiple gpio interrupts
*
* GPIO status:
* GPIO2 : output ( built-in led on Devkit-V1 )
* GPIO34 : output ( externally pulled up )
* GPIO35 : output ( externally pulled up )
*
* Test:
* Connect GPIO34 with simple switch and ground
* Connect GPIO35 with simple switch and ground
*/
#define ESP_INTR_FLAG_DEFAULT 0
#define CALIB_N 64
#define CALIB_PAUSE 50
#define BUILTIN_LED 2
#define GPIO_INPUT_DET_A 25
#define GPIO_INPUT_CAL_A 17
#define GPIO_INPUT_DET_B 26
#define GPIO_INPUT_CAL_B 18
#define GPIO_INPUT_DET_C 27
#define GPIO_INPUT_CAL_C 19
#define TIMER_DIVIDER 2 // Hardware timer clock divider
#define TIMER_SCALE (TIMER_BASE_CLK / TIMER_DIVIDER) // convert counter value to seconds
SemaphoreHandle_t xSemaphore_a = NULL;
SemaphoreHandle_t xSemaphore_b = NULL;
SemaphoreHandle_t xSemaphore_c = NULL;
bool detection_armed = false;
uint64_t a_timestamp_cal = 0ULL;
uint64_t a_timestamp_det = 0ULL;
uint64_t b_timestamp_cal = 0ULL;
uint64_t b_timestamp_det = 0ULL;
uint64_t c_timestamp_cal = 0ULL;
uint64_t c_timestamp_det = 0ULL;
float ab_delta = 0;
float ac_delta = 0;
float ba_delta = 0;
float bc_delta = 0;
float ca_delta = 0;
float cb_delta = 0;
void IRAM_ATTR gpio_a_isr_handler(void* arg){ //++ First ISR
timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &a_timestamp_det);
gpio_intr_disable(GPIO_INPUT_DET_A);
xSemaphoreGiveFromISR(xSemaphore_a, NULL);
}
void IRAM_ATTR gpio_b_isr_handler(void* arg){ //++ Second ISR
timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &b_timestamp_det);
gpio_intr_disable(GPIO_INPUT_DET_B);
xSemaphoreGiveFromISR(xSemaphore_b, NULL);
}
void IRAM_ATTR gpio_c_isr_handler(void* arg) {
timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &c_timestamp_det);
gpio_intr_disable(GPIO_INPUT_DET_C);
xSemaphoreGiveFromISR(xSemaphore_c, NULL);
}
static bool measure_a(uint32_t *p_delta_b, uint32_t *p_delta_c)
{
bool ab_okay = false;
bool ac_okay = false;
uint64_t timestep_cal = 0ULL;
// reset timestamps
b_timestamp_det = 0;
c_timestamp_det = 0;
timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &timestep_cal);
// pulse the IO
gpio_intr_enable(GPIO_INPUT_DET_B);
gpio_intr_enable(GPIO_INPUT_DET_C);
ledc_set_pin(GPIO_INPUT_CAL_A, LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
for (int i = 0; i < 0xFFFF; ++i);
gpio_set_direction(GPIO_INPUT_CAL_A, GPIO_MODE_INPUT);
// get the a-b delta
if (b_timestamp_det > 0)
{
uint64_t delta = b_timestamp_det - timestep_cal;
*p_delta_b = delta;
ab_okay = true;
}
// get the a-c delta
if (c_timestamp_det > 0)
{
uint64_t delta = c_timestamp_det - timestep_cal;
*p_delta_c = delta;
ac_okay = true;
}
return ab_okay && ac_okay;
}
void calibrate_a(float *p_delta_b, float *p_delta_c)
{
uint32_t delta_b = 0UL;
uint32_t delta_c = 0UL;
uint32_t sum_b = 0UL;
uint32_t sum_c = 0UL;
uint32_t samples = 0UL;
for (int i = 0; i < CALIB_N; ++i)
{
bool okay = measure_a(&delta_b, &delta_c);
if (okay)
{
sum_b += delta_b;
sum_c += delta_c;
samples++;
}
vTaskDelay(CALIB_PAUSE/portTICK_PERIOD_MS);
}
if (samples > 0)
{
*p_delta_b = sum_b / samples;
*p_delta_c = sum_c / samples;
}
}
static bool measure_b(uint32_t *p_delta_a, uint32_t *p_delta_c)
{
bool ba_okay = false;
bool bc_okay = false;
uint64_t timestep_cal = 0ULL;
// reset timestamps
a_timestamp_det = 0;
c_timestamp_det = 0;
timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &timestep_cal);
// pulse the IO
gpio_intr_enable(GPIO_INPUT_DET_A);
gpio_intr_enable(GPIO_INPUT_DET_C);
ledc_set_pin(GPIO_INPUT_CAL_B, LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
for (int i = 0; i < 0xFFFF; ++i);
gpio_set_direction(GPIO_INPUT_CAL_B, GPIO_MODE_INPUT);
// get the b-a delta
if (a_timestamp_det > 0)
{
uint64_t delta = a_timestamp_det - timestep_cal;
*p_delta_a = delta;
ba_okay = true;
}
// get the b-c delta
if (c_timestamp_det > 0)
{
uint64_t delta = c_timestamp_det - timestep_cal;
*p_delta_c = delta;
bc_okay = true;
}
return ba_okay && bc_okay;
}
void calibrate_b(float *p_delta_a, float *p_delta_c)
{
uint32_t delta_a = 0UL;
uint32_t delta_c = 0UL;
uint32_t sum_a = 0UL;
uint32_t sum_c = 0UL;
uint32_t samples = 0UL;
for (int i = 0; i < CALIB_N; ++i)
{
bool okay = measure_b(&delta_a, &delta_c);
if (okay)
{
sum_a += delta_a;
sum_c += delta_c;
samples++;
}
vTaskDelay(CALIB_PAUSE/portTICK_PERIOD_MS);
}
if (samples > 0)
{
*p_delta_a = sum_a / samples;
*p_delta_c = sum_c / samples;
}
}
static bool measure_c(uint32_t *p_delta_a, uint32_t *p_delta_b)
{
bool ca_okay = false;
bool cb_okay = false;
uint64_t timestep_cal = 0ULL;
// reset timestamps
a_timestamp_det = 0;
b_timestamp_det = 0;
timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &timestep_cal);
// pulse the IO
gpio_intr_enable(GPIO_INPUT_DET_A);
gpio_intr_enable(GPIO_INPUT_DET_B);
ledc_set_pin(GPIO_INPUT_CAL_C, LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
for (int i = 0; i < 0xFFFF; ++i);
gpio_set_direction(GPIO_INPUT_CAL_C, GPIO_MODE_INPUT);
// get the c-a delta
if (a_timestamp_det > 0)
{
uint64_t delta = a_timestamp_det - timestep_cal;
*p_delta_a = delta;
ca_okay = true;
}
// get the c-b delta
if (b_timestamp_det > 0)
{
uint64_t delta = b_timestamp_det - timestep_cal;
*p_delta_b = delta;
cb_okay = true;
}
return ca_okay && cb_okay;
}
void calibrate_c(float *p_delta_a, float *p_delta_b)
{
uint32_t delta_a = 0UL;
uint32_t delta_b = 0UL;
uint32_t sum_a = 0UL;
uint32_t sum_b = 0UL;
uint32_t samples = 0UL;
for (int i = 0; i < CALIB_N; ++i)
{
bool okay = measure_c(&delta_a, &delta_b);
if (okay)
{
sum_a += delta_a;
sum_b += delta_b;
samples++;
}
vTaskDelay(CALIB_PAUSE/portTICK_PERIOD_MS);
}
if (samples > 0)
{
*p_delta_a = sum_a / samples;
*p_delta_b = sum_b / samples;
}
}
static struct {
struct arg_end *end;
} calib_args;
static int calibrate(int argc, char **argv)
{
ab_delta = 0;
ac_delta = 0;
ba_delta = 0;
bc_delta = 0;
ca_delta = 0;
cb_delta = 0;
//for(;;)
calibrate_a(&ab_delta, &ac_delta);
calibrate_b(&ba_delta, &bc_delta);
calibrate_c(&ca_delta, &cb_delta);
printf("AB: %f\n", ab_delta);
printf("AC: %f\n", ac_delta);
printf("BA: %f\n", ba_delta);
printf("BC: %f\n", bc_delta);
printf("CA: %f\n", ca_delta);
printf("CB: %f\n", cb_delta);
return 0;
}
static void example_tg0_timer_init(int timer_idx, bool auto_reload)
{
/* Select and initialize basic parameters of the timer */
timer_config_t config = {
.divider = TIMER_DIVIDER,
.counter_dir = TIMER_COUNT_UP,
.counter_en = TIMER_PAUSE,
.alarm_en = TIMER_ALARM_EN,
.auto_reload = auto_reload,
}; // default clock source is APB
timer_init(TIMER_GROUP_0, timer_idx, &config);
/* Timer's counter will initially start from value below.
Also, if auto_reload is set, this value will be automatically reload on alarm */
timer_set_counter_value(TIMER_GROUP_0, timer_idx, 0x00000000ULL);
timer_start(TIMER_GROUP_0, timer_idx);
}
static void initialize_nvs(void) {
esp_err_t err = nvs_flash_init();
if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
ESP_ERROR_CHECK(nvs_flash_erase());
err = nvs_flash_init();
}
ESP_ERROR_CHECK(err);
}
static struct {
struct arg_end *end;
} arm_args;
static int arm_command(int argc, char **argv)
{
while( xSemaphoreTake( xSemaphore_a, 0 ) == pdPASS );
while( xSemaphoreTake( xSemaphore_b, 0 ) == pdPASS );
while( xSemaphoreTake( xSemaphore_c, 0 ) == pdPASS );
a_timestamp_det = 0ULL;
b_timestamp_det = 0ULL;
c_timestamp_det = 0ULL;
gpio_intr_enable(GPIO_INPUT_DET_A);
gpio_intr_enable(GPIO_INPUT_DET_B);
gpio_intr_enable(GPIO_INPUT_DET_C);
detection_armed = true;
return 0;
}
void detect_task(void* arg)
{
for(;;)
{
if (detection_armed)
{
while( xSemaphoreTake( xSemaphore_a, portMAX_DELAY ) != pdPASS );
while( xSemaphoreTake( xSemaphore_b, portMAX_DELAY ) != pdPASS );
while( xSemaphoreTake( xSemaphore_c, portMAX_DELAY ) != pdPASS );
printf("A: %ju\n", a_timestamp_det);
printf("B: %ju\n", b_timestamp_det);
printf("C: %ju\n", c_timestamp_det);
detection_armed = false;
}
else
{
vTaskDelay( 250 / portTICK_PERIOD_MS);
}
}
}
void app_main(void)
{
gpio_reset_pin(GPIO_INPUT_DET_A);
gpio_reset_pin(GPIO_INPUT_DET_B);
gpio_reset_pin(GPIO_INPUT_DET_C);
//PWM
// Prepare and then apply the LEDC PWM timer configuration
ledc_timer_config_t ledc_timer = {
.speed_mode = LEDC_LOW_SPEED_MODE,
.timer_num = LEDC_TIMER_0,
.duty_resolution = LEDC_TIMER_13_BIT,
.freq_hz = 3200, // Set output frequency at 5 kHz
.clk_cfg = LEDC_AUTO_CLK
};
ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
// Prepare and then apply the LEDC PWM channel configuration
ledc_channel_config_t ledc_channel = {
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = LEDC_CHANNEL_0,
.timer_sel = LEDC_TIMER_0,
.intr_type = LEDC_INTR_DISABLE,
.gpio_num = GPIO_INPUT_CAL_A,
.duty = 4096, // Set duty to 50%
.hpoint = 0
};
ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
gpio_set_direction(GPIO_INPUT_CAL_A, GPIO_MODE_INPUT);
/* Set the GPIO & LED direction */
gpio_set_direction(GPIO_INPUT_DET_A, GPIO_MODE_INPUT);
gpio_set_pull_mode(GPIO_INPUT_DET_A, GPIO_PULLDOWN_ONLY); //GPIO_PULLUP_ONLY
gpio_set_direction(GPIO_INPUT_DET_B, GPIO_MODE_INPUT);
gpio_set_pull_mode(GPIO_INPUT_DET_B, GPIO_PULLDOWN_ONLY);
gpio_set_direction(GPIO_INPUT_DET_C, GPIO_MODE_INPUT);
gpio_set_pull_mode(GPIO_INPUT_DET_C, GPIO_PULLDOWN_ONLY);
gpio_set_direction(GPIO_INPUT_CAL_A, GPIO_MODE_INPUT);
gpio_set_pull_mode(GPIO_INPUT_CAL_A, GPIO_FLOATING);
gpio_set_direction(GPIO_INPUT_CAL_B, GPIO_MODE_INPUT);
gpio_set_pull_mode(GPIO_INPUT_CAL_B, GPIO_FLOATING);
gpio_set_direction(GPIO_INPUT_CAL_C, GPIO_MODE_INPUT);
gpio_set_pull_mode(GPIO_INPUT_CAL_C, GPIO_FLOATING);
example_tg0_timer_init(TIMER_0, false);
//enable interrupt on anyedge for button pin
gpio_set_intr_type(GPIO_INPUT_DET_A, GPIO_INTR_POSEDGE);
gpio_set_intr_type(GPIO_INPUT_DET_B, GPIO_INTR_POSEDGE);
gpio_set_intr_type(GPIO_INPUT_DET_C, GPIO_INTR_POSEDGE);
//install ISR service with default configuration
gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT);
//attach the interrupt service routine
gpio_isr_handler_add(GPIO_INPUT_DET_A, gpio_a_isr_handler, NULL);
gpio_isr_handler_add(GPIO_INPUT_DET_B, gpio_b_isr_handler, NULL);
gpio_isr_handler_add(GPIO_INPUT_DET_C, gpio_c_isr_handler, NULL);
gpio_intr_disable(GPIO_INPUT_DET_A);
gpio_intr_disable(GPIO_INPUT_DET_B);
gpio_intr_disable(GPIO_INPUT_DET_C);
//semaphores
xSemaphore_a = xSemaphoreCreateBinary();
xSemaphore_b = xSemaphoreCreateBinary();
xSemaphore_c = xSemaphoreCreateBinary();
xTaskCreate(detect_task, "detect_task", 2048, NULL, configMAX_PRIORITIES-1, NULL);
esp_console_repl_t *repl = NULL;
esp_console_repl_config_t repl_config = ESP_CONSOLE_REPL_CONFIG_DEFAULT();
/* Prompt to be printed before each line.
* This can be customized, made dynamic, etc.
*/
repl_config.prompt = ">";
repl_config.max_cmdline_length = 255;
initialize_nvs();
esp_console_dev_uart_config_t hw_config = ESP_CONSOLE_DEV_UART_CONFIG_DEFAULT();
ESP_ERROR_CHECK(esp_console_new_repl_uart(&hw_config, &repl_config, &repl));
calib_args.end = arg_end(1);
esp_console_cmd_t calib_cmd = {
.command = "c",
.help = "Perform calibration",
.func = &calibrate,
.argtable = &calib_args
};
ESP_ERROR_CHECK(esp_console_cmd_register(&calib_cmd));
arm_args.end = arg_end(1);
esp_console_cmd_t arm_cmd = {
.command = "a",
.help = "Arm detections",
.func = &arm_command,
.argtable = &arm_args
};
ESP_ERROR_CHECK(esp_console_cmd_register(&arm_cmd));
// Start the REPL
ESP_ERROR_CHECK(esp_console_start_repl(repl));
}
Reference in New Issue View Git Blame Copy Permalink