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

Merge branch 'master' of https://git.sparksoftdesign.com/firstpass/soundshot

Browse Source
This commit is contained in:
Brent Perteet
2025-07-22 14:09:47 -05:00
parent dc2885e02e 4feb4c0a98
commit 4697b369db
28 changed files with 7441 additions and 2090 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
main/CMakeLists.txt
View File

@@ -1,3 +1,17 @@
<<<<<<< HEAD
idf_component_register(SRCS "system.c.LOCAL.c" "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")
=======
idf_component_register(SRCS "bt_app.c" "system.c" "bubble.c" "keypad.c" "main.c"
"gui.c"
@@ -10,3 +24,5 @@ idf_component_register(SRCS "bt_app.c" "system.c" "bubble.c" "keypad.c" "main.c"
"esp_timer"
"nvs_flash"
"bt")
>>>>>>> 4feb4c0a98bddb1f2a172ea4b195ce31ba18d442

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

1159
main/bt_app.c
View File

File diff suppressed because it is too large Load Diff

140
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

31
main/gpio.h
View File

@@ -1,3 +1,4 @@
<<<<<<< HEAD
#ifndef GPIO_H
#define GPIO_H
@@ -26,4 +27,34 @@
#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
>>>>>>> 4feb4c0a98bddb1f2a172ea4b195ce31ba18d442
#endif

621
main/gui.c
View File

@@ -1,3 +1,4 @@
<<<<<<< HEAD
#include <stdio.h>
#include <math.h>
#include "freertos/FreeRTOS.h"
@@ -616,4 +617,624 @@ static void gui_task(void)
=======
#include <stdio.h>
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_lcd_panel_io.h"
#include "esp_lcd_panel_vendor.h"
#include "esp_lcd_panel_ops.h"
#include "driver/gpio.h"
#include "driver/spi_master.h"
#include "esp_err.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "lvgl.h"
#include "esp_lvgl_port.h"
#include "gui.h"
#include "gpio.h"
#include "keypad.h"
#include "bubble.h"
#include "system.h"
#define DEVKIT
#undef DEVKIT
static const char *TAG = "gui";
#define UNLOCK() lvgl_port_unlock()
#define LOCK() lvgl_port_lock(0)
// LCD Pin Configuration
#define LCD_PIXEL_CLOCK_HZ (5 * 1000 * 1000)
#define LCD_SPI_HOST SPI2_HOST
#define PIN_NUM_nON 26
// ST7735S properties
#define LCD_H_RES 160
#define LCD_V_RES 80
#define LCD_CMD_BITS 8
#define LCD_PARAM_BITS 8
#define LCD_COLOR_SPACE ESP_LCD_COLOR_SPACE_RGB
#define LCD_BITS_PER_PIXEL 16
static esp_lcd_panel_handle_t panel_handle = NULL;
esp_lcd_panel_io_handle_t io_handle = NULL;
static lv_disp_t *disp = NULL;
static lv_obj_t *imu_label = NULL; // Label for IMU data
static lv_style_t style_mono8;
typedef struct
{
int selected;
int count;
lv_obj_t *obj;
} menu_context_t;
static void gui_task(void);
static void createBubble(lv_obj_t * scr);
static void build_scrollable_menu(void);
static void currentFocusIndex(menu_context_t *ctx);
#define MAX_ITEMS 10
#define VISIBLE_ITEMS 3
#define MENU_MAX_STRING_LENGTH 30
static const char *menu_items[MAX_ITEMS] = {
"V-Moda Crossfade", "Item 2", "Item 3", "Item 4",
"Item 5", "Item 6", "Item 7", "Item 8",
"Item 9", "Item 10"
};
static lv_obj_t *list;
static lv_obj_t *buttons[MAX_ITEMS];
static int selected_index = 0;
static lv_obj_t *_bubble = NULL;
static lv_obj_t * _menu = NULL;
static lv_obj_t *_currentPage = NULL;
static GuiMode_t _mode = GUI_BUBBLE;
/* 1. Prepare a default (unfocused) style */
static lv_style_t _styleUnfocusedBtn;
/* 2. Prepare a focus style */
static lv_style_t _styleFocusedBtn;
static menu_context_t _menuContext;
static bool notify_lvgl_flush_ready(void *user_ctx) {
if (disp) {
lv_display_flush_ready(disp);
}
return true;
}
static void create_lvgl_demo(void)
{
lvgl_port_lock(0);
// Create a screen with black background
lv_obj_t *scr = lv_scr_act();
createBubble(scr);
//build_scrollable_menu();
lvgl_port_unlock();
}
static void lcd_init(void)
{
ESP_LOGI(TAG, "Initialize SPI bus");
spi_bus_config_t buscfg = {
.sclk_io_num = PIN_NUM_CLK,
.mosi_io_num = PIN_NUM_MOSI,
.miso_io_num = -1,
.quadwp_io_num = -1,
.quadhd_io_num = -1,
.max_transfer_sz = LCD_H_RES * LCD_V_RES * 2
};
ESP_ERROR_CHECK(spi_bus_initialize(LCD_SPI_HOST, &buscfg, SPI_DMA_CH_AUTO));
ESP_LOGI(TAG, "Install panel IO");
esp_lcd_panel_io_spi_config_t io_config = {
.dc_gpio_num = PIN_NUM_DC,
.cs_gpio_num = PIN_NUM_CS,
.pclk_hz = LCD_PIXEL_CLOCK_HZ,
.lcd_cmd_bits = LCD_CMD_BITS,
.lcd_param_bits = LCD_PARAM_BITS,
.spi_mode = 3,
.trans_queue_depth = 10,
.user_ctx = NULL,
};
ESP_ERROR_CHECK(esp_lcd_new_panel_io_spi((esp_lcd_spi_bus_handle_t)LCD_SPI_HOST, &io_config, &io_handle));
esp_lcd_panel_dev_config_t panel_config = {
.reset_gpio_num = PIN_NUM_RST,
.rgb_endian = LCD_RGB_ENDIAN_BGR,
.bits_per_pixel = LCD_BITS_PER_PIXEL,
};
ESP_ERROR_CHECK(esp_lcd_new_panel_st7789(io_handle, &panel_config, &panel_handle));
ESP_ERROR_CHECK(esp_lcd_panel_reset(panel_handle));
ESP_ERROR_CHECK(esp_lcd_panel_init(panel_handle));
ESP_ERROR_CHECK(esp_lcd_panel_invert_color(panel_handle, true));
ESP_ERROR_CHECK(esp_lcd_panel_set_gap(panel_handle, 1, 26)); // ST7735S typically needs these offsets
ESP_ERROR_CHECK(esp_lcd_panel_disp_on_off(panel_handle, true));
}
static void lvgl_init(void)
{
lv_init();
#if 1
const lvgl_port_cfg_t lvgl_cfg = {
.task_priority = 4, // LVGL task priority
.task_stack = 16384, // LVGL task stack size
.task_affinity = 0, // LVGL task can run on any core
.task_max_sleep_ms = 500, // Maximum sleep in LVGL task
.timer_period_ms = 5 // LVGL timer period
};
ESP_ERROR_CHECK(lvgl_port_init(&lvgl_cfg));
#endif
const lvgl_port_display_cfg_t disp_cfg = {
.io_handle = io_handle,
.panel_handle = panel_handle,
.buffer_size = LCD_H_RES * LCD_V_RES * 2,
.double_buffer = false,
.hres = LCD_H_RES,
.vres = LCD_V_RES,
.monochrome = false,
.rotation = {
.swap_xy = true,
.mirror_x = true,
.mirror_y = false,
},
.flags = {
//.buff_dma = true,
.swap_bytes = true,
}
};
disp = lvgl_port_add_disp(&disp_cfg);
//lv_display_set_color_format(disp, LV_COLOR_FORMAT_RGB565);
}
static void createBubble(lv_obj_t * scr)
{
// 2) Create a bubble level of size 200×60, with range [−30°, +30°], initial 0°:
lv_obj_t * level = bubble_create(scr, 150, 40, -10.0f, +10.0f, 0.0f);
lv_obj_align(level, LV_ALIGN_CENTER, 0, 0);
// 3) … Later, when you read your accelerometer or keypad‐derived angle …
float new_angle = 10.0f;
bubble_setValue(level, new_angle);
// 4) You can call bubble_level_set_value(level, …) as often as you like.
// Each call invalidates the object and LVGL will call the draw callback
// (usually on the next tick, or immediately if LVGL is idle).
_bubble = level;
}
void gui_start(void)
{
// Initialize LCD
lcd_init();
// Initialize LVGL
lvgl_init();
// Create UI
create_lvgl_demo();
keypad_start();
gpio_set_level(PIN_NUM_BK_LIGHT, 1);
xTaskCreate(gui_task, "gui_task", 4096, NULL, 5, NULL);
}
static uint32_t waitForKeyPress(void)
{
QueueHandle_t q = keypad_getQueue();
uint32_t ev = 0;
if (xQueueReceive(q, &ev, portMAX_DELAY) == pdTRUE)
{
return ev;
}
return 0;
}
static void handleMainMenu(void)
{
lvgl_port_lock(0);
// Create a screen with black background
lv_obj_t *scr = lv_scr_act();
//create_menu(scr);
lvgl_port_unlock();
}
// ───── MENU CONFIG ─────
#define ROW_H 20 // height of each row
static const char * items[] = { // your menu entries
"VMode Crossmade",
"Second Choice",
"Another Option",
"Yet Another",
"Yet Another",
"Last One"
};
#define ITEM_COUNT (sizeof(items)/sizeof(items[0]))
// ───── STATE & HELPERS ─────
static lv_obj_t * btn_array[ITEM_COUNT];
static int selected_idx = 0;
// Called whenever a button is “activated” (Enter/Click)
static void btn_click_cb(lv_event_t * e) {
lv_obj_t * btn = lv_event_get_target(e);
const char * txt = lv_label_get_text(lv_obj_get_child(btn, 0));
ESP_LOGI(TAG, "Activated: %s\n", txt);
}
// Repaint all rows so only btn_array[selected_idx] is highlighted
static void refresh_highlight(void) {
return;
lvgl_port_lock(0);
int index = 0;
lv_obj_t *page = _currentPage;
lv_obj_t * child = NULL;
lv_obj_t * next = lv_obj_get_child(page, index);
lv_obj_t * selected = NULL;
while(next) {
child = next;
ESP_LOGI(TAG, "Child: %p", child);
if (lv_obj_has_flag(child, LV_OBJ_FLAG_USER_1))
{
lv_obj_set_style_bg_color(child, lv_color_hex(0xFF8800), 0);
lv_obj_set_style_text_color(lv_obj_get_child(child,0),
lv_color_white(), 0);
selected = child;
ESP_LOGI(TAG, "Selected");
}
else
{
lv_obj_set_style_bg_color(child, lv_color_white(), 0);
lv_obj_set_style_text_color(lv_obj_get_child(child,0),
lv_color_black(), 0);
}
index++;
next = lv_obj_get_child(page, index);
}
if (selected)
{
lv_obj_scroll_to_view(selected, LV_ANIM_ON);
}
lvgl_port_unlock();
}
static void menuInc(int inc)
{
LOCK();
currentFocusIndex(&_menuContext);
ESP_LOGI(TAG, "Current Index: %d", _menuContext.selected);
lv_obj_t *next = NULL;
// check if we are at the first or last in the page
lv_obj_t *test = lv_obj_get_child(_currentPage, (inc > 0 ? -1 : 0));
if (_menuContext.obj != test)
{
next = lv_obj_get_child(_currentPage, _menuContext.selected + inc);
lv_obj_add_state(next, LV_STATE_FOCUSED);
lv_obj_clear_state(_menuContext.obj, LV_STATE_FOCUSED);
lv_obj_scroll_to_view(next, LV_ANIM_ON);
_menuContext.obj = next;
_menuContext.selected += inc;
}
UNLOCK();
}
static void menuNext(void)
{
menuInc(1);
}
static void menuPrevious(void)
{
menuInc(-1);
}
// Fire the “clicked” event on the selected row
static void activate_selected(void)
{
LOCK();
lv_obj_send_event(_menuContext.obj, LV_EVENT_CLICKED, NULL);
UNLOCK();
}
static lv_obj_t * addMenuItem(lv_obj_t *page, const char *text)
{
lv_obj_t * btn = lv_btn_create(page);
lv_obj_set_size(btn, LV_PCT(100), ROW_H);
lv_obj_add_style(btn, &_styleUnfocusedBtn, LV_PART_MAIN | LV_STATE_DEFAULT);
lv_obj_add_style(btn, &_styleFocusedBtn, LV_PART_MAIN | LV_STATE_FOCUSED);
lv_obj_add_state(btn, LV_STATE_DEFAULT);
// style it just like your old list
// lv_obj_set_style_bg_color(btn, lv_color_white(), 0);
lv_obj_set_style_radius(btn, 0, LV_PART_MAIN);
lv_obj_set_style_border_width(btn, 0, LV_PART_MAIN);
lv_obj_clear_flag(btn, LV_OBJ_FLAG_SCROLLABLE);
// label & center
lv_obj_t * lbl = lv_label_create(btn);
lv_label_set_text(lbl, text);
lv_obj_set_style_radius(lbl, 0, LV_PART_MAIN | LV_STATE_ANY);
lv_obj_set_style_border_width(lbl, 0, LV_PART_MAIN | LV_STATE_ANY);
// click callback
lv_obj_add_event_cb(btn, btn_click_cb, LV_EVENT_CLICKED, NULL);
return btn;
}
static int getSelectedIndex(lv_obj_t *page)
{
return -1;
}
static void currentFocusIndex(menu_context_t *ctx)
{
ctx->count = lv_obj_get_child_cnt(_currentPage);
ctx->obj = NULL;
ctx->selected = -1;
// return the index of the currently focused object
for(int i = 0; i < ctx->count; i++) {
lv_obj_t * child = lv_obj_get_child(_currentPage, i);
if (lv_obj_has_state(child, LV_STATE_FOCUSED))
{
ctx->obj = child;
ctx->selected = i;
return;
}
}
}
// ───── BUILD THE MENU ─────
static void build_scrollable_menu(void) {
lv_style_init(&_styleFocusedBtn);
lv_style_init(&_styleUnfocusedBtn);
lv_style_set_bg_color(&_styleUnfocusedBtn, lv_color_make(0xff,0xff,0xff)); // gray bg
lv_style_set_text_color(&_styleUnfocusedBtn, lv_color_hex(0xFF8800));
lv_style_set_bg_color(&_styleFocusedBtn, lv_color_make(0x33,0x99,0xFF)); // blue bg
lv_style_set_text_color(&_styleUnfocusedBtn,lv_color_black());
// 2) Inside it, create the lv_menu and hide its sidebar/header
lv_obj_t *menu = lv_menu_create(lv_scr_act());
_menu = menu;
lv_obj_set_style_radius(menu, 0, LV_PART_MAIN | LV_STATE_ANY);
lv_obj_set_style_border_width(menu, 0, LV_PART_MAIN | LV_STATE_ANY);
lv_obj_set_size(menu, lv_pct(100), lv_pct(100));
lv_obj_center(menu);
lv_obj_set_scrollbar_mode(menu, LV_SCROLLBAR_MODE_AUTO);
lv_obj_t * main = lv_menu_page_create(menu, NULL);
lv_obj_set_style_radius(main, 0, LV_PART_MAIN | LV_STATE_ANY);
lv_obj_set_style_border_width(main, 0, LV_PART_MAIN | LV_STATE_ANY);
lv_obj_set_scrollbar_mode(main, LV_SCROLLBAR_MODE_AUTO);
lv_obj_set_size(main, lv_pct(100), lv_pct(100));
lv_menu_set_page(menu, main);
lv_obj_t * tmpObj;
lv_obj_t * calMenu = lv_menu_page_create(menu, NULL);
lv_obj_set_style_radius(calMenu, 0, LV_PART_MAIN | LV_STATE_ANY);
lv_obj_set_style_border_width(calMenu, 0, LV_PART_MAIN | LV_STATE_ANY);
lv_obj_set_scrollbar_mode(calMenu, LV_SCROLLBAR_MODE_AUTO);
tmpObj = addMenuItem(main, "Bluetooth");
lv_obj_add_state(tmpObj, LV_STATE_FOCUSED);
//lv_obj_add_flag(tmpObj, LV_OBJ_FLAG_USER_1);
tmpObj = addMenuItem(main, "Calibration");
lv_menu_set_load_page_event(menu, tmpObj, calMenu);
tmpObj = addMenuItem(main, "Volume");
addMenuItem(main, "About");
addMenuItem(main, "Exit");
addMenuItem(calMenu, "Calibrate Level");
addMenuItem(calMenu, "Reset Calibration");
addMenuItem(calMenu, "Exit");
_currentPage = main;
// 6) Initial highlight
selected_idx = 0;
refresh_highlight();
}
static void gui_task(void)
{
system_subscribe(xTaskGetCurrentTaskHandle());
// Grab queue handle
QueueHandle_t q = keypad_getQueue();
uint32_t ev = 0;
LOCK();
// _mode = GUI_MENU;
// lv_obj_remove_flag(_menu, LV_OBJ_FLAG_HIDDEN);
lv_obj_remove_flag(_bubble, LV_OBJ_FLAG_HIDDEN);
//lv_obj_add_flag(_menu, LV_OBJ_FLAG_HIDDEN);
UNLOCK();
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;
}
#if 0
case (KEY0 << KEY_SHORT_PRESS):
if (_mode == GUI_MENU)
{
menuNext();
}
ESP_LOGI(TAG, "MAIN: Button 1 SHORT");
break;
case (KEY0 << KEY_LONG_PRESS):
{
if (_mode != GUI_MENU)
{
_mode = GUI_MENU;
lv_obj_remove_flag(_menu, LV_OBJ_FLAG_HIDDEN);
lv_obj_add_flag(_bubble, LV_OBJ_FLAG_HIDDEN);
}
else
if (_mode == GUI_MENU)
{
activate_selected();
_mode = GUI_BUBBLE;
// lv_obj_remove_flag(_bubble, LV_OBJ_FLAG_HIDDEN);
// lv_obj_add_flag(_menu, LV_OBJ_FLAG_HIDDEN);
}
break;
}
case (KEY1 << KEY_SHORT_PRESS):
{
if (_mode == GUI_MENU)
{
menuPrevious();
}
break;
}
case (KEY1 << KEY_LONG_PRESS):
ESP_LOGI(TAG, "MAIN: Button 2 LONG");
gpio_set_level(PIN_NUM_nON, 0);
break;
#endif
default:
break;
}
}
if (_mode == GUI_BUBBLE)
{
uint32_t notifiedBits = 0;
// clear on entry (first param), wait for any bit, block forever
xTaskNotifyWait(
0xFFFFFFFF, // clear any old bits on entry
0xFFFFFFFF, // clear bits on exit
&notifiedBits,
pdMS_TO_TICKS(100));
if (notifiedBits & EM_EVENT_NEW_DATA)
{
ImuData_t d = system_getImuData();
bubble_setValue(_bubble, -d.angle);
}
}
}
>>>>>>> 4feb4c0a98bddb1f2a172ea4b195ce31ba18d442
}

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);
}

