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

Audio working

Browse Source
This commit is contained in:
Brent Perteet
2025-07-13 03:02:18 +00:00
parent 311981ceb4
commit 4feb4c0a98
28 changed files with 7419 additions and 4198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
main/CMakeLists.txt
View File

@@ -1,12 +1,12 @@
idf_component_register(SRCS "system.c" "bubble.c" "keypad.c" "main.c"
"bt_app_core.c"
"gui.c"
"lsm6dsv.c"
INCLUDE_DIRS "."
REQUIRES "driver"
"esp_lcd"
"lvgl"
"esp_lvgl_port"
"esp_timer"
"nvs_flash"
"bt")
idf_component_register(SRCS "bt_app.c" "system.c" "bubble.c" "keypad.c" "main.c"
"gui.c"
"lsm6dsv.c"
INCLUDE_DIRS "."
REQUIRES "driver"
"esp_lcd"
"lvgl"
"esp_lvgl_port"
"esp_timer"
"nvs_flash"
"bt")

18
main/Kconfig.projbuild
View File

@@ -1,9 +1,9 @@
menu "A2DP Example Configuration"
config EXAMPLE_SSP_ENABLED
bool "Secure Simple Pairing"
depends on BT_CLASSIC_ENABLED
default y
help
This enables the Secure Simple Pairing. If disable this option,
Bluedroid will only support Legacy Pairing
endmenu
menu "A2DP Example Configuration"
config EXAMPLE_SSP_ENABLED
bool "Secure Simple Pairing"
depends on BT_CLASSIC_ENABLED
default y
help
This enables the Secure Simple Pairing. If disable this option,
Bluedroid will only support Legacy Pairing
endmenu

2076
main/bt_app_core.c → main/bt_app.c
View File

File diff suppressed because it is too large Load Diff

140
main/bt_app_core.h → main/bt_app.h
View File

@@ -1,70 +1,70 @@
/*
* SPDX-FileCopyrightText: 2021-2022 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Unlicense OR CC0-1.0
*/
#ifndef __BT_APP_CORE_H__
#define __BT_APP_CORE_H__
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
/* log tag */
#define BT_APP_CORE_TAG "BT_APP_CORE"
/* signal for dispatcher */
#define BT_APP_SIG_WORK_DISPATCH (0x01)
/**
* @brief handler for the dispatched work
*
* @param [in] event message event id
* @param [in] param pointer to the parameter
*/
typedef void (* bt_app_cb_t) (uint16_t event, void *param);
/* message to be sent */
typedef struct {
uint16_t sig; /*!< signal to bt_app_task */
uint16_t event; /*!< message event id */
bt_app_cb_t cb; /*!< context switch callback */
void *param; /*!< parameter area needs to be last */
} bt_app_msg_t;
/**
* @brief parameter deep-copy function to be customized
*
* @param [in] p_dest pointer to the destination
* @param [in] p_src pointer to the source
* @param [in] len data length in byte
*/
typedef void (* bt_app_copy_cb_t) (void *p_dest, void *p_src, int len);
/**
* @brief work dispatcher for the application task
*
* @param [in] p_cback handler for the dispatched work (event handler)
* @param [in] event message event id
* @param [in] p_params pointer to the parameter
* @param [in] param_len length of the parameter
* @param [in] p_copy_cback parameter deep-copy function
*
* @return true if work dispatch successfully, false otherwise
*/
bool bt_app_work_dispatch(bt_app_cb_t p_cback, uint16_t event, void *p_params, int param_len, bt_app_copy_cb_t p_copy_cback);
/**
* @brief start up the application task
*/
void bt_app_task_start_up(void);
/**
* @brief shut down the application task
*/
void bt_app_task_shut_down(void);
void bt_app_init(void);
#endif /* __BT_APP_CORE_H__ */
/*
* SPDX-FileCopyrightText: 2021-2022 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Unlicense OR CC0-1.0
*/
#ifndef __BT_APP_CORE_H__
#define __BT_APP_CORE_H__
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
/* log tag */
#define BT_APP_CORE_TAG "BT_APP_CORE"
/* signal for dispatcher */
#define BT_APP_SIG_WORK_DISPATCH (0x01)
/**
* @brief handler for the dispatched work
*
* @param [in] event message event id
* @param [in] param pointer to the parameter
*/
typedef void (* bt_app_cb_t) (uint16_t event, void *param);
/* message to be sent */
typedef struct {
uint16_t sig; /*!< signal to bt_app_task */
uint16_t event; /*!< message event id */
bt_app_cb_t cb; /*!< context switch callback */
void *param; /*!< parameter area needs to be last */
} bt_app_msg_t;
/**
* @brief parameter deep-copy function to be customized
*
* @param [in] p_dest pointer to the destination
* @param [in] p_src pointer to the source
* @param [in] len data length in byte
*/
typedef void (* bt_app_copy_cb_t) (void *p_dest, void *p_src, int len);
/**
* @brief work dispatcher for the application task
*
* @param [in] p_cback handler for the dispatched work (event handler)
* @param [in] event message event id
* @param [in] p_params pointer to the parameter
* @param [in] param_len length of the parameter
* @param [in] p_copy_cback parameter deep-copy function
*
* @return true if work dispatch successfully, false otherwise
*/
bool bt_app_work_dispatch(bt_app_cb_t p_cback, uint16_t event, void *p_params, int param_len, bt_app_copy_cb_t p_copy_cback);
/**
* @brief start up the application task
*/
void bt_app_task_start_up(void);
/**
* @brief shut down the application task
*/
void bt_app_task_shut_down(void);
void bt_app_init(void);
#endif /* __BT_APP_CORE_H__ */

264
main/bubble.c
View File

