Audio working

.vscode/c_cpp_properties.json

@@ -1,23 +0,0 @@
-{
-    "configurations": [
-        {
-            "name": "ESP-IDF",
-            "compilerPath": "${config:idf.toolsPathWin}\\tools\\xtensa-esp-elf\\esp-13.2.0_20240530\\xtensa-esp-elf\\bin\\xtensa-esp32-elf-gcc.exe",
-            "compileCommands": "${config:idf.buildPath}/compile_commands.json",
-            "includePath": [
-                "${config:idf.espIdfPath}/components/**",
-                "${config:idf.espIdfPathWin}/components/**",
-                "${workspaceFolder}/**"
-            ],
-            "browse": {
-                "path": [
-                    "${config:idf.espIdfPath}/components",
-                    "${config:idf.espIdfPathWin}/components",
-                    "${workspaceFolder}"
-                ],
-                "limitSymbolsToIncludedHeaders": true
-            }
-        }
-    ],
-    "version": 4
-}

.vscode/launch.json

@@ -1,15 +0,0 @@
-{
-  "version": "0.2.0",
-  "configurations": [
-    {
-      "type": "gdbtarget",
-      "request": "attach",
-      "name": "Eclipse CDT GDB Adapter"
-    },
-    {
-      "type": "espidf",
-      "name": "Launch",
-      "request": "launch"
-    }
-  ]
-}

.vscode/settings.json

@@ -1,41 +1,3 @@
 {
-    "C_Cpp.intelliSenseEngine": "default",
+    "idf.pythonInstallPath": "/usr/bin/python"
-    "idf.espIdfPathWin": "C:\\Users\\brent.RPX\\esp\\v5.3.1\\esp-idf",
-    "idf.openOcdConfigs": [
-        "board/esp32-wrover-kit-3.3v.cfg"
-    ],
-    "idf.portWin": "COM4",
-    "idf.toolsPathWin": "C:\\Users\\brent.RPX\\.espressif\\tools",
-    "idf.flashType": "UART",
-    "files.associations": {
-        "esp_system.h": "c",
-        "task.h": "c",
-        "bt_app_core.h": "c",
-        "math.h": "c",
-        "esp_log.h": "c",
-        "freertos.h": "c",
-        "queue.h": "c",
-        "unistd.h": "c",
-        "inttypes.h": "c",
-        "esp_idf_version.h": "c",
-        "timers.h": "c",
-        "lvgl.h": "c",
-        "esp_timer.h": "c",
-        "esp_bt_device.h": "c",
-        "esp_lvgl_port.h": "c",
-        "lv_global.h": "c",
-        "stdio.h": "c",
-        "compare": "c",
-        "cstdint": "c",
-        "spi_master.h": "c",
-        "bitset": "c",
-        "format": "c",
-        "system.h": "c",
-        "esp_lcd_panel_ops.h": "c",
-        "semphr.h": "c",
-        "lv_spinbox.h": "c",
-        "lv_slider.h": "c",
-        "lv_menu.h": "c"
-    },
-    "git.ignoreLimitWarning": true
 }

README.md

