1 /* main.c - Application main entry point */
2
3 /*
4 * Copyright (c) 2019 Andrei Stoica
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/bluetooth/gap.h>
10 #include <zephyr/types.h>
11 #include <stddef.h>
12 #include <zephyr/sys/printk.h>
13 #include <zephyr/sys/util.h>
14 #include <zephyr/sys/byteorder.h>
15
16 #include <zephyr/bluetooth/bluetooth.h>
17 #include <zephyr/bluetooth/hci.h>
18 #include <zephyr/bluetooth/hci_vs.h>
19
20 #include <zephyr/bluetooth/conn.h>
21 #include <zephyr/bluetooth/uuid.h>
22 #include <zephyr/bluetooth/gatt.h>
23 #include <zephyr/bluetooth/services/hrs.h>
24
25 BUILD_ASSERT(IS_ENABLED(CONFIG_BT_HAS_HCI_VS),
26 "This app requires Zephyr-specific HCI vendor extensions");
27
28 static struct bt_conn *default_conn;
29 static uint16_t default_conn_handle;
30
31 static const struct bt_data ad[] = {
32 BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
33 BT_DATA_BYTES(BT_DATA_UUID16_ALL, BT_UUID_16_ENCODE(BT_UUID_HRS_VAL)),
34 };
35
36 static const struct bt_data sd[] = {
37 BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1),
38 };
39
40 #define DEVICE_NAME CONFIG_BT_DEVICE_NAME
41 #define DEVICE_NAME_LEN (sizeof(DEVICE_NAME) - 1)
42 #define DEVICE_BEACON_TXPOWER_NUM 8
43
44 static struct k_thread pwr_thread_data;
45 static K_THREAD_STACK_DEFINE(pwr_thread_stack, 512);
46
47 static const int8_t txpower[DEVICE_BEACON_TXPOWER_NUM] = {4, 0, -3, -8,
48 -15, -18, -23, -30};
49 static const struct bt_le_adv_param *param = BT_LE_ADV_PARAM(
50 BT_LE_ADV_OPT_CONN, BT_GAP_MS_TO_ADV_INTERVAL(20), BT_GAP_MS_TO_ADV_INTERVAL(20), NULL);
51
read_conn_rssi(uint16_t handle,int8_t * rssi)52 static void read_conn_rssi(uint16_t handle, int8_t *rssi)
53 {
54 struct net_buf *buf, *rsp = NULL;
55 struct bt_hci_cp_read_rssi *cp;
56 struct bt_hci_rp_read_rssi *rp;
57
58 int err;
59
60 buf = bt_hci_cmd_create(BT_HCI_OP_READ_RSSI, sizeof(*cp));
61 if (!buf) {
62 printk("Unable to allocate command buffer\n");
63 return;
64 }
65
66 cp = net_buf_add(buf, sizeof(*cp));
67 cp->handle = sys_cpu_to_le16(handle);
68
69 err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_RSSI, buf, &rsp);
70 if (err) {
71 printk("Read RSSI err: %d\n", err);
72 return;
73 }
74
75 rp = (void *)rsp->data;
76 *rssi = rp->rssi;
77
78 net_buf_unref(rsp);
79 }
80
81
set_tx_power(uint8_t handle_type,uint16_t handle,int8_t tx_pwr_lvl)82 static void set_tx_power(uint8_t handle_type, uint16_t handle, int8_t tx_pwr_lvl)
83 {
84 struct bt_hci_cp_vs_write_tx_power_level *cp;
85 struct bt_hci_rp_vs_write_tx_power_level *rp;
86 struct net_buf *buf, *rsp = NULL;
87 int err;
88
89 buf = bt_hci_cmd_create(BT_HCI_OP_VS_WRITE_TX_POWER_LEVEL,
90 sizeof(*cp));
91 if (!buf) {
92 printk("Unable to allocate command buffer\n");
93 return;
94 }
95
96 cp = net_buf_add(buf, sizeof(*cp));
97 cp->handle = sys_cpu_to_le16(handle);
98 cp->handle_type = handle_type;
99 cp->tx_power_level = tx_pwr_lvl;
100
101 err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_WRITE_TX_POWER_LEVEL,
102 buf, &rsp);
103 if (err) {
104 printk("Set Tx power err: %d\n", err);
105 return;
106 }
107
108 rp = (void *)rsp->data;
109 printk("Actual Tx Power: %d\n", rp->selected_tx_power);
110
111 net_buf_unref(rsp);
112 }
113
get_tx_power(uint8_t handle_type,uint16_t handle,int8_t * tx_pwr_lvl)114 static void get_tx_power(uint8_t handle_type, uint16_t handle, int8_t *tx_pwr_lvl)
115 {
116 struct bt_hci_cp_vs_read_tx_power_level *cp;
117 struct bt_hci_rp_vs_read_tx_power_level *rp;
118 struct net_buf *buf, *rsp = NULL;
119 int err;
120
121 *tx_pwr_lvl = 0xFF;
122 buf = bt_hci_cmd_create(BT_HCI_OP_VS_READ_TX_POWER_LEVEL,
123 sizeof(*cp));
124 if (!buf) {
125 printk("Unable to allocate command buffer\n");
126 return;
127 }
128
129 cp = net_buf_add(buf, sizeof(*cp));
130 cp->handle = sys_cpu_to_le16(handle);
131 cp->handle_type = handle_type;
132
133 err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_TX_POWER_LEVEL,
134 buf, &rsp);
135 if (err) {
136 printk("Read Tx power err: %d\n", err);
137 return;
138 }
139
140 rp = (void *)rsp->data;
141 *tx_pwr_lvl = rp->tx_power_level;
142
143 net_buf_unref(rsp);
144 }
145
connected(struct bt_conn * conn,uint8_t err)146 static void connected(struct bt_conn *conn, uint8_t err)
147 {
148 char addr[BT_ADDR_LE_STR_LEN];
149 int8_t txp;
150 int ret;
151
152 if (err) {
153 printk("Connection failed, err 0x%02x %s\n", err, bt_hci_err_to_str(err));
154 } else {
155 default_conn = bt_conn_ref(conn);
156 ret = bt_hci_get_conn_handle(default_conn,
157 &default_conn_handle);
158 if (ret) {
159 printk("No connection handle (err %d)\n", ret);
160 } else {
161 /* Send first at the default selected power */
162 bt_addr_le_to_str(bt_conn_get_dst(conn),
163 addr, sizeof(addr));
164 printk("Connected via connection (%d) at %s\n",
165 default_conn_handle, addr);
166 get_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
167 default_conn_handle, &txp);
168 printk("Connection (%d) - Initial Tx Power = %d\n",
169 default_conn_handle, txp);
170
171 set_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
172 default_conn_handle,
173 BT_HCI_VS_LL_TX_POWER_LEVEL_NO_PREF);
174 get_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
175 default_conn_handle, &txp);
176 printk("Connection (%d) - Tx Power = %d\n",
177 default_conn_handle, txp);
178 }
179 }
180 }
181
disconnected(struct bt_conn * conn,uint8_t reason)182 static void disconnected(struct bt_conn *conn, uint8_t reason)
183 {
184 printk("Disconnected, reason 0x%02x %s\n", reason, bt_hci_err_to_str(reason));
185
186 if (default_conn) {
187 bt_conn_unref(default_conn);
188 default_conn = NULL;
189 }
190 }
191
192 BT_CONN_CB_DEFINE(conn_callbacks) = {
193 .connected = connected,
194 .disconnected = disconnected,
195 };
196
bt_ready(int err)197 static void bt_ready(int err)
198 {
199 if (err) {
200 printk("Bluetooth init failed (err %d)\n", err);
201 return;
202 }
203
204 printk("Bluetooth initialized\n");
205
206 /* Start advertising */
207 err = bt_le_adv_start(param, ad, ARRAY_SIZE(ad),
208 sd, ARRAY_SIZE(sd));
209 if (err) {
210 printk("Advertising failed to start (err %d)\n", err);
211 return;
212 }
213
214 printk("Dynamic Tx power Beacon started\n");
215 }
216
hrs_notify(void)217 static void hrs_notify(void)
218 {
219 static uint8_t heartrate = 90U;
220
221 /* Heartrate measurements simulation */
222 heartrate++;
223 if (heartrate == 160U) {
224 heartrate = 90U;
225 }
226
227 bt_hrs_notify(heartrate);
228 }
229
modulate_tx_power(void * p1,void * p2,void * p3)230 void modulate_tx_power(void *p1, void *p2, void *p3)
231 {
232 int8_t txp_get = 0;
233 uint8_t idx = 0;
234
235 while (1) {
236 if (!default_conn) {
237 printk("Set Tx power level to %d\n", txpower[idx]);
238 set_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_ADV,
239 0, txpower[idx]);
240
241 k_sleep(K_SECONDS(5));
242
243 printk("Get Tx power level -> ");
244 get_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_ADV,
245 0, &txp_get);
246 printk("TXP = %d\n", txp_get);
247
248 idx = (idx+1) % DEVICE_BEACON_TXPOWER_NUM;
249 } else {
250 int8_t rssi = 0xFF;
251 int8_t txp_adaptive;
252
253 idx = 0;
254
255 read_conn_rssi(default_conn_handle, &rssi);
256 printk("Connected (%d) - RSSI = %d\n",
257 default_conn_handle, rssi);
258 if (rssi > -70) {
259 txp_adaptive = -20;
260 } else if (rssi > -90) {
261 txp_adaptive = -12;
262 } else {
263 txp_adaptive = -4;
264 }
265 printk("Adaptive Tx power selected = %d\n",
266 txp_adaptive);
267 set_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
268 default_conn_handle, txp_adaptive);
269 get_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
270 default_conn_handle, &txp_get);
271 printk("Connection (%d) TXP = %d\n",
272 default_conn_handle, txp_get);
273
274 k_sleep(K_SECONDS(1));
275 }
276 }
277 }
278
main(void)279 int main(void)
280 {
281 int8_t txp_get = 0xFF;
282 int err;
283
284 default_conn = NULL;
285 printk("Starting Dynamic Tx Power Beacon Demo\n");
286
287 /* Initialize the Bluetooth Subsystem */
288 err = bt_enable(bt_ready);
289 if (err) {
290 printk("Bluetooth init failed (err %d)\n", err);
291 }
292
293 printk("Get Tx power level ->");
294 get_tx_power(BT_HCI_VS_LL_HANDLE_TYPE_ADV, 0, &txp_get);
295 printk("-> default TXP = %d\n", txp_get);
296
297 /* Wait for 5 seconds to give a chance users/testers
298 * to check that default Tx power is indeed the one
299 * selected in Kconfig.
300 */
301 k_sleep(K_SECONDS(5));
302
303 k_thread_create(&pwr_thread_data, pwr_thread_stack,
304 K_THREAD_STACK_SIZEOF(pwr_thread_stack),
305 modulate_tx_power, NULL, NULL, NULL,
306 K_PRIO_COOP(10),
307 0, K_NO_WAIT);
308 k_thread_name_set(&pwr_thread_data, "DYN TX");
309
310 while (1) {
311 hrs_notify();
312 k_sleep(K_SECONDS(2));
313 }
314 return 0;
315 }
316
