332 lines
8.8 KiB
C
332 lines
8.8 KiB
C
/*
|
|
* 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 "esp_heap_caps.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 const char *TAG = "main";
|
|
|
|
/*********************************
|
|
* STATIC FUNCTION DECLARATIONS
|
|
********************************/
|
|
|
|
static void print_heap_info(const char* tag) {
|
|
size_t free_heap = esp_get_free_heap_size();
|
|
size_t min_free_heap = esp_get_minimum_free_heap_size();
|
|
size_t largest_block = heap_caps_get_largest_free_block(MALLOC_CAP_DEFAULT);
|
|
|
|
ESP_LOGI(TAG, "[%s] Free heap: %zu bytes, Min free: %zu bytes, Largest block: %zu bytes",
|
|
tag, free_heap, min_free_heap, largest_block);
|
|
}
|
|
typedef struct {
|
|
float alpha; // smoothing factor, 0<alpha<1
|
|
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.
|
|
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 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;
|
|
}
|
|
|
|
|
|
/*********************************
|
|
* STATIC VARIABLE DEFINITIONS
|
|
********************************/
|
|
|
|
/*********************************
|
|
* 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)
|
|
{
|
|
print_heap_info("STARTUP");
|
|
|
|
/* 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);
|
|
print_heap_info("POST_NVS");
|
|
|
|
init_gpio();
|
|
print_heap_info("POST_GPIO");
|
|
|
|
system_init();
|
|
print_heap_info("POST_SYSTEM");
|
|
|
|
// Initialize IMU
|
|
ESP_ERROR_CHECK(lsm6dsv_init(22, 21)); // SCL = IO14, SDA = IO15
|
|
print_heap_info("POST_IMU");
|
|
|
|
// Create IMU task
|
|
TaskHandle_t h = xTaskCreate(imu_task, "imu_task", 2048, NULL, 5, NULL);
|
|
print_heap_info("POST_IMU_TASK");
|
|
|
|
bt_app_init();
|
|
print_heap_info("POST_BLUETOOTH");
|
|
|
|
gui_start();
|
|
print_heap_info("POST_GUI");
|
|
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
|
|
}
|