@@ -0,0 +1,90 @@
+| Supported Targets | ESP32 |
+| ----------------- | ----- |
+
+A2DP-SOURCE EXAMPLE
+========================
+
+Example of A2DP audio source role
+
+This is the example of using Advanced Audio Distribution Profile (A2DP) APIs to transmit audio stream. Application can take advantage of this example to implement portable audio players or microphones to transmit audio stream to A2DP sink devices.
+
+## How to use this example
+
+### Hardware Required
+
+This example is able to run on any commonly available ESP32 development board, and is supposed to connect to [A2DP sink example](../a2dp_sink) in ESP-IDF.
+
+### Configure the project
+
+```
+idf.py menuconfig
+```
+
+* Enable Classic Bluetooth and A2DP under Component config --> Bluetooth --> Bluedroid Enable
+
+### Build and Flash
+
+Build the project and flash it to the board, then run monitor tool to view serial output.
+
+```
+idf.py -p PORT flash monitor
+```
+
+(Replace PORT with the name of the serial port to use.)
+
+(To exit the serial monitor, type ``Ctrl-]``.)
+
+See the Getting Started Guide for full steps to configure and use ESP-IDF to build projects.
+
+## Example Output
+
+For the first step, this example performs device discovery to search for a target device (A2DP sink) whose device name is "ESP_SPEAKER" and whose "Rendering" bit of its Service Class field is set in its Class of Device (COD). If a candidate target is found, the local device will initiate connection with it.
+
+After connection with A2DP sink is established, the example performs the following running loop 1-2-3-4-1:
+1. audio transmission starts and lasts for a while
+2. audio transmission stops
+3. disconnect with target device
+4. reconnect to target device
+
+The example implements an event loop triggered by a periodic "heart beat" timer and events from Bluetooth protocol stack callback functions.
+
+After the local device discovers the target device and initiates connection, there will be logging message like this:
+
+```
+I (4090) BT_AV: Found a target device, address xx:xx:xx:xx:xx:xx, name ESP_SPEAKER
+I (4090) BT_AV: Cancel device discovery ...
+I (4100) BT_AV: Device discovery stopped.
+I (4100) BT_AV: a2dp connecting to peer: ESP_SPEAKER
+```
+
+If connection is set up successfully, there will be such message:
+
+```
+I (5100) BT_AV: a2dp connected
+```
+
+Starting of audio transmission has the following notification message:
+
+```
+I (10880) BT_AV: a2dp media ready checking ...
+...
+I (10880) BT_AV: a2dp media ready, starting ...
+...
+I (11400) BT_AV: a2dp media start successfully.
+```
+
+Stop of audio transmission, and disconnection with remote device generate the following notification message:
+
+```
+I (110880) BT_AV: a2dp media stopping...
+...
+I (110920) BT_AV: a2dp media stopped successfully, disconnecting...
+...
+I (111040) BT_AV: a2dp disconnected
+```
+
+## Troubleshooting
+* For current stage, the supported audio codec in ESP32 A2DP is SBC. SBC audio stream is encoded from PCM data normally formatted as 44.1kHz sampling rate, two-channel 16-bit sample data.
+* The raw PCM media stream in the example is generated by a sequence of random number, so the sound played on the sink side will be piercing noise.
+* As a usage limitation, ESP32 A2DP source can support at most one connection with remote A2DP sink devices. Also, A2DP source cannot be used together with A2DP sink at the same time, but can be used with other profiles such as SPP and HFP.
+* Usually, the `ACL_CONN_NUM` is set to `2` but not `1` as one is used for a2dp connection and the other is used for avrcp connection.

main/CMakeLists.txt