@@ -1,132 +1,132 @@
#include "bubble.h"
typedef struct {
float min_val;
float max_val;
float cur_val;
} bubble_level_dsc_t;
static void bubble_draw_cb(lv_event_t * e)
{
lvgl_port_lock(0);
lv_obj_t * obj = lv_event_get_target(e);
bubble_level_dsc_t * d = lv_obj_get_user_data(obj);
if(!d) return;
// 1) Get the LVGL draw layer (clipping is automatic)
lv_layer_t * layer = lv_event_get_layer(e);
// 2) Figure out the object's inner rectangle
lv_area_t coords;
lv_obj_get_coords(obj, &coords);
lv_area_t inner = {
.x1 = coords.x1 + 2, .y1 = coords.y1 + 2,
.x2 = coords.x2 - 2, .y2 = coords.y2 - 2
};
// 3) Draw the border
lv_draw_rect_dsc_t rect_dsc;
lv_draw_rect_dsc_init(&rect_dsc);
rect_dsc.border_color = lv_color_hex(0xAAAAAA);
rect_dsc.border_width = 2;
rect_dsc.bg_opa = LV_OPA_TRANSP;
lv_draw_rect(layer, &rect_dsc, &inner);
// 4) Draw guide lines at the center
lv_coord_t mid_x = (inner.x1 + inner.x2) / 2;
lv_coord_t mid_y = (inner.y1 + inner.y2) / 2;
lv_draw_line_dsc_t line_dsc;
lv_draw_line_dsc_init(&line_dsc);
line_dsc.color = lv_color_hex(0x444444);
line_dsc.width = 1;
// Set up points and convert to 'precise' type
lv_point_t tl = { inner.x1 + 10, mid_y };
lv_point_t tr = { inner.x2 - 10, mid_y };
line_dsc.p1 = lv_point_to_precise(&tl);
line_dsc.p2 = lv_point_to_precise(&tr);
lv_draw_line(layer, &line_dsc);
lv_point_t tt = { mid_x, inner.y1 + 10 };
lv_point_t tb = { mid_x, inner.y2 - 10 };
line_dsc.p1 = lv_point_to_precise(&tt);
line_dsc.p2 = lv_point_to_precise(&tb);
lv_draw_line(layer, &line_dsc);
// 5) Compute bubble position (normalized between min_val → max_val)
float norm = (d->cur_val - d->min_val) / (d->max_val - d->min_val);
norm = norm < 0 ? 0 : (norm > 1 ? 1 : norm);
const lv_coord_t bubble_d = 16;
lv_coord_t avail_w = (inner.x2 - inner.x1 + 1) - bubble_d;
lv_coord_t bx = inner.x1 + (lv_coord_t)(norm * avail_w);
lv_coord_t by = mid_y - (bubble_d / 2);
// 6) Draw the bubble as a filled circle (rounded rect)
lv_draw_rect_dsc_t circ_dsc;
lv_draw_rect_dsc_init(&circ_dsc);
circ_dsc.bg_color = lv_color_hex(0x00CC00);
circ_dsc.bg_opa = LV_OPA_COVER;
circ_dsc.radius = bubble_d / 2;
circ_dsc.border_opa = LV_OPA_TRANSP;
lv_area_t circ_area = {
.x1 = bx,
.y1 = by,
.x2 = bx + bubble_d - 1,
.y2 = by + bubble_d - 1
};
lv_draw_rect(layer, &circ_dsc, &circ_area);
lvgl_port_unlock();
}
lv_obj_t * bubble_create(lv_obj_t * parent,
lv_coord_t width, lv_coord_t height,
float min_val, float max_val, float initial)
{
// 1) Create a container object (no special style)
lv_obj_t * obj = lv_obj_create(parent);
lv_obj_set_size(obj, width, height);
// (Optional) center it or let the caller position it:
// lv_obj_center(obj);
// 2) Allocate or attach our descriptor
// In LVGL v9 you can use USER_DATA: need to enable LV_USE_USER_DATA = 1
bubble_level_dsc_t * dsc = lv_malloc(sizeof(bubble_level_dsc_t));
if(!dsc) return NULL; /* handle out-of-memory */
dsc->min_val = min_val;
dsc->max_val = max_val;
dsc->cur_val = initial;
lv_obj_set_user_data(obj, dsc);
// 3) Register our draw callback
lv_obj_add_event_cb(obj, bubble_draw_cb, LV_EVENT_DRAW_MAIN, NULL);
// 4) Make sure the object does NOT get a default background fill
lv_obj_set_style_bg_opa(obj, LV_OPA_TRANSP, 0);
return obj;
}
void bubble_setValue(lv_obj_t * bubble, float v)
{
lvgl_port_lock(0);
// 1) Retrieve the descriptor
bubble_level_dsc_t * d = lv_obj_get_user_data(bubble);
if(!d) return;
// 2) Update the stored value
d->cur_val = v;
// 3) Tell LVGL that object must be re‐drawn
lv_obj_invalidate(bubble);
lvgl_port_unlock();
}
#include "bubble.h"
typedef struct {
float min_val;
float max_val;
float cur_val;
} bubble_level_dsc_t;
static void bubble_draw_cb(lv_event_t * e)
{
lvgl_port_lock(0);
lv_obj_t * obj = lv_event_get_target(e);
bubble_level_dsc_t * d = lv_obj_get_user_data(obj);
if(!d) return;
// 1) Get the LVGL draw layer (clipping is automatic)
lv_layer_t * layer = lv_event_get_layer(e);
// 2) Figure out the object's inner rectangle
lv_area_t coords;
lv_obj_get_coords(obj, &coords);
lv_area_t inner = {
.x1 = coords.x1 + 2, .y1 = coords.y1 + 2,
.x2 = coords.x2 - 2, .y2 = coords.y2 - 2
};
// 3) Draw the border
lv_draw_rect_dsc_t rect_dsc;
lv_draw_rect_dsc_init(&rect_dsc);
rect_dsc.border_color = lv_color_hex(0xAAAAAA);
rect_dsc.border_width = 2;
rect_dsc.bg_opa = LV_OPA_TRANSP;
lv_draw_rect(layer, &rect_dsc, &inner);
// 4) Draw guide lines at the center
lv_coord_t mid_x = (inner.x1 + inner.x2) / 2;
lv_coord_t mid_y = (inner.y1 + inner.y2) / 2;
lv_draw_line_dsc_t line_dsc;
lv_draw_line_dsc_init(&line_dsc);
line_dsc.color = lv_color_hex(0x444444);
line_dsc.width = 1;
// Set up points and convert to 'precise' type
lv_point_t tl = { inner.x1 + 10, mid_y };
lv_point_t tr = { inner.x2 - 10, mid_y };
line_dsc.p1 = lv_point_to_precise(&tl);
line_dsc.p2 = lv_point_to_precise(&tr);
lv_draw_line(layer, &line_dsc);
lv_point_t tt = { mid_x, inner.y1 + 10 };
lv_point_t tb = { mid_x, inner.y2 - 10 };
line_dsc.p1 = lv_point_to_precise(&tt);
line_dsc.p2 = lv_point_to_precise(&tb);
lv_draw_line(layer, &line_dsc);
// 5) Compute bubble position (normalized between min_val → max_val)
float norm = (d->cur_val - d->min_val) / (d->max_val - d->min_val);
norm = norm < 0 ? 0 : (norm > 1 ? 1 : norm);
const lv_coord_t bubble_d = 16;
lv_coord_t avail_w = (inner.x2 - inner.x1 + 1) - bubble_d;
lv_coord_t bx = inner.x1 + (lv_coord_t)(norm * avail_w);
lv_coord_t by = mid_y - (bubble_d / 2);
// 6) Draw the bubble as a filled circle (rounded rect)
lv_draw_rect_dsc_t circ_dsc;
lv_draw_rect_dsc_init(&circ_dsc);
circ_dsc.bg_color = lv_color_hex(0x00CC00);
circ_dsc.bg_opa = LV_OPA_COVER;
circ_dsc.radius = bubble_d / 2;
circ_dsc.border_opa = LV_OPA_TRANSP;
lv_area_t circ_area = {
.x1 = bx,
.y1 = by,
.x2 = bx + bubble_d - 1,
.y2 = by + bubble_d - 1
};
lv_draw_rect(layer, &circ_dsc, &circ_area);
lvgl_port_unlock();
}
lv_obj_t * bubble_create(lv_obj_t * parent,
lv_coord_t width, lv_coord_t height,
float min_val, float max_val, float initial)
{
// 1) Create a container object (no special style)
lv_obj_t * obj = lv_obj_create(parent);
lv_obj_set_size(obj, width, height);
// (Optional) center it or let the caller position it:
// lv_obj_center(obj);
// 2) Allocate or attach our descriptor
// In LVGL v9 you can use USER_DATA: need to enable LV_USE_USER_DATA = 1
bubble_level_dsc_t * dsc = lv_malloc(sizeof(bubble_level_dsc_t));
if(!dsc) return NULL; /* handle out-of-memory */
dsc->min_val = min_val;
dsc->max_val = max_val;
dsc->cur_val = initial;
lv_obj_set_user_data(obj, dsc);
// 3) Register our draw callback
lv_obj_add_event_cb(obj, bubble_draw_cb, LV_EVENT_DRAW_MAIN, NULL);
// 4) Make sure the object does NOT get a default background fill
lv_obj_set_style_bg_opa(obj, LV_OPA_TRANSP, 0);
return obj;
}
void bubble_setValue(lv_obj_t * bubble, float v)
{
lvgl_port_lock(0);
// 1) Retrieve the descriptor
bubble_level_dsc_t * d = lv_obj_get_user_data(bubble);
if(!d) return;
// 2) Update the stored value
d->cur_val = v;
// 3) Tell LVGL that object must be re‐drawn
lv_obj_invalidate(bubble);
lvgl_port_unlock();
}

24
main/bubble.h
View File

@@ -1,13 +1,13 @@
#ifndef BUBBLE_H
#define BUBBLE_H
#include "lvgl.h"
#include "esp_lvgl_port.h"
lv_obj_t * bubble_create(lv_obj_t * parent,
lv_coord_t width, lv_coord_t height,
float min_val, float max_val, float initial);
void bubble_setValue(lv_obj_t * bubble, float v);
#ifndef BUBBLE_H
#define BUBBLE_H
#include "lvgl.h"
#include "esp_lvgl_port.h"
lv_obj_t * bubble_create(lv_obj_t * parent,
lv_coord_t width, lv_coord_t height,
float min_val, float max_val, float initial);
void bubble_setValue(lv_obj_t * bubble, float v);
#endif

55
main/gpio.h
View File

@@ -1,28 +1,29 @@
#ifndef GPIO_H
#define GPIO_H
// Define keypad buttons
#define PIN_NUM_BUTTON_0 36
#define PIN_NUM_BUTTON_1 39
#define PIN_NUM_LED_1 32
#define PIN_NUM_nON 26
// Define GPIO pins
#ifdef DEVKIT
#define PIN_NUM_MOSI 23 // SDA pin for LCD
#define PIN_NUM_CLK 18 // SCL pin for LCD
#define PIN_NUM_CS 2 // CS pin
#define PIN_NUM_DC 12 // Data/Command pin (RS)
#define PIN_NUM_RST 13 // Reset pin
#define PIN_NUM_BK_LIGHT -1 // Backlight control pin, -1 if not used
#else
#define PIN_NUM_MOSI 23 // SDA pin for LCD
#define PIN_NUM_CLK 18 // SCL pin for LCD
#define PIN_NUM_CS 12 // CS pin
#define PIN_NUM_DC 15 // Data/Command pin (RS)
#define PIN_NUM_RST 13 // Reset pin
#define PIN_NUM_BK_LIGHT 5 // Backlight control pin, -1 if not used
#endif
#ifndef GPIO_H
#define GPIO_H
// Define keypad buttons
#define PIN_NUM_BUTTON_0 36
#define PIN_NUM_BUTTON_1 39
#define PIN_NUM_LED_1 32
#define PIN_NUM_LED_2 33
#define PIN_NUM_nON 26
// Define GPIO pins
#ifdef DEVKIT
#define PIN_NUM_MOSI 23 // SDA pin for LCD
#define PIN_NUM_CLK 18 // SCL pin for LCD
#define PIN_NUM_CS 2 // CS pin
#define PIN_NUM_DC 12 // Data/Command pin (RS)
#define PIN_NUM_RST 13 // Reset pin
#define PIN_NUM_BK_LIGHT -1 // Backlight control pin, -1 if not used
#else
#define PIN_NUM_MOSI 23 // SDA pin for LCD
#define PIN_NUM_CLK 18 // SCL pin for LCD
#define PIN_NUM_CS 12 // CS pin
#define PIN_NUM_DC 15 // Data/Command pin (RS)
#define PIN_NUM_RST 13 // Reset pin
#define PIN_NUM_BK_LIGHT 5 // Backlight control pin, -1 if not used
#endif
#endif

1266
main/gui.c
View File

File diff suppressed because it is too large Load Diff

26
main/gui.h
View File

@@ -1,13 +1,13 @@
#ifndef GUI_H
#define GUI_H
typedef enum
{
GUI_MAIN = 0,
GUI_MENU,
GUI_BUBBLE
} GuiMode_t;
void gui_start(void);
#endif
#ifndef GUI_H
#define GUI_H
typedef enum
{
GUI_MAIN = 0,
GUI_MENU,
GUI_BUBBLE
} GuiMode_t;
void gui_start(void);
#endif

