ble connect and disconnect

include/ble.h

@@ -44,5 +44,7 @@ void ble_init(void);
 bool ble_is_ready(void);
 void ble_scan(enum ble_scan_mode mode);
 int ble_get_scan_results(struct ble_scan_result *buf, uint len);
+void ble_connect(bd_addr_t addr, bd_addr_type_t type);
+void ble_disconnect(void);
 
 #endif // __BLE_H__

src/ble.c

@@ -26,23 +26,22 @@
 #include "util.h"
 #include "ble.h"
 
+#define BLE_MAX_SCAN_AGE_MS (10 * 1000)
+
 enum ble_state {
     TC_OFF = 0,
     TC_IDLE,
-    TC_W4_SCAN_RESULT,
+    TC_W4_SCAN,
     TC_W4_CONNECT,
-    TC_W4_SERVICE_RESULT,
-    TC_W4_CHARACTERISTIC_RESULT,
-    TC_W4_ENABLE_NOTIFICATIONS_COMPLETE,
-    TC_W4_READY
+    TC_READY
 };
 
 static btstack_packet_callback_registration_t hci_event_callback_registration;
+static hci_con_handle_t connection_handle;
+
 static enum ble_state state = TC_OFF;
 static struct ble_scan_result scans[BLE_MAX_SCAN_RESULTS] = {0};
 
-// TODO scan result entries are not aging out
-
 static void hci_add_scan_result(bd_addr_t addr, bd_addr_type_t type, int8_t rssi) {
     int unused = -1;
 
@@ -55,8 +54,9 @@ static void hci_add_scan_result(bd_addr_t addr, bd_addr_type_t type, int8_t rssi
         }
 
         if (memcmp(addr, scans[i].addr, sizeof(bd_addr_t)) == 0) {
-            // already in list, just update time for aging
+            // already in list, just update changing values
             scans[i].time = to_ms_since_boot(get_absolute_time());
+            scans[i].rssi = rssi;
             return;
         }
     }
@@ -102,7 +102,7 @@ static void hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa
     UNUSED(channel);
 
     if (packet_type != HCI_EVENT_PACKET) {
-        debug("unexpected packet 0x%02X", packet_type);
+        //debug("unexpected packet 0x%02X", packet_type);
         return;
     }
 
@@ -119,16 +119,8 @@ static void hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa
             }
         break;
 
-    case HCI_EVENT_INQUIRY_COMPLETE:
-    case HCI_EVENT_COMMAND_STATUS:
-    case HCI_EVENT_REMOTE_HOST_SUPPORTED_FEATURES:
-    case BTSTACK_EVENT_SCAN_MODE_CHANGED:
-    case HCI_EVENT_COMMAND_COMPLETE:
-    case HCI_EVENT_TRANSPORT_PACKET_SENT:
-        break;
-
     case GAP_EVENT_ADVERTISING_REPORT: {
-        if (state != TC_W4_SCAN_RESULT) {
+        if (state != TC_W4_SCAN) {
             debug("scan result in invalid state %d", state);
             return;
         }
@@ -163,17 +155,9 @@ static void hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa
                 hci_scan_result_add_name(addr, data, data_size);
                 break;
 
-            case BLUETOOTH_DATA_TYPE_FLAGS:
-            case BLUETOOTH_DATA_TYPE_TX_POWER_LEVEL:
-            case BLUETOOTH_DATA_TYPE_COMPLETE_LIST_OF_16_BIT_SERVICE_CLASS_UUIDS:
-            case BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID:
-            case BLUETOOTH_DATA_TYPE_MANUFACTURER_SPECIFIC_DATA:
-            case BLUETOOTH_DATA_TYPE_SLAVE_CONNECTION_INTERVAL_RANGE:
-                break;
-
             default:
-                debug("Unexpected advertisement type 0x%02X from %s", data_type, bd_addr_to_str(addr));
-                hexdump(data, data_size);
+                //debug("Unexpected advertisement type 0x%02X from %s", data_type, bd_addr_to_str(addr));
+                //hexdump(data, data_size);
                 break;
             }
         }
@@ -182,22 +166,29 @@ static void hci_event_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa
 
     case HCI_EVENT_LE_META:
         switch (hci_event_le_meta_get_subevent_code(packet)) {
-            case HCI_SUBEVENT_LE_ADVERTISING_REPORT:
-                // handled internally by BTstack
-                break;
-
             case HCI_SUBEVENT_LE_CONNECTION_COMPLETE:
-                debug("connection complete?!");
+                if (state != TC_W4_CONNECT) {
+                    return;
+                }
+                debug("connection complete");
+                connection_handle = hci_subevent_le_connection_complete_get_connection_handle(packet);
+                state = TC_READY;
                 break;
 
             default:
-                debug("unexpected LE meta event 0x%02X", hci_event_le_meta_get_subevent_code(packet));
+                //debug("unexpected LE meta event 0x%02X", hci_event_le_meta_get_subevent_code(packet));
                 break;
         }
         break;
 
+    case HCI_EVENT_DISCONNECTION_COMPLETE:
+        debug("disconnected");
+        connection_handle = HCI_CON_HANDLE_INVALID;
+        state = TC_IDLE;
+        break;
+
     default:
-        debug("unexpected event 0x%02X", hci_event_packet_get_type(packet));
+        //debug("unexpected event 0x%02X", hci_event_packet_get_type(packet));
         break;
     }
 }