@@ -1,5 +1,5 @@
-idf_component_register(SRCS "system.c" "bubble.c" "keypad.c" "main.c"
+idf_component_register(SRCS "bt_app.c" "system.c" "bubble.c" "keypad.c" "main.c"
-                        "bt_app_core.c"                            
+                                                   
                             "gui.c"
                             "lsm6dsv.c"
                     INCLUDE_DIRS "."

main/bt_app.c

@@ -12,9 +12,11 @@
 #include "freertos/FreeRTOS.h"
 #include "freertos/queue.h"
 #include "freertos/task.h"
+#include "freertos/stream_buffer.h"
 #include "esp_log.h"
 
-#include "bt_app_core.h"
+#include "bt_app.h"
+#include "system.h"
 
 #include "esp_bt.h"
 #include "esp_bt_main.h"
@@ -368,18 +370,215 @@ static void bt_app_a2d_cb(esp_a2d_cb_event_t event, esp_a2d_cb_param_t *param)
     bt_app_work_dispatch(bt_app_av_sm_hdlr, event, param, sizeof(esp_a2d_cb_param_t), NULL);
 }
 
-static uint8_t btData[512];
-
-static void fillBuffer(uint8_t *data, int32_t len)
-{
 
 
+#define SAMPLE_RATE     44100
+#define TONE_FREQ       440.0f     // A4
+#define FADE_FREQ       1.0f       // 1 Hz fade L->R
+#define STREAM_BUF_SIZE (8192)
+#define CHUNK_SIZE      (512)
+
+StreamBufferHandle_t audio_stream_buf;
+
+void generate_synth_pcm(uint8_t *buf, int len) {
+    static float phase = 0.0f;
+    static float fade_phase = 0.0f;
+
+    int16_t *samples = (int16_t *)buf;
+    int samples_needed = len / 4;  // 4 bytes per stereo frame (2 bytes L, 2 bytes R)
+
+    for (int i = 0; i < samples_needed; i++) {
+        // Fade L/R amplitude: 0..1 for L, inverted for R
+        float fade = (sinf(2 * M_PI * fade_phase) + 1.0f) * 0.5f;
+
+        // Sine wave sample
+        float sample = sinf(2 * M_PI * phase);
+
+        int16_t left  = (int16_t)(sample * fade * 32767);
+        int16_t right = (int16_t)(sample * (1.0f - fade) * 32767);
+
+        samples[i * 2 + 0] = left;
+        samples[i * 2 + 1] = right;
+
+        // Advance phases
+        phase += TONE_FREQ / SAMPLE_RATE;
+        if (phase >= 1.0f) phase -= 1.0f;
+
+        fade_phase += FADE_FREQ / SAMPLE_RATE;
+        if (fade_phase >= 1.0f) fade_phase -= 1.0f;
+    }
 }
 
+#define MIN_RATE_HZ 1.0f
+#define MAX_RATE_HZ 440.0f
+#define CLICK_AMPLITUDE 32000  // 16-bit
+
+static float click_timer = 0.0f;
+
+void generate_synth_clicks(uint8_t *buf, int len, float balance) {
+
+    int16_t *samples = (int16_t *)buf;
+    int samples_needed = len / 4;
+
+    float abs_balance = fabsf(balance);
+    float rate_hz = MIN_RATE_HZ * powf(MAX_RATE_HZ / MIN_RATE_HZ, abs_balance);
+    float samples_per_click = SAMPLE_RATE / rate_hz;
+
+    for (int i = 0; i < samples_needed; i++) {
+        int16_t left = 0;
+        int16_t right = 0;
+
+        if (click_timer <= 0.0f) {
+            // Insert a short pulse: single sample spike
+            if (balance < 0) {
+                left = CLICK_AMPLITUDE;
+            } else if (balance > 0) {
+                right = CLICK_AMPLITUDE;
+            }
+            click_timer += samples_per_click;
+        }
+
+        click_timer -= 1.0f;
+
+        samples[i * 2 + 0] = left;
+        samples[i * 2 + 1] = right;
+
+    }
+}
+
+void audio_producer_task(void *pvParameters) {
+    uint8_t chunk[CHUNK_SIZE];
+
+    int core_id = xPortGetCoreID();
+
+    ESP_LOGI("Producer", "Running on core %d", core_id);
+
+    system_subscribe(xTaskGetCurrentTaskHandle());
+#if 0    
+    
+
+    while (1) {
+        generate_synth_pcm(chunk, sizeof(chunk));
+
+        size_t sent = xStreamBufferSend(audio_stream_buf, chunk, sizeof(chunk), portMAX_DELAY);
+
+        if (sent < sizeof(chunk)) {
+            ESP_LOGW(BT_APP_CORE_TAG, "Dropped %d bytes", sizeof(chunk) - sent);
+        }
+    }
+#endif
+
+    static float phase = 0.0f;
+
+    float balance = 0.0f;  // -1.0 = left, +1.0 = right
+
+    uint32_t raw_value;
+    uint32_t notifiedBits = 0;
+
+    while (1) {
+        // Non-blocking check for new notification:
+                    
+            // clear on entry (first param), wait for any bit, block forever
+        xTaskNotifyWait(
+            0,        // clear any old bits on entry
+            0,        // clear bits on exit
+            &notifiedBits,
+            0);
+
+        if (notifiedBits & EM_EVENT_NEW_DATA) 
+        {
+            ImuData_t d = system_getImuData();
+
+            if (fabs(d.raw[system_getPrimaryAxis()]) > 45.0f)
+            {
+                balance = 0.0f;
+            }
+            else
+            {
+                balance = -d.angle / MAX_INDICATION_ANGLE;
+            }
+        }
+    
+        if (balance > 1.0f)
+        {
+            balance = 1.0f;
+        }
+        else
+        if (balance < -1.0f)
+        {
+            balance = -1.0f;
+        }
+        generate_synth_clicks(chunk, sizeof(chunk), balance);
+
+#if 0
+        // Synthesize audio using latest balance
+        int16_t *samples = (int16_t *)chunk;
+        int samples_needed = CHUNK_SIZE / 4;
+
+        for (int i = 0; i < samples_needed; i++) {
+            float sample = sinf(2 * M_PI * phase);
+
+            float left_gain  = 0.5f * (1.0f - balance);
+            float right_gain = 0.5f * (1.0f + balance);
+
+            int16_t left  = (int16_t)(sample * left_gain * 32767);
+            int16_t right = (int16_t)(sample * right_gain * 32767);
+
+            samples[i * 2 + 0] = left;
+            samples[i * 2 + 1] = right;
+
+            phase += TONE_FREQ / SAMPLE_RATE;
+            if (phase >= 1.0f) phase -= 1.0f;
+        }
+#endif
+
+        // Push chunk to FIFO
+        xStreamBufferSend(audio_stream_buf, chunk, sizeof(chunk), portMAX_DELAY);
+    }
+}
+
+/* generate some random noise to simulate source audio */
+static int32_t bt_app_a2d_data_cb(uint8_t *data, int32_t len)
+{
+        size_t bytes_read = xStreamBufferReceive(audio_stream_buf, data, len, 0);
+
+    if (bytes_read < len) {
+        memset(data + bytes_read, 0, len - bytes_read);  // fill silence
+    }
+
+    return len;
+#if 0    
+    if (data == NULL || len < 0) {
+        return 0;
+    }
+
+    int16_t *p_buf = (int16_t *)data;
+    for (int i = 0; i < (len >> 1); i++) {
+        p_buf[i] = rand() % (1 << 16);
+    }
+
+    return len;
+#endif    
+}
+
+#if 0
+
 /* Generate a continuous 440Hz sine wave */
 static int32_t bt_app_a2d_data_cb(uint8_t *data, int32_t len)
 {
-        static double phase = 0.0;
+
+        if (data == NULL || len < 0) {
+        return 0;
+    }
+
+    int16_t *p_buf = (int16_t *)data;
+    for (int i = 0; i < (len >> 1); i++) {
+        p_buf[i] = rand() % (1 << 16);
+    }
+
+    return len;
+#if 0
+    static double phase = 0.0;
     const double frequency = 440.0; // A4 tone (440 Hz)
     const double sample_rate = 44100.0; // 44.1 kHz sample rate
     const double phase_increment = 2.0 * M_PI * frequency / sample_rate;
@@ -406,7 +605,9 @@ static int32_t bt_app_a2d_data_cb(uint8_t *data, int32_t len)
         }
     }
     return len;
+#endif    
 }