8
main/idf_component.yml
View File

@@ -1,4 +1,4 @@
dependencies:
lvgl/lvgl:
version: "^9.2.0"
espressif/esp_lvgl_port: "^2.6.0"
dependencies:
lvgl/lvgl:
version: "^9.2.0"
espressif/esp_lvgl_port: "^2.6.0"

400
main/keypad.c
View File

@@ -1,200 +1,200 @@
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "keypad.h"
// ───────────── CONFIGURATION ─────────────
typedef enum {
BUTTON1_GPIO = GPIO_NUM_36,
BUTTON2_GPIO = GPIO_NUM_39,
} button_gpio_t;
#define BUTTON_COUNT 2
// If your buttons are active‐low, set this to 0. If active‐high, set it to 1.
#define KEYPAD_INACTIVE 1
#define KEYPAD_ACTIVE 0
// Now we run the task every 50 ms and assume that is enough to debounce:
#define DEBOUNCE_INTERVAL_MS 50 // Poll once every 50 ms
// No more stable‐count logic—one sample/50 ms is considered “debounced.”
// Short vs. long press threshold (in ms):
#define LONG_PRESS_MS 1000 // ≥1000 ms = “long press”
// Queue length for pending button events:
#define BTN_EVT_QUEUE_LEN 10
// ───────────── EVENT ENUM & QUEUE HANDLE ─────────────
typedef struct
{
keycode_t key;
gpio_num_t gpio;
int prevState;
int currentState;
TickType_t activeTime;
uint32_t event;
} button_t;
static button_t _buttons[2];
// The queue handle that the debounce task will push events into:
static QueueHandle_t s_btn_evt_queue = NULL;
// Call this to get the queue, so the main task can xQueueReceive():
QueueHandle_t keypad_getQueue(void) {
return s_btn_evt_queue;
}
// ───────────── INTERNAL STATE ─────────────
// Current debounced state: 0 = released, 1 = pressed
static int s_debounced[2] = { 0, 0 };
// Tick count when each button was debounced to “pressed”
static TickType_t s_press_tick[2] = { 0, 0 };
// If we suppress an individual‐button event (because it was part of a “both” press), this is true
static bool s_suppress_event[2] = { false, false };
// Tracks if we are currently in “both buttons pressed” phase:
static bool s_was_both = false;
// Tick count when “both buttons” first became debounced‐pressed:
static TickType_t s_both_press_tick = 0;
// ───────────── TAG FOR LOGGING ─────────────
static const char *TAG = "btn_debounce";
// ───────────── GPIO SETUP ─────────────
static void init_button_gpio(void)
{
// Configure both as inputs with pull‐up (assuming active‐low buttons)
gpio_config_t io_conf = {
.pin_bit_mask = (1ULL << BUTTON1_GPIO) | (1ULL << BUTTON2_GPIO),
.mode = GPIO_MODE_INPUT,
.pull_up_en = GPIO_PULLUP_ENABLE,
.pull_down_en = GPIO_PULLDOWN_DISABLE,
.intr_type = GPIO_INTR_DISABLE
};
ESP_ERROR_CHECK(gpio_config(&io_conf));
_buttons[0].activeTime = 0;
_buttons[0].gpio = BUTTON1_GPIO;
_buttons[0].key = KEY0;
_buttons[0].prevState = KEYPAD_INACTIVE;
_buttons[0].currentState = KEYPAD_INACTIVE;
_buttons[1].activeTime = 0;
_buttons[1].gpio = BUTTON2_GPIO;
_buttons[1].key = KEY1;
_buttons[1].prevState = KEYPAD_INACTIVE;
_buttons[1].currentState = KEYPAD_INACTIVE;
}
// ───────────── BUTTON DEBOUNCE & PRESS‐TYPE TASK ─────────────
static void vButtonDebounceTask(void *arg)
{
(void)arg;
TickType_t now = xTaskGetTickCount();
TickType_t xLastWake = now;
const TickType_t long_press_thresh = pdMS_TO_TICKS(LONG_PRESS_MS);
while (1) {
now = xTaskGetTickCount();
for (int i=0; i < BUTTON_COUNT; ++i)
{
button_t *button = &_buttons[i];
button->currentState = gpio_get_level(button->gpio);
if (button->currentState == KEYPAD_ACTIVE)
{
// just pressed
if (button->prevState == KEYPAD_INACTIVE)
{
button->activeTime = now;
}
else
{
if ((now - button->activeTime) >= long_press_thresh)
{
if (button->event == 0)
{
ESP_LOGI(TAG, "%d LONG", button->key);
button->event = (button->key << KEY_LONG_PRESS);
xQueueSend(s_btn_evt_queue, &button->event, 0);
}
}
}
}
else
if (button->currentState == KEYPAD_INACTIVE)
{
// just released
if (button->prevState == KEYPAD_ACTIVE)
{
if (button->event == 0)
{
ESP_LOGI(TAG, "%d SHORT", button->key);
button->event = (button->key << KEY_SHORT_PRESS);
xQueueSend(s_btn_evt_queue, &button->event, 0);
}
else
{
ESP_LOGI(TAG, "%d LONG RELEASE", button->key);
}
button->event = 0;
}
button->activeTime = 0;
}
button->prevState = button->currentState;
}
// 7) Wait 50 ms and repeat
vTaskDelayUntil(&xLastWake, pdMS_TO_TICKS(DEBOUNCE_INTERVAL_MS));
xLastWake = now;
}
}
// ───────────── PUBLIC “START” FUNCTION ─────────────
void keypad_start(void)
{
// 1) Create the queue for button events
s_btn_evt_queue = xQueueCreate(BTN_EVT_QUEUE_LEN, sizeof(uint32_t));
if (s_btn_evt_queue == NULL) {
ESP_LOGE(TAG, "Failed to create button event queue");
return;
}
// 2) Initialize GPIOs & initial state
init_button_gpio();
// 3) Launch the FreeRTOS task (low priority)
xTaskCreate(vButtonDebounceTask,
"btn_debounce",
2048,
NULL,
tskIDLE_PRIORITY + 1,
NULL);
}
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "keypad.h"
// ───────────── CONFIGURATION ─────────────
typedef enum {
BUTTON1_GPIO = GPIO_NUM_36,
BUTTON2_GPIO = GPIO_NUM_39,
} button_gpio_t;
#define BUTTON_COUNT 2
// If your buttons are active‐low, set this to 0. If active‐high, set it to 1.
#define KEYPAD_INACTIVE 1
#define KEYPAD_ACTIVE 0
// Now we run the task every 50 ms and assume that is enough to debounce:
#define DEBOUNCE_INTERVAL_MS 50 // Poll once every 50 ms
// No more stable‐count logic—one sample/50 ms is considered “debounced.”
// Short vs. long press threshold (in ms):
#define LONG_PRESS_MS 1000 // ≥1000 ms = “long press”
// Queue length for pending button events:
#define BTN_EVT_QUEUE_LEN 10
// ───────────── EVENT ENUM & QUEUE HANDLE ─────────────
typedef struct
{
keycode_t key;
gpio_num_t gpio;
int prevState;
int currentState;
TickType_t activeTime;
uint32_t event;
} button_t;
static button_t _buttons[2];
// The queue handle that the debounce task will push events into:
static QueueHandle_t s_btn_evt_queue = NULL;
// Call this to get the queue, so the main task can xQueueReceive():
QueueHandle_t keypad_getQueue(void) {
return s_btn_evt_queue;
}
// ───────────── INTERNAL STATE ─────────────
// Current debounced state: 0 = released, 1 = pressed
static int s_debounced[2] = { 0, 0 };
// Tick count when each button was debounced to “pressed”
static TickType_t s_press_tick[2] = { 0, 0 };
// If we suppress an individual‐button event (because it was part of a “both” press), this is true
static bool s_suppress_event[2] = { false, false };
// Tracks if we are currently in “both buttons pressed” phase:
static bool s_was_both = false;
// Tick count when “both buttons” first became debounced‐pressed:
static TickType_t s_both_press_tick = 0;
// ───────────── TAG FOR LOGGING ─────────────
static const char *TAG = "btn_debounce";
// ───────────── GPIO SETUP ─────────────
static void init_button_gpio(void)
{
// Configure both as inputs with pull‐up (assuming active‐low buttons)
gpio_config_t io_conf = {
.pin_bit_mask = (1ULL << BUTTON1_GPIO) | (1ULL << BUTTON2_GPIO),
.mode = GPIO_MODE_INPUT,
.pull_up_en = GPIO_PULLUP_ENABLE,
.pull_down_en = GPIO_PULLDOWN_DISABLE,
.intr_type = GPIO_INTR_DISABLE
};
ESP_ERROR_CHECK(gpio_config(&io_conf));
_buttons[0].activeTime = 0;
_buttons[0].gpio = BUTTON1_GPIO;
_buttons[0].key = KEY0;
_buttons[0].prevState = KEYPAD_INACTIVE;
_buttons[0].currentState = KEYPAD_INACTIVE;
_buttons[1].activeTime = 0;
_buttons[1].gpio = BUTTON2_GPIO;
_buttons[1].key = KEY1;
_buttons[1].prevState = KEYPAD_INACTIVE;
_buttons[1].currentState = KEYPAD_INACTIVE;
}
// ───────────── BUTTON DEBOUNCE & PRESS‐TYPE TASK ─────────────
static void vButtonDebounceTask(void *arg)
{
(void)arg;
TickType_t now = xTaskGetTickCount();
TickType_t xLastWake = now;
const TickType_t long_press_thresh = pdMS_TO_TICKS(LONG_PRESS_MS);
while (1) {
now = xTaskGetTickCount();
for (int i=0; i < BUTTON_COUNT; ++i)
{
button_t *button = &_buttons[i];
button->currentState = gpio_get_level(button->gpio);
if (button->currentState == KEYPAD_ACTIVE)
{
// just pressed
if (button->prevState == KEYPAD_INACTIVE)
{
button->activeTime = now;
}
else
{
if ((now - button->activeTime) >= long_press_thresh)
{
if (button->event == 0)
{
ESP_LOGI(TAG, "%d LONG", button->key);
button->event = (button->key << KEY_LONG_PRESS);
xQueueSend(s_btn_evt_queue, &button->event, 0);
}
}
}
}
else
if (button->currentState == KEYPAD_INACTIVE)
{
// just released
if (button->prevState == KEYPAD_ACTIVE)
{
if (button->event == 0)
{
ESP_LOGI(TAG, "%d SHORT", button->key);
button->event = (button->key << KEY_SHORT_PRESS);
xQueueSend(s_btn_evt_queue, &button->event, 0);
}
else
{
ESP_LOGI(TAG, "%d LONG RELEASE", button->key);
}
button->event = 0;
}
button->activeTime = 0;
}
button->prevState = button->currentState;
}
// 7) Wait 50 ms and repeat
vTaskDelayUntil(&xLastWake, pdMS_TO_TICKS(DEBOUNCE_INTERVAL_MS));
xLastWake = now;
}
}
// ───────────── PUBLIC “START” FUNCTION ─────────────
void keypad_start(void)
{
// 1) Create the queue for button events
s_btn_evt_queue = xQueueCreate(BTN_EVT_QUEUE_LEN, sizeof(uint32_t));
if (s_btn_evt_queue == NULL) {
ESP_LOGE(TAG, "Failed to create button event queue");
return;
}
// 2) Initialize GPIOs & initial state
init_button_gpio();
// 3) Launch the FreeRTOS task (low priority)
xTaskCreate(vButtonDebounceTask,
"btn_debounce",
2048,
NULL,
tskIDLE_PRIORITY + 1,
NULL);
}

