dominic/pestdetector
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
pestdetector/sw/firmware/main/main.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

622 lines
18 KiB
C
Raw 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"
#include "esp_private/esp_clk.h"
#include "driver/mcpwm_cap.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 33 //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;
mcpwm_cap_timer_handle_t timer_a = NULL;
mcpwm_cap_channel_handle_t cap_chan_a = NULL;
mcpwm_cap_channel_handle_t cap_chan_b = NULL;
mcpwm_cap_channel_handle_t cap_chan_c = NULL;
TaskHandle_t xDetectTask = 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
ESP_ERROR_CHECK(mcpwm_capture_channel_enable(cap_chan_b));
ESP_ERROR_CHECK(mcpwm_capture_channel_enable(cap_chan_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;
} else ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan_b));
// 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;
} else ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan_c));
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_b = 0UL;
uint32_t samples_c = 0UL;
for (int i = 0; i < CALIB_N; ++i)
{
delta_b = 0UL;
delta_c = 0UL;
bool okay = measure_a(&delta_b, &delta_c);
if (delta_b > 0)
{
sum_b += delta_b;
samples_b++;
}
if (delta_c > 0)
{
sum_c += delta_c;
samples_c++;
}
vTaskDelay(CALIB_PAUSE/portTICK_PERIOD_MS);
}
if (samples_b > 0)
{
*p_delta_b = sum_b / samples_b;
}
if (samples_c > 0)
{
*p_delta_c = sum_c / samples_c;
}
}
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
ESP_ERROR_CHECK(mcpwm_capture_channel_enable(cap_chan_a));
ESP_ERROR_CHECK(mcpwm_capture_channel_enable(cap_chan_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;
} else ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan_a));
// 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;
} else ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan_c));
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_a = 0UL;
uint32_t samples_c = 0UL;
for (int i = 0; i < CALIB_N; ++i)
{
delta_a = 0UL;
delta_c = 0UL;
bool okay = measure_b(&delta_a, &delta_c);
if (delta_a > 0)
{
sum_a += delta_a;
samples_a++;
}
if (delta_c > 0)
{
sum_c += delta_c;
samples_c++;
}
vTaskDelay(CALIB_PAUSE/portTICK_PERIOD_MS);
}
if (samples_a > 0)
{
*p_delta_a = sum_a / samples_a;
}
if (samples_c > 0)
{
*p_delta_c = sum_c / samples_c;
}
}
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
ESP_ERROR_CHECK(mcpwm_capture_channel_enable(cap_chan_a));
ESP_ERROR_CHECK(mcpwm_capture_channel_enable(cap_chan_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;
} else ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan_a));
// 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;
} else ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan_b));
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_a = 0UL;
uint32_t samples_b = 0UL;
for (int i = 0; i < CALIB_N; ++i)
{
delta_a = 0UL;
delta_b = 0UL;
bool okay = measure_c(&delta_a, &delta_b);
if (delta_a > 0)
{
sum_a += delta_a;
samples_a++;
}
if (delta_b > 0)
{
sum_b += delta_b;
samples_b++;
}
vTaskDelay(CALIB_PAUSE/portTICK_PERIOD_MS);
}
if (samples_a > 0)
{
*p_delta_a = sum_a / samples_a;
}
if (samples_b > 0)
{
*p_delta_b = sum_b / samples_b;
}
}
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;
//ESP_ERROR_CHECK(mcpwm_capture_timer_start(timer_a));
ESP_ERROR_CHECK(mcpwm_capture_channel_enable(cap_chan_a));
ESP_ERROR_CHECK(mcpwm_capture_channel_enable(cap_chan_b));
ESP_ERROR_CHECK(mcpwm_capture_channel_enable(cap_chan_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);
}
}
}
static bool detection_a_callback(mcpwm_cap_channel_handle_t cap_chan, const mcpwm_capture_event_data_t *edata, void *user_data)
{
TaskHandle_t task_to_notify = (TaskHandle_t)user_data;
BaseType_t high_task_wakeup = pdFALSE;
if ((edata->cap_edge == MCPWM_CAP_EDGE_POS) && (a_timestamp_det == 0)) {
// store the timestamp when pos edge is detected
a_timestamp_det = edata->cap_value;
}
//ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan_a));
ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan));
xSemaphoreGiveFromISR(xSemaphore_a, NULL);
return high_task_wakeup == pdTRUE;
}
static bool detection_b_callback(mcpwm_cap_channel_handle_t cap_chan, const mcpwm_capture_event_data_t *edata, void *user_data)
{
//TaskHandle_t task_to_notify = (TaskHandle_t)user_data;
BaseType_t high_task_wakeup = pdFALSE;
if ((edata->cap_edge == MCPWM_CAP_EDGE_POS) && (b_timestamp_det == 0)) {
// store the timestamp when pos edge is detected
b_timestamp_det = edata->cap_value;
}
//ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan_b));
ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan));
xSemaphoreGiveFromISR(xSemaphore_b, NULL);
return high_task_wakeup == pdTRUE;
}
static bool detection_c_callback(mcpwm_cap_channel_handle_t cap_chan, const mcpwm_capture_event_data_t *edata, void *user_data)
{
//TaskHandle_t task_to_notify = (TaskHandle_t)user_data;
BaseType_t high_task_wakeup = pdFALSE;
if ((edata->cap_edge == MCPWM_CAP_EDGE_POS) && (c_timestamp_det == 0)) {
// store the timestamp when pos edge is detected
c_timestamp_det = edata->cap_value;
}
//ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan_c));
ESP_ERROR_CHECK(mcpwm_capture_channel_disable(cap_chan_c));
xSemaphoreGiveFromISR(xSemaphore_c, NULL);
return high_task_wakeup == pdTRUE;
}
mcpwm_cap_timer_handle_t mcpwm_capture_init_a(mcpwm_cap_channel_handle_t *p_cap_chan_a, mcpwm_cap_channel_handle_t *p_cap_chan_b, mcpwm_cap_channel_handle_t *p_cap_chan_c)
{
uint32_t res = 0UL;
const uint32_t PRESCALER = 1;
mcpwm_cap_timer_handle_t cap_timer = NULL;
mcpwm_capture_timer_config_t cap_conf = {
.clk_src = MCPWM_CAPTURE_CLK_SRC_DEFAULT,
.group_id = 0,
};
ESP_ERROR_CHECK(mcpwm_new_capture_timer(&cap_conf, &cap_timer));
ESP_ERROR_CHECK(mcpwm_capture_timer_get_resolution(cap_timer, &res));
printf("TIMER RES: %lu\n", res);
//ESP_LOGI(TAG, "Install capture channel");
mcpwm_capture_channel_config_t cap_ch_a_conf = {
.gpio_num = GPIO_INPUT_DET_A,
.prescale = PRESCALER,
//.intr_priority = 10,
// capture on pos edge only
.flags.neg_edge = false,
.flags.pos_edge = true,
// pull up internally
.flags.pull_down = true,
};
ESP_ERROR_CHECK(mcpwm_new_capture_channel(cap_timer, &cap_ch_a_conf, p_cap_chan_a));
//ESP_LOGI(TAG, "Register capture callback");
TaskHandle_t cur_task = xTaskGetCurrentTaskHandle();
mcpwm_capture_event_callbacks_t cbs = {
.on_cap = detection_a_callback,
};
ESP_ERROR_CHECK(mcpwm_capture_channel_register_event_callbacks(*p_cap_chan_a, &cbs, cur_task));
//ESP_LOGI(TAG, "Enable capture channel");
//ESP_ERROR_CHECK(mcpwm_capture_channel_enable(*p_cap_chan_a));
mcpwm_capture_channel_config_t cap_ch_b_conf = {
.gpio_num = GPIO_INPUT_DET_B,
.prescale = PRESCALER,
//.intr_priority = 9,
// capture on pos edge only
.flags.neg_edge = false,
.flags.pos_edge = true,
// pull up internally
.flags.pull_down = true,
};
ESP_ERROR_CHECK(mcpwm_new_capture_channel(cap_timer, &cap_ch_b_conf, p_cap_chan_b));
//ESP_LOGI(TAG, "Register capture callback");
mcpwm_capture_event_callbacks_t cbs_b = {
.on_cap = detection_b_callback,
};
ESP_ERROR_CHECK(mcpwm_capture_channel_register_event_callbacks(*p_cap_chan_b, &cbs_b, cur_task));
//ESP_LOGI(TAG, "Enable capture channel");
//ESP_ERROR_CHECK(mcpwm_capture_channel_enable(*p_cap_chan_b));
mcpwm_capture_channel_config_t cap_ch_c_conf = {
.gpio_num = GPIO_INPUT_DET_C,
.prescale = PRESCALER,
//.intr_priority = 8,
// capture on pos edge only
.flags.neg_edge = false,
.flags.pos_edge = true,
// pull up internally
.flags.pull_down = true,
};
ESP_ERROR_CHECK(mcpwm_new_capture_channel(cap_timer, &cap_ch_c_conf, p_cap_chan_c));
//ESP_LOGI(TAG, "Register capture callback");
mcpwm_capture_event_callbacks_t cbs_c = {
.on_cap = detection_c_callback,
};
ESP_ERROR_CHECK(mcpwm_capture_channel_register_event_callbacks(*p_cap_chan_c, &cbs_c, cur_task));
//ESP_LOGI(TAG, "Enable capture channel");
//ESP_ERROR_CHECK(mcpwm_capture_channel_enable(*p_cap_chan_c));
//ESP_LOGI(TAG, "Enable and start capture timer");
ESP_ERROR_CHECK(mcpwm_capture_timer_enable(cap_timer));
ESP_ERROR_CHECK(mcpwm_capture_timer_start(cap_timer));
//ESP_ERROR_CHECK(mcpwm_capture_timer_stop(cap_timer));
return cap_timer;
}
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_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);
//install ISR service with default configuration
gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT);
//semaphores
xSemaphore_a = xSemaphoreCreateBinary();
xSemaphore_b = xSemaphoreCreateBinary();
xSemaphore_c = xSemaphoreCreateBinary();
timer_a = mcpwm_capture_init_a(&cap_chan_a, &cap_chan_b, &cap_chan_c);
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