+#endif
 
 static void bt_app_a2d_heart_beat(TimerHandle_t arg)
 {
@@ -551,6 +752,7 @@ static void bt_app_av_media_proc(uint16_t event, void *param)
     }
     case APP_AV_MEDIA_STATE_STARTED: {
         if (event == BT_APP_HEART_BEAT_EVT) {
+#if 0            
             /* stop media after 10 heart beat intervals */
             if (++s_intv_cnt >= 10) {
                 ESP_LOGI(BT_AV_TAG, "a2dp media suspending...");
@@ -558,6 +760,7 @@ static void bt_app_av_media_proc(uint16_t event, void *param)
                 s_media_state = APP_AV_MEDIA_STATE_STOPPING;
                 s_intv_cnt = 0;
             }
+#endif                
         }
         break;
     }
@@ -791,6 +994,10 @@ static void bt_app_task_handler(void *arg)
 {
     bt_app_msg_t msg;
 
+    int core_id = xPortGetCoreID();
+
+    ESP_LOGI("MY_TASK", "Running on core %d", core_id);
+
     for (;;) {
         /* receive message from work queue and handle it */
         if (pdTRUE == xQueueReceive(s_bt_app_task_queue, &msg, (TickType_t)portMAX_DELAY)) {
@@ -846,7 +1053,9 @@ bool bt_app_work_dispatch(bt_app_cb_t p_cback, uint16_t event, void *p_params, i
 void bt_app_task_start_up(void)
 {
     s_bt_app_task_queue = xQueueCreate(10, sizeof(bt_app_msg_t));
-    xTaskCreate(bt_app_task_handler, "BtAppTask", 4096, NULL, 10, &s_bt_app_task_handle);
+    //xTaskCreate(bt_app_task_handler, "BtAppTask", 8192, NULL, 10, &s_bt_app_task_handle);
+
+    xTaskCreatePinnedToCore(bt_app_task_handler, "BtAppTask", 8192, NULL, 10, NULL, 1);
 }
 
 void bt_app_task_shut_down(void)
@@ -867,7 +1076,6 @@ void bt_app_init(void)
     esp_err_t ret;
     char bda_str[18] = {0};
 
-    fillBuffer(btData, 512);
 
     /*
      * This example only uses the functions of Classical Bluetooth.
@@ -918,4 +1126,32 @@ void bt_app_init(void)
     bt_app_task_start_up();
     /* Bluetooth device name, connection mode and profile set up */
     bt_app_work_dispatch(bt_av_hdl_stack_evt, BT_APP_STACK_UP_EVT, NULL, 0, NULL);
+
+
+
+    audio_stream_buf = xStreamBufferCreate(STREAM_BUF_SIZE, 1);
+    assert(audio_stream_buf != NULL);
+
+    //xTaskCreate(audio_producer_task, "audio_producer", 4096, NULL, 5, NULL);
+    xTaskCreatePinnedToCore(audio_producer_task, "audio_producer", 4096, NULL, 5, NULL, 1);
+
+    ESP_LOGI(BT_APP_CORE_TAG, "Audio synth producer started");
+
+    vTaskDelay(pdMS_TO_TICKS(1000));
+
+    //esp_err_t esp_bredr_tx_power_get(esp_power_level_t *min_power_level, esp_power_level_t *max_power_level)
+    esp_power_level_t pmin;
+    esp_power_level_t pmax;
+    esp_bredr_tx_power_get(&pmin, &pmax);
+
+    ESP_LOGI(BT_APP_CORE_TAG, "BT Power min = %d\nBT Power max = %d", pmin, pmax);
+
+    pmin = ESP_PWR_LVL_P7;
+    pmax = ESP_PWR_LVL_P7;
+    esp_bredr_tx_power_set(pmin, pmax);
+
+    esp_bredr_tx_power_get(&pmin, &pmax);
+
+    ESP_LOGI(BT_APP_CORE_TAG, "BT Power min = %d\nBT Power max = %d", pmin, pmax);
+
 }

main/gpio.h

@@ -5,6 +5,7 @@
 #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
 
 

main/gui.c

@@ -105,9 +105,9 @@ static void create_lvgl_demo(void)
     lvgl_port_lock(0);
     // Create a screen with black background
     lv_obj_t *scr = lv_scr_act();
-    //create_menu(scr);
+    
     createBubble(scr);
-    build_scrollable_menu();
+    //build_scrollable_menu();
 
     lvgl_port_unlock();
 
@@ -167,8 +167,8 @@ static void lvgl_init(void)
 #if 1
     const lvgl_port_cfg_t lvgl_cfg = {
         .task_priority = 4,        // LVGL task priority
-        .task_stack = 32768,        // LVGL task stack size
+        .task_stack = 16384,        // LVGL task stack size
-        .task_affinity = -1,       // LVGL task can run on any core
+        .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
     };
@@ -180,7 +180,7 @@ static void lvgl_init(void)
         .io_handle = io_handle,
         .panel_handle = panel_handle,
         .buffer_size = LCD_H_RES * LCD_V_RES * 2,
-        .double_buffer = true,
+        .double_buffer = false,
         .hres = LCD_H_RES,
         .vres = LCD_V_RES,
         .monochrome = false,
@@ -219,15 +219,10 @@ static void createBubble(lv_obj_t * scr)
     _bubble = level;
 }
 
-void ui_test()
-{
-    lv_obj_t *label = lv_label_create(lv_screen_active());
-    lv_label_set_text(label, "Hello, LVGL 9.2!");
-    lv_obj_center(label);
-}
 
 void gui_start(void)
 {
+
 	// Initialize LCD
     lcd_init();
 
@@ -237,7 +232,6 @@ void gui_start(void)
 	// Create UI
     create_lvgl_demo();
 
-
     keypad_start();
 
 	gpio_set_level(PIN_NUM_BK_LIGHT, 1);
@@ -333,21 +327,9 @@ static void refresh_highlight(void) {
         next = lv_obj_get_child(page, index);
     }
 
-    // for(int i = 0; i < ITEM_COUNT; i++) {
-    //     if(i == selected_idx) {
-    //         lv_obj_set_style_bg_color(btn_array[i], lv_color_hex(0xFF8800), 0);
-    //         lv_obj_set_style_text_color(lv_obj_get_child(btn_array[i],0),
-    //                                     lv_color_white(), 0);
-    //     } else {
-    //         lv_obj_set_style_bg_color(btn_array[i], lv_color_white(), 0);
-    //         lv_obj_set_style_text_color(lv_obj_get_child(btn_array[i],0),
-    //                                     lv_color_black(), 0);
-    //     }
-    // }
-
     if (selected)
     {
-    lv_obj_scroll_to_view(selected, LV_ANIM_ON);
+        lv_obj_scroll_to_view(selected, LV_ANIM_ON);
     }
 
     lvgl_port_unlock();
@@ -522,39 +504,6 @@ static void build_scrollable_menu(void) {
 
     _currentPage = main;
 
-
-    
-#if 0
-    for(int i = 0; i < ITEM_COUNT; i++) {
-        // create a full-width button in that section
-        lv_obj_t * btn = lv_btn_create(main);
-        lv_obj_set_size(btn, LV_PCT(100), ROW_H);
-
-        // 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, items[i]);
-        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);
-        //lv_obj_center(lbl);
-
-        // click callback
-        lv_obj_add_event_cb(btn, btn_click_cb, LV_EVENT_CLICKED, NULL);
-
-        // save for highlight/scroll
-        btn_array[i] = btn;
-    }
-
-//     lv_obj_t * content = lv_obj_get_child(page, 0);  
-// lv_obj_set_style_radius(content, 0, LV_PART_MAIN | LV_STATE_ANY);
-// lv_obj_set_style_border_width(content, 0, LV_PART_MAIN | LV_STATE_ANY);
-#endif
     // 6) Initial highlight
     selected_idx = 0;
     refresh_highlight();
@@ -563,6 +512,7 @@ static void build_scrollable_menu(void) {
 
 }
 
+
 static void gui_task(void)
 {
     system_subscribe(xTaskGetCurrentTaskHandle());
@@ -571,8 +521,12 @@ static void gui_task(void)
     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);
+    //lv_obj_add_flag(_menu, LV_OBJ_FLAG_HIDDEN);
+    UNLOCK();
 
     while (1)
     {
@@ -581,6 +535,19 @@ static void gui_task(void)
         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)
@@ -590,6 +557,8 @@ static void gui_task(void)
                 ESP_LOGI(TAG, "MAIN: Button 1 SHORT");
                 break;
             case (KEY0 << KEY_LONG_PRESS):
+            {
+
              
                 if (_mode != GUI_MENU)
                 {
@@ -605,8 +574,8 @@ static void gui_task(void)
                     // 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)
@@ -619,6 +588,7 @@ static void gui_task(void)
                 ESP_LOGI(TAG, "MAIN: Button 2 LONG");
                 gpio_set_level(PIN_NUM_nON, 0);
                 break;
+#endif                
             default:
                 break;
             }
@@ -633,7 +603,7 @@ static void gui_task(void)
                 0xFFFFFFFF,        // clear any old bits on entry
                 0xFFFFFFFF,        // clear bits on exit
                 &notifiedBits,
-                pdMS_TO_TICKS(10));
+                pdMS_TO_TICKS(100));
 
             if (notifiedBits & EM_EVENT_NEW_DATA) 
             {

main/keypad.h

@@ -14,7 +14,8 @@ typedef enum
 #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);

main/main.c

@@ -21,7 +21,7 @@
 #include "nvs_flash.h"
 
 
-#include "bt_app_core.h"
+#include "bt_app.h"
 #include "lsm6dsv.h"
 #include "gui.h"
 #include "gpio.h"
@@ -71,8 +71,9 @@ static void init_gpio(void)
 
 
     // Configure output GPIO
-    io_conf.pin_bit_mask = (1ULL << PIN_NUM_LED_1);
+    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);
@@ -86,7 +87,8 @@ static void init_gpio(void)
     
     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.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
@@ -95,8 +97,8 @@ static void init_gpio(void)
 
     // 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);
@@ -116,7 +118,7 @@ static void imu_task(void *pvParameters) {
     ImuData_t imu;
 
     LowPassFilter lpf;
-    LPF_Init(&lpf, 0.2, 0);
+    LPF_Init(&lpf, FILTER_COEFF, 0);
 
     while (1) {
 
@@ -170,20 +172,20 @@ static void imu_task(void *pvParameters) {
             
             float angle;
 
-            if (imu.raw[ANGLE_XY] > MAX_INDICATION_ANGLE)
+
+            angle = system_getAngle(); 
+
+            if (angle > MAX_INDICATION_ANGLE)
             {
                 angle = MAX_INDICATION_ANGLE;
             }
             else
-            if (imu.raw[ANGLE_XY] > MAX_INDICATION_ANGLE)
+            if (angle < -MAX_INDICATION_ANGLE)
             {
                 angle = -MAX_INDICATION_ANGLE;
             }
-            else
-            {
-                angle = imu.raw[ANGLE_XY];
-            }
             
+            // low pass filter the angle measurement
             imu.angle = LPF_Update(&lpf, angle);
 
             //imu.filtered[ANGLE_XY] = LPF_Update(&lpf, imu.raw[ANGLE_XY]);
@@ -197,7 +199,7 @@ static void imu_task(void *pvParameters) {
 #if 0
             snprintf(data_str, sizeof(data_str),
                     "(%.1f, %.1f, %.1f) (%.2f, %.2f, %.2f)", 
-                    xz, yz, xy,
+                    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
@@ -218,12 +220,12 @@ void app_main(void)
 {
     
     /* initialize NVS — it is used to store PHY calibration data */
-    // esp_err_t ret = nvs_flash_init();
+    esp_err_t ret = nvs_flash_init();
-    // if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
+    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
-    //     ESP_ERROR_CHECK(nvs_flash_erase());
+        ESP_ERROR_CHECK(nvs_flash_erase());
-    //     ret = nvs_flash_init();
+        ret = nvs_flash_init();
-    // }
+    }
-    // ESP_ERROR_CHECK(ret);
+    ESP_ERROR_CHECK(ret);
 
     init_gpio();
 
@@ -241,26 +243,49 @@ void app_main(void)
     
 
     
-
+    bt_app_init();
 
     gui_start();
     
+    gpio_set_level(PIN_NUM_LED_2, 1);
 
 
-    //bt_app_init();
+#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) {
         
-        // int level = gpio_get_level(PIN_NUM_BUTTON_1); // Read input GPIO
+        if (xQueueReceive(q, &ev, pdMS_TO_TICKS(10)) == pdTRUE) 
-        // gpio_set_level(PIN_NUM_LED_1, level);
+        {
+            switch (ev) 
+            {
+                case (KEY_UP << KEY_LONG_PRESS):
+                {
+                    system_setZeroAngle();
+                    break;
+                }
 
-        //gpio_set_level(PIN_NUM_nON, (level ? 0 : 1));
+                case (KEY_DOWN << KEY_LONG_PRESS):
+                {
+                    system_clearZeroAngle();
+                    break;
+                }
+            }
             
-        vTaskDelay(pdMS_TO_TICKS(100));
+        }
  
-        //gui_service();
     }
+#else
+    while (1)
+    {
+        vTaskDelay(pdMS_TO_TICKS(1000));
+    }    
+#endif
 }

main/system.c

@@ -8,12 +8,77 @@ 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);

main/system.h

@@ -5,6 +5,8 @@
 #include "freertos/task.h"
 #include "freertos/semphr.h"
 
+#define DISABLE_GUI
+
 enum
 {
     ANGLE_XY = 0,
@@ -27,11 +29,15 @@ typedef struct SystemState_s
 
 	EventGroupHandle_t event;
 
+    float zeroAngle;
+    int primaryAxis;
 
 } SystemState_t;
 
 
-#define MAX_INDICATION_ANGLE    10.0f
+#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)
@@ -49,6 +55,13 @@ 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);
 

sdkconfig

@@ -353,14 +353,14 @@ CONFIG_ESPTOOLPY_FLASHFREQ_40M=y
 # CONFIG_ESPTOOLPY_FLASHFREQ_20M is not set
 CONFIG_ESPTOOLPY_FLASHFREQ="40m"
 # CONFIG_ESPTOOLPY_FLASHSIZE_1MB is not set
-CONFIG_ESPTOOLPY_FLASHSIZE_2MB=y
+# CONFIG_ESPTOOLPY_FLASHSIZE_2MB is not set
-# CONFIG_ESPTOOLPY_FLASHSIZE_4MB is not set
+CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y
 # CONFIG_ESPTOOLPY_FLASHSIZE_8MB is not set
 # CONFIG_ESPTOOLPY_FLASHSIZE_16MB is not set
 # CONFIG_ESPTOOLPY_FLASHSIZE_32MB is not set
 # CONFIG_ESPTOOLPY_FLASHSIZE_64MB is not set
 # CONFIG_ESPTOOLPY_FLASHSIZE_128MB is not set
-CONFIG_ESPTOOLPY_FLASHSIZE="2MB"
+CONFIG_ESPTOOLPY_FLASHSIZE="4MB"
 # CONFIG_ESPTOOLPY_HEADER_FLASHSIZE_UPDATE is not set
 CONFIG_ESPTOOLPY_BEFORE_RESET=y
 # CONFIG_ESPTOOLPY_BEFORE_NORESET is not set
@@ -393,8 +393,8 @@ CONFIG_EXAMPLE_SSP_ENABLED=y
 #
 # Compiler options
 #
-CONFIG_COMPILER_OPTIMIZATION_DEBUG=y
+# CONFIG_COMPILER_OPTIMIZATION_DEBUG is not set
-# CONFIG_COMPILER_OPTIMIZATION_SIZE is not set
+CONFIG_COMPILER_OPTIMIZATION_SIZE=y
 # CONFIG_COMPILER_OPTIMIZATION_PERF is not set
 # CONFIG_COMPILER_OPTIMIZATION_NONE is not set
 CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_ENABLE=y
@@ -1448,7 +1448,6 @@ CONFIG_FREERTOS_TASK_NOTIFICATION_ARRAY_ENTRIES=1
 #
 # Port
 #
-CONFIG_FREERTOS_TASK_FUNCTION_WRAPPER=y
 # CONFIG_FREERTOS_WATCHPOINT_END_OF_STACK is not set
 CONFIG_FREERTOS_TLSP_DELETION_CALLBACKS=y
 # CONFIG_FREERTOS_TASK_PRE_DELETION_HOOK is not set
@@ -1497,7 +1496,7 @@ CONFIG_HEAP_TRACING_OFF=y
 # CONFIG_HEAP_TRACING_TOHOST is not set
 # CONFIG_HEAP_USE_HOOKS is not set
 # CONFIG_HEAP_TASK_TRACKING is not set
-# CONFIG_HEAP_ABORT_WHEN_ALLOCATION_FAILS is not set
+CONFIG_HEAP_ABORT_WHEN_ALLOCATION_FAILS=y
 # CONFIG_HEAP_PLACE_FUNCTION_INTO_FLASH is not set
 # end of Heap memory debugging
 
@@ -1561,7 +1560,7 @@ CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y
 # CONFIG_LWIP_DHCP_DISABLE_CLIENT_ID is not set
 CONFIG_LWIP_DHCP_DISABLE_VENDOR_CLASS_ID=y
 # CONFIG_LWIP_DHCP_RESTORE_LAST_IP is not set
-CONFIG_LWIP_DHCP_OPTIONS_LEN=68
+CONFIG_LWIP_DHCP_OPTIONS_LEN=69
 CONFIG_LWIP_NUM_NETIF_CLIENT_DATA=0
 CONFIG_LWIP_DHCP_COARSE_TIMER_SECS=1
 
@@ -2496,11 +2495,11 @@ CONFIG_LOG_BOOTLOADER_LEVEL=3
 CONFIG_FLASHMODE_DIO=y
 # CONFIG_FLASHMODE_DOUT is not set
 CONFIG_MONITOR_BAUD=115200
-CONFIG_OPTIMIZATION_LEVEL_DEBUG=y
+# CONFIG_OPTIMIZATION_LEVEL_DEBUG is not set
-CONFIG_COMPILER_OPTIMIZATION_LEVEL_DEBUG=y
+# CONFIG_COMPILER_OPTIMIZATION_LEVEL_DEBUG is not set
-CONFIG_COMPILER_OPTIMIZATION_DEFAULT=y
+# CONFIG_COMPILER_OPTIMIZATION_DEFAULT is not set
-# CONFIG_OPTIMIZATION_LEVEL_RELEASE is not set
+CONFIG_OPTIMIZATION_LEVEL_RELEASE=y
-# CONFIG_COMPILER_OPTIMIZATION_LEVEL_RELEASE is not set
+CONFIG_COMPILER_OPTIMIZATION_LEVEL_RELEASE=y
 CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED=y
 # CONFIG_OPTIMIZATION_ASSERTIONS_SILENT is not set
 # CONFIG_OPTIMIZATION_ASSERTIONS_DISABLED is not set

sdkconfig.defaults

@@ -0,0 +1,9 @@
+# Override some defaults so BT stack is enabled and
+# Classic BT is enabled
+CONFIG_BT_ENABLED=y
+CONFIG_BTDM_CTRL_MODE_BLE_ONLY=n
+CONFIG_BTDM_CTRL_MODE_BR_EDR_ONLY=y
+CONFIG_BTDM_CTRL_MODE_BTDM=n
+CONFIG_BT_BLUEDROID_ENABLED=y
+CONFIG_BT_CLASSIC_ENABLED=y
+CONFIG_BT_A2DP_ENABLE=y