43
main/keypad.h
View File

@@ -1,22 +1,23 @@
#ifndef _KEYPAD_H
#define _KEYPAD_H
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
typedef enum
{
KEY0 = (1 << 0),
KEY1 = (1 << 1),
} keycode_t;
#define KEY_SHORT_PRESS 0
#define KEY_LONG_PRESS 4
void keypad_start(void);
QueueHandle_t keypad_getQueue(void);
#ifndef _KEYPAD_H
#define _KEYPAD_H
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
typedef enum
{
KEY0 = (1 << 0),
KEY1 = (1 << 1),
} keycode_t;
#define KEY_SHORT_PRESS 0
#define KEY_LONG_PRESS 4
#define KEY_UP KEY0
#define KEY_DOWN KEY1
void keypad_start(void);
QueueHandle_t keypad_getQueue(void);
#endif

696
main/lsm6dsv.c
View File

@@ -1,349 +1,349 @@
#include <math.h>
#include "lsm6dsv.h"
#include "driver/i2c.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "sdkconfig.h"
static const char *TAG = "LSM6DSV";
static const int I2C_PORT = 0;
// I2C helper functions
static esp_err_t i2c_write_reg(uint8_t reg_addr, uint8_t data) {
uint8_t write_buf[2] = {reg_addr, data};
return i2c_master_write_to_device(I2C_PORT, LSM6DSV_ADDR, write_buf, sizeof(write_buf), pdMS_TO_TICKS(100));
}
static esp_err_t i2c_read_reg(uint8_t reg_addr, uint8_t *data) {
return i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg_addr, 1, data, 1, pdMS_TO_TICKS(100));
}
/**
* @brief Converts uint16_t half-precision number into a uint32_t single-precision number.
* @param h half-precision number
* @param f pointer where the result of the conversion is written
* @retval 0
*/
esp_err_t npy_halfbits_to_floatbits(uint16_t h, uint32_t *f)
{
uint16_t h_exp, h_sig;
uint32_t f_sgn, f_exp, f_sig;
h_exp = (h & 0x7c00u);
f_sgn = ((uint32_t)h & 0x8000u) << 16;
switch (h_exp) {
case 0x0000u: /* 0 or subnormal */
h_sig = (h & 0x03ffu);
/* Signed zero */
if (h_sig == 0) {
*f = f_sgn;
return ESP_OK;
}
/* Subnormal */
h_sig <<= 1;
while ((h_sig & 0x0400u) == 0) {
h_sig <<= 1;
h_exp++;
}
f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23;
f_sig = ((uint32_t)(h_sig & 0x03ffu)) << 13;
*f = f_sgn + f_exp + f_sig;
return ESP_OK;
case 0x7c00u: /* inf or NaN */
/* All-ones exponent and a copy of the significand */
*f = f_sgn + 0x7f800000u + (((uint32_t)(h & 0x03ffu)) << 13);
return ESP_OK;
default: /* normalized */
/* Just need to adjust the exponent and shift */
*f = f_sgn + (((uint32_t)(h & 0x7fffu) + 0x1c000u) << 13);
return ESP_OK;
}
}
esp_err_t npy_half_to_float(uint16_t h, float *f)
{
union {
float ret;
uint32_t retbits;
} conv;
npy_halfbits_to_floatbits(h, &conv.retbits);
*f = conv.ret;
return ESP_OK;
}
/**
* @brief Compute quaternions.
* @param quat results of the computation
* @param sflp raw value of the quaternions
* @retval 0
*/
esp_err_t sflp2q(float quat[4], uint16_t sflp[3])
{
float sumsq = 0;
npy_half_to_float(sflp[0], &quat[0]);
npy_half_to_float(sflp[1], &quat[1]);
npy_half_to_float(sflp[2], &quat[2]);
for (uint8_t i = 0; i < 3; i++) {
sumsq += quat[i] * quat[i];
}
if (sumsq > 1.0f) {
float n = sqrtf(sumsq);
quat[0] /= n;
quat[1] /= n;
quat[2] /= n;
sumsq = 1.0f;
}
quat[3] = sqrtf(1.0f - sumsq);
return ESP_OK;
}
void quaternion_to_euler(float qx, float qy, float qz, float qw, float *roll, float *pitch, float *yaw) {
// Roll (X-axis rotation)
*roll = atan2f(2.0f * (qw * qx + qy * qz), 1.0f - 2.0f * (qx * qx + qy * qy));
// Pitch (Y-axis rotation)
*pitch = asinf(2.0f * (qw * qy - qz * qx));
// Yaw (Z-axis rotation)
*yaw = atan2f(2.0f * (qw * qz + qx * qy), 1.0f - 2.0f * (qy * qy + qz * qz));
}
esp_err_t lsm6dsv_init(int scl_pin, int sda_pin) {
// Configure I2C
i2c_config_t conf = {
.mode = I2C_MODE_MASTER,
.sda_io_num = sda_pin,
.scl_io_num = scl_pin,
.sda_pullup_en = GPIO_PULLUP_ENABLE,
.scl_pullup_en = GPIO_PULLUP_ENABLE,
.master.clk_speed = 400000 // 400 KHz
};
esp_err_t ret = i2c_param_config(I2C_PORT, &conf);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "I2C parameter configuration failed");
return ret;
}
ret = i2c_driver_install(I2C_PORT, I2C_MODE_MASTER, 0, 0, 0);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "I2C driver installation failed");
return ret;
}
// Verify device ID
uint8_t who_am_i;
ret = i2c_read_reg(LSM6DSV_WHO_AM_I, &who_am_i);
if (ret != ESP_OK || who_am_i != 0x70) { // 0x70 is the expected WHO_AM_I value
ESP_LOGE(TAG, "Failed to verify device ID or incorrect device (expected: 0x70, got: 0x%02x)", who_am_i);
return ESP_FAIL;
}
// Configure accelerometer (±2g, 104 Hz)
ret = i2c_write_reg(LSM6DSV_CTRL1, 0x44); // ODR = 104 Hz (0x04), ±2g (0x40)
if (ret != ESP_OK) {
ESP_LOGE(TAG, "Failed to configure accelerometer");
return ret;
}
// Verify accelerometer configuration
uint8_t ctrl1_val;
ret = i2c_read_reg(LSM6DSV_CTRL1, &ctrl1_val);
if (ret == ESP_OK) {
ESP_LOGI(TAG, "CTRL1 value: 0x%02x", ctrl1_val);
}
// Configure gyroscope (±250 dps, 104 Hz)
ret = i2c_write_reg(LSM6DSV_CTRL2, 0x04); // ODR = 104 Hz (0x04), ±250 dps (0x40)
if (ret != ESP_OK) {
ESP_LOGE(TAG, "Failed to configure gyroscope");
return ret;
}
// Verify gyroscope configuration
uint8_t regval, tmp1;
ret = i2c_read_reg(LSM6DSV_CTRL2, &regval);
if (ret == ESP_OK) {
ESP_LOGI(TAG, "CTRL2 value: 0x%02x", regval);
}
// ret = i2c_read_reg(LSM6DSV_SFLP_ODR, &regval);
// regval |= SFLP_ODR_15HZ;
// ret = i2c_write_reg(LSM6DSV_SFLP_ODR, regval);
// // get current value of FUNC_CFG_ACCESS
// ret = i2c_read_reg(LSM6DSV_FUNC_CFG_ACCESS, &regval);
// // enable embedded function access
// regval |= LSM6DSV_EMB_FUNC_REG_ACCESS;
// ret = i2c_write_reg(LSM6DSV_FUNC_CFG_ACCESS, regval);
// // enable game vector FIDO
// ret = i2c_write_reg(LSM6DSV_EMB_FUNC_FIFO_EN_A, SFLP_GAME_FIFO_EN);
// // enable sensor fusion algorithm
// ret = i2c_write_reg(LSM6DSV_EMB_FUNC_EN_A, SFLP_GAME_EN);
// ret = i2c_write_reg(LSM6DSV_EMB_FUNC_INIT_A, SFLP_GAME_INIT);
// // restore FUNC_CFG_ACCESS
// regval &= ~LSM6DSV_EMB_FUNC_REG_ACCESS;
// ret = i2c_write_reg(LSM6DSV_FUNC_CFG_ACCESS, regval);
// ret = i2c_write_reg(LSM6DSV_FIFO_CTRL4, 0x06);
// ret = i2c_read_reg(LSM6DSV_FIFO_CTRL4, &regval);
// if (ret == ESP_OK) {
// ESP_LOGI(TAG, "FIFO_CTRL4 value: 0x%02x", regval);
// }
// Add a small delay after configuration
vTaskDelay(pdMS_TO_TICKS(200));
ESP_LOGI(TAG, "LSM6DSV initialized successfully");
return ESP_OK;
}
esp_err_t lsm6dsv_read_data(lsm6dsv_data_t *data) {
if (data == NULL) {
return ESP_ERR_INVALID_ARG;
}
esp_err_t ret;
uint8_t buffer[12];
uint8_t reg = LSM6DSV_OUTX_L_A;
// Check status register
uint8_t status;
ret = i2c_read_reg(LSM6DSV_STATUS, &status);
if (ret == ESP_OK) {
ESP_LOGD(TAG, "Status register: 0x%02x", status);
if (!(status & 0x01)) { // Check if accelerometer data is ready
ESP_LOGW(TAG, "Accelerometer data not ready");
return ESP_ERR_NOT_FOUND;
}
}
// Read accelerometer data (6 bytes)
ret = i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg, 1, buffer, 6, pdMS_TO_TICKS(100));
if (ret != ESP_OK) {
ESP_LOGE(TAG, "Failed to read accelerometer data");
return ret;
}
ESP_LOGD(TAG, "ACC raw: x=0x%02x%02x y=0x%02x%02x z=0x%02x%02x",
buffer[1], buffer[0], buffer[3], buffer[2], buffer[5], buffer[4]);
// Check if gyroscope data is ready
ret = i2c_read_reg(LSM6DSV_STATUS, &status);
if (ret == ESP_OK) {
if (!(status & 0x02)) { // Check if gyroscope data is ready
ESP_LOGW(TAG, "Gyroscope data not ready");
return ESP_ERR_NOT_FOUND;
}
}
// Read gyroscope data (6 bytes)
reg = LSM6DSV_OUTX_L_G;
ret = i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg, 1, buffer + 6, 6, pdMS_TO_TICKS(100));
if (ret != ESP_OK) {
ESP_LOGE(TAG, "Failed to read gyroscope data");
return ret;
}
ESP_LOGD(TAG, "GYRO raw: x=0x%02x%02x y=0x%02x%02x z=0x%02x%02x",
buffer[7], buffer[6], buffer[9], buffer[8], buffer[11], buffer[10]);
// Convert accelerometer data (±2g scale)
const float acc_scale = 2.0f / 32768.0f; // ±2g range
data->acc_x = -(int16_t)(buffer[0] | (buffer[1] << 8)) * acc_scale;
data->acc_y = -(int16_t)(buffer[2] | (buffer[3] << 8)) * acc_scale;
data->acc_z = -(int16_t)(buffer[4] | (buffer[5] << 8)) * acc_scale;
// Convert gyroscope data (±250 dps scale)
const float gyro_scale = 250.0f / 32768.0f; // ±250 dps range
data->gyro_x = (int16_t)(buffer[6] | (buffer[7] << 8)) * gyro_scale;
data->gyro_y = (int16_t)(buffer[8] | (buffer[9] << 8)) * gyro_scale;
data->gyro_z = (int16_t)(buffer[10] | (buffer[11] << 8)) * gyro_scale;
return ESP_OK;
}
uint8_t lsm6dsv_fifo_ready(void)
{
esp_err_t ret;
uint8_t status;
// Check if fifo data is ready
ret = i2c_read_reg(LSM6DSV_FIFO_STATUS2, &status);
if (ret == ESP_OK) {
//ESP_LOGI(TAG, "FIFO_STATUS2: %d", status);
}
ret = i2c_read_reg(LSM6DSV_FIFO_STATUS1, &status);
if (ret == ESP_OK) {
return status;
}
return 0;
}
esp_err_t lsm6dsv_fifo_read(lsm6dsv_fifo_t *fifo)
{
esp_err_t ret;
uint8_t data;
uint8_t reg;
ret = i2c_read_reg(LSM6DSV_FIFO_DATA_OUT_TAG, &data);
reg = LSM6DSV_FIFO_DATA_OUT_X_L;
ret = i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg, 1, fifo->data, 6, pdMS_TO_TICKS(100));
if (ret == ESP_OK)
{
fifo->count = (data & 0x06) >> 1;
fifo->tag = (data & 0xf8) >> 3;
fifo->v[0] = (uint16_t)(fifo->data[0] | (fifo->data[1] << 8));
fifo->v[1] = (uint16_t)(fifo->data[2] | (fifo->data[3] << 8));
fifo->v[2] = (uint16_t)(fifo->data[4] | (fifo->data[5] << 8));
return ESP_OK;
}
return ESP_FAIL;
}
esp_err_t lsm6dsv_getAttitude(lsm6dsv_fifo_t *fifo, float *roll, float *pitch, float *yaw)
{
float q[4];
sflp2q(q, fifo->v);
float w = q[3];
float x = q[0];
float y = q[1];
float z = q[2];
// Yaw (z-axis rotation)
*yaw = atan2(2.0f * (x * y + w * z), w * w + x * x - y * y - z * z);
// Pitch (y-axis rotation)
*pitch = -asin(2.0f * (x * z - w * y));
// Roll (x-axis rotation)
*roll = atan2(2.0f * (w * x + y * z), w * w - x * x - y * y + z * z);
return ESP_OK;
#include <math.h>
#include "lsm6dsv.h"
#include "driver/i2c.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "sdkconfig.h"
static const char *TAG = "LSM6DSV";
static const int I2C_PORT = 0;
// I2C helper functions
static esp_err_t i2c_write_reg(uint8_t reg_addr, uint8_t data) {
uint8_t write_buf[2] = {reg_addr, data};
return i2c_master_write_to_device(I2C_PORT, LSM6DSV_ADDR, write_buf, sizeof(write_buf), pdMS_TO_TICKS(100));
}
static esp_err_t i2c_read_reg(uint8_t reg_addr, uint8_t *data) {
return i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg_addr, 1, data, 1, pdMS_TO_TICKS(100));
}
/**
* @brief Converts uint16_t half-precision number into a uint32_t single-precision number.
* @param h half-precision number
* @param f pointer where the result of the conversion is written
* @retval 0
*/
esp_err_t npy_halfbits_to_floatbits(uint16_t h, uint32_t *f)
{
uint16_t h_exp, h_sig;
uint32_t f_sgn, f_exp, f_sig;
h_exp = (h & 0x7c00u);
f_sgn = ((uint32_t)h & 0x8000u) << 16;
switch (h_exp) {
case 0x0000u: /* 0 or subnormal */
h_sig = (h & 0x03ffu);
/* Signed zero */
if (h_sig == 0) {
*f = f_sgn;
return ESP_OK;
}
/* Subnormal */
h_sig <<= 1;
while ((h_sig & 0x0400u) == 0) {
h_sig <<= 1;
h_exp++;
}
f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23;
f_sig = ((uint32_t)(h_sig & 0x03ffu)) << 13;
*f = f_sgn + f_exp + f_sig;
return ESP_OK;
case 0x7c00u: /* inf or NaN */
/* All-ones exponent and a copy of the significand */
*f = f_sgn + 0x7f800000u + (((uint32_t)(h & 0x03ffu)) << 13);
return ESP_OK;
default: /* normalized */
/* Just need to adjust the exponent and shift */
*f = f_sgn + (((uint32_t)(h & 0x7fffu) + 0x1c000u) << 13);
return ESP_OK;
}
}
esp_err_t npy_half_to_float(uint16_t h, float *f)
{
union {
float ret;
uint32_t retbits;
} conv;
npy_halfbits_to_floatbits(h, &conv.retbits);
*f = conv.ret;
return ESP_OK;
}
/**
* @brief Compute quaternions.
* @param quat results of the computation
* @param sflp raw value of the quaternions
* @retval 0
*/
esp_err_t sflp2q(float quat[4], uint16_t sflp[3])
{
float sumsq = 0;
npy_half_to_float(sflp[0], &quat[0]);
npy_half_to_float(sflp[1], &quat[1]);
npy_half_to_float(sflp[2], &quat[2]);
for (uint8_t i = 0; i < 3; i++) {
sumsq += quat[i] * quat[i];
}
if (sumsq > 1.0f) {
float n = sqrtf(sumsq);
quat[0] /= n;
quat[1] /= n;
quat[2] /= n;
sumsq = 1.0f;
}
quat[3] = sqrtf(1.0f - sumsq);
return ESP_OK;
}
void quaternion_to_euler(float qx, float qy, float qz, float qw, float *roll, float *pitch, float *yaw) {
// Roll (X-axis rotation)
*roll = atan2f(2.0f * (qw * qx + qy * qz), 1.0f - 2.0f * (qx * qx + qy * qy));
// Pitch (Y-axis rotation)
*pitch = asinf(2.0f * (qw * qy - qz * qx));
// Yaw (Z-axis rotation)
*yaw = atan2f(2.0f * (qw * qz + qx * qy), 1.0f - 2.0f * (qy * qy + qz * qz));
}
esp_err_t lsm6dsv_init(int scl_pin, int sda_pin) {
// Configure I2C
i2c_config_t conf = {
.mode = I2C_MODE_MASTER,
.sda_io_num = sda_pin,
.scl_io_num = scl_pin,
.sda_pullup_en = GPIO_PULLUP_ENABLE,
.scl_pullup_en = GPIO_PULLUP_ENABLE,
.master.clk_speed = 400000 // 400 KHz
};
esp_err_t ret = i2c_param_config(I2C_PORT, &conf);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "I2C parameter configuration failed");
return ret;
}
ret = i2c_driver_install(I2C_PORT, I2C_MODE_MASTER, 0, 0, 0);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "I2C driver installation failed");
return ret;
}
// Verify device ID
uint8_t who_am_i;
ret = i2c_read_reg(LSM6DSV_WHO_AM_I, &who_am_i);
if (ret != ESP_OK || who_am_i != 0x70) { // 0x70 is the expected WHO_AM_I value
ESP_LOGE(TAG, "Failed to verify device ID or incorrect device (expected: 0x70, got: 0x%02x)", who_am_i);
return ESP_FAIL;
}
// Configure accelerometer (±2g, 104 Hz)
ret = i2c_write_reg(LSM6DSV_CTRL1, 0x44); // ODR = 104 Hz (0x04), ±2g (0x40)
if (ret != ESP_OK) {
ESP_LOGE(TAG, "Failed to configure accelerometer");
return ret;
}
// Verify accelerometer configuration
uint8_t ctrl1_val;
ret = i2c_read_reg(LSM6DSV_CTRL1, &ctrl1_val);
if (ret == ESP_OK) {
ESP_LOGI(TAG, "CTRL1 value: 0x%02x", ctrl1_val);
}
// Configure gyroscope (±250 dps, 104 Hz)
ret = i2c_write_reg(LSM6DSV_CTRL2, 0x04); // ODR = 104 Hz (0x04), ±250 dps (0x40)
if (ret != ESP_OK) {
ESP_LOGE(TAG, "Failed to configure gyroscope");
return ret;
}
// Verify gyroscope configuration
uint8_t regval, tmp1;
ret = i2c_read_reg(LSM6DSV_CTRL2, &regval);
if (ret == ESP_OK) {
ESP_LOGI(TAG, "CTRL2 value: 0x%02x", regval);
}
// ret = i2c_read_reg(LSM6DSV_SFLP_ODR, &regval);
// regval |= SFLP_ODR_15HZ;
// ret = i2c_write_reg(LSM6DSV_SFLP_ODR, regval);
// // get current value of FUNC_CFG_ACCESS
// ret = i2c_read_reg(LSM6DSV_FUNC_CFG_ACCESS, &regval);
// // enable embedded function access
// regval |= LSM6DSV_EMB_FUNC_REG_ACCESS;
// ret = i2c_write_reg(LSM6DSV_FUNC_CFG_ACCESS, regval);
// // enable game vector FIDO
// ret = i2c_write_reg(LSM6DSV_EMB_FUNC_FIFO_EN_A, SFLP_GAME_FIFO_EN);
// // enable sensor fusion algorithm
// ret = i2c_write_reg(LSM6DSV_EMB_FUNC_EN_A, SFLP_GAME_EN);
// ret = i2c_write_reg(LSM6DSV_EMB_FUNC_INIT_A, SFLP_GAME_INIT);
// // restore FUNC_CFG_ACCESS
// regval &= ~LSM6DSV_EMB_FUNC_REG_ACCESS;
// ret = i2c_write_reg(LSM6DSV_FUNC_CFG_ACCESS, regval);
// ret = i2c_write_reg(LSM6DSV_FIFO_CTRL4, 0x06);
// ret = i2c_read_reg(LSM6DSV_FIFO_CTRL4, &regval);
// if (ret == ESP_OK) {
// ESP_LOGI(TAG, "FIFO_CTRL4 value: 0x%02x", regval);
// }
// Add a small delay after configuration
vTaskDelay(pdMS_TO_TICKS(200));
ESP_LOGI(TAG, "LSM6DSV initialized successfully");
return ESP_OK;
}
esp_err_t lsm6dsv_read_data(lsm6dsv_data_t *data) {
if (data == NULL) {
return ESP_ERR_INVALID_ARG;
}
esp_err_t ret;
uint8_t buffer[12];
uint8_t reg = LSM6DSV_OUTX_L_A;
// Check status register
uint8_t status;
ret = i2c_read_reg(LSM6DSV_STATUS, &status);
if (ret == ESP_OK) {
ESP_LOGD(TAG, "Status register: 0x%02x", status);
if (!(status & 0x01)) { // Check if accelerometer data is ready
ESP_LOGW(TAG, "Accelerometer data not ready");
return ESP_ERR_NOT_FOUND;
}
}
// Read accelerometer data (6 bytes)
ret = i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg, 1, buffer, 6, pdMS_TO_TICKS(100));
if (ret != ESP_OK) {
ESP_LOGE(TAG, "Failed to read accelerometer data");
return ret;
}
ESP_LOGD(TAG, "ACC raw: x=0x%02x%02x y=0x%02x%02x z=0x%02x%02x",
buffer[1], buffer[0], buffer[3], buffer[2], buffer[5], buffer[4]);
// Check if gyroscope data is ready
ret = i2c_read_reg(LSM6DSV_STATUS, &status);
if (ret == ESP_OK) {
if (!(status & 0x02)) { // Check if gyroscope data is ready
ESP_LOGW(TAG, "Gyroscope data not ready");
return ESP_ERR_NOT_FOUND;
}
}
// Read gyroscope data (6 bytes)
reg = LSM6DSV_OUTX_L_G;
ret = i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg, 1, buffer + 6, 6, pdMS_TO_TICKS(100));
if (ret != ESP_OK) {
ESP_LOGE(TAG, "Failed to read gyroscope data");
return ret;
}
ESP_LOGD(TAG, "GYRO raw: x=0x%02x%02x y=0x%02x%02x z=0x%02x%02x",
buffer[7], buffer[6], buffer[9], buffer[8], buffer[11], buffer[10]);
// Convert accelerometer data (±2g scale)
const float acc_scale = 2.0f / 32768.0f; // ±2g range
data->acc_x = -(int16_t)(buffer[0] | (buffer[1] << 8)) * acc_scale;
data->acc_y = -(int16_t)(buffer[2] | (buffer[3] << 8)) * acc_scale;
data->acc_z = -(int16_t)(buffer[4] | (buffer[5] << 8)) * acc_scale;
// Convert gyroscope data (±250 dps scale)
const float gyro_scale = 250.0f / 32768.0f; // ±250 dps range
data->gyro_x = (int16_t)(buffer[6] | (buffer[7] << 8)) * gyro_scale;
data->gyro_y = (int16_t)(buffer[8] | (buffer[9] << 8)) * gyro_scale;
data->gyro_z = (int16_t)(buffer[10] | (buffer[11] << 8)) * gyro_scale;
return ESP_OK;
}
uint8_t lsm6dsv_fifo_ready(void)
{
esp_err_t ret;
uint8_t status;
// Check if fifo data is ready
ret = i2c_read_reg(LSM6DSV_FIFO_STATUS2, &status);
if (ret == ESP_OK) {
//ESP_LOGI(TAG, "FIFO_STATUS2: %d", status);
}
ret = i2c_read_reg(LSM6DSV_FIFO_STATUS1, &status);
if (ret == ESP_OK) {
return status;
}
return 0;
}
esp_err_t lsm6dsv_fifo_read(lsm6dsv_fifo_t *fifo)
{
esp_err_t ret;
uint8_t data;
uint8_t reg;
ret = i2c_read_reg(LSM6DSV_FIFO_DATA_OUT_TAG, &data);
reg = LSM6DSV_FIFO_DATA_OUT_X_L;
ret = i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg, 1, fifo->data, 6, pdMS_TO_TICKS(100));
if (ret == ESP_OK)
{
fifo->count = (data & 0x06) >> 1;
fifo->tag = (data & 0xf8) >> 3;
fifo->v[0] = (uint16_t)(fifo->data[0] | (fifo->data[1] << 8));
fifo->v[1] = (uint16_t)(fifo->data[2] | (fifo->data[3] << 8));
fifo->v[2] = (uint16_t)(fifo->data[4] | (fifo->data[5] << 8));
return ESP_OK;
}
return ESP_FAIL;
}
esp_err_t lsm6dsv_getAttitude(lsm6dsv_fifo_t *fifo, float *roll, float *pitch, float *yaw)
{
float q[4];
sflp2q(q, fifo->v);
float w = q[3];
float x = q[0];
float y = q[1];
float z = q[2];
// Yaw (z-axis rotation)
*yaw = atan2(2.0f * (x * y + w * z), w * w + x * x - y * y - z * z);
// Pitch (y-axis rotation)
*pitch = -asin(2.0f * (x * z - w * y));
// Roll (x-axis rotation)
*roll = atan2(2.0f * (w * x + y * z), w * w - x * x - y * y + z * z);
return ESP_OK;
}

