#include "bt_spp_server.h" #include "drive_control.h" #include "protocol.h" #include "speed_sensor.h" #include "battery_monitor.h" #include #include #include #include #include "esp_bt.h" #include "esp_bt_device.h" #include "esp_bt_main.h" #include "esp_err.h" #include "esp_gap_bt_api.h" #include "esp_log.h" #include "esp_spp_api.h" #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "esp_timer.h" static const char *TAG = "BT_SPP"; static const char *DEVICE_NAME = "ESP32_SKATEBOARD"; static const char *SPP_SERVER_NAME = "ESP32_SKATEBOARD_SPP"; static const TickType_t WRITER_TASK_DELAY = pdMS_TO_TICKS(10); static const TickType_t TELEMETRY_TASK_DELAY = pdMS_TO_TICKS(500); static const int64_t WRITE_TIMEOUT_US = 1200000; static const int64_t CONGESTION_TIMEOUT_US = 1800000; static uint32_t s_spp_handle = 0; static bool s_client_connected = false; static bool s_spp_congested = false; static bool s_write_in_flight = false; static int64_t s_last_write_started_us = 0; static int64_t s_last_congestion_change_us = 0; static portMUX_TYPE s_tx_lock = portMUX_INITIALIZER_UNLOCKED; static char s_rx_line[256]; static size_t s_rx_len = 0; static char s_pending_high[160]; static bool s_has_pending_high = false; static char s_pending_telemetry[160]; static bool s_has_pending_telemetry = false; static void tx_clear_pending(void) { portENTER_CRITICAL(&s_tx_lock); s_pending_high[0] = '\0'; s_has_pending_high = false; s_pending_telemetry[0] = '\0'; s_has_pending_telemetry = false; portEXIT_CRITICAL(&s_tx_lock); } static void tx_enqueue_high(const char *text) { if (text == NULL || text[0] == '\0') { return; } portENTER_CRITICAL(&s_tx_lock); strncpy(s_pending_high, text, sizeof(s_pending_high) - 1); s_pending_high[sizeof(s_pending_high) - 1] = '\0'; s_has_pending_high = true; portEXIT_CRITICAL(&s_tx_lock); } static void tx_enqueue_telemetry(const char *text) { if (text == NULL || text[0] == '\0') { return; } portENTER_CRITICAL(&s_tx_lock); strncpy(s_pending_telemetry, text, sizeof(s_pending_telemetry) - 1); s_pending_telemetry[sizeof(s_pending_telemetry) - 1] = '\0'; s_has_pending_telemetry = true; portEXIT_CRITICAL(&s_tx_lock); } static void process_received_char(char ch) { if (ch == '\r') { return; } if (ch == '\n') { s_rx_line[s_rx_len] = '\0'; if (s_rx_len > 0) { char response[128]; if (protocol_build_response(s_rx_line, response, sizeof(response))) { tx_enqueue_high(response); } } s_rx_len = 0; return; } if (s_rx_len < sizeof(s_rx_line) - 1) { s_rx_line[s_rx_len++] = ch; } else { s_rx_len = 0; } } static void handle_received_data(const uint8_t *data, uint16_t len) { if (data == NULL || len == 0) { return; } for (uint16_t i = 0; i < len; i++) { process_received_char((char)data[i]); } } static void writer_task(void *arg) { (void)arg; char message[160]; while (1) { message[0] = '\0'; const int64_t now_us = esp_timer_get_time(); if (s_write_in_flight && s_last_write_started_us != 0) { if ((now_us - s_last_write_started_us) > WRITE_TIMEOUT_US) { ESP_LOGW(TAG, "TX watchdog released stalled write state"); s_write_in_flight = false; s_last_write_started_us = 0; } } if (s_spp_congested && s_last_congestion_change_us != 0) { if ((now_us - s_last_congestion_change_us) > CONGESTION_TIMEOUT_US) { ESP_LOGW(TAG, "TX watchdog cleared stale congestion flag"); s_spp_congested = false; s_last_congestion_change_us = now_us; } } if (s_client_connected && s_spp_handle != 0 && !s_spp_congested && !s_write_in_flight) { portENTER_CRITICAL(&s_tx_lock); if (s_has_pending_high) { strncpy(message, s_pending_high, sizeof(message) - 1); message[sizeof(message) - 1] = '\0'; s_has_pending_high = false; s_pending_high[0] = '\0'; } else if (s_has_pending_telemetry) { strncpy(message, s_pending_telemetry, sizeof(message) - 1); message[sizeof(message) - 1] = '\0'; s_has_pending_telemetry = false; s_pending_telemetry[0] = '\0'; } portEXIT_CRITICAL(&s_tx_lock); if (message[0] != '\0') { esp_err_t err = esp_spp_write(s_spp_handle, strlen(message), (uint8_t *)message); if (err == ESP_OK) { s_write_in_flight = true; s_last_write_started_us = now_us; } else { ESP_LOGW(TAG, "esp_spp_write failed: %s", esp_err_to_name(err)); s_write_in_flight = false; s_last_write_started_us = 0; } } } vTaskDelay(WRITER_TASK_DELAY); } } static void telemetry_task(void *arg) { (void)arg; while (1) { if (s_client_connected && protocol_is_handshake_verified()) { speed_sensor_update_timeout(); const float speed_kmh = speed_sensor_get_speed_kmh(); const float distance_km = speed_sensor_get_distance_km(); const uint32_t pulse_total = speed_sensor_get_pulse_total(); const char *state = drive_control_get_state_string(); battery_monitor_update(); const int battery_percent = battery_monitor_get_percent(); const float battery_voltage = battery_monitor_get_voltage(); if (!s_spp_congested && !s_write_in_flight) { char telemetry_line[192]; snprintf( telemetry_line, sizeof(telemetry_line), "TEL:SPD=%.2f;DST_KM=%.5f;PULSE=%lu;STATE=%s;BAT=%d;VBAT=%.2f\n", (double)speed_kmh, (double)distance_km, (unsigned long)pulse_total, state, battery_percent, (double)battery_voltage ); tx_enqueue_telemetry(telemetry_line); } } vTaskDelay(TELEMETRY_TASK_DELAY); } } static void spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param) { switch (event) { case ESP_SPP_INIT_EVT: ESP_LOGI(TAG, "ESP_SPP_INIT_EVT"); esp_bt_gap_set_device_name(DEVICE_NAME); esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE); esp_spp_start_srv(ESP_SPP_SEC_NONE, ESP_SPP_ROLE_SLAVE, 0, SPP_SERVER_NAME); break; case ESP_SPP_START_EVT: ESP_LOGI(TAG, "ESP_SPP_START_EVT"); break; case ESP_SPP_SRV_OPEN_EVT: ESP_LOGI(TAG, "ESP_SPP_SRV_OPEN_EVT"); s_spp_handle = param->srv_open.handle; s_client_connected = true; s_rx_len = 0; s_spp_congested = false; s_write_in_flight = false; s_last_write_started_us = 0; s_last_congestion_change_us = esp_timer_get_time(); protocol_reset_session(); tx_clear_pending(); // Socket connected is not enough for motor control. // Control is enabled only after HELLO handshake is verified. drive_control_set_client_connected(false); break; case ESP_SPP_CLOSE_EVT: ESP_LOGI(TAG, "ESP_SPP_CLOSE_EVT"); s_spp_handle = 0; s_client_connected = false; s_rx_len = 0; s_spp_congested = false; s_write_in_flight = false; s_last_write_started_us = 0; s_last_congestion_change_us = esp_timer_get_time(); protocol_reset_session(); tx_clear_pending(); drive_control_set_client_connected(false); break; case ESP_SPP_DATA_IND_EVT: handle_received_data(param->data_ind.data, param->data_ind.len); break; case ESP_SPP_WRITE_EVT: s_write_in_flight = false; s_last_write_started_us = 0; s_spp_congested = param->write.cong; s_last_congestion_change_us = esp_timer_get_time(); break; case ESP_SPP_CONG_EVT: s_spp_congested = param->cong.cong; s_last_congestion_change_us = esp_timer_get_time(); break; default: break; } } esp_err_t bt_spp_server_init(void) { esp_err_t err; esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT(); err = esp_bt_controller_init(&bt_cfg); if (err != ESP_OK) { ESP_LOGE(TAG, "esp_bt_controller_init failed: %s", esp_err_to_name(err)); return err; } err = esp_bt_controller_enable(ESP_BT_MODE_BTDM); if (err != ESP_OK) { ESP_LOGE(TAG, "esp_bt_controller_enable failed: %s", esp_err_to_name(err)); return err; } err = esp_bluedroid_init(); if (err != ESP_OK) { ESP_LOGE(TAG, "esp_bluedroid_init failed: %s", esp_err_to_name(err)); return err; } err = esp_bluedroid_enable(); if (err != ESP_OK) { ESP_LOGE(TAG, "esp_bluedroid_enable failed: %s", esp_err_to_name(err)); return err; } err = esp_spp_register_callback(spp_cb); if (err != ESP_OK) { ESP_LOGE(TAG, "esp_spp_register_callback failed: %s", esp_err_to_name(err)); return err; } esp_spp_cfg_t spp_cfg = { .mode = ESP_SPP_MODE_CB, .enable_l2cap_ertm = false, .tx_buffer_size = 0, }; err = esp_spp_enhanced_init(&spp_cfg); if (err != ESP_OK) { ESP_LOGE(TAG, "esp_spp_enhanced_init failed: %s", esp_err_to_name(err)); return err; } BaseType_t writer_ok = xTaskCreate(writer_task, "bt_writer_task", 3072, NULL, 6, NULL); if (writer_ok != pdPASS) { ESP_LOGE(TAG, "Failed to create bt_writer_task"); return ESP_FAIL; } BaseType_t telemetry_ok = xTaskCreate(telemetry_task, "telemetry_task", 3072, NULL, 5, NULL); if (telemetry_ok != pdPASS) { ESP_LOGE(TAG, "Failed to create telemetry_task"); return ESP_FAIL; } ESP_LOGI(TAG, "Bluetooth Classic SPP server init done"); return ESP_OK; }