25
main/keypad.h
View File

@@ -1,3 +1,4 @@
<<<<<<< HEAD
#ifndef _KEYPAD_H
#define _KEYPAD_H
@@ -20,4 +21,28 @@ typedef enum
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);
>>>>>>> 4feb4c0a98bddb1f2a172ea4b195ce31ba18d442
#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

294
main/main.c
View File

@@ -1,3 +1,4 @@
<<<<<<< HEAD
/*
* SPDX-FileCopyrightText: 2021-2024 Espressif Systems (Shanghai) CO LTD
*
@@ -289,3 +290,296 @@ 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

171
main/system.c
View File

@@ -1,3 +1,4 @@
<<<<<<< HEAD
#include "system.h"
#include "esp_log.h"
@@ -166,4 +167,174 @@ void system_notifyAll(uint32_t eventBits) {
}
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);
}
>>>>>>> 4feb4c0a98bddb1f2a172ea4b195ce31ba18d442
}

77
main/system.h
View File

@@ -1,3 +1,4 @@
<<<<<<< HEAD
#ifndef SYSTEM_H
#define SYSTEM_H
@@ -72,4 +73,80 @@ BaseType_t system_unsubscribe(TaskHandle_t task);
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);
>>>>>>> 4feb4c0a98bddb1f2a172ea4b195ce31ba18d442
#endif