@@ -236,6 +227,11 @@ bool ble_is_ready(void) {
 void ble_scan(enum ble_scan_mode mode) {
     cyw43_thread_enter();
 
+    if (state == TC_OFF) {
+        cyw43_thread_exit();
+        return;
+    }
+
     switch (mode) {
     case BLE_SCAN_OFF:
         debug("stopping BLE scan");
@@ -245,14 +241,14 @@ void ble_scan(enum ble_scan_mode mode) {
 
     case BLE_SCAN_ON:
         debug("starting BLE scan");
-        state = TC_W4_SCAN_RESULT;
+        state = TC_W4_SCAN;
         gap_set_scan_parameters(1, 0x0030, 0x0030);
         gap_start_scan();
         break;
 
     case BLE_SCAN_TOGGLE:
         switch (state) {
-        case TC_W4_SCAN_RESULT:
+        case TC_W4_SCAN:
             cyw43_thread_exit();
             ble_scan(0);
             return;
@@ -283,12 +279,23 @@ int ble_get_scan_results(struct ble_scan_result *buf, uint len) {
 
     cyw43_thread_enter();
 
+    if (state == TC_OFF) {
+        cyw43_thread_exit();
+        return -1;
+    }
+
     uint pos = 0;
     for (uint i = 0; i < BLE_MAX_SCAN_RESULTS; i++) {
         if (!scans[i].set) {
             continue;
         }
 
+        uint32_t diff = to_ms_since_boot(get_absolute_time()) - scans[i].time;
+        if (diff >= BLE_MAX_SCAN_AGE_MS) {
+            //debug("removing %s due to age", bd_addr_to_str(scans[i].addr));
+            scans[i].set = false;
+        }
+
         memcpy(buf + pos, scans + i, sizeof(struct ble_scan_result));
         pos++;
 
@@ -300,3 +307,42 @@ int ble_get_scan_results(struct ble_scan_result *buf, uint len) {
     cyw43_thread_exit();
     return pos;
 }
+
+void ble_connect(bd_addr_t addr, bd_addr_type_t type) {
+    cyw43_thread_enter();
+
+    switch (state) {
+    case TC_OFF:
+        cyw43_thread_exit();
+        return;
+
+    case TC_W4_SCAN:
+        cyw43_thread_exit();
+        ble_scan(0);
+        cyw43_thread_enter();
+        break;
+
+    case TC_READY:
+        gap_disconnect(connection_handle);
+        break;
+
+    default:
+        break;
+    }
+
+    debug("connecting to %s", bd_addr_to_str(addr));
+    state = TC_W4_CONNECT;
+    gap_connect(addr, type);
+
+    cyw43_thread_exit();
+}
+
+void ble_disconnect(void) {
+    cyw43_thread_enter();
+
+    if (state == TC_READY) {
+        gap_disconnect(connection_handle);
+    }
+
+    cyw43_thread_exit();
+}

src/console.c

@@ -20,6 +20,7 @@
 #include <string.h>
 #include "pico/stdlib.h"
 #include <unistd.h>
+#include <stdio.h>
 
 #include "config.h"
 #include "log.h"
@@ -37,8 +38,6 @@
 #define CNSL_BUFF_SIZE 1024
 #define CNSL_REPEAT_MS 500
 
-//#define CNSL_REPEAT_PMW_STATUS_BY_DEFAULT
-
 static char cnsl_line_buff[CNSL_BUFF_SIZE + 1];
 static uint32_t cnsl_buff_pos = 0;
 
@@ -71,19 +70,25 @@ static void cnsl_interpret(const char *line) {
             || (strcmp(line, "h") == 0)
             || (strcmp(line, "?") == 0)) {
         println("VolcanoRC Firmware Usage:");
+        println("");
         println("  reset - reset back into this firmware");
         println("   \\x18 - reset to bootloader");
         println(" repeat - repeat last command every %d milliseconds", CNSL_REPEAT_MS);
         println("   help - print this message");
         println("  mount - make mass storage medium (un)available");
         println("  power - show Lipo battery status");
+        println("");
         println("   scan - start or stop BLE scan");
         println("scanres - print list of found BLE devices");
+        println("con M T - connect to (M)AC and (T)ype");
+        println(" discon - disconnect from BLE device");
+        println("");
         println("  clear - blank screen");
         println(" splash - draw image on screen");
         println("  fonts - show font list");
         println("   text - draw text on screen");
         println("    bat - draw battery indicator");
+        println("");
         println("Press Enter with no input to repeat last command.");
         println("Use repeat to continuously execute last command.");
         println("Stop this by calling repeat again.");
@@ -117,6 +122,21 @@ static void cnsl_interpret(const char *line) {
                         age / 1000.0, results[i].name);
             }
         }
+    } else if (str_startswith(line, "con ")) {
+        bd_addr_t addr;
+        bd_addr_type_t type;
+        int r = sscanf(line, "con %02hhX:%02hhX:%02hhX:%02hhX:%02hhX:%02hhX %hhu",
+                       &addr[0], &addr[1], &addr[2], &addr[3],
+                       &addr[4], &addr[5], &type);
+
+        if (r == 7) {
+            debug("connecting");
+            ble_connect(addr, type);
+        } else {
+            debug("invalid input (%d)", r);
+        }
+    } else if (strcmp(line, "discon") == 0) {
+        ble_disconnect();
     } else if (strcmp(line, "clear") == 0) {
         lcd_clear();
     } else if (strcmp(line, "splash") == 0) {
@@ -186,11 +206,6 @@ void cnsl_init(void) {
         cnsl_last_command[i] = '\0';
         cnsl_repeated_command[i] = '\0';
     }
-
-#ifdef CNSL_REPEAT_PMW_STATUS_BY_DEFAULT
-    strcpy(cnsl_repeated_command, "pmws");
-    repeat_command = true;
-#endif // CNSL_REPEAT_PMW_STATUS_BY_DEFAULT
 }
 
 static int32_t cnsl_find_line_end(void) {