198
main/lsm6dsv.h
View File

@@ -1,99 +1,99 @@
#pragma once
#include <stdint.h>
#include "esp_err.h"
// LSM6DSV Register Addresses
#define LSM6DSV_ADDR 0x6A // I2C address (SA0=0)
#define LSM6DSV_EMB_FUNC_EN_A 0x04
#define SFLP_GAME_EN (1 << 1)
#define LSM6DSV_FUNC_CFG_ACCESS 0x01
#define LSM6DSV_EMB_FUNC_REG_ACCESS (1 << 7)
// FIFO Registers
#define LSM6DSV_FIFO_CTRL1 0x07 // FIFO Control Register 1
#define LSM6DSV_FIFO_CTRL2 0x08 // FIFO Control Register 2
#define LSM6DSV_FIFO_CTRL3 0x09 // FIFO Control Register 3
#define LSM6DSV_FIFO_CTRL4 0x0A // FIFO Control Register 4
#define FIFO_MODE_CONTINUOUS 0x06
#define LSM6DSV_FIFO_STATUS1 0x1B // FIFO Status Register 1
#define LSM6DSV_FIFO_STATUS2 0x1C // FIFO Status Register 2
#define LSM6DSV_FIFO_DATA_OUT_TAG 0x78 // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_X_L 0x79 // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_X_H 0x7A // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Y_L 0x7B // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Y_H 0x7C // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Z_L 0x7D // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Z_H 0x7E // FIFO Data Output Register
#define LSM6DSV_WHO_AM_I 0x0F // Device ID register
#define LSM6DSV_CTRL1 0x10 // Control register 1
#define LSM6DSV_CTRL2 0x11 // Control register 2
#define LSM6DSV_FIFO_STATUS1 0x1B
#define LSM6DSV_STATUS 0x1E // Status register
#define LSM6DSV_OUTX_L_A 0x28 // Accelerometer X-axis LSB
#define LSM6DSV_OUTX_H_A 0x29 // Accelerometer X-axis MSB
#define LSM6DSV_OUTY_L_A 0x2A // Accelerometer Y-axis LSB
#define LSM6DSV_OUTY_H_A 0x2B // Accelerometer Y-axis MSB
#define LSM6DSV_OUTZ_L_A 0x2C // Accelerometer Z-axis LSB
#define LSM6DSV_OUTZ_H_A 0x2D // Accelerometer Z-axis MSB
#define LSM6DSV_OUTX_L_G 0x22 // Gyroscope X-axis LSB
#define LSM6DSV_OUTX_H_G 0x23 // Gyroscope X-axis MSB
#define LSM6DSV_OUTY_L_G 0x24 // Gyroscope Y-axis LSB
#define LSM6DSV_OUTY_H_G 0x25 // Gyroscope Y-axis MSB
#define LSM6DSV_OUTZ_L_G 0x26 // Gyroscope Z-axis LSB
#define LSM6DSV_OUTZ_H_G 0x27 // Gyroscope Z-axis MSB
#define LSM6DSV_SFLP_ODR 0x5E
#define SFLP_ODR_15HZ (0 << 3)
#define SFLP_ODR_30HZ (1 << 3)
#define SFLP_ODR_60HZ (2 << 3)
#define SFLP_ODR_120HZ (3 << 3)
#define SFLP_ODR_240HZ (4 << 3)
#define SFLP_ODR_480HZ (5 << 3)
#define LSM6DSV_EMB_FUNC_FIFO_EN_A 0x44
#define SFLP_GAME_FIFO_EN (1 << 1)
#define LSM6DSV_EMB_FUNC_INIT_A 0x66
#define SFLP_GAME_INIT (1 << 1)
typedef struct lsm6dsv_fifo_s
{
uint8_t tag;
uint8_t count;
uint8_t data[6];
uint16_t v[3];
} lsm6dsv_fifo_t;
// Sensor data structure
typedef struct {
float acc_x;
float acc_y;
float acc_z;
float gyro_x;
float gyro_y;
float gyro_z;
} lsm6dsv_data_t;
// Function declarations
esp_err_t lsm6dsv_init(int scl_pin, int sda_pin);
esp_err_t lsm6dsv_read_data(lsm6dsv_data_t *data);
uint8_t lsm6dsv_fifo_ready(void);
esp_err_t lsm6dsv_fifo_read(lsm6dsv_fifo_t *fifo);
esp_err_t lsm6dsv_getAttitude(lsm6dsv_fifo_t *fifo, float *roll, float *pitch, float *yaw);
#pragma once
#include <stdint.h>
#include "esp_err.h"
// LSM6DSV Register Addresses
#define LSM6DSV_ADDR 0x6A // I2C address (SA0=0)
#define LSM6DSV_EMB_FUNC_EN_A 0x04
#define SFLP_GAME_EN (1 << 1)
#define LSM6DSV_FUNC_CFG_ACCESS 0x01
#define LSM6DSV_EMB_FUNC_REG_ACCESS (1 << 7)
// FIFO Registers
#define LSM6DSV_FIFO_CTRL1 0x07 // FIFO Control Register 1
#define LSM6DSV_FIFO_CTRL2 0x08 // FIFO Control Register 2
#define LSM6DSV_FIFO_CTRL3 0x09 // FIFO Control Register 3
#define LSM6DSV_FIFO_CTRL4 0x0A // FIFO Control Register 4
#define FIFO_MODE_CONTINUOUS 0x06
#define LSM6DSV_FIFO_STATUS1 0x1B // FIFO Status Register 1
#define LSM6DSV_FIFO_STATUS2 0x1C // FIFO Status Register 2
#define LSM6DSV_FIFO_DATA_OUT_TAG 0x78 // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_X_L 0x79 // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_X_H 0x7A // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Y_L 0x7B // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Y_H 0x7C // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Z_L 0x7D // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Z_H 0x7E // FIFO Data Output Register
#define LSM6DSV_WHO_AM_I 0x0F // Device ID register
#define LSM6DSV_CTRL1 0x10 // Control register 1
#define LSM6DSV_CTRL2 0x11 // Control register 2
#define LSM6DSV_FIFO_STATUS1 0x1B
#define LSM6DSV_STATUS 0x1E // Status register
#define LSM6DSV_OUTX_L_A 0x28 // Accelerometer X-axis LSB
#define LSM6DSV_OUTX_H_A 0x29 // Accelerometer X-axis MSB
#define LSM6DSV_OUTY_L_A 0x2A // Accelerometer Y-axis LSB
#define LSM6DSV_OUTY_H_A 0x2B // Accelerometer Y-axis MSB
#define LSM6DSV_OUTZ_L_A 0x2C // Accelerometer Z-axis LSB
#define LSM6DSV_OUTZ_H_A 0x2D // Accelerometer Z-axis MSB
#define LSM6DSV_OUTX_L_G 0x22 // Gyroscope X-axis LSB
#define LSM6DSV_OUTX_H_G 0x23 // Gyroscope X-axis MSB
#define LSM6DSV_OUTY_L_G 0x24 // Gyroscope Y-axis LSB
#define LSM6DSV_OUTY_H_G 0x25 // Gyroscope Y-axis MSB
#define LSM6DSV_OUTZ_L_G 0x26 // Gyroscope Z-axis LSB
#define LSM6DSV_OUTZ_H_G 0x27 // Gyroscope Z-axis MSB
#define LSM6DSV_SFLP_ODR 0x5E
#define SFLP_ODR_15HZ (0 << 3)
#define SFLP_ODR_30HZ (1 << 3)
#define SFLP_ODR_60HZ (2 << 3)
#define SFLP_ODR_120HZ (3 << 3)
#define SFLP_ODR_240HZ (4 << 3)
#define SFLP_ODR_480HZ (5 << 3)
#define LSM6DSV_EMB_FUNC_FIFO_EN_A 0x44
#define SFLP_GAME_FIFO_EN (1 << 1)
#define LSM6DSV_EMB_FUNC_INIT_A 0x66
#define SFLP_GAME_INIT (1 << 1)
typedef struct lsm6dsv_fifo_s
{
uint8_t tag;
uint8_t count;
uint8_t data[6];
uint16_t v[3];
} lsm6dsv_fifo_t;
// Sensor data structure
typedef struct {
float acc_x;
float acc_y;
float acc_z;
float gyro_x;
float gyro_y;
float gyro_z;
} lsm6dsv_data_t;
// Function declarations
esp_err_t lsm6dsv_init(int scl_pin, int sda_pin);
esp_err_t lsm6dsv_read_data(lsm6dsv_data_t *data);
uint8_t lsm6dsv_fifo_ready(void);
esp_err_t lsm6dsv_fifo_read(lsm6dsv_fifo_t *fifo);
esp_err_t lsm6dsv_getAttitude(lsm6dsv_fifo_t *fifo, float *roll, float *pitch, float *yaw);

