344 lines
10 KiB
C
344 lines
10 KiB
C
#include "bt_spp_server.h"
|
|
#include "drive_control.h"
|
|
#include "protocol.h"
|
|
#include "speed_sensor.h"
|
|
#include "battery_monitor.h"
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#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;
|
|
}
|