firstpass/soundshot
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added tweaks to filter

Browse Source
This commit is contained in:
Brent Perteet
2025-08-08 17:31:27 -05:00
parent fb9fd84bf1
commit c4ca91bf84
17 changed files with 210 additions and 2440 deletions
Download Patch File Download Diff File Expand all files Collapse all files

325
main/main.c
View File

@@ -1,4 +1,3 @@
<<<<<<< HEAD
/*
* SPDX-FileCopyrightText: 2021-2024 Espressif Systems (Shanghai) CO LTD
*
@@ -40,6 +39,31 @@ typedef struct {
float prev_output; // y[n-1]
} LowPassFilter;
#define ALPHA_MIN 0.2f
#define ALPHA_MAX 0.4f
// Clamp function to bound value between min and max
double clamp(double value, double min, double max) {
if (value < min) return min;
if (value > max) return max;
return value;
}
// Compute alpha using linear ramp
float compute_alpha_linear(double x, float alpha_min, float alpha_max, float threshold) {
float abs_x = fabs(x);
float t = clamp(abs_x / threshold, 0.0, 1.0); // Normalize |x| to [0,1]
return alpha_min + (alpha_max - alpha_min) * t;
}
// Compute sigmoid-based dynamic alpha
float compute_alpha_sigmoid(float x, float alpha_min, float alpha_max, float k, float midpoint) {
float abs_x = fabs(x);
float exponent = -k * (abs_x - midpoint);
float sigmoid = 1.0 / (1.0 + exp(exponent));
return alpha_min + (alpha_max - alpha_min) * sigmoid;
}
// Initialize the filter. alpha = smoothing factor.
// e.g. alpha = dt/(RC+dt) if you derive from cutoff freq & sample time.
// init_output can be 0 or your first sample to avoid startup glitch.
@@ -51,7 +75,11 @@ static inline void LPF_Init(LowPassFilter *f, float alpha, float init_output) {
// Run one input sample through the filter.
// Returns y[n] = alpha * x[n] + (1-alpha) * y[n-1].
static inline float LPF_Update(LowPassFilter *f, float input) {
float out = f->alpha * input + (1.0f - f->alpha) * f->prev_output;
//float alpha = compute_alpha_sigmoid(input, ALPHA_MIN, ALPHA_MAX, 1.0f, 0.0f);
float alpha = compute_alpha_linear(input, ALPHA_MIN, ALPHA_MAX, 2.0f);
float out = alpha * input + (1.0f - alpha) * f->prev_output;
//float out = f->alpha * input + (1.0f - f->alpha) * f->prev_output;
f->prev_output = out;
return out;
}
@@ -290,296 +318,3 @@ void app_main(void)
}
#endif
}
=======
/*
* SPDX-FileCopyrightText: 2021-2024 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Unlicense OR CC0-1.0
*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <inttypes.h>
#include "math.h"
#include "esp_system.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/timers.h"
#include "driver/gpio.h"
#include "nvs.h"
#include "nvs_flash.h"
#include "bt_app.h"
#include "lsm6dsv.h"
#include "gui.h"
#include "gpio.h"
#include "keypad.h"
#include "system.h"
/*********************************
* STATIC FUNCTION DECLARATIONS
********************************/
typedef struct {
float alpha; // smoothing factor, 0<alpha<1
float prev_output; // y[n-1]
} LowPassFilter;
// Initialize the filter. alpha = smoothing factor.
// e.g. alpha = dt/(RC+dt) if you derive from cutoff freq & sample time.
// init_output can be 0 or your first sample to avoid startup glitch.
static inline void LPF_Init(LowPassFilter *f, float alpha, float init_output) {
f->alpha = alpha;
f->prev_output = init_output;
}
// Run one input sample through the filter.
// Returns y[n] = alpha * x[n] + (1-alpha) * y[n-1].
static inline float LPF_Update(LowPassFilter *f, float input) {
float out = f->alpha * input + (1.0f - f->alpha) * f->prev_output;
f->prev_output = out;
return out;
}
/*********************************
* STATIC VARIABLE DEFINITIONS
********************************/
static const char *TAG = "main";
/*********************************
* STATIC FUNCTION DEFINITIONS
********************************/
static void init_gpio(void)
{
gpio_config_t io_conf;
// Configure output GPIO
io_conf.pin_bit_mask = (1ULL << PIN_NUM_LED_1) | (1ULL << PIN_NUM_LED_2);
io_conf.mode = GPIO_MODE_OUTPUT;
io_conf.intr_type = GPIO_INTR_DISABLE;
io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
gpio_config(&io_conf);
#if 1
// Configure output power signal
gpio_set_level(PIN_NUM_nON, 1);
io_conf.pin_bit_mask = (1ULL << PIN_NUM_nON);
io_conf.mode = GPIO_MODE_OUTPUT;
io_conf.intr_type = GPIO_INTR_DISABLE;
io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
gpio_config(&io_conf);
#endif
// Configure LCD Backlight GPIO
io_conf.pin_bit_mask = (1ULL << PIN_NUM_BK_LIGHT);
io_conf.mode = GPIO_MODE_OUTPUT;
io_conf.intr_type = GPIO_INTR_DISABLE;
io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
gpio_config(&io_conf);
}
// IMU task to read sensor data
static void imu_task(void *pvParameters) {
lsm6dsv_data_t imu_data;
char data_str[128];
float roll, pitch, yaw;
lsm6dsv_fifo_t fifo;
ImuData_t imu;
LowPassFilter lpf;
LPF_Init(&lpf, FILTER_COEFF, 0);
while (1) {
// uint8_t samples = lsm6dsv_fifo_ready();
//ESP_LOGI(TAG, "Samples: %d", samples);
// while (samples)
// {
// lsm6dsv_fifo_read(&fifo);
// // snprintf(data_str, sizeof(data_str),
// // "tag: %d, count: %d, (%d, %d, %d)",
// // fifo.tag, fifo.count, x, y, z);
// // ESP_LOGI(TAG, "%s", data_str);
// samples--;
// }
// lsm6dsv_getAttitude(&fifo, &roll, &pitch, &yaw);
// snprintf(data_str, sizeof(data_str),
// "r: %0.3f, p: %0.3f, y: %0.3f",
// roll, pitch, yaw);
// ESP_LOGI(TAG, "%s", data_str);
float xz;
float yz;
float xy;
#if 1
if (lsm6dsv_read_data(&imu_data) == ESP_OK)
{
// snprintf(data_str, sizeof(data_str),
// "Acc: %.2f, %.2f, %.2f; "
// "Gyro: %.2f, %.2f, %.2f (%d)\n",
// imu_data.acc_x, imu_data.acc_y, imu_data.acc_z,
// imu_data.gyro_x, imu_data.gyro_y, imu_data.gyro_z, lsm6dsv_fifo_ready());
#if 1
imu.raw[ANGLE_XZ] = atan2f((float)imu_data.acc_z, (float)imu_data.acc_x) * 180 / M_PI;
imu.raw[ANGLE_YZ] = atan2f((float)imu_data.acc_z, (float)imu_data.acc_y) * 180 / M_PI;
imu.raw[ANGLE_XY] = atan2f((float)imu_data.acc_x, (float)imu_data.acc_y) * 180 / M_PI;
float angle;
angle = system_getAngle();
if (angle > MAX_INDICATION_ANGLE)
{
angle = MAX_INDICATION_ANGLE;
}
else
if (angle < -MAX_INDICATION_ANGLE)
{
angle = -MAX_INDICATION_ANGLE;
}
// low pass filter the angle measurement
imu.angle = LPF_Update(&lpf, angle);
//imu.filtered[ANGLE_XY] = LPF_Update(&lpf, imu.raw[ANGLE_XY]);
system_setImuData(imu);
// snprintf(data_str, sizeof(data_str),
// "(%.2f, %.2f, %.2f)", xy, yz, xy);
#endif
#if 0
snprintf(data_str, sizeof(data_str),
"(%.1f, %.1f, %.1f) (%.2f, %.2f, %.2f)",
imu.raw[ANGLE_XZ], imu.raw[ANGLE_YZ], imu.raw[ANGLE_XY],
(float)imu_data.acc_x, (float)imu_data.acc_y, (float)imu_data.acc_z);
ESP_LOGI(TAG, "%s", data_str);
#endif
// Update label text in LVGL task
//lv_label_set_text(imu_label, data_str);
}
#endif
vTaskDelay(pdMS_TO_TICKS(100)); // Update every 100ms
}
}
/*********************************
* MAIN ENTRY POINT
********************************/
void app_main(void)
{
/* initialize NVS — it is used to store PHY calibration data */
esp_err_t ret = nvs_flash_init();
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
ESP_ERROR_CHECK(nvs_flash_erase());
ret = nvs_flash_init();
}
ESP_ERROR_CHECK(ret);
init_gpio();
system_init();
// Initialize IMU
ESP_ERROR_CHECK(lsm6dsv_init(22, 21)); // SCL = IO14, SDA = IO15
// Create IMU task
TaskHandle_t h = xTaskCreate(imu_task, "imu_task", 4096, NULL, 5, NULL);
bt_app_init();
gui_start();
gpio_set_level(PIN_NUM_LED_2, 1);
#if 0
keypad_start();
QueueHandle_t q = keypad_getQueue();
uint32_t ev = 0;
//gpio_set_level(PIN_NUM_LED_1, 1);
gpio_set_level(PIN_NUM_LED_2, 1);
// Main loop - LVGL task will run automatically
while (1) {
if (xQueueReceive(q, &ev, pdMS_TO_TICKS(10)) == pdTRUE)
{
switch (ev)
{
case (KEY_UP << KEY_LONG_PRESS):
{
system_setZeroAngle();
break;
}
case (KEY_DOWN << KEY_LONG_PRESS):
{
system_clearZeroAngle();
break;
}
}
}
}
#else
while (1)
{
vTaskDelay(pdMS_TO_TICKS(1000));
}
#endif
}
>>>>>>> 4feb4c0a98bddb1f2a172ea4b195ce31ba18d442