jh.chun/VesiScan-Basic_Zephyr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

BLE TX power/RSSI 모니터 로직 분리

Browse Source
This commit is contained in:
jhchun
2026-07-20 12:27:12 +09:00
parent 3f2c2e774b
commit 4a3e71774f
3 changed files with 462 additions and 331 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/ble/ble_service.c
+76 -331
View File
@@ -1,4 +1,4 @@
/*******************************************************************************
/*******************************************************************************
* @file ble_service.c
* @brief BLE NUS service module
*
@@ -12,8 +12,6 @@
#include <zephyr/kernel.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/hci_vs.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>
@@ -25,13 +23,14 @@
#include <errno.h>
#include "ble_service.h"
#include "ble_tx_power.h"
#include "main.h"
#include "debug_print.h"
#include "led_control.h"
LOG_MODULE_REGISTER(ble_svc, LOG_LEVEL_INF);
/* Module variables */
/* BLE service 상태 */
static struct bt_conn *current_conn;
static ble_data_rx_cb_t rx_callback;
static bool dfu_advertising_mode;
@@ -39,29 +38,6 @@ static bool advertising_unlimited;
static uint8_t conn_param_update_attempts;
static int64_t last_rx_conn_param_update_ms;
#define BLE_TX_POWER_NORMAL_DBM 4
#define BLE_TX_POWER_STRESS_DBM 8
#define BLE_RSSI_MONITOR_ENABLED 0
#define BLE_RSSI_STRESS_DBM (-80)
#define BLE_RSSI_STRESS_COUNT 3
#define BLE_RSSI_MONITOR_INTERVAL_MS 500
#define BLE_TX_SLOW_MS 50
#define BLE_TX_CRITICAL_MS 100
#define BLE_TX_STUCK_MS 200
#define BLE_TX_SLOW_COUNT 3
#define BLE_TX_BUSY_STRESS_RETRY 4
#define BLE_TX_RECOVER_HOLD_MS 10000 // 마지막 트리거 이후 이 시간 동안 트리거 없는 경우 TX power 복귀
extern int bt_hci_get_conn_handle(const struct bt_conn *conn, uint16_t *conn_handle);
static uint16_t current_conn_handle;
static bool current_conn_handle_valid;
static int8_t current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
static uint8_t low_rssi_count;
static uint8_t slow_tx_count;
static int64_t last_stress_ms;
static struct k_work_delayable tx_power_monitor_work;
#if IS_ENABLED(CONFIG_BT_SMP)
#define BLE_REQUIRED_SECURITY_LEVEL BT_SECURITY_L4
static uint32_t pairing_passkey = BT_PASSKEY_RAND;
@@ -79,211 +55,7 @@ static struct k_work adv_restart_work;
static struct k_work_delayable adv_timeout_work;
static struct k_work_delayable conn_param_update_work;
static int ble_tx_power_write(uint8_t handle_type, uint16_t handle, int8_t dbm,
const char *reason)
{
struct net_buf *buf;
struct net_buf *rsp;
struct bt_hci_cp_vs_write_tx_power_level *cp;
struct bt_hci_rp_vs_write_tx_power_level *rp;
int err;
buf = bt_hci_cmd_alloc(K_NO_WAIT);
if (buf == NULL)
{
return -ENOBUFS;
}
cp = net_buf_add(buf, sizeof(*cp));
cp->handle_type = handle_type;
cp->handle = sys_cpu_to_le16(handle);
cp->tx_power_level = dbm;
err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_WRITE_TX_POWER_LEVEL, buf, &rsp);
if (err)
{
DBG_ERR("[BLE] TX power set fail reason=%s err=%d\r\n", reason, err);
return err;
}
rp = (void *)rsp->data;
if (rp->status != 0U)
{
err = -EIO;
DBG_ERR("[BLE] TX power set rejected reason=%s status=0x%02x\r\n",
reason, rp->status);
}
else
{
current_tx_power_dbm = rp->selected_tx_power;
DBG_CORE("[BLE] TX power %d dBm reason=%s\r\n",
current_tx_power_dbm, reason);
}
net_buf_unref(rsp);
return err;
}
static int ble_set_conn_tx_power(int8_t dbm, const char *reason)
{
if (!current_conn_handle_valid || current_tx_power_dbm == dbm)
{
return 0;
}
return ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
current_conn_handle,
dbm,
reason ? reason : "unknown");
}
static void ble_tx_power_stress(const char *reason)
{
last_stress_ms = k_uptime_get(); // stress 발생 시각 기록 (복귀 판정 기준)
(void)ble_set_conn_tx_power(BLE_TX_POWER_STRESS_DBM, reason);
}
// 마지막 stress 이후 충분히 조용하면(BLE_TX_RECOVER_HOLD_MS) NORMAL(4)로 복귀
static void ble_tx_power_try_recover(void)
{
if ((current_tx_power_dbm != BLE_TX_POWER_NORMAL_DBM) &&
((k_uptime_get() - last_stress_ms) >= BLE_TX_RECOVER_HOLD_MS))
{
(void)ble_set_conn_tx_power(BLE_TX_POWER_NORMAL_DBM, "link-recovered");
}
}
// 컨트롤러에 설정된 현재 연결 TX power를 조회 (실제 적용값 확인용)
static int ble_read_conn_tx_power(int8_t *dbm)
{
struct net_buf *buf;
struct net_buf *rsp;
struct bt_hci_cp_vs_read_tx_power_level *cp;
struct bt_hci_rp_vs_read_tx_power_level *rp;
int err;
if (!current_conn_handle_valid || dbm == NULL)
{
return -ENOTCONN;
}
buf = bt_hci_cmd_alloc(K_NO_WAIT);
if (buf == NULL)
{
return -ENOBUFS;
}
cp = net_buf_add(buf, sizeof(*cp));
cp->handle_type = BT_HCI_VS_LL_HANDLE_TYPE_CONN;
cp->handle = sys_cpu_to_le16(current_conn_handle);
err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_TX_POWER_LEVEL, buf, &rsp);
if (err)
{
return err;
}
rp = (void *)rsp->data;
if (rp->status != 0U)
{
err = -EIO;
}
else
{
*dbm = rp->tx_power_level;
}
net_buf_unref(rsp);
return err;
}
static int ble_read_conn_rssi(int8_t *rssi)
{
struct net_buf *buf;
struct net_buf *rsp;
struct bt_hci_cp_read_rssi *cp;
struct bt_hci_rp_read_rssi *rp;
int err;
if (!current_conn_handle_valid || rssi == NULL)
{
return -ENOTCONN;
}
buf = bt_hci_cmd_alloc(K_NO_WAIT);
if (buf == NULL)
{
return -ENOBUFS;
}
cp = net_buf_add(buf, sizeof(*cp));
cp->handle = sys_cpu_to_le16(current_conn_handle);
err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_RSSI, buf, &rsp);
if (err)
{
return err;
}
rp = (void *)rsp->data;
if (rp->status != 0U)
{
err = -EIO;
}
else
{
*rssi = rp->rssi;
}
net_buf_unref(rsp);
return err;
}
static void tx_power_monitor_handler(struct k_work *work)
{
int8_t rssi = 0;
int err;
ARG_UNUSED(work);
if (!current_conn_handle_valid)
{
return;
}
if (!BLE_RSSI_MONITOR_ENABLED)
{
return;
}
err = ble_read_conn_rssi(&rssi);
if (err)
{
ble_tx_power_stress("rssi-silence");
}
else if (rssi <= BLE_RSSI_STRESS_DBM)
{
if (low_rssi_count < UINT8_MAX)
{
low_rssi_count++;
}
if (low_rssi_count >= BLE_RSSI_STRESS_COUNT)
{
ble_tx_power_stress("rssi-low");
}
}
else
{
low_rssi_count = 0U;
}
// 전송이 없어도 주기적으로 복귀 판정 (마지막 stress 후 충분히 조용하면 NORMAL)
ble_tx_power_try_recover();
k_work_schedule(&tx_power_monitor_work, K_MSEC(BLE_RSSI_MONITOR_INTERVAL_MS));
}
/* 로컬 BLE MAC 주소 로그 */
static void ble_log_local_identity(void)
{
bt_addr_le_t addr = {0};
@@ -302,15 +74,18 @@ static void ble_log_local_identity(void)
}
#if IS_ENABLED(CONFIG_BT_SMP)
/* NUS 접근 전 보안 level 확인 */
static bool ble_conn_is_secure(const struct bt_conn *conn)
{
return (conn != NULL) && (bt_conn_get_security(conn) >= BLE_REQUIRED_SECURITY_LEVEL);
}
/* 6자리 ASCII passkey를 숫자로 변환 */
static int ble_passkey_to_uint(const char *passkey, unsigned int *value)
{
unsigned int parsed = 0U;
// 입력 포인터 방어
if ((passkey == NULL) || (value == NULL))
{
return -EINVAL;
@@ -318,6 +93,7 @@ static int ble_passkey_to_uint(const char *passkey, unsigned int *value)
for (size_t i = 0; i < PASSKEY_LENGTH; i++)
{
// 숫자 6자리만 허용
if ((passkey[i] < '0') || (passkey[i] > '9'))
{
return -EINVAL;
@@ -330,6 +106,7 @@ static int ble_passkey_to_uint(const char *passkey, unsigned int *value)
return 0;
}
/* 저장된 passkey 검증, 실패 시 기본값 사용 */
static int ble_security_configure(void)
{
unsigned int passkey;
@@ -353,6 +130,7 @@ static int ble_security_configure(void)
return 0;
}
/* Just Works/passkey pairing 요청 자동 승인 */
static void auth_pairing_confirm(struct bt_conn *conn)
{
int err = bt_conn_auth_pairing_confirm(conn);
@@ -366,6 +144,7 @@ static void auth_pairing_confirm(struct bt_conn *conn)
DBG_CORE("[BLE] Pairing confirmed\r\n");
}
/* pairing 취소 로그 */
static void auth_cancel(struct bt_conn *conn)
{
ARG_UNUSED(conn);
@@ -373,6 +152,7 @@ static void auth_cancel(struct bt_conn *conn)
}
#if IS_ENABLED(CONFIG_BT_APP_PASSKEY)
/* Zephyr pairing callback에 현재 passkey 제공 */
static uint32_t auth_app_passkey(struct bt_conn *conn)
{
unsigned int passkey;
@@ -389,6 +169,7 @@ static uint32_t auth_app_passkey(struct bt_conn *conn)
}
#endif
/* pairing 처리 callback 묶음 */
static struct bt_conn_auth_cb auth_cb =
{
.pairing_confirm = auth_pairing_confirm,
@@ -398,24 +179,28 @@ static struct bt_conn_auth_cb auth_cb =
#endif
};
/* pairing 성공 로그 */
static void auth_pairing_complete(struct bt_conn *conn, bool bonded)
{
ARG_UNUSED(conn);
DBG_CORE("[BLE] Pairing complete bonded=%u\r\n", bonded ? 1U : 0U);
}
/* pairing 실패 시 연결 종료 */
static void auth_pairing_failed(struct bt_conn *conn, enum bt_security_err reason)
{
DBG_ERR("[BLE] Pairing failed reason=%u\r\n", reason);
(void)bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL);
}
/* pairing 결과 callback 묶음 */
static struct bt_conn_auth_info_cb auth_info_cb =
{
.pairing_complete = auth_pairing_complete,
.pairing_failed = auth_pairing_failed,
};
/* BLE 보안 callback 등록, passkey 설정 */
static int ble_security_init(void)
{
int err = bt_conn_auth_cb_register(&auth_cb);
@@ -437,6 +222,7 @@ static int ble_security_init(void)
}
#endif
/* callback context 밖에서 advertising 재시작 */
static void adv_restart_handler(struct k_work *work)
{
ARG_UNUSED(work);
@@ -453,14 +239,15 @@ static bool ble_disconnect_reason_uses_unlimited_adv(uint8_t reason)
{
switch (reason)
{
case BT_HCI_ERR_REMOTE_USER_TERM_CONN: // 0x13 앱이 연결을 정상 종료
return false; // → 10분 타임아웃 후 전원 OFF
case BT_HCI_ERR_REMOTE_USER_TERM_CONN: // 0x13 앱이 연결을 정상 종료
return false; // 10분 타임아웃 후 전원 OFF
default:
return true; // 그 외 전부 → 무제한 광고
default:
return true; // 그 외 전부 → 무제한 광고
}
}
/* 광고 timeout 시 자동 전원 OFF 판단 */
static void adv_timeout_handler(struct k_work *work)
{
ARG_UNUSED(work);
@@ -474,14 +261,14 @@ static void adv_timeout_handler(struct k_work *work)
}
}
/* 현재 연결 파라미터가 목표값인지 비교 */
static bool ble_conn_param_is_preferred(uint16_t interval, uint16_t latency,
uint16_t timeout)
{
return (interval == BLE_MIN_CONN_INTERVAL) &&
(latency == BLE_SLAVE_LATENCY) &&
(timeout == BLE_CONN_SUP_TIMEOUT);
return (interval == BLE_MIN_CONN_INTERVAL) && (latency == BLE_SLAVE_LATENCY) && (timeout == BLE_CONN_SUP_TIMEOUT);
}
/* 15ms interval 목표로 connection parameter update 요청 */
static int ble_request_preferred_conn_params(bool count_attempt)
{
struct bt_le_conn_param conn_param =
@@ -494,6 +281,7 @@ static int ble_request_preferred_conn_params(bool count_attempt)
struct bt_conn_info info;
int err;
// 연결 없으면 요청 불가
if (current_conn == NULL)
{
return -ENOTCONN;
@@ -522,6 +310,7 @@ static int ble_request_preferred_conn_params(bool count_attempt)
}
}
// 예약 재시도에서만 attempt 증가
if (count_attempt)
{
conn_param_update_attempts++;
@@ -546,6 +335,7 @@ static int ble_request_preferred_conn_params(bool count_attempt)
return 0;
}
/* connection parameter 재시도 work */
static void conn_param_update_handler(struct k_work *work)
{
ARG_UNUSED(work);
@@ -555,21 +345,19 @@ static void conn_param_update_handler(struct k_work *work)
return;
}
// central이 계속 거부하면 최대 횟수에서 중지
if (conn_param_update_attempts >= BLE_CONN_PARAM_MAX_ATTEMPTS)
{
return;
}
if (ble_request_preferred_conn_params(true) == 0 &&
conn_param_update_attempts < BLE_CONN_PARAM_MAX_ATTEMPTS)
if (ble_request_preferred_conn_params(true) == 0 && conn_param_update_attempts < BLE_CONN_PARAM_MAX_ATTEMPTS)
{
k_work_schedule(&conn_param_update_work,
K_MSEC(BLE_CONN_PARAM_RETRY_INTERVAL_MS));
k_work_schedule(&conn_param_update_work, K_MSEC(BLE_CONN_PARAM_RETRY_INTERVAL_MS));
}
}
/* Advertising data */
/* Advertising scan response data */
static const struct bt_data sd[] =
{
// 평상시 Web Bluetooth 앱이 찾을 수 있도록 NUS UUID 광고
@@ -582,7 +370,7 @@ static const struct bt_data sd_dfu[] =
BT_DATA_BYTES(BT_DATA_UUID128_SOME, SMP_BT_SVC_UUID_VAL),
};
/* BLE TX Power Control */
/* BLE 연결 완료 처리 */
static void connected(struct bt_conn *conn, uint8_t err)
{
if (err)
@@ -594,41 +382,16 @@ static void connected(struct bt_conn *conn, uint8_t err)
current_conn = bt_conn_ref(conn);
ble_connection_st = true;
current_conn_handle_valid = (bt_hci_get_conn_handle(conn, &current_conn_handle) == 0);
current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
low_rssi_count = 0U;
slow_tx_count = 0U;
last_stress_ms = k_uptime_get();
// 컨트롤러 기본 출력이 최대(+8)이므로, 연결 시 NORMAL(4)로 강제 적용
if (current_conn_handle_valid)
{
(void)ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_CONN, current_conn_handle, BLE_TX_POWER_NORMAL_DBM, "connected");
}
ble_tx_power_on_connected(conn); // TX power/RSSI 상태는 별도 모듈에서 관리
int8_t cur_txp = 0;
int txp_ret = ble_read_conn_tx_power(&cur_txp);
if (txp_ret == 0)
{
DBG_CORE("[BLE] TX power (current) = %d dBm\r\n", cur_txp);
}
else
{
DBG_CORE("[BLE] TX power read fail ret=%d\r\n", txp_ret);
}
if (BLE_RSSI_MONITOR_ENABLED)
{
k_work_schedule(&tx_power_monitor_work, K_MSEC(BLE_RSSI_MONITOR_INTERVAL_MS));
}
/* 연결 정보 출력 */
char addr_str[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(bt_conn_get_dst(conn), addr_str, sizeof(addr_str));
DBG_CORE("[BLE] Peer: %s\r\n", addr_str);
DBG_CORE("[BLE] Peer: %s\r\n", addr_str); // 연결 정보 출력
conn_param_update_attempts = 0U;
last_rx_conn_param_update_ms = 0;
// 연결 직후 약간 기다린 뒤 preferred parameter 요청
k_work_schedule(&conn_param_update_work, K_MSEC(BLE_CONN_PARAM_FIRST_DELAY_MS));
#if IS_ENABLED(CONFIG_BT_SMP)
@@ -646,17 +409,13 @@ static void connected(struct bt_conn *conn, uint8_t err)
DBG_CORE("[BLE] Connected\r\n");
}
/* BLE 연결 해제 처리 */
static void disconnected(struct bt_conn *conn, uint8_t reason)
{
k_work_cancel_delayable(&conn_param_update_work);
k_work_cancel_delayable(&tx_power_monitor_work);
ble_tx_power_on_disconnected();
conn_param_update_attempts = 0U;
last_rx_conn_param_update_ms = 0;
current_conn_handle_valid = false;
current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
low_rssi_count = 0U;
slow_tx_count = 0U;
last_stress_ms = 0;
if (current_conn)
{
@@ -682,6 +441,7 @@ static void disconnected(struct bt_conn *conn, uint8_t reason)
k_work_submit(&adv_restart_work);
}
/* central의 connection parameter 요청 허용 */
static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param)
{
ARG_UNUSED(conn);
@@ -690,6 +450,7 @@ static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param)
return true;
}
/* connection parameter 변경 결과 확인 */
static void le_param_updated(struct bt_conn *conn, uint16_t interval, uint16_t latency, uint16_t timeout)
{
ARG_UNUSED(conn);
@@ -702,20 +463,20 @@ static void le_param_updated(struct bt_conn *conn, uint16_t interval, uint16_t l
timeout,
timeout * 10U);
// 목표값 도달 시 재시도 work 중지
if (ble_conn_param_is_preferred(interval, latency, timeout))
{
k_work_cancel_delayable(&conn_param_update_work);
}
else if (conn_param_update_attempts < BLE_CONN_PARAM_MAX_ATTEMPTS)
{
k_work_schedule(&conn_param_update_work,
K_MSEC(BLE_CONN_PARAM_RETRY_INTERVAL_MS));
k_work_schedule(&conn_param_update_work, K_MSEC(BLE_CONN_PARAM_RETRY_INTERVAL_MS));
}
}
#if IS_ENABLED(CONFIG_BT_SMP)
static void security_changed(struct bt_conn *conn, bt_security_t level,
enum bt_security_err err)
/* BLE security level 변경 결과 확인 */
static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err)
{
ARG_UNUSED(conn);
@@ -740,7 +501,7 @@ BT_CONN_CB_DEFINE(conn_callbacks) =
#endif
};
/* NUS callbacks */
/* NUS RX: 보안 확인 후 app parser로 전달 */
static void nus_received(struct bt_conn *conn, const uint8_t *data, uint16_t len)
{
#if IS_ENABLED(CONFIG_BT_SMP)
@@ -768,6 +529,7 @@ static void nus_received(struct bt_conn *conn, const uint8_t *data, uint16_t len
}
}
/* NUS TX 완료 callback */
static void nus_sent(struct bt_conn *conn)
{
ARG_UNUSED(conn);
@@ -782,7 +544,7 @@ static struct bt_nus_cb nus_cb =
.sent = nus_sent,
};
/* Public functions */
/* BLE stack, security, NUS service 초기화 */
int ble_service_init(ble_data_rx_cb_t rx_cb)
{
int err;
@@ -794,9 +556,8 @@ int ble_service_init(ble_data_rx_cb_t rx_cb)
k_work_init(&adv_restart_work, adv_restart_handler);
k_work_init_delayable(&adv_timeout_work, adv_timeout_handler);
k_work_init_delayable(&conn_param_update_work, conn_param_update_handler);
k_work_init_delayable(&tx_power_monitor_work, tx_power_monitor_handler);
// Enable BLE stack
ble_tx_power_init();
// BLE stack 활성화
err = bt_enable(NULL);
if (err)
{
@@ -825,7 +586,7 @@ int ble_service_init(ble_data_rx_cb_t rx_cb)
}
#endif
// Initialize NUS
// NUS service 초기화
err = bt_nus_init(&nus_cb);
if (err)
{
@@ -837,6 +598,7 @@ int ble_service_init(ble_data_rx_cb_t rx_cb)
return 0;
}
/* 현재 모드에 맞는 advertising 시작 */
int ble_advertising_start(void)
{
int err;
@@ -870,8 +632,7 @@ int ble_advertising_start(void)
return err;
}
// 컨트롤러 기본 출력이 최대(+8)이므로, 광고는 항상 NORMAL(4)로 고정 (stress 무관)
(void)ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_ADV, 0, BLE_TX_POWER_NORMAL_DBM, "adv");
ble_tx_power_apply_advertising();
k_work_cancel_delayable(&adv_timeout_work);
if (!advertising_unlimited) // DFU 광고 포함, 미연결 10분 시 전원 OFF
@@ -883,6 +644,7 @@ int ble_advertising_start(void)
return 0;
}
/* advertising 중지, timeout work 취소 */
int ble_advertising_stop(void)
{
int err = bt_le_adv_stop();
@@ -897,6 +659,7 @@ int ble_advertising_stop(void)
return 0;
}
/* DFU 모드 진입, SMP advertising 준비 */
int ble_dfu_advertising_enable(void)
{
if (dfu_advertising_mode)
@@ -930,9 +693,10 @@ bool ble_dfu_advertising_is_enabled(void)
return dfu_advertising_mode;
}
/* DFU watchdog */
/* DFU watchdog: 현재 연결 강제 disconnect */
int ble_disconnect_active(void)
{
// 연결 없으면 요청 불가
if (current_conn == NULL)
{
return -ENOTCONN;
@@ -941,6 +705,7 @@ int ble_disconnect_active(void)
return bt_conn_disconnect(current_conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
}
/* NUS TX: 직렬화, retry, 완료 대기 */
int ble_data_send(const uint8_t *data, uint16_t len)
{
int err;
@@ -964,7 +729,9 @@ int ble_data_send(const uint8_t *data, uint16_t len)
k_mutex_lock(&nus_tx_lock, K_FOREVER);
// 혹시 남아 있는 완료 신호가 있으면 비우고 이번 전송 전용 상태로 만듦
while (k_sem_take(&nus_tx_done_sem, K_NO_WAIT) == 0) {
while (k_sem_take(&nus_tx_done_sem, K_NO_WAIT) == 0)
{
// stale completion 비우기
}
//DBG_CORE("[NUS TX] send len=%u\r\n", len);
@@ -972,19 +739,17 @@ int ble_data_send(const uint8_t *data, uint16_t len)
for (int retry = 0; ; retry++)
{
err = bt_nus_send(current_conn, data, len);
if (err != -ENOMEM || retry >= 20) {
// -ENOMEM만 짧게 재시도, 그 외 error는 즉시 종료
if (err != -ENOMEM || retry >= 20)
{
break;
}
/*
* -ENOMEM : BLE TX 버퍼가 가득참
* 바로 포기하지 않고 짧게 쉬었다가 다시 시도
*/
// -ENOMEM : BLE TX 버퍼가 가득찬 경우: 바로 포기하지 않고 짧게 쉬었다가 다시 시도
DBG_ERR("[NUS TX] busy, retry=%d\r\n", retry + 1);
if ((retry + 1) >= BLE_TX_BUSY_STRESS_RETRY)
{
ble_tx_power_stress("tx-queue-busy");
}
// busy 반복 시 TX power boost 판단
ble_tx_power_on_tx_busy_retry(retry + 1);
k_msleep(5);
}
@@ -996,11 +761,12 @@ int ble_data_send(const uint8_t *data, uint16_t len)
}
// bt_nus_send() 성공 후 sent 콜백이 올 때까지 기다림 (다음 패킷을 보내도 되는 시점)
err = k_sem_take(&nus_tx_done_sem, K_MSEC(BLE_TX_STUCK_MS));
err = k_sem_take(&nus_tx_done_sem, K_MSEC(ble_tx_power_tx_stuck_ms()));
if (err)
{
ble_tx_power_stress("tx-complete-stuck");
err = k_sem_take(&nus_tx_done_sem, K_MSEC(1000 - BLE_TX_STUCK_MS));
// sent callback 지연도 링크 stress로 처리
ble_tx_power_on_tx_complete_timeout();
err = k_sem_take(&nus_tx_done_sem, K_MSEC(1000 - ble_tx_power_tx_stuck_ms()));
if (err)
{
DBG_ERR("[NUS TX] completion timeout (err %d)\r\n", err);
@@ -1011,36 +777,15 @@ int ble_data_send(const uint8_t *data, uint16_t len)
{
int64_t tx_elapsed_ms = k_uptime_get() - tx_start_ms;
if (tx_elapsed_ms >= BLE_TX_CRITICAL_MS)
{
ble_tx_power_stress("tx-complete-slow100");
slow_tx_count = 0U;
}
else if (tx_elapsed_ms >= BLE_TX_SLOW_MS)
{
if (slow_tx_count < UINT8_MAX)
{
slow_tx_count++;
}
if (slow_tx_count >= BLE_TX_SLOW_COUNT)
{
ble_tx_power_stress("tx-complete-slow50");
}
}
else
{
slow_tx_count = 0U;
// 정상 전송 완료 → 마지막 stress 이후 충분히 조용했으면 복귀
ble_tx_power_try_recover();
}
// 완료 시간 기반 TX power 복구/boost 판단
ble_tx_power_on_tx_complete(tx_elapsed_ms);
}
k_mutex_unlock(&nus_tx_lock);
return err;
}
/* BLE 연결 상태 */
bool ble_is_connected(void)
{
return (current_conn != NULL);
src/ble/ble_tx_power.c
+364
View File
@@ -0,0 +1,364 @@
/*******************************************************************************
* @file ble_tx_power.c
* @brief BLE TX power and RSSI monitor helper
******************************************************************************/
#include <zephyr/kernel.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/hci_vs.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/sys/byteorder.h>
#include <errno.h>
#include "ble_tx_power.h"
#include "debug_print.h"
#define BLE_TX_POWER_NORMAL_DBM 4 // NORMAL: 평상시에는 전류 소모를 낮추기 위해 +4 dBm을 기본값으로 사용
#define BLE_TX_POWER_STRESS_DBM 8 // BOOST: RSSI 또는 TX 지연이 나빠지면 일시적으로 +8 dBm으로 boost
#define BLE_RSSI_MONITOR_ENABLED 1
#define BLE_RSSI_STRESS_DBM (-80)
#define BLE_RSSI_STRESS_COUNT 3
#define BLE_RSSI_MONITOR_INTERVAL_MS 500
#define BLE_TX_SLOW_MS 50
#define BLE_TX_CRITICAL_MS 100
#define BLE_TX_STUCK_MS 200
#define BLE_TX_SLOW_COUNT 3
#define BLE_TX_BUSY_STRESS_RETRY 4
#define BLE_TX_RECOVER_HOLD_MS 10000
extern int bt_hci_get_conn_handle(const struct bt_conn *conn, uint16_t *conn_handle);
/* HCI vendor command는 bt_conn 포인터 대신 controller handle을 사용 */
static uint16_t current_conn_handle;
static bool current_conn_handle_valid;
static int8_t current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
static uint8_t low_rssi_count;
static uint8_t slow_tx_count;
static int64_t last_stress_ms;
static struct k_work_delayable tx_power_monitor_work;
/* Nordic vendor HCI command로 advertising/connection TX power 직접 설정 */
static int ble_tx_power_write(uint8_t handle_type, uint16_t handle, int8_t dbm, const char *reason)
{
struct net_buf *buf;
struct net_buf *rsp;
struct bt_hci_cp_vs_write_tx_power_level *cp;
struct bt_hci_rp_vs_write_tx_power_level *rp;
int err;
buf = bt_hci_cmd_alloc(K_NO_WAIT);
if (buf == NULL)
{
return -ENOBUFS;
}
cp = net_buf_add(buf, sizeof(*cp));
cp->handle_type = handle_type;
cp->handle = sys_cpu_to_le16(handle);
cp->tx_power_level = dbm;
err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_WRITE_TX_POWER_LEVEL, buf, &rsp);
if (err)
{
DBG_ERR("[BLE] TX power set fail reason=%s err=%d\r\n", reason, err);
return err;
}
rp = (void *)rsp->data;
if (rp->status != 0U)
{
err = -EIO;
DBG_ERR("[BLE] TX power set rejected reason=%s status=0x%02x\r\n", reason, rp->status);
}
else
{
if (handle_type == BT_HCI_VS_LL_HANDLE_TYPE_CONN)
{
current_tx_power_dbm = rp->selected_tx_power;
}
DBG_CORE("[BLE] TX power %d dBm reason=%s\r\n", rp->selected_tx_power, reason);
}
net_buf_unref(rsp);
return err;
}
/* 연결 handle이 유효할 때만 connection TX power 변경 */
static int ble_set_conn_tx_power(int8_t dbm, const char *reason)
{
// 연결 전이거나 이미 목표 출력이면 HCI 명령을 보내지 않음
if (!current_conn_handle_valid || current_tx_power_dbm == dbm)
{
return 0;
}
return ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_CONN, current_conn_handle, dbm, reason ? reason : "unknown");
}
static void ble_tx_power_stress(const char *reason)
{
// stress 발생 시각을 기록해서 이후 NORMAL 복귀 시점 판단
last_stress_ms = k_uptime_get();
(void)ble_set_conn_tx_power(BLE_TX_POWER_STRESS_DBM, reason);
}
/* 마지막 stress 이후 일정 시간 stress 없는 경우 NORMAL 출력으로 회복 */
static void ble_tx_power_try_recover(void)
{
// 이미 NORMAL이면 불필요한 HCI 명령 피함
if ((current_tx_power_dbm != BLE_TX_POWER_NORMAL_DBM) &&
((k_uptime_get() - last_stress_ms) >= BLE_TX_RECOVER_HOLD_MS))
{
(void)ble_set_conn_tx_power(BLE_TX_POWER_NORMAL_DBM, "link-recovered");
}
}
/* 컨트롤러에 실제 적용된 연결 TX power 확인 */
static int ble_read_conn_tx_power(int8_t *dbm)
{
struct net_buf *buf;
struct net_buf *rsp;
struct bt_hci_cp_vs_read_tx_power_level *cp;
struct bt_hci_rp_vs_read_tx_power_level *rp;
int err;
// 연결 handle 없이는 controller에 연결 TX power 조회 불가
if (!current_conn_handle_valid || dbm == NULL)
{
return -ENOTCONN;
}
buf = bt_hci_cmd_alloc(K_NO_WAIT);
if (buf == NULL)
{
return -ENOBUFS;
}
cp = net_buf_add(buf, sizeof(*cp));
cp->handle_type = BT_HCI_VS_LL_HANDLE_TYPE_CONN;
cp->handle = sys_cpu_to_le16(current_conn_handle);
err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_TX_POWER_LEVEL, buf, &rsp);
if (err)
{
return err;
}
rp = (void *)rsp->data;
if (rp->status != 0U)
{
err = -EIO;
}
else
{
*dbm = rp->tx_power_level;
}
net_buf_unref(rsp);
return err;
}
/* 현재 연결의 RSSI를 읽어 링크 품질 판단에 사용 */
static int ble_read_conn_rssi(int8_t *rssi)
{
struct net_buf *buf;
struct net_buf *rsp;
struct bt_hci_cp_read_rssi *cp;
struct bt_hci_rp_read_rssi *rp;
int err;
// RSSI 조회도 연결 handle이 있어야 가능
if (!current_conn_handle_valid || rssi == NULL)
{
return -ENOTCONN;
}
buf = bt_hci_cmd_alloc(K_NO_WAIT);
if (buf == NULL)
{
return -ENOBUFS;
}
cp = net_buf_add(buf, sizeof(*cp));
cp->handle = sys_cpu_to_le16(current_conn_handle);
err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_RSSI, buf, &rsp);
if (err)
{
return err;
}
rp = (void *)rsp->data;
if (rp->status != 0U)
{
err = -EIO;
}
else
{
*rssi = rp->rssi;
}
net_buf_unref(rsp);
return err;
}
/* 연결 중에는 주기적으로 RSSI를 확인하고, 필요하면 TX power boost */
static void tx_power_monitor_handler(struct k_work *work)
{
int8_t rssi = 0;
int err;
ARG_UNUSED(work);
if (!current_conn_handle_valid)
{
return;
}
if (!BLE_RSSI_MONITOR_ENABLED)
{
return;
}
err = ble_read_conn_rssi(&rssi);
// RSSI를 못 읽는 상황도 링크 품질 저하로 간주
if (err)
{
ble_tx_power_stress("rssi-silence");
}
else if (rssi <= BLE_RSSI_STRESS_DBM)
{
// 순간적인 RSSI 흔들림은 무시하고, 연속 low RSSI일 때만 boost
if (low_rssi_count < UINT8_MAX)
{
low_rssi_count++;
}
if (low_rssi_count >= BLE_RSSI_STRESS_COUNT)
{
ble_tx_power_stress("rssi-low");
}
}
else
{
// RSSI가 회복되면 연속 low RSSI 카운터 초기화
low_rssi_count = 0U;
}
// TX가 없어도 monitor 주기마다 NORMAL 복귀 여부 확인
ble_tx_power_try_recover();
// 연결이 유지되는 동안 monitor work 계속 반복
k_work_schedule(&tx_power_monitor_work, K_MSEC(BLE_RSSI_MONITOR_INTERVAL_MS));
}
/* RSSI monitor용 delayable work 초기화 */
void ble_tx_power_init(void)
{
k_work_init_delayable(&tx_power_monitor_work, tx_power_monitor_handler);
}
/* 연결 직후 TX power 상태를 초기화하고 RSSI monitor를 시작 */
void ble_tx_power_on_connected(struct bt_conn *conn)
{
int8_t cur_txp = 0;
int txp_ret;
// 이후 HCI 명령에서 사용할 controller connection handle 저장
current_conn_handle_valid = (bt_hci_get_conn_handle(conn, &current_conn_handle) == 0);
current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
low_rssi_count = 0U;
slow_tx_count = 0U;
last_stress_ms = k_uptime_get();
if (current_conn_handle_valid)
{
// 새 연결은 항상 NORMAL (4 dBm) 시작
(void)ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_CONN, current_conn_handle, BLE_TX_POWER_NORMAL_DBM, "connected");
}
txp_ret = ble_read_conn_tx_power(&cur_txp); // 실제 적용값
if (txp_ret == 0)
{
DBG_CORE("[BLE] TX power (current) = %d dBm\r\n", cur_txp);
}
else
{
DBG_CORE("[BLE] TX power read fail ret=%d\r\n", txp_ret);
}
if (BLE_RSSI_MONITOR_ENABLED)
{
// 연결이 유지되는 동안만 RSSI monitor
k_work_schedule(&tx_power_monitor_work, K_MSEC(BLE_RSSI_MONITOR_INTERVAL_MS));
}
}
/* 연결 해제 시 monitor와 링크별 상태 초기화 */
void ble_tx_power_on_disconnected(void)
{
k_work_cancel_delayable(&tx_power_monitor_work);
current_conn_handle_valid = false;
current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
low_rssi_count = 0U;
slow_tx_count = 0U;
last_stress_ms = 0;
}
/* advertising handle의 TX power를 NORMAL 값으로 설정 */
void ble_tx_power_apply_advertising(void)
{
(void)ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_ADV, 0, BLE_TX_POWER_NORMAL_DBM, "adv");
}
/* ble_service.c가 TX 완료 대기 timeout을 TX power 정책값과 공유 */
uint32_t ble_tx_power_tx_stuck_ms(void)
{
return BLE_TX_STUCK_MS;
}
/* NUS TX queue가 여러 번 busy이면 링크가 불안정한 상황으로 보고 출력 boost */
void ble_tx_power_on_tx_busy_retry(uint8_t retry_count)
{
if (retry_count >= BLE_TX_BUSY_STRESS_RETRY)
{
ble_tx_power_stress("tx-queue-busy");
}
}
/* sent callback이 늦게 오면 TX path가 막힌 것으로 보고 boost */
void ble_tx_power_on_tx_complete_timeout(void)
{
ble_tx_power_stress("tx-complete-stuck");
}
/* TX 완료 시간이 길어지는 것도 링크 품질 저하 신호로 사용 */
void ble_tx_power_on_tx_complete(int64_t elapsed_ms)
{
if (elapsed_ms >= BLE_TX_CRITICAL_MS)
{
// 100ms 이상이면 단발성이라도 즉시 boost
ble_tx_power_stress("tx-complete-slow100");
slow_tx_count = 0U;
}
else if (elapsed_ms >= BLE_TX_SLOW_MS)
{
// 50ms 이상은 연속 발생할 때만 boost
if (slow_tx_count < UINT8_MAX)
{
slow_tx_count++;
}
if (slow_tx_count >= BLE_TX_SLOW_COUNT)
{
ble_tx_power_stress("tx-complete-slow50");
}
}
else
{
// 정상 속도로 전송되면 slow 카운터 지우고 복귀 가능성 확인
slow_tx_count = 0U;
ble_tx_power_try_recover();
}
}
src/ble/ble_tx_power.h
+22
View File
@@ -0,0 +1,22 @@
/*******************************************************************************
* @file ble_tx_power.h
* @brief BLE TX power and RSSI monitor helper
******************************************************************************/
#ifndef BLE_TX_POWER_H__
#define BLE_TX_POWER_H__
#include <stdint.h>
struct bt_conn;
void ble_tx_power_init(void);
void ble_tx_power_on_connected(struct bt_conn *conn);
void ble_tx_power_on_disconnected(void);
void ble_tx_power_apply_advertising(void);
uint32_t ble_tx_power_tx_stuck_ms(void);
void ble_tx_power_on_tx_busy_retry(uint8_t retry_count);
void ble_tx_power_on_tx_complete_timeout(void);
void ble_tx_power_on_tx_complete(int64_t elapsed_ms);
#endif /* BLE_TX_POWER_H__ */