2256
main/lv_conf.h
View File

File diff suppressed because it is too large Load Diff

557
main/main.c
View File

@@ -1,266 +1,291 @@
/*
* 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_core.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);
io_conf.mode = GPIO_MODE_OUTPUT;
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.pull_up_en = GPIO_PULLUP_DISABLE;
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.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, 0.2, 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;
if (imu.raw[ANGLE_XY] > MAX_INDICATION_ANGLE)
{
angle = MAX_INDICATION_ANGLE;
}
else
if (imu.raw[ANGLE_XY] > MAX_INDICATION_ANGLE)
{
angle = -MAX_INDICATION_ANGLE;
}
else
{
angle = imu.raw[ANGLE_XY];
}
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)",
xz, yz, 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);
gui_start();
//bt_app_init();
// Main loop - LVGL task will run automatically
while (1) {
// int level = gpio_get_level(PIN_NUM_BUTTON_1); // Read input GPIO
// gpio_set_level(PIN_NUM_LED_1, level);
//gpio_set_level(PIN_NUM_nON, (level ? 0 : 1));
vTaskDelay(pdMS_TO_TICKS(100));
//gui_service();
}
}
/*
* 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
}

271
main/system.c
View File

@@ -1,104 +1,169 @@
#include "system.h"
#include "esp_log.h"
static SystemState_t _systemState;
static EventGroupHandle_t _systemEvent;
static EventManager_t _eventManager;
void system_init(void)
{
EventGroupHandle_t evt = xEventGroupCreate();
_eventManager.count = 0;
_eventManager.mutex = xSemaphoreCreateMutex();
}
void system_setImuData(ImuData_t imu)
{
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
_systemState.imu = imu;
xSemaphoreGive(_eventManager.mutex);
//ESP_LOGI("g", "New IMU Data: %.2f", xy);
system_notifyAll(EM_EVENT_NEW_DATA);
}
ImuData_t system_getImuData(void)
{
ImuData_t imu;
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
imu = _systemState.imu;
xSemaphoreGive(_eventManager.mutex);
return imu;
}
void system_waitForEvent(void)
{
}
SystemState_t *system_getState(void)
{
return &_systemState;
}
BaseType_t system_subscribe(TaskHandle_t task) {
EventManager_t *em = &_eventManager;
ESP_LOGI("g", "Subscribe: %p", task);
if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdFALSE) {
return pdFAIL;
}
if (em->count < EM_MAX_SUBSCRIBERS) {
// avoid duplicates?
ESP_LOGI("g", "PASS");
em->subscribers[em->count++] = task;
xSemaphoreGive(em->mutex);
return pdPASS;
}
xSemaphoreGive(em->mutex);
return pdFAIL; // full
}
BaseType_t system_unsubscribe(TaskHandle_t task) {
EventManager_t *em = &_eventManager;
BaseType_t removed = pdFAIL;
if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdTRUE) {
for (size_t i = 0; i < em->count; ++i) {
if (em->subscribers[i] == task) {
// shift down
for (size_t j = i; j + 1 < em->count; ++j)
em->subscribers[j] = em->subscribers[j+1];
em->count--;
removed = pdPASS;
break;
}
}
xSemaphoreGive(em->mutex);
}
return removed;
}
void system_notifyAll(uint32_t eventBits) {
EventManager_t *em = &_eventManager;
if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdTRUE) {
for (size_t i = 0; i < em->count; ++i) {
// set the bits in each task's notification value
//ESP_LOGI("g", "Notify: %p", em->subscribers[i]);
xTaskNotify(em->subscribers[i],
eventBits,
eSetBits);
}
xSemaphoreGive(em->mutex);
}
#include "system.h"
#include "esp_log.h"
static SystemState_t _systemState;
static EventGroupHandle_t _systemEvent;
static EventManager_t _eventManager;
void system_init(void)
{
_systemState.zeroAngle = 0.0f;
_systemState.primaryAxis = PRIMARY_AXIS;
EventGroupHandle_t evt = xEventGroupCreate();
_eventManager.count = 0;
_eventManager.mutex = xSemaphoreCreateMutex();
}
int system_getPrimaryAxis(void)
{
int axis;
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
axis = _systemState.primaryAxis;
xSemaphoreGive(_eventManager.mutex);
return axis;
}
float system_getAngle(void)
{
float angle;
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
angle = _systemState.imu.raw[_systemState.primaryAxis] - _systemState.zeroAngle;
xSemaphoreGive(_eventManager.mutex);
return angle;
}
void system_setZeroAngle(void)
{
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
_systemState.zeroAngle = _systemState.imu.raw[_systemState.primaryAxis];
ESP_LOGI("system", "Zero: %.1f", _systemState.zeroAngle);
xSemaphoreGive(_eventManager.mutex);
}
void system_clearZeroAngle(void)
{
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
_systemState.zeroAngle = 0.0f;
ESP_LOGI("system", "Zero: %.1f", _systemState.zeroAngle);
xSemaphoreGive(_eventManager.mutex);
}
float system_getZeroAngle(void)
{
float angle;
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
angle = _systemState.zeroAngle;
xSemaphoreGive(_eventManager.mutex);
return angle;
}
void system_setImuData(ImuData_t imu)
{
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
_systemState.imu = imu;
xSemaphoreGive(_eventManager.mutex);
//ESP_LOGI("g", "New IMU Data: %.2f", xy);
system_notifyAll(EM_EVENT_NEW_DATA);
}
ImuData_t system_getImuData(void)
{
ImuData_t imu;
xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
imu = _systemState.imu;
xSemaphoreGive(_eventManager.mutex);
return imu;
}
void system_waitForEvent(void)
{
}
SystemState_t *system_getState(void)
{
return &_systemState;
}
BaseType_t system_subscribe(TaskHandle_t task) {
EventManager_t *em = &_eventManager;
ESP_LOGI("g", "Subscribe: %p", task);
if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdFALSE) {
return pdFAIL;
}
if (em->count < EM_MAX_SUBSCRIBERS) {
// avoid duplicates?
ESP_LOGI("g", "PASS");
em->subscribers[em->count++] = task;
xSemaphoreGive(em->mutex);
return pdPASS;
}
xSemaphoreGive(em->mutex);
return pdFAIL; // full
}
BaseType_t system_unsubscribe(TaskHandle_t task) {
EventManager_t *em = &_eventManager;
BaseType_t removed = pdFAIL;
if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdTRUE) {
for (size_t i = 0; i < em->count; ++i) {
if (em->subscribers[i] == task) {
// shift down
for (size_t j = i; j + 1 < em->count; ++j)
em->subscribers[j] = em->subscribers[j+1];
em->count--;
removed = pdPASS;
break;
}
}
xSemaphoreGive(em->mutex);
}
return removed;
}
void system_notifyAll(uint32_t eventBits) {
EventManager_t *em = &_eventManager;
if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdTRUE) {
for (size_t i = 0; i < em->count; ++i) {
// set the bits in each task's notification value
//ESP_LOGI("g", "Notify: %p", em->subscribers[i]);
xTaskNotify(em->subscribers[i],
eventBits,
eSetBits);
}
xSemaphoreGive(em->mutex);
}
}

135
main/system.h
View File

@@ -1,62 +1,75 @@
#ifndef SYSTEM_H
#define SYSTEM_H
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
enum
{
ANGLE_XY = 0,
ANGLE_XZ,
ANGLE_YZ,
};
typedef struct
{
float raw[3];
float filtered[3];
float angle;
} ImuData_t;
typedef struct SystemState_s
{
ImuData_t imu;
EventGroupHandle_t event;
} SystemState_t;
#define MAX_INDICATION_ANGLE 10.0f
#define EM_MAX_SUBSCRIBERS 8 // tweak as needed
#define EM_EVENT_NEW_DATA (1UL<<0)
#define EM_EVENT_ERROR (1UL<<1)
// …add more event bit-masks here…
typedef struct {
TaskHandle_t subscribers[EM_MAX_SUBSCRIBERS];
size_t count;
SemaphoreHandle_t mutex;
} EventManager_t;
void system_init(void);
void system_setImuData(ImuData_t imu);
ImuData_t system_getImuData(void);
// Subscribe (register) current task to receive events
BaseType_t system_subscribe(TaskHandle_t task);
// Unsubscribe if you ever need
BaseType_t system_unsubscribe(TaskHandle_t task);
// Notify all subscribers of one or more event bits
void system_notifyAll(uint32_t eventBits);
#ifndef SYSTEM_H
#define SYSTEM_H
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#define DISABLE_GUI
enum
{
ANGLE_XY = 0,
ANGLE_XZ,
ANGLE_YZ,
};
typedef struct
{
float raw[3];
float filtered[3];
float angle;
} ImuData_t;
typedef struct SystemState_s
{
ImuData_t imu;
EventGroupHandle_t event;
float zeroAngle;
int primaryAxis;
} SystemState_t;
#define MAX_INDICATION_ANGLE 5.0f
#define FILTER_COEFF 0.2f // 0 to 1 Smaller number is heavier filter
#define PRIMARY_AXIS ANGLE_YZ
#define EM_MAX_SUBSCRIBERS 8 // tweak as needed
#define EM_EVENT_NEW_DATA (1UL<<0)
#define EM_EVENT_ERROR (1UL<<1)
// …add more event bit-masks here…
typedef struct {
TaskHandle_t subscribers[EM_MAX_SUBSCRIBERS];
size_t count;
SemaphoreHandle_t mutex;
} EventManager_t;
void system_init(void);
void system_setImuData(ImuData_t imu);
ImuData_t system_getImuData(void);
int system_getPrimaryAxis(void);
float system_getAngle(void);
void system_setZeroAngle(void);
void system_clearZeroAngle(void);
float system_getZeroAngle(void);
// Subscribe (register) current task to receive events
BaseType_t system_subscribe(TaskHandle_t task);
// Unsubscribe if you ever need
BaseType_t system_unsubscribe(TaskHandle_t task);
// Notify all subscribers of one or more event bits
void system_notifyAll(uint32_t eventBits);
#endif