drv/bluetooth_stm32_cc2640: use actual adv tx power

This reads the advertisement tx power from the bluetooth chip instead of
hard-coding it. The hard-coded value was correct, so the value doesn't
actually change.

Author: dlech

lib/ble5stack/central/hci.c

@@ -10,3 +10,8 @@ HCI_StatusCodes_t HCI_readLocalVersionInfo(void)
 {
     return HCI_sendHCICommand(HCI_READ_LOCAL_VERSION_INFO, NULL, 0);
 }
+
+HCI_StatusCodes_t HCI_LE_readAdvertisingChannelTxPower(void)
+{
+    return HCI_sendHCICommand(HCI_LE_READ_ADVERTISING_CHANNEL_TX_POWER, NULL, 0);
+}

lib/ble5stack/central/hci.h

@@ -5,5 +5,6 @@
 
 HCI_StatusCodes_t HCI_readBdaddr(void);
 HCI_StatusCodes_t HCI_readLocalVersionInfo(void);
+HCI_StatusCodes_t HCI_LE_readAdvertisingChannelTxPower(void);
 
 #endif // HCI_H

lib/ble5stack/central/hci_tl.h

@@ -96,6 +96,9 @@
 // Status Parameters
 #define HCI_READ_RSSI                                     0x1405	//!< opcode of @ref HCI_ReadRssiCmd
 
+// Low energy commands
+#define HCI_LE_READ_ADVERTISING_CHANNEL_TX_POWER          0x2007	//!< opcode of @ref HCI_LE_readAdvertisingChannelTxPower
+
 /* HCI Status return types  */
 typedef enum
 {

lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c

@@ -270,6 +270,18 @@ static PT_THREAD(set_discoverable(struct pt *pt, pbio_task_t *task)) {
 
     PT_BEGIN(pt);
 
+    static int8_t tx_power;
+    PT_WAIT_WHILE(pt, write_xfer_size);
+    HCI_LE_readAdvertisingChannelTxPower();
+    PT_WAIT_UNTIL(pt, hci_command_complete);
+
+    {
+        HCI_StatusCodes_t status = read_buf[9];
+        if (status == bleSUCCESS) {
+            tx_power = read_buf[10];
+        }
+    }
+
     // Set advertising data
 
     PT_WAIT_WHILE(pt, write_xfer_size);
@@ -281,7 +293,7 @@ static PT_THREAD(set_discoverable(struct pt *pt, pbio_task_t *task)) {
     pbio_uuid128_reverse_copy(&data[5], pbio_pybricks_service_uuid);
     data[21] = 2; // length
     data[22] = GAP_ADTYPE_POWER_LEVEL;
-    data[23] = 0;
+    data[23] = tx_power;
     GAP_updateAdvertistigData(GAP_AD_TYPE_ADVERTISEMNT_DATA, 24, data);
     PT_WAIT_UNTIL(pt, hci_command_complete);
     // ignoring response data