From f1468219fdf61d103ad77abd3ba06ed103f9d3ce Mon Sep 17 00:00:00 2001
From: Guo-Rong <5484552+gkoh@users.noreply.github.com>
Date: Wed, 18 Sep 2024 11:42:23 +0930
Subject: [PATCH] Implement simultaneous camera connect (#127)

* Add per-camera task and queue.

Lay the foundation for supporting simultaneous, parallel camera
connections.

The UI now sends commands to the control task, which relays them to
per-camera tasks.
This allows the UI and each connected camera to operate asynchronously.

Increase limit maximum clients from 3 to 8.

* Display human readable mobile device names.

Fix #100 by updating to a development cut of NimBLE-Arduino.
Whilst here improve device bond handling, this makes subsequent
pair/forget sequences a little less iffy.

* Implement multi-connect.

M5ez:
Add context to menu item advanced function.

Furble:
Tweak NimBLE settings.
Remove scan duration, now scan forever as we can stop on demand.
Drop global interval_t, load as needed to reduce variable scope.

Add setting to toggle multi-connect.
Modify connection menu to support tagging cameras.
Add ability to connect to all tagged cameras.

Cannot get directed advertising to mobile devices so still disabled.

Multi-connect to mobile devices is super iffy, not recommended for
production use.

* Tweak the code style a little.

Fix minor error in shutter lock formatting, introduced when migrating
from String to std::string.

Also removed unused macros.

* Update clang-format action version.

* Fix m5stack-core build.

* Minor style updates.

Move some more code into std::string native and const a few more
parameters.

* Further C++ refactoring.

Refactor the Camera type into the base Camera class.
Refactor fillSaveName into CameraList class, it is only used there.
Style adjustments.
---
 .clang-format               |   1 +
 .github/workflows/main.yml  |   4 +-
 README.md                   |  27 ++++-
 include/furble_control.h    |  77 +++++++++++-
 include/furble_gps.h        |   5 +-
 include/furble_ui.h         |   4 +-
 include/settings.h          |   7 +-
 include/spinner.h           |   4 +-
 lib/M5ez/src/M5ez.cpp       |  26 ++---
 lib/M5ez/src/M5ez.h         |  20 ++--
 lib/furble/Camera.cpp       |  14 ++-
 lib/furble/Camera.h         |  26 ++++-
 lib/furble/CameraList.cpp   |  58 ++++-----
 lib/furble/CameraList.h     |  20 +++-
 lib/furble/CanonEOS.cpp     |  10 +-
 lib/furble/CanonEOS.h       |  10 +-
 lib/furble/CanonEOSM6.cpp   |   4 -
 lib/furble/CanonEOSM6.h     |   5 +-
 lib/furble/CanonEOSRP.cpp   |   4 -
 lib/furble/CanonEOSRP.h     |   5 +-
 lib/furble/Fujifilm.cpp     |  56 +++++----
 lib/furble/Fujifilm.h       |   5 +-
 lib/furble/Furble.cpp       |  24 ++--
 lib/furble/Furble.h         |   8 +-
 lib/furble/FurbleTypes.h    |   7 --
 lib/furble/HIDServer.cpp    |  43 ++++---
 lib/furble/HIDServer.h      |  15 +--
 lib/furble/MobileDevice.cpp |  27 ++---
 lib/furble/MobileDevice.h   |  19 ++-
 platformio.ini              |  13 ++-
 src/furble.cpp              | 227 ++++++++++++++++++++++++------------
 src/furble_control.cpp      | 187 ++++++++++++++++++++++++-----
 src/furble_gps.cpp          |  29 ++---
 src/interval.cpp            |  19 +--
 src/main.cpp                |  13 +--
 src/settings.cpp            |  93 +++++++++------
 src/spinner.cpp             |  31 +++--
 37 files changed, 737 insertions(+), 410 deletions(-)

diff --git a/.clang-format b/.clang-format
index e747460..31f6b35 100644
--- a/.clang-format
+++ b/.clang-format
@@ -4,3 +4,4 @@ ColumnLimit: 100
 PointerAlignment: Right
 BreakBeforeBinaryOperators: NonAssignment
 SpaceBeforeInheritanceColon: false
+AlignArrayOfStructures: Left
diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index ddae157..55b41fc 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -17,9 +17,9 @@ jobs:
       - uses: actions/checkout@v4
 
       - name: Run clang-format style check
-        uses: jidicula/clang-format-action@v4.11.0
+        uses: jidicula/clang-format-action@v4.13.0
         with:
-          clang-format-version: '13'
+          clang-format-version: '17'
           check-path: '.'
           exclude-regex: 'lib/M5ez/examples'
 
diff --git a/README.md b/README.md
index 697798c..20f0ea8 100644
--- a/README.md
+++ b/README.md
@@ -7,7 +7,6 @@ cameras.
 
 ![furble - M5StickC-Plus2](https://github.com/user-attachments/assets/e0eebd87-3ac0-4a8b-871a-014eb8c71395)
 
-
 The remote uses the camera's native Bluetooth Low Energy interface so additional
 adapters are not required.
 
@@ -45,6 +44,7 @@ Currently supported features in `furble`:
 - focus
 - GPS location tagging
 - intervalometer
+- multi-connect
 
 ### Table of Features
 
@@ -100,8 +100,10 @@ Connection to mobile devices is a little iffy:
 - hit `Scan`
 - on the mobile device:
    - pair with `furble`
-- on `furble` the mobile device bluetooth MAC address should appear as a connectable target
-   - fixing the target name is tracked in [#100](https://github.com/gkoh/furble/issues/100).
+- on `furble` the mobile device should appear as a connectable target if the pairing was successful
+- connect to the mobile device to save the pairing
+  - the devices will remain paired even if you do not connect and save
+  - forget `furble` on the mobile device to remove such a pair
 
 ### GPS Location Tagging
 
@@ -124,6 +126,25 @@ Delay and shutter time can be figured with custom or preset values from 0 to 999
 
 When in `Shutter` remote control, holding focus (button B) then release (button A) will engage shutter lock, holding the shutter open until a button is pressed.
 
+### Multi-Connect
+
+Multi-Connect enables simultaneous connection to multiple cameras to synchronise
+remote shutter control. Up to 9 (ESP32 hardware limit) cameras can be
+simultaneously controlled.
+
+To use:
+* Pair with one or more cameras
+* Enable `Settings->Multi-Connect`
+* In `Connect` select cameras
+   * Selected cameras will have a `*`
+* Select `Connect *`
+   * Selected cameras will be connected in sequence
+* If all cameras are connected, the standard remote control is shown
+
+WARNING:
+* mobile device connections are extremely finnicky
+* multi-connect involving mobile devices is not well tested and can easily crash
+
 ## Motivation
 
 I found current smartphone apps for basic wireless remote shutter control to be
diff --git a/include/furble_control.h b/include/furble_control.h
index d60275b..9ce0ea5 100644
--- a/include/furble_control.h
+++ b/include/furble_control.h
@@ -4,16 +4,89 @@
 #include <Camera.h>
 
 #define CONTROL_CMD_QUEUE_LEN (32)
+#define TARGET_CMD_QUEUE_LEN (8)
 
 typedef enum {
   CONTROL_CMD_SHUTTER_PRESS,
   CONTROL_CMD_SHUTTER_RELEASE,
   CONTROL_CMD_FOCUS_PRESS,
   CONTROL_CMD_FOCUS_RELEASE,
-  CONTROL_CMD_GPS_UPDATE
+  CONTROL_CMD_GPS_UPDATE,
+  CONTROL_CMD_DISCONNECT,
+  CONTROL_CMD_ERROR
 } control_cmd_t;
 
-void control_update_gps(Furble::Camera::gps_t &gps, Furble::Camera::timesync_t &timesync);
+namespace Furble {
+
+class Control {
+ public:
+  class Target {
+   public:
+    Target(Camera *camera);
+    ~Target();
+
+    Camera *getCamera(void);
+    control_cmd_t getCommand(void);
+    void sendCommand(control_cmd_t cmd);
+    const Camera::gps_t &getGPS(void);
+    const Camera::timesync_t &getTimesync(void);
+
+    void updateGPS(Camera::gps_t &gps, Camera::timesync_t &timesync);
+
+   private:
+    QueueHandle_t m_Queue = NULL;
+    Furble::Camera *m_Camera = NULL;
+    Camera::gps_t m_GPS;
+    Camera::timesync_t m_Timesync;
+  };
+
+  Control(void);
+  ~Control();
+
+  /**
+   * FreeRTOS control task function.
+   */
+  void task(void);
+
+  /**
+   * Send control command to active connections.
+   */
+  BaseType_t sendCommand(control_cmd_t cmd);
+
+  /**
+   * Update GPS and timesync values.
+   */
+  BaseType_t updateGPS(Camera::gps_t &gps, Camera::timesync_t &timesync);
+
+  /**
+   * Are all active cameras still connected?
+   */
+  bool isConnected(void);
+
+  /**
+   * Get list of connected targets.
+   */
+  const std::vector<std::unique_ptr<Control::Target>> &getTargets(void);
+
+  /**
+   * Disconnect all connected cameras.
+   */
+  void disconnect(void);
+
+  /**
+   * Add specified camera to active target list.
+   */
+  void addActive(Camera *camera);
+
+ private:
+  QueueHandle_t m_Queue = NULL;
+  std::vector<std::unique_ptr<Control::Target>> m_Targets;
+};
+
+};  // namespace Furble
+
+extern "C" {
 void control_task(void *param);
+}
 
 #endif
diff --git a/include/furble_gps.h b/include/furble_gps.h
index bf0fa1c..9286c7d 100644
--- a/include/furble_gps.h
+++ b/include/furble_gps.h
@@ -1,14 +1,15 @@
 #ifndef FURBLE_GPS_H
 #define FURBLE_GPS_H
 
-#include <Camera.h>
 #include <TinyGPS++.h>
 
+#include "furble_control.h"
+
 extern TinyGPSPlus furble_gps;
 
 extern bool furble_gps_enable;
 
 void furble_gps_init(void);
-void furble_gps_update(Furble::Camera *camera);
+void furble_gps_update(Furble::Control *control);
 
 #endif
diff --git a/include/furble_ui.h b/include/furble_ui.h
index 7dc9b73..7d78927 100644
--- a/include/furble_ui.h
+++ b/include/furble_ui.h
@@ -1,10 +1,10 @@
 #ifndef FURBLE_UI_H
 #define FURBLE_UI_H
 
-#include <Camera.h>
+#include "furble_control.h"
 
 struct FurbleCtx {
-  Furble::Camera *camera;
+  Furble::Control *control;
   bool reconnected;
 };
 
diff --git a/include/settings.h b/include/settings.h
index 9c758b5..fc3fd31 100644
--- a/include/settings.h
+++ b/include/settings.h
@@ -6,8 +6,6 @@
 
 #include "interval.h"
 
-extern interval_t interval;
-
 void settings_menu_tx_power(void);
 esp_power_level_t settings_load_esp_tx_power(void);
 
@@ -17,7 +15,10 @@ void settings_menu_gps(void);
 void settings_load_interval(interval_t *interval);
 void settings_save_interval(interval_t *interval);
 
-void settings_add_interval_items(ezMenu *submenu);
+void settings_add_interval_items(ezMenu *submenu, interval_t *interval);
 void settings_menu_interval(void);
 
+bool settings_load_multiconnect(void);
+void settings_save_multiconnect(bool multiconnect);
+
 #endif
diff --git a/include/spinner.h b/include/spinner.h
index e99e09c..8683d43 100644
--- a/include/spinner.h
+++ b/include/spinner.h
@@ -16,8 +16,8 @@ struct __attribute__((packed)) SpinValue {
 
 void spinner_modify_value(const char *title, bool preset, SpinValue *sv);
 
-std::string sv2str(SpinValue *sv);
-unsigned long sv2ms(SpinValue *sv);
+std::string sv2str(const SpinValue *sv);
+unsigned long sv2ms(const SpinValue *sv);
 void ms2hms(unsigned long ms, unsigned int *h, unsigned int *m, unsigned int *s);
 
 #endif
diff --git a/lib/M5ez/src/M5ez.cpp b/lib/M5ez/src/M5ez.cpp
index 858af36..46a699c 100644
--- a/lib/M5ez/src/M5ez.cpp
+++ b/lib/M5ez/src/M5ez.cpp
@@ -450,7 +450,7 @@ size_t ezCanvas::write(const uint8_t *buffer, size_t size) {
   return size;
 }
 
-uint16_t ezCanvas::loop(void *private_data) {
+uint16_t ezCanvas::loop(void *context) {
   if (_next_scroll && millis() >= _next_scroll) {
     ez.setFont(_font);
     uint8_t h = ez.fontHeight();
@@ -907,7 +907,7 @@ void changeCpuPower(bool reduce) {
   }
 }
 
-uint16_t ezBacklight::loop(void *private_data) {
+uint16_t ezBacklight::loop(void *context) {
   if (!_backlight_off && _inactivity) {
     if (millis() > _last_activity + 30000 * _inactivity) {
       _backlight_off = true;
@@ -955,7 +955,7 @@ void ezBattery::begin() {
   }
 }
 
-uint16_t ezBattery::loop(void *private_data) {
+uint16_t ezBattery::loop(void *context) {
   if (!_on)
     return 0;
   ez.header.draw("battery");
@@ -1059,11 +1059,12 @@ void M5ez::begin() {
 }
 
 void M5ez::yield() {
-  ::yield();  // execute the Arduino yield in the root namespace
+  vTaskDelay(1);  // allow lower priority tasks to run
+  ::yield();      // execute the Arduino yield in the root namespace
   M5.update();
   for (uint8_t n = 0; n < _events.size(); n++) {
     if (millis() > _events[n].when) {
-      uint16_t r = (_events[n].function)(_events[n].private_data);
+      uint16_t r = (_events[n].function)(_events[n].context);
       if (r) {
         _events[n].when = millis() + r - 1;
       } else {
@@ -1074,17 +1075,15 @@ void M5ez::yield() {
   }
 }
 
-void M5ez::addEvent(uint16_t (*function)(void *private_data),
-                    void *private_data,
-                    uint32_t when /* = 1 */) {
+void M5ez::addEvent(uint16_t (*function)(void *context), void *context, uint32_t when /* = 1 */) {
   event_t n;
   n.function = function;
-  n.private_data = private_data;
+  n.context = context;
   n.when = millis() + when - 1;
   _events.push_back(n);
 }
 
-void M5ez::removeEvent(uint16_t (*function)(void *private_data)) {
+void M5ez::removeEvent(uint16_t (*function)(void *context)) {
   uint8_t n = 0;
   while (n < _events.size()) {
     if (_events[n].function == function) {
@@ -1274,13 +1273,14 @@ void ezMenu::txtFont(const GFXfont *font) {
 bool ezMenu::addItem(std::string name,
                      std::string caption,
                      void (*simpleFunction)() /* = NULL */,
-                     bool (*advancedFunction)(ezMenu *callingMenu) /* = NULL */,
+                     void *context /* = NULL */,
+                     bool (*advancedFunction)(ezMenu *callingMenu, void *context) /* = NULL */,
                      void (*drawFunction)(ezMenu *callingMenu,
                                           int16_t x,
                                           int16_t y,
                                           int16_t w,
                                           int16_t h) /* = NULL */) {
-  MenuItem_t new_item = {name, caption, simpleFunction, advancedFunction, drawFunction};
+  MenuItem_t new_item = {name, caption, context, simpleFunction, advancedFunction, drawFunction};
   if (_selected == -1)
     _selected = _items.size();
   _items.push_back(new_item);
@@ -1437,7 +1437,7 @@ int16_t ezMenu::_runTextOnce(bool dynamic) {
         (_items[_selected].simpleFunction)();
       }
       if (_items[_selected].advancedFunction) {
-        if (!(_items[_selected].advancedFunction)(this))
+        if (!(_items[_selected].advancedFunction)(this, _items[_selected].context))
           return 0;
       }
       return _selected
diff --git a/lib/M5ez/src/M5ez.h b/lib/M5ez/src/M5ez.h
index c3af86c..35b7d0a 100644
--- a/lib/M5ez/src/M5ez.h
+++ b/lib/M5ez/src/M5ez.h
@@ -233,7 +233,7 @@ class ezCanvas: public Print {
       uint8_t c);  // These three are used to inherit print and println from Print class
   virtual size_t write(const char *str);
   virtual size_t write(const uint8_t *buffer, size_t size);
-  static uint16_t loop(void *private_data);
+  static uint16_t loop(void *context);
 
  private:
   static ezTFT tft;
@@ -297,7 +297,8 @@ class ezMenu {
       std::string name,
       std::string caption = "",
       void (*simpleFunction)() = NULL,
-      bool (*advancedFunction)(ezMenu *callingMenu) = NULL,
+      void *context = nullptr,
+      bool (*advancedFunction)(ezMenu *callingMenu, void *context) = NULL,
       void (*drawFunction)(ezMenu *callingMenu, int16_t x, int16_t y, int16_t w, int16_t h) = NULL);
   bool deleteItem(int16_t index);
   bool deleteItem(std::string name);
@@ -338,8 +339,9 @@ class ezMenu {
   struct MenuItem_t {
     std::string name;
     std::string caption;
+    void *context;
     void (*simpleFunction)();
-    bool (*advancedFunction)(ezMenu *callingMenu);
+    bool (*advancedFunction)(ezMenu *callingMenu, void *context);
     void (*drawFunction)(ezMenu *callingMenu, int16_t x, int16_t y, int16_t w, int16_t h);
   };
   std::vector<MenuItem_t> _items;
@@ -425,7 +427,7 @@ class ezBacklight {
   static void menu();
   static void inactivity(uint8_t half_minutes);
   static void activity();
-  static uint16_t loop(void *private_data);
+  static uint16_t loop(void *context);
 
  private:
   static uint8_t _brightness;
@@ -450,7 +452,7 @@ class ezBacklight {
 class ezBattery {
  public:
   static void begin();
-  static uint16_t loop(void *private_data);
+  static uint16_t loop(void *context);
   static uint8_t getTransformedBatteryLevel();
   static uint16_t getBatteryBarColor(uint8_t batteryLevel);
 
@@ -471,7 +473,7 @@ class ezBattery {
 
 struct event_t {
   uint16_t (*function)(void *);
-  void *private_data;
+  void *context;
   uint32_t when;
 };
 
@@ -503,10 +505,8 @@ class M5ez {
 
   static void yield();
 
-  static void addEvent(uint16_t (*function)(void *),
-                       void *private_data = nullptr,
-                       uint32_t when = 1);
-  static void removeEvent(uint16_t (*function)(void *private_data));
+  static void addEvent(uint16_t (*function)(void *), void *context = nullptr, uint32_t when = 1);
+  static void removeEvent(uint16_t (*function)(void *context));
   static void redraw();
 
   // ez.msgBox
diff --git a/lib/furble/Camera.cpp b/lib/furble/Camera.cpp
index 9777e11..a377363 100644
--- a/lib/furble/Camera.cpp
+++ b/lib/furble/Camera.cpp
@@ -4,7 +4,7 @@
 
 namespace Furble {
 
-Camera::Camera() {
+Camera::Camera(Type type) : m_Type(type) {
   m_Client = NimBLEDevice::createClient();
 }
 
@@ -32,12 +32,16 @@ void Camera::setActive(bool active) {
   m_Active = active;
 }
 
-const char *Camera::getName(void) {
-  return m_Name.c_str();
+const Camera::Type &Camera::getType(void) {
+  return m_Type;
 }
 
-void Camera::fillSaveName(char *name) {
-  snprintf(name, 16, "%08llX", (uint64_t)m_Address);
+const std::string &Camera::getName(void) {
+  return m_Name;
+}
+
+const NimBLEAddress &Camera::getAddress(void) {
+  return m_Address;
 }
 
 void Camera::updateProgress(progressFunc pFunc, void *ctx, float value) {
diff --git a/lib/furble/Camera.h b/lib/furble/Camera.h
index 651fcc3..55513eb 100644
--- a/lib/furble/Camera.h
+++ b/lib/furble/Camera.h
@@ -19,8 +19,15 @@ namespace Furble {
  */
 class Camera {
  public:
-  Camera();
-  ~Camera();
+  /**
+   * Camera types.
+   */
+  enum class Type : uint32_t {
+    FUJIFILM = 1,
+    CANON_EOS_M6 = 2,
+    CANON_EOS_RP = 3,
+    MOBILE_DEVICE = 4,
+  };
 
   /**
    * GPS data type.
@@ -43,6 +50,8 @@ class Camera {
     unsigned int second;
   } timesync_t;
 
+  ~Camera();
+
   /**
    * Wrapper for protected pure virtual Camera::connect().
    */
@@ -76,9 +85,8 @@ class Camera {
   /**
    * Update geotagging data.
    */
-  virtual void updateGeoData(gps_t &gps, timesync_t &timesync) = 0;
+  virtual void updateGeoData(const gps_t &gps, const timesync_t &timesync) = 0;
 
-  virtual device_type_t getDeviceType(void) = 0;
   virtual size_t getSerialisedBytes(void) = 0;
   virtual bool serialise(void *buffer, size_t bytes) = 0;
 
@@ -97,11 +105,15 @@ class Camera {
    */
   void setActive(bool active);
 
-  const char *getName(void);
+  const Type &getType(void);
+
+  const std::string &getName(void);
 
-  void fillSaveName(char *name);
+  const NimBLEAddress &getAddress(void);
 
  protected:
+  Camera(Type type);
+
   /**
    * Connect to the target camera such that it is ready for shutter control.
    *
@@ -126,6 +138,8 @@ class Camera {
   // double the disconnect timeout
   const uint16_t m_Timeout = (2 * BLE_GAP_INITIAL_SUPERVISION_TIMEOUT);
 
+  const Type m_Type;
+
   bool m_Active = false;
 };
 }  // namespace Furble
diff --git a/lib/furble/CameraList.cpp b/lib/furble/CameraList.cpp
index 1712130..a6cfaf6 100644
--- a/lib/furble/CameraList.cpp
+++ b/lib/furble/CameraList.cpp
@@ -13,7 +13,7 @@
 namespace Furble {
 
 std::vector<std::unique_ptr<Furble::Camera>> CameraList::m_ConnectList;
-static Preferences m_Prefs;
+Preferences CameraList::m_Prefs;
 
 /**
  * Non-volatile storage index entry.
@@ -23,10 +23,15 @@ static Preferences m_Prefs;
  */
 typedef struct {
   char name[16];
-  device_type_t type;
+  Camera::Type type;
 } index_entry_t;
 
-static void save_index(std::vector<index_entry_t> &index) {
+void CameraList::fillSaveEntry(CameraList::index_entry_t &entry, Camera *camera) {
+  snprintf(entry.name, 16, "%08llX", (uint64_t)camera->getAddress());
+  entry.type = camera->getType();
+}
+
+void CameraList::save_index(std::vector<CameraList::index_entry_t> &index) {
   if (index.size() > 0) {
     m_Prefs.putBytes(FURBLE_PREF_INDEX, index.data(), sizeof(index[0]) * index.size());
   } else {
@@ -34,19 +39,19 @@ static void save_index(std::vector<index_entry_t> &index) {
   }
 }
 
-static std::vector<index_entry_t> load_index(void) {
+std::vector<CameraList::index_entry_t> CameraList::load_index(void) {
   std::vector<index_entry_t> index;
 
   size_t bytes = m_Prefs.getBytesLength(FURBLE_PREF_INDEX);
   if (bytes > 0 && (bytes % sizeof(index_entry_t) == 0)) {
     uint8_t buffer[bytes] = {0};
     size_t count = bytes / sizeof(index_entry_t);
-    ESP_LOGI(LOG_TAG, "Index entries: %d\r\n", count);
+    ESP_LOGI(LOG_TAG, "Index entries: %d", count);
     m_Prefs.getBytes(FURBLE_PREF_INDEX, buffer, bytes);
     index_entry_t *entry = (index_entry_t *)buffer;
 
     for (int i = 0; i < count; i++) {
-      ESP_LOGI(LOG_TAG, "Loading index entry: %s\r\n", entry[i].name);
+      ESP_LOGI(LOG_TAG, "Loading index entry: %s", entry[i].name);
       index.push_back(entry[i]);
     }
   }
@@ -54,12 +59,12 @@ static std::vector<index_entry_t> load_index(void) {
   return index;
 }
 
-static void add_index(std::vector<index_entry_t> &index, index_entry_t &entry) {
+void CameraList::add_index(std::vector<CameraList::index_entry_t> &index, index_entry_t &entry) {
   bool exists = false;
   for (size_t i = 0; i < index.size(); i++) {
-    ESP_LOGI(LOG_TAG, "[%d] %s : %s\r\n", i, index[i].name, entry.name);
+    ESP_LOGI(LOG_TAG, "[%d] %s : %s", i, index[i].name, entry.name);
     if (strcmp(index[i].name, entry.name) == 0) {
-      ESP_LOGI(LOG_TAG, "Overwriting existing entry");
+      ESP_LOGI(LOG_TAG, "Overwriting existing entry: %s", entry.name);
       index[i] = entry;
       exists = true;
       break;
@@ -67,7 +72,7 @@ static void add_index(std::vector<index_entry_t> &index, index_entry_t &entry) {
   }
 
   if (!exists) {
-    ESP_LOGI(LOG_TAG, "Adding new entry");
+    ESP_LOGI(LOG_TAG, "Adding new entry: %s", entry.name);
     index.push_back(entry);
   }
 }
@@ -77,8 +82,7 @@ void CameraList::save(Furble::Camera *camera) {
   std::vector<index_entry_t> index = load_index();
 
   index_entry_t entry = {0};
-  camera->fillSaveName(entry.name);
-  entry.type = camera->getDeviceType();
+  fillSaveEntry(entry, camera);
 
   add_index(index, entry);
 
@@ -87,9 +91,9 @@ void CameraList::save(Furble::Camera *camera) {
   if (camera->serialise(dbuffer, dbytes)) {
     // Store the entry and the index if serialisation succeeds
     m_Prefs.putBytes(entry.name, dbuffer, dbytes);
-    ESP_LOGI(LOG_TAG, "Saved %s\r\n", entry.name);
+    ESP_LOGI(LOG_TAG, "Saved %s", entry.name);
     save_index(index);
-    ESP_LOGI(LOG_TAG, "Index entries: %d\r\n", index.size());
+    ESP_LOGI(LOG_TAG, "Index entries: %d", index.size());
   }
 
   m_Prefs.end();
@@ -100,13 +104,12 @@ void CameraList::remove(Furble::Camera *camera) {
   std::vector<index_entry_t> index = load_index();
 
   index_entry_t entry = {0};
-  camera->fillSaveName(entry.name);
+  fillSaveEntry(entry, camera);
 
   size_t i = 0;
   for (i = 0; i < index.size(); i++) {
     if (strcmp(index[i].name, entry.name) == 0) {
-      ESP_LOGI(LOG_TAG, "Deleting: ");
-      ESP_LOGI(LOG_TAG, "%s", entry.name);
+      ESP_LOGI(LOG_TAG, "Deleting: %s", entry.name);
       break;
     }
   }
@@ -117,6 +120,11 @@ void CameraList::remove(Furble::Camera *camera) {
   save_index(index);
 
   m_Prefs.end();
+
+  // delete bond if required
+  if (NimBLEDevice::isBonded(camera->getAddress())) {
+    NimBLEDevice::deleteBond(camera->getAddress());
+  }
 }
 
 /**
@@ -139,16 +147,16 @@ void CameraList::load(void) {
     m_Prefs.getBytes(index[i].name, dbuffer, dbytes);
 
     switch (index[i].type) {
-      case FURBLE_FUJIFILM:
+      case Camera::Type::FUJIFILM:
         m_ConnectList.push_back(std::unique_ptr<Furble::Camera>(new Fujifilm(dbuffer, dbytes)));
         break;
-      case FURBLE_CANON_EOS_M6:
+      case Camera::Type::CANON_EOS_M6:
         m_ConnectList.push_back(std::unique_ptr<Furble::Camera>(new CanonEOSM6(dbuffer, dbytes)));
         break;
-      case FURBLE_CANON_EOS_RP:
+      case Camera::Type::CANON_EOS_RP:
         m_ConnectList.push_back(std::unique_ptr<Furble::Camera>(new CanonEOSRP(dbuffer, dbytes)));
         break;
-      case FURBLE_MOBILE_DEVICE:
+      case Camera::Type::MOBILE_DEVICE:
         m_ConnectList.push_back(std::unique_ptr<Furble::Camera>(new MobileDevice(dbuffer, dbytes)));
         break;
     }
@@ -176,10 +184,6 @@ Furble::Camera *CameraList::get(size_t n) {
   return m_ConnectList[n].get();
 }
 
-Furble::Camera *CameraList::back(void) {
-  return m_ConnectList.back().get();
-}
-
 bool CameraList::match(NimBLEAdvertisedDevice *pDevice) {
   if (Fujifilm::matches(pDevice)) {
     m_ConnectList.push_back(std::unique_ptr<Furble::Camera>(new Furble::Fujifilm(pDevice)));
@@ -195,8 +199,8 @@ bool CameraList::match(NimBLEAdvertisedDevice *pDevice) {
   return false;
 }
 
-void CameraList::add(NimBLEAddress address) {
-  m_ConnectList.push_back(std::unique_ptr<Furble::Camera>(new Furble::MobileDevice(address)));
+void CameraList::add(const NimBLEAddress &address, const std::string &name) {
+  m_ConnectList.push_back(std::unique_ptr<Furble::Camera>(new Furble::MobileDevice(address, name)));
 }
 
 }  // namespace Furble
diff --git a/lib/furble/CameraList.h b/lib/furble/CameraList.h
index dd0cac6..39a31bc 100644
--- a/lib/furble/CameraList.h
+++ b/lib/furble/CameraList.h
@@ -1,6 +1,7 @@
 #ifndef CAMERALIST_H
 #define CAMERALIST_H
 
+#include <Preferences.h>
 #include <memory>
 
 #include "Camera.h"
@@ -41,7 +42,7 @@ class CameraList {
   /**
    * Add mobile device to the list.
    */
-  static void add(NimBLEAddress address);
+  static void add(const NimBLEAddress &address, const std::string &name);
 
   /**
    * Number of connectable devices.
@@ -58,16 +59,23 @@ class CameraList {
    */
   static Furble::Camera *get(size_t n);
 
-  /**
-   * Retrieve last entry.
-   */
-  static Furble::Camera *back(void);
-
  private:
+  typedef struct {
+    char name[16];
+    Camera::Type type;
+  } index_entry_t;
+
+  static void fillSaveEntry(index_entry_t &entry, Camera *camera);
+  static std::vector<index_entry_t> load_index(void);
+  static void save_index(std::vector<index_entry_t> &index);
+  static void add_index(std::vector<index_entry_t> &index, index_entry_t &entry);
+
   /**
    * List of connectable devices.
    */
   static std::vector<std::unique_ptr<Furble::Camera>> m_ConnectList;
+
+  static Preferences m_Prefs;
 };
 }  // namespace Furble
 
diff --git a/lib/furble/CanonEOS.cpp b/lib/furble/CanonEOS.cpp
index 637132e..00eb749 100644
--- a/lib/furble/CanonEOS.cpp
+++ b/lib/furble/CanonEOS.cpp
@@ -9,7 +9,7 @@
 
 namespace Furble {
 
-CanonEOS::CanonEOS(const void *data, size_t len) {
+CanonEOS::CanonEOS(Type type, const void *data, size_t len) : Camera(type) {
   if (len != sizeof(eos_t))
     throw;
 
@@ -19,7 +19,7 @@ CanonEOS::CanonEOS(const void *data, size_t len) {
   memcpy(&m_Uuid, &eos->uuid, sizeof(Device::uuid128_t));
 }
 
-CanonEOS::CanonEOS(NimBLEAdvertisedDevice *pDevice) {
+CanonEOS::CanonEOS(Type type, NimBLEAdvertisedDevice *pDevice) : Camera(type) {
   m_Name = pDevice->getName();
   m_Address = pDevice->getAddress();
   ESP_LOGI(LOG_TAG, "Name = %s", m_Name.c_str());
@@ -106,7 +106,9 @@ bool CanonEOS::connect(progressFunc pFunc, void *pCtx) {
     NimBLERemoteCharacteristic *pChr = pSvc->getCharacteristic(CANON_EOS_CHR_NAME_UUID);
     if ((pChr != nullptr) && pChr->canIndicate()) {
       ESP_LOGI(LOG_TAG, "Subscribed for pairing indication");
-      pChr->subscribe(false, std::bind(&CanonEOS::pairCallback, this, _1, _2, _3, _4));
+      pChr->subscribe(false,
+                      [this](BLERemoteCharacteristic *pChr, uint8_t *pData, size_t length,
+                             bool isNotify) { this->pairCallback(pChr, pData, length, isNotify); });
     }
   }
 
@@ -204,7 +206,7 @@ void CanonEOS::focusRelease(void) {
   return;
 }
 
-void CanonEOS::updateGeoData(gps_t &gps, timesync_t &timesync) {
+void CanonEOS::updateGeoData(const gps_t &gps, const timesync_t &timesync) {
   // do nothing
   return;
 }
diff --git a/lib/furble/CanonEOS.h b/lib/furble/CanonEOS.h
index 204d9ec..cae2a4e 100644
--- a/lib/furble/CanonEOS.h
+++ b/lib/furble/CanonEOS.h
@@ -10,10 +10,6 @@ namespace Furble {
  */
 class CanonEOS: public Camera {
  public:
-  CanonEOS(const void *data, size_t len);
-  CanonEOS(NimBLEAdvertisedDevice *pDevice);
-  ~CanonEOS(void);
-
  protected:
   typedef struct _eos_t {
     char name[MAX_NAME];    /** Human readable device name. */
@@ -22,6 +18,10 @@ class CanonEOS: public Camera {
     Device::uuid128_t uuid; /** Our UUID. */
   } eos_t;
 
+  CanonEOS(Type type, const void *data, size_t len);
+  CanonEOS(Type type, NimBLEAdvertisedDevice *pDevice);
+  ~CanonEOS(void);
+
   const char *CANON_EOS_SVC_IDEN_UUID = "00010000-0000-1000-0000-d8492fffa821";
   /** 0xf108 */
   const char *CANON_EOS_CHR_NAME_UUID = "00010006-0000-1000-0000-d8492fffa821";
@@ -61,7 +61,7 @@ class CanonEOS: public Camera {
   void shutterRelease(void) override;
   void focusPress(void) override;
   void focusRelease(void) override;
-  void updateGeoData(gps_t &gps, timesync_t &timesync) override;
+  void updateGeoData(const gps_t &gps, const timesync_t &timesync) override;
   void disconnect(void) override;
   size_t getSerialisedBytes(void) override;
   bool serialise(void *buffer, size_t bytes) override;
diff --git a/lib/furble/CanonEOSM6.cpp b/lib/furble/CanonEOSM6.cpp
index efc7741..6b77a88 100644
--- a/lib/furble/CanonEOSM6.cpp
+++ b/lib/furble/CanonEOSM6.cpp
@@ -32,8 +32,4 @@ bool CanonEOSM6::matches(NimBLEAdvertisedDevice *pDevice) {
   return false;
 }
 
-device_type_t CanonEOSM6::getDeviceType(void) {
-  return FURBLE_CANON_EOS_M6;
-}
-
 }  // namespace Furble
diff --git a/lib/furble/CanonEOSM6.h b/lib/furble/CanonEOSM6.h
index 01e24c4..18d9920 100644
--- a/lib/furble/CanonEOSM6.h
+++ b/lib/furble/CanonEOSM6.h
@@ -9,8 +9,8 @@ namespace Furble {
  */
 class CanonEOSM6: public CanonEOS {
  public:
-  CanonEOSM6(const void *data, size_t len) : CanonEOS(data, len){};
-  CanonEOSM6(NimBLEAdvertisedDevice *pDevice) : CanonEOS(pDevice){};
+  CanonEOSM6(const void *data, size_t len) : CanonEOS(Type::CANON_EOS_M6, data, len){};
+  CanonEOSM6(NimBLEAdvertisedDevice *pDevice) : CanonEOS(Type::CANON_EOS_M6, pDevice){};
 
   /**
    * Determine if the advertised BLE device is a Canon EOS M6.
@@ -18,7 +18,6 @@ class CanonEOSM6: public CanonEOS {
   static bool matches(NimBLEAdvertisedDevice *pDevice);
 
  private:
-  device_type_t getDeviceType(void) override;
 };
 
 }  // namespace Furble
diff --git a/lib/furble/CanonEOSRP.cpp b/lib/furble/CanonEOSRP.cpp
index cf30eed..3bbedb4 100644
--- a/lib/furble/CanonEOSRP.cpp
+++ b/lib/furble/CanonEOSRP.cpp
@@ -32,8 +32,4 @@ bool CanonEOSRP::matches(NimBLEAdvertisedDevice *pDevice) {
   return false;
 }
 
-device_type_t CanonEOSRP::getDeviceType(void) {
-  return FURBLE_CANON_EOS_RP;
-}
-
 }  // namespace Furble
diff --git a/lib/furble/CanonEOSRP.h b/lib/furble/CanonEOSRP.h
index 671eb0a..4b7b853 100644
--- a/lib/furble/CanonEOSRP.h
+++ b/lib/furble/CanonEOSRP.h
@@ -9,8 +9,8 @@ namespace Furble {
  */
 class CanonEOSRP: public CanonEOS {
  public:
-  CanonEOSRP(const void *data, size_t len) : CanonEOS(data, len){};
-  CanonEOSRP(NimBLEAdvertisedDevice *pDevice) : CanonEOS(pDevice){};
+  CanonEOSRP(const void *data, size_t len) : CanonEOS(Type::CANON_EOS_RP, data, len){};
+  CanonEOSRP(NimBLEAdvertisedDevice *pDevice) : CanonEOS(Type::CANON_EOS_RP, pDevice){};
 
   /**
    * Determine if the advertised BLE device is a Canon EOS RP.
@@ -18,7 +18,6 @@ class CanonEOSRP: public CanonEOS {
   static bool matches(NimBLEAdvertisedDevice *pDevice);
 
  private:
-  device_type_t getDeviceType(void) override;
 };
 
 }  // namespace Furble
diff --git a/lib/furble/Fujifilm.cpp b/lib/furble/Fujifilm.cpp
index 699edaf..46b87e1 100644
--- a/lib/furble/Fujifilm.cpp
+++ b/lib/furble/Fujifilm.cpp
@@ -49,10 +49,10 @@ static const NimBLEUUID FUJIFILM_CHR_GEOTAG_UUID =
 
 static const NimBLEUUID FUJIFILM_GEOTAG_UPDATE = NimBLEUUID("ad06c7b7-f41a-46f4-a29a-712055319122");
 
-static const std::array<uint8_t, 2> FUJIFILM_SHUTTER_RELEASE = {0x00, 0x00};
-static const std::array<uint8_t, 2> FUJIFILM_SHUTTER_CMD = {0x01, 0x00};
-static const std::array<uint8_t, 2> FUJIFILM_SHUTTER_PRESS = {0x02, 0x00};
-static const std::array<uint8_t, 2> FUJIFILM_SHUTTER_FOCUS = {0x03, 0x00};
+static constexpr std::array<uint8_t, 2> FUJIFILM_SHUTTER_RELEASE = {0x00, 0x00};
+static constexpr std::array<uint8_t, 2> FUJIFILM_SHUTTER_CMD = {0x01, 0x00};
+static constexpr std::array<uint8_t, 2> FUJIFILM_SHUTTER_PRESS = {0x02, 0x00};
+static constexpr std::array<uint8_t, 2> FUJIFILM_SHUTTER_FOCUS = {0x03, 0x00};
 
 namespace Furble {
 
@@ -82,7 +82,7 @@ void Fujifilm::notify(BLERemoteCharacteristic *pChr, uint8_t *pData, size_t leng
   }
 }
 
-Fujifilm::Fujifilm(const void *data, size_t len) {
+Fujifilm::Fujifilm(const void *data, size_t len) : Camera(Type::FUJIFILM) {
   if (len != sizeof(fujifilm_t))
     throw;
 
@@ -92,7 +92,7 @@ Fujifilm::Fujifilm(const void *data, size_t len) {
   memcpy(m_Token.data(), fujifilm->token, FUJIFILM_TOKEN_LEN);
 }
 
-Fujifilm::Fujifilm(NimBLEAdvertisedDevice *pDevice) {
+Fujifilm::Fujifilm(NimBLEAdvertisedDevice *pDevice) : Camera(Type::FUJIFILM) {
   const char *data = pDevice->getManufacturerData().data();
   m_Name = pDevice->getName();
   m_Address = pDevice->getAddress();
@@ -107,13 +107,13 @@ Fujifilm::~Fujifilm(void) {
   m_Client = nullptr;
 }
 
-const size_t FUJIFILM_ADV_TOKEN_LEN = 7;
-const uint8_t FUJIFILM_ID_0 = 0xd8;
-const uint8_t FUJIFILM_ID_1 = 0x04;
-const uint8_t FUJIFILM_TYPE_TOKEN = 0x02;
+constexpr size_t FUJIFILM_ADV_TOKEN_LEN = 7;
+constexpr uint8_t FUJIFILM_ID_0 = 0xd8;
+constexpr uint8_t FUJIFILM_ID_1 = 0x04;
+constexpr uint8_t FUJIFILM_TYPE_TOKEN = 0x02;
 
 /**
- * Determine if the advertised BLE device is a Fujifilm X-T30.
+ * Determine if the advertised BLE device is a Fujifilm.
  */
 bool Fujifilm::matches(NimBLEAdvertisedDevice *pDevice) {
   if (pDevice->haveManufacturerData()
@@ -261,7 +261,7 @@ void Fujifilm::focusRelease(void) {
   shutterRelease();
 }
 
-void Fujifilm::sendGeoData(gps_t &gps, timesync_t &timesync) {
+void Fujifilm::sendGeoData(const gps_t &gps, const timesync_t &timesync) {
   NimBLERemoteService *pSvc = m_Client->getService(FUJIFILM_SVC_GEOTAG_UUID);
   if (pSvc == nullptr) {
     return;
@@ -273,18 +273,20 @@ void Fujifilm::sendGeoData(gps_t &gps, timesync_t &timesync) {
   }
 
   if (pChr->canWrite()) {
-    geotag_t geotag = {.latitude = (int32_t)(gps.latitude * 10000000),
-                       .longitude = (int32_t)(gps.longitude * 10000000),
-                       .altitude = (int32_t)gps.altitude,
-                       .pad = {0},
-                       .gps_time = {
-                           .year = (uint16_t)timesync.year,
-                           .day = (uint8_t)timesync.day,
-                           .month = (uint8_t)timesync.month,
-                           .hour = (uint8_t)timesync.hour,
-                           .minute = (uint8_t)timesync.minute,
-                           .second = (uint8_t)timesync.second,
-                       }};
+    geotag_t geotag = {
+        .latitude = (int32_t)(gps.latitude * 10000000),
+        .longitude = (int32_t)(gps.longitude * 10000000),
+        .altitude = (int32_t)gps.altitude,
+        .pad = {0},
+        .gps_time = {
+                .year = (uint16_t)timesync.year,
+                .day = (uint8_t)timesync.day,
+                .month = (uint8_t)timesync.month,
+                .hour = (uint8_t)timesync.hour,
+                .minute = (uint8_t)timesync.minute,
+                .second = (uint8_t)timesync.second,
+                }
+    };
 
     ESP_LOGI(LOG_TAG, "Sending geotag data (%u bytes) to 0x%04x", sizeof(geotag),
              pChr->getHandle());
@@ -296,7 +298,7 @@ void Fujifilm::sendGeoData(gps_t &gps, timesync_t &timesync) {
   }
 }
 
-void Fujifilm::updateGeoData(gps_t &gps, timesync_t &timesync) {
+void Fujifilm::updateGeoData(const gps_t &gps, const timesync_t &timesync) {
   if (m_GeoRequested) {
     sendGeoData(gps, timesync);
     m_GeoRequested = false;
@@ -313,10 +315,6 @@ void Fujifilm::disconnect(void) {
   m_Client->disconnect();
 }
 
-device_type_t Fujifilm::getDeviceType(void) {
-  return FURBLE_FUJIFILM;
-}
-
 size_t Fujifilm::getSerialisedBytes(void) {
   return sizeof(fujifilm_t);
 }
diff --git a/lib/furble/Fujifilm.h b/lib/furble/Fujifilm.h
index 02dfe7a..7e4262f 100644
--- a/lib/furble/Fujifilm.h
+++ b/lib/furble/Fujifilm.h
@@ -27,9 +27,8 @@ class Fujifilm: public Camera {
   void shutterRelease(void) override;
   void focusPress(void) override;
   void focusRelease(void) override;
-  void updateGeoData(gps_t &gps, timesync_t &timesync) override;
+  void updateGeoData(const gps_t &gps, const timesync_t &timesync) override;
   void disconnect(void) override;
-  device_type_t getDeviceType(void) override;
   size_t getSerialisedBytes(void) override;
   bool serialise(void *buffer, size_t bytes) override;
 
@@ -59,7 +58,7 @@ class Fujifilm: public Camera {
 
   void print(void);
   void notify(NimBLERemoteCharacteristic *, uint8_t *, size_t, bool);
-  void sendGeoData(gps_t &gps, timesync_t &timesync);
+  void sendGeoData(const gps_t &gps, const timesync_t &timesync);
 
   template <std::size_t N>
   void sendShutterCommand(const std::array<uint8_t, N> &cmd, const std::array<uint8_t, N> &param);
diff --git a/lib/furble/Furble.cpp b/lib/furble/Furble.cpp
index f97f849..6489641 100644
--- a/lib/furble/Furble.cpp
+++ b/lib/furble/Furble.cpp
@@ -1,5 +1,6 @@
 #include <NimBLEAdvertisedDevice.h>
 #include <NimBLEDevice.h>
+#include <NimBLEScan.h>
 
 #include "Device.h"
 #include "Furble.h"
@@ -14,14 +15,10 @@ scanResultCallback *Scan::m_ScanResultCallback = nullptr;
 void *Scan::m_ScanResultPrivateData = nullptr;
 HIDServer *Scan::m_HIDServer = nullptr;
 
-void scanEndCB(NimBLEScanResults results) {
-  ESP_LOGI(LOG_TAG, "Scan ended");
-}
-
 /**
  * BLE Advertisement callback.
  */
-class Scan::AdvertisedCallback: public NimBLEAdvertisedDeviceCallbacks {
+class Scan::ScanCallback: public NimBLEScanCallbacks {
   void onResult(NimBLEAdvertisedDevice *pDevice) {
     if (CameraList::match(pDevice)) {
       if (m_ScanResultCallback != nullptr) {
@@ -35,10 +32,8 @@ class Scan::AdvertisedCallback: public NimBLEAdvertisedDeviceCallbacks {
  * HID server callback.
  */
 class Scan::HIDServerCallback: public HIDServerCallbacks {
-  void onConnect(NimBLEAddress address) { return; }
-
-  void onComplete(NimBLEAddress address) {
-    CameraList::add(address);
+  void onComplete(const NimBLEAddress &address, const std::string &name) {
+    CameraList::add(address, name);
     if (m_ScanResultCallback != nullptr) {
       (m_ScanResultCallback)(m_ScanResultPrivateData);
     }
@@ -49,25 +44,24 @@ void Scan::init(esp_power_level_t power) {
   NimBLEDevice::init(Device::getStringID());
   NimBLEDevice::setPower(power);
   NimBLEDevice::setSecurityAuth(true, true, true);
+  NimBLEDevice::setOwnAddrType(BLE_OWN_ADDR_PUBLIC);
 
   // NimBLE requires configuring server before scan
   m_HIDServer = HIDServer::getInstance();
 
   Scan::m_Scan = NimBLEDevice::getScan();
-  m_Scan->setAdvertisedDeviceCallbacks(new AdvertisedCallback());
+  m_Scan->setScanCallbacks(new ScanCallback());
   m_Scan->setActiveScan(true);
   m_Scan->setInterval(6553);
   m_Scan->setWindow(6553);
 }
 
-void Scan::start(const uint32_t scanDuration,
-                 scanResultCallback scanCallback,
-                 void *scanPrivateData) {
-  m_HIDServer->start(scanDuration, new HIDServerCallback());
+void Scan::start(scanResultCallback scanCallback, void *scanPrivateData) {
+  m_HIDServer->start(nullptr, new HIDServerCallback());
 
   m_ScanResultCallback = scanCallback;
   m_ScanResultPrivateData = scanPrivateData;
-  m_Scan->start(scanDuration, scanEndCB, false);
+  m_Scan->start(BLE_HS_FOREVER, false);
 }
 
 void Scan::stop(void) {
diff --git a/lib/furble/Furble.h b/lib/furble/Furble.h
index 017452d..55a419f 100644
--- a/lib/furble/Furble.h
+++ b/lib/furble/Furble.h
@@ -11,7 +11,7 @@
 #define FURBLE_VERSION "unknown"
 #endif
 
-typedef void(scanResultCallback(void *private_data));
+typedef void(scanResultCallback(void *context));
 
 namespace Furble {
 /**
@@ -30,9 +30,7 @@ class Scan {
    * Start the scan for BLE advertisements with a callback function when a matching reseult is
    * encountered.
    */
-  static void start(const uint32_t scanDuration,
-                    scanResultCallback scanCallBack,
-                    void *scanResultPrivateData);
+  static void start(scanResultCallback scanCallBack, void *scanResultPrivateData);
 
   /**
    * Stop the scan.
@@ -48,7 +46,7 @@ class Scan {
   ~Scan();
 
  private:
-  class AdvertisedCallback;
+  class ScanCallback;
   class HIDServerCallback;
 
   static NimBLEScan *m_Scan;
diff --git a/lib/furble/FurbleTypes.h b/lib/furble/FurbleTypes.h
index d818a1f..8ef7a2b 100644
--- a/lib/furble/FurbleTypes.h
+++ b/lib/furble/FurbleTypes.h
@@ -5,11 +5,4 @@
 
 extern const char *LOG_TAG;
 
-typedef enum {
-  FURBLE_FUJIFILM = 1,
-  FURBLE_CANON_EOS_M6 = 2,
-  FURBLE_CANON_EOS_RP = 3,
-  FURBLE_MOBILE_DEVICE = 4,
-} device_type_t;
-
 #endif
diff --git a/lib/furble/HIDServer.cpp b/lib/furble/HIDServer.cpp
index 182f4d9..4611a33 100644
--- a/lib/furble/HIDServer.cpp
+++ b/lib/furble/HIDServer.cpp
@@ -58,7 +58,9 @@ HIDServer::HIDServer() {
   m_Advertising->addServiceUUID(m_HID->hidService()->getUUID());
 }
 
-HIDServer::~HIDServer() {}
+HIDServer::~HIDServer() {
+  m_hidCallbacks = nullptr;
+}
 
 HIDServer *HIDServer::getInstance(void) {
   if (hidServer == nullptr) {
@@ -68,15 +70,17 @@ HIDServer *HIDServer::getInstance(void) {
   return hidServer;
 }
 
-void HIDServer::start(unsigned int duration,
-                      HIDServerCallbacks *hidCallback,
-                      NimBLEAddress *address) {
+void HIDServer::start(NimBLEAddress *address, HIDServerCallbacks *hidCallback) {
   m_hidCallbacks = hidCallback;
   m_HID->startServices();
 
-  // Directed advertising not yet released
-  // m_Advertising->start(0, nullptr, address);
-  m_Advertising->start(duration);
+  m_Server->getPeerNameOnConnect((address == nullptr) ? true : false);
+
+  // Cannot get directed advertising working properly.
+  // m_Advertising->setAdvertisementType((address == nullptr) ? BLE_GAP_CONN_MODE_UND :
+  // BLE_GAP_CONN_MODE_DIR);
+  m_Advertising->setAdvertisementType(BLE_GAP_CONN_MODE_UND);
+  m_Advertising->start(BLE_HS_FOREVER, nullptr, address);
 }
 
 void HIDServer::stop(void) {
@@ -84,23 +88,16 @@ void HIDServer::stop(void) {
   m_Server->stopAdvertising();
 }
 
-void HIDServer::onConnect(NimBLEServer *pServer, ble_gap_conn_desc *desc) {
-  NimBLEAddress address = NimBLEAddress(desc->peer_ota_addr);
+void HIDServer::onAuthenticationComplete(const NimBLEConnInfo &connInfo, const std::string &name) {
+  NimBLEAddress address = connInfo.getIdAddress();
   if (m_hidCallbacks != nullptr) {
-    m_hidCallbacks->onConnect(address);
+    m_hidCallbacks->onComplete(address, name);
   }
 }
 
-void HIDServer::onDisconnect(NimBLEServer *pServer, ble_gap_conn_desc *desc) {
-  m_Connected = false;
-}
-
-void HIDServer::onAuthenticationComplete(ble_gap_conn_desc *desc) {
-  NimBLEAddress address = NimBLEAddress(desc->peer_ota_addr);
-  if (m_hidCallbacks != nullptr) {
-    m_hidCallbacks->onComplete(address);
-  }
-  m_Connected = true;
+void HIDServer::onIdentity(const NimBLEConnInfo &connInfo) {
+  ESP_LOGI("HID", "identity resolved: address = %s, type = %d",
+           connInfo.getIdAddress().toString().c_str(), connInfo.getIdAddress().getType());
 }
 
 NimBLECharacteristic *HIDServer::getInput(void) {
@@ -116,7 +113,9 @@ void HIDServer::disconnect(NimBLEAddress &address) {
   m_Server->disconnect(info.getConnHandle());
 }
 
-bool HIDServer::isConnected(void) {
-  return m_Connected;
+bool HIDServer::isConnected(NimBLEAddress &address) {
+  NimBLEConnInfo info = m_Server->getPeerInfo(address);
+
+  return (!info.getIdAddress().isNull());
 }
 }  // namespace Furble
diff --git a/lib/furble/HIDServer.h b/lib/furble/HIDServer.h
index c3afbed..c75f1cb 100644
--- a/lib/furble/HIDServer.h
+++ b/lib/furble/HIDServer.h
@@ -13,8 +13,7 @@ class HIDServer;
  */
 class HIDServerCallbacks {
  public:
-  virtual void onConnect(NimBLEAddress address);
-  virtual void onComplete(NimBLEAddress address);
+  virtual void onComplete(const NimBLEAddress &address, const std::string &name);
 };
 
 /**
@@ -29,16 +28,14 @@ class HIDServer: public NimBLEServerCallbacks {
    *
    * @param[in] address If specified, start directed advertising.
    */
-  void start(unsigned int duration,
-             HIDServerCallbacks *hidCallbacks,
-             NimBLEAddress *address = nullptr);
+  void start(NimBLEAddress *address = nullptr, HIDServerCallbacks *hidCallbacks = nullptr);
 
   void stop(void);
 
   NimBLECharacteristic *getInput(void);
   NimBLEConnInfo getConnInfo(NimBLEAddress &address);
   void disconnect(NimBLEAddress &address);
-  bool isConnected(void);
+  bool isConnected(NimBLEAddress &address);
 
  private:
   HIDServer();
@@ -46,11 +43,9 @@ class HIDServer: public NimBLEServerCallbacks {
 
   static HIDServer *hidServer;  // singleton
 
-  void onConnect(NimBLEServer *pServer, ble_gap_conn_desc *desc);
-  void onDisconnect(NimBLEServer *pServer, ble_gap_conn_desc *desc);
-  void onAuthenticationComplete(ble_gap_conn_desc *desc);
+  void onAuthenticationComplete(const NimBLEConnInfo &connInfo, const std::string &name) override;
+  void onIdentity(const NimBLEConnInfo &connInfo) override;
 
-  bool m_Connected = false;
   NimBLEServer *m_Server = nullptr;
   NimBLEHIDDevice *m_HID = nullptr;
   NimBLECharacteristic *m_Input = nullptr;
diff --git a/lib/furble/MobileDevice.cpp b/lib/furble/MobileDevice.cpp
index dadcabc..2b3a043 100644
--- a/lib/furble/MobileDevice.cpp
+++ b/lib/furble/MobileDevice.cpp
@@ -9,7 +9,7 @@
 
 namespace Furble {
 
-MobileDevice::MobileDevice(const void *data, size_t len) {
+MobileDevice::MobileDevice(const void *data, size_t len) : Camera(Type::MOBILE_DEVICE) {
   if (len != sizeof(mobile_device_t))
     throw;
 
@@ -19,8 +19,9 @@ MobileDevice::MobileDevice(const void *data, size_t len) {
   m_HIDServer = HIDServer::getInstance();
 }
 
-MobileDevice::MobileDevice(NimBLEAddress address) {
-  m_Name = address.toString();
+MobileDevice::MobileDevice(const NimBLEAddress &address, const std::string &name)
+    : Camera(Type::MOBILE_DEVICE) {
+  m_Name = name;
   m_Address = address;
   m_HIDServer = HIDServer::getInstance();
 }
@@ -43,25 +44,25 @@ bool MobileDevice::matches(NimBLEAdvertisedDevice *pDevice) {
  * All this logic is encapsulated in the HIDServer class.
  */
 bool MobileDevice::connect(progressFunc pFunc, void *pCtx) {
+  unsigned int timeout_secs = 60;
   float progress = 0.0f;
   updateProgress(pFunc, pCtx, progress);
 
-  m_HIDServer->start(60, nullptr, &m_Address);
+  m_HIDServer->start(&m_Address);
 
-  unsigned int timeout = 60;
-
-  ESP_LOGI(LOG_TAG, "Waiting for connection.");
-  while (--timeout && !isConnected()) {
+  ESP_LOGI(LOG_TAG, "Waiting for %us for connection from %s", timeout_secs, m_Name.c_str());
+  while (--timeout_secs && !isConnected()) {
     progress += 1.0f;
     updateProgress(pFunc, pCtx, progress);
     delay(1000);
   };
 
-  if (timeout == 0) {
+  if (timeout_secs == 0) {
     ESP_LOGI(LOG_TAG, "Connection timed out.");
     return false;
   }
 
+  ESP_LOGI(LOG_TAG, "Connected to %s.", m_Name.c_str());
   progress = 100.0f;
   updateProgress(pFunc, pCtx, progress);
   m_HIDServer->stop();
@@ -89,7 +90,7 @@ void MobileDevice::focusRelease(void) {
   // not supported
 }
 
-void MobileDevice::updateGeoData(gps_t &gps, timesync_t &timesync) {
+void MobileDevice::updateGeoData(const gps_t &gps, const timesync_t &timesync) {
   // not supported
 }
 
@@ -98,11 +99,7 @@ void MobileDevice::disconnect(void) {
 }
 
 bool MobileDevice::isConnected(void) {
-  return m_HIDServer->isConnected();
-}
-
-device_type_t MobileDevice::getDeviceType(void) {
-  return FURBLE_MOBILE_DEVICE;
+  return m_HIDServer->isConnected(m_Address);
 }
 
 size_t MobileDevice::getSerialisedBytes(void) {
diff --git a/lib/furble/MobileDevice.h b/lib/furble/MobileDevice.h
index 33ebf5d..2378a6f 100644
--- a/lib/furble/MobileDevice.h
+++ b/lib/furble/MobileDevice.h
@@ -13,20 +13,20 @@ namespace Furble {
 class MobileDevice: public Camera {
  public:
   MobileDevice(const void *data, size_t len);
-  MobileDevice(NimBLEAddress address);
+  MobileDevice(const NimBLEAddress &address, const std::string &name);
   ~MobileDevice(void);
 
   static bool matches(NimBLEAdvertisedDevice *pDevice);
 
-  bool connect(progressFunc pFunc = nullptr, void *pCtx = nullptr);
-  void shutterPress(void);
-  void shutterRelease(void);
-  void focusPress(void);
-  void focusRelease(void);
-  void updateGeoData(gps_t &gps, timesync_t &timesync);
-  void disconnect(void);
+  bool connect(progressFunc pFunc = nullptr, void *pCtx = nullptr) override;
+  void shutterPress(void) override;
+  void shutterRelease(void) override;
+  void focusPress(void) override;
+  void focusRelease(void) override;
+  void updateGeoData(const gps_t &gps, const timesync_t &timesync) override;
+  void disconnect(void) override;
 
-  bool isConnected(void);
+  bool isConnected(void) override;
 
  private:
   typedef struct _mobile_device_t {
@@ -35,7 +35,6 @@ class MobileDevice: public Camera {
     uint8_t type;        /** Address type. */
   } mobile_device_t;
 
-  device_type_t getDeviceType(void);
   size_t getSerialisedBytes(void);
   bool serialise(void *buffer, size_t bytes);
   void sendKeyReport(const uint8_t key);
diff --git a/platformio.ini b/platformio.ini
index 94a1c15..2c31298 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -1,5 +1,14 @@
 [furble]
-build_flags = -Wall -DFURBLE_VERSION=\"${sysenv.FURBLE_VERSION}\" -DCORE_DEBUG_LEVEL=3
+build_flags = -Wall
+  -DFURBLE_VERSION=\"${sysenv.FURBLE_VERSION}\"
+  -DCORE_DEBUG_LEVEL=3
+  -DCONFIG_BT_NIMBLE_LOG_LEVEL=3
+  -DCONFIG_BT_NIMBLE_MAX_CONNECTIONS=9
+  -DCONFIG_BT_NIMBLE_MAX_BONDS=15
+  -DCONFIG_BT_NIMBLE_MAX_CCCDS=16
+  -DCONFIG_NIMBLE_CPP_LOG_LEVEL=1
+  -DCONFIG_NIMBLE_CPP_ENABLE_GAP_EVENT_CODE_TEXT=1
+  -DCONFIG_NIMBLE_CPP_ENABLE_RETURN_CODE_TEXT=1
 
 [env]
 platform = espressif32
@@ -9,8 +18,8 @@ framework = arduino
 lib_deps =
   M5GFX@0.1.16
   M5Unified@0.1.16
-  NimBLE-Arduino@1.4.2
   mikalhart/TinyGPSPlus@1.0.3
+  https://github.com/h2zero/NimBLE-Arduino#6ece29f
 
 [env:m5stick-c]
 board = m5stick-c
diff --git a/src/furble.cpp b/src/furble.cpp
index df5468d..c829763 100644
--- a/src/furble.cpp
+++ b/src/furble.cpp
@@ -1,17 +1,23 @@
+#include <M5Unified.h>
+
 #include <Device.h>
 #include <Furble.h>
 #include <M5ez.h>
 
-#include <M5Unified.h>
-
 #include "furble_control.h"
 #include "furble_gps.h"
 #include "furble_ui.h"
 #include "interval.h"
 #include "settings.h"
 
-const uint32_t SCAN_DURATION = (60 * 5);
-static QueueHandle_t queue;
+typedef struct {
+  Furble::Control *control;
+  ezMenu *menu;
+  bool scan;
+  bool multiconnect;
+} ui_context_t;
+
+#define CONNECT_STAR "Connect *"
 
 /**
  * Progress bar update function.
@@ -56,11 +62,11 @@ static void show_shutter_control(bool shutter_locked, unsigned long lock_start_m
     snprintf(duration, 16, "%02lu:%02lu", minutes, seconds);
 
 #if ARDUINO_M5STACK_CORE_ESP32 || ARDUINO_M5STACK_CORE2
-    ez.msgBox("Remote Shutter", {std::string("Shutter Locked") + std::string(duration)},
+    ez.msgBox("Remote Shutter", {std::string("Shutter Locked"), std::string(duration)},
               {"Unlock", "Unlock", "Back"}, false);
 #else
     ez.msgBox("Remote Shutter",
-              {std::string("Shutter Locked") + std::string(duration), "", "Back: Power"},
+              {std::string("Shutter Locked"), std::string(duration), "", "Back: Power"},
               {"Unlock", "Unlock"}, false);
 #endif
   } else {
@@ -74,17 +80,14 @@ static void show_shutter_control(bool shutter_locked, unsigned long lock_start_m
 }
 
 static void remote_control(FurbleCtx *fctx) {
-  auto camera = fctx->camera;
+  auto control = fctx->control;
   static unsigned long shutter_lock_start_ms = 0;
   static bool shutter_lock = false;
-  control_cmd_t cmd;
 
   ESP_LOGI(LOG_TAG, "Remote Control");
 
   show_shutter_control(false, 0);
 
-  ESP_LOGW(LOG_TAG, "queue = %p", queue);
-
   do {
     ez.yield();
 
@@ -96,8 +99,7 @@ static void remote_control(FurbleCtx *fctx) {
     if (M5.BtnPWR.wasClicked() || M5.BtnC.wasPressed()) {
       if (shutter_lock) {
         // ensure shutter is released on exit
-        cmd = CONTROL_CMD_SHUTTER_RELEASE;
-        xQueueSend(queue, &cmd, 0);
+        control->sendCommand(CONTROL_CMD_SHUTTER_RELEASE);
       }
       ESP_LOGI(LOG_TAG, "Exit shutter");
       break;
@@ -107,16 +109,14 @@ static void remote_control(FurbleCtx *fctx) {
       // release shutter if either shutter or focus is pressed
       if (M5.BtnA.wasClicked() || M5.BtnB.wasClicked()) {
         shutter_lock = false;
-        cmd = CONTROL_CMD_SHUTTER_RELEASE;
-        xQueueSend(queue, &cmd, 0);
+        control->sendCommand(CONTROL_CMD_SHUTTER_RELEASE);
         show_shutter_control(false, 0);
       } else {
         show_shutter_control(true, shutter_lock_start_ms);
       }
     } else {
       if (M5.BtnA.wasPressed()) {
-        cmd = CONTROL_CMD_SHUTTER_PRESS;
-        xQueueSend(queue, &cmd, 0);
+        control->sendCommand(CONTROL_CMD_SHUTTER_PRESS);
         continue;
       }
 
@@ -128,25 +128,22 @@ static void remote_control(FurbleCtx *fctx) {
           show_shutter_control(true, shutter_lock_start_ms);
           ESP_LOGI(LOG_TAG, "shutter lock");
         } else {
-          cmd = CONTROL_CMD_SHUTTER_RELEASE;
-          xQueueSend(queue, &cmd, 0);
+          control->sendCommand(CONTROL_CMD_SHUTTER_RELEASE);
         }
         continue;
       }
 
       if (M5.BtnB.wasPressed()) {
-        cmd = CONTROL_CMD_FOCUS_PRESS;
-        xQueueSend(queue, &cmd, 0);
+        control->sendCommand(CONTROL_CMD_FOCUS_PRESS);
         continue;
       }
 
       if (M5.BtnB.wasReleased()) {
-        cmd = CONTROL_CMD_FOCUS_RELEASE;
-        xQueueSend(queue, &cmd, 0);
+        control->sendCommand(CONTROL_CMD_FOCUS_RELEASE);
         continue;
       }
     }
-  } while (camera->isConnected());
+  } while (control->isConnected());
 }
 
 /**
@@ -154,28 +151,33 @@ static void remote_control(FurbleCtx *fctx) {
  * * disconnect detection
  * * geotag updates
  */
-static uint16_t statusRefresh(void *private_data) {
-  FurbleCtx *fctx = static_cast<FurbleCtx *>(private_data);
-  auto camera = fctx->camera;
+static uint16_t statusRefresh(void *context) {
+  FurbleCtx *fctx = static_cast<FurbleCtx *>(context);
+  auto control = fctx->control;
 
-  if (camera->isConnected()) {
-    furble_gps_update(camera);
+  if (control->isConnected()) {
+    furble_gps_update(control);
     return 500;
   }
 
   auto buttons = ez.buttons.get();
   std::string header = ez.header.title();
 
-  ezProgressBar progress_bar(FURBLE_STR, {"Reconnecting ..."}, {""});
-  if (camera->connect(settings_load_esp_tx_power(), &update_progress_bar, &progress_bar)) {
-    ez.screen.clear();
-    ez.header.show(header);
-    ez.buttons.show(buttons);
+  for (const auto &target : control->getTargets()) {
+    auto camera = target->getCamera();
+    if (!camera->isConnected()) {
+      ezProgressBar progress_bar(FURBLE_STR, {"Reconnecting ..."}, {""});
+      if (camera->connect(settings_load_esp_tx_power(), &update_progress_bar, &progress_bar)) {
+        ez.screen.clear();
+        ez.header.show(header);
+        ez.buttons.show(buttons);
 
-    fctx->reconnected = true;
+        fctx->reconnected = true;
 
-    ez.redraw();
-    return 500;
+        ez.redraw();
+        return 500;
+      }
+    }
   }
 
   ez.screen.clear();
@@ -209,25 +211,86 @@ static void menu_remote(FurbleCtx *fctx) {
 
   ez.removeEvent(statusRefresh);
 
-  fctx->camera->disconnect();
-  fctx->camera->setActive(false);
+  fctx->control->disconnect();
   ez.backlight.inactivity(USER_SET);
 }
 
+static bool do_connect(ezMenu *menu, void *context) {
+  auto ctx = static_cast<ui_context_t *>(context);
+
+  FurbleCtx fctx = {ctx->control, false};
+
+  if (!ctx->scan && ctx->multiconnect) {
+    for (int n = 0; n < Furble::CameraList::size(); n++) {
+      auto camera = Furble::CameraList::get(n);
+      if (camera->isActive()) {
+        ezProgressBar progress_bar(FURBLE_STR, {std::string("Connecting to ") + camera->getName()},
+                                   {""});
+        if (camera->connect(settings_load_esp_tx_power(), &update_progress_bar, &progress_bar)) {
+          ctx->control->addActive(camera);
+        } else {
+          // Fail all if any connect fails
+          return false;
+        }
+      }
+    }
+  } else {
+    auto camera = Furble::CameraList::get(menu->pick() - 1);
+
+    ezProgressBar progress_bar(FURBLE_STR, {std::string("Connecting to ") + camera->getName()},
+                               {""});
+    if (camera->connect(settings_load_esp_tx_power(), &update_progress_bar, &progress_bar)) {
+      if (ctx->scan) {
+        Furble::CameraList::save(camera);
+      }
+      ctx->control->addActive(camera);
+    } else {
+      return false;
+    }
+  }
+
+  menu_remote(&fctx);
+  return true;
+}
+
+static bool toggleCameraSelect(ezMenu *menu, void *context) {
+  auto camera = static_cast<Furble::Camera *>(context);
+  camera->setActive(!camera->isActive());
+  menu->setCaption(camera->getAddress().toString(),
+                   camera->getName() + "\t" + (camera->isActive() ? "*" : ""));
+
+  return true;
+}
+
 /**
  * Scan callback to update connection menu with new devices.
  */
-void updateConnectItems(void *private_data) {
-  ezMenu *submenu = (ezMenu *)private_data;
+static void updateConnectItems(void *context) {
+  auto ctx = static_cast<ui_context_t *>(context);
+  auto menu = ctx->menu;
 
-  submenu->deleteItem("Back");
-  for (int i = submenu->countItems(); i < Furble::CameraList::size(); i++) {
-    submenu->addItem(Furble::CameraList::get(i)->getName());
+  if (!ctx->scan && ctx->multiconnect) {
+    menu->deleteItem(CONNECT_STAR);
   }
-  submenu->addItem("Back");
+  menu->deleteItem("Back");
+  for (int i = menu->countItems(); i < Furble::CameraList::size(); i++) {
+    auto camera = Furble::CameraList::get(i);
+
+    if (!ctx->scan && ctx->multiconnect) {
+      menu->addItem(camera->getAddress().toString(),
+                    camera->getName() + "\t" + (camera->isActive() ? "*" : ""), NULL,
+                    static_cast<void *>(camera), toggleCameraSelect);
+    } else {
+      menu->addItem(camera->getAddress().toString(), camera->getName(), NULL, ctx, do_connect);
+    }
+  }
+  if (!ctx->scan && ctx->multiconnect) {
+    menu->addItem(CONNECT_STAR, "", NULL, ctx, do_connect);
+  }
+  menu->addItem("Back");
 }
 
-static void menu_connect(bool scan) {
+static void menu_connect(Furble::Control *control, bool scan) {
   std::string header = FURBLE_STR " - ";
   if (scan) {
     header += "Scanning";
@@ -236,11 +299,12 @@ static void menu_connect(bool scan) {
   }
 
   ezMenu submenu(header);
+  ui_context_t ui_ctx = (ui_context_t){control, &submenu, scan, settings_load_multiconnect()};
   if (scan) {
     ez.backlight.inactivity(NEVER);
-    Furble::Scan::start(SCAN_DURATION, updateConnectItems, &submenu);
+    Furble::Scan::start(updateConnectItems, &ui_ctx);
   } else {
-    updateConnectItems(&submenu);
+    updateConnectItems(&ui_ctx);
   }
 
   submenu.buttons({"OK", "down"});
@@ -249,45 +313,43 @@ static void menu_connect(bool scan) {
   }
   submenu.downOnLast("first");
 
-  submenu.runOnce(true);
-
-  if (scan) {
-    Furble::Scan::stop();
-    ez.backlight.inactivity(USER_SET);
-  }
-
-  int16_t i = submenu.pick();
-  if (i == 0)
-    return;
-
-  FurbleCtx fctx = {Furble::CameraList::get(i - 1), false};
+  do {
+    submenu.runOnce(true);
 
-  ezProgressBar progress_bar(FURBLE_STR, {std::string("Connecting to ") + fctx.camera->getName()},
-                             {""});
-  if (fctx.camera->connect(settings_load_esp_tx_power(), &update_progress_bar, &progress_bar)) {
     if (scan) {
-      Furble::CameraList::save(fctx.camera);
+      Furble::Scan::stop();
+      ez.backlight.inactivity(USER_SET);
     }
-    fctx.camera->setActive(true);
-    menu_remote(&fctx);
-  }
+
+    if (submenu.pick() == 0) {
+      break;
+    }
+  } while (!scan);
 }
 
 /**
  * Scan for devices, then present connection menu.
  */
-static void do_scan(void) {
+static bool do_scan(ezMenu *menu, void *context) {
+  auto control = static_cast<Furble::Control *>(context);
+
   Furble::CameraList::clear();
   Furble::Scan::clear();
-  menu_connect(true);
+  menu_connect(control, true);
+
+  return true;
 }
 
 /**
  * Retrieve saved devices, then present connection menu.
  */
-static void do_saved(void) {
+static bool do_saved(ezMenu *menu, void *context) {
+  auto control = static_cast<Furble::Control *>(context);
+
   Furble::CameraList::load();
-  menu_connect(false);
+  menu_connect(control, false);
+
+  return true;
 }
 
 static void menu_delete(void) {
@@ -307,13 +369,32 @@ static void menu_delete(void) {
   Furble::CameraList::remove(Furble::CameraList::get(i - 1));
 }
 
+/**
+ * Toggle Multi-Connect menu setting.
+ */
+static bool multiconnect_toggle(ezMenu *menu, void *context) {
+  bool *multiconnect = static_cast<bool *>(context);
+  *multiconnect = !*multiconnect;
+  menu->setCaption("multiconnectonoff",
+                   std::string("Multi-Connect\t") + (*multiconnect ? "ON" : "OFF"));
+
+  settings_save_multiconnect(*multiconnect);
+
+  return true;
+}
+
 static void menu_settings(void) {
   ezMenu submenu(FURBLE_STR " - Settings");
 
+  bool multiconnect = settings_load_multiconnect();
+
   submenu.buttons({"OK", "down"});
   submenu.addItem("Backlight", "", ez.backlight.menu);
   submenu.addItem("GPS", "", settings_menu_gps);
   submenu.addItem("Intervalometer", "", settings_menu_interval);
+  submenu.addItem("multiconnectonoff",
+                  std::string("Multi-Connect\t") + (multiconnect ? "ON" : "OFF"), nullptr,
+                  &multiconnect, multiconnect_toggle);
   submenu.addItem("Theme", "", ez.theme->menu);
   submenu.addItem("Transmit Power", "", settings_menu_tx_power);
   submenu.addItem("About", "", about);
@@ -327,7 +408,7 @@ static void mainmenu_poweroff(void) {
 }
 
 void vUITask(void *param) {
-  queue = static_cast<QueueHandle_t>(param);
+  auto control = static_cast<Furble::Control *>(param);
 
 #include <themes/dark.h>
 #include <themes/default.h>
@@ -342,9 +423,9 @@ void vUITask(void *param) {
     ezMenu mainmenu(FURBLE_STR);
     mainmenu.buttons({"OK", "down"});
     if (save_count > 0) {
-      mainmenu.addItem("Connect", "", do_saved);
+      mainmenu.addItem("Connect", "", nullptr, control, do_saved);
     }
-    mainmenu.addItem("Scan", "", do_scan);
+    mainmenu.addItem("Scan", "", nullptr, control, do_scan);
     if (save_count > 0) {
       mainmenu.addItem("Delete Saved", "", menu_delete);
     }
diff --git a/src/furble_control.cpp b/src/furble_control.cpp
index 84f9a15..a769b4c 100644
--- a/src/furble_control.cpp
+++ b/src/furble_control.cpp
@@ -2,57 +2,182 @@
 
 #include "furble_control.h"
 
-static QueueHandle_t queue;
+static void target_task(void *param) {
+  auto *target = static_cast<Furble::Control::Target *>(param);
 
-static Furble::Camera::gps_t last_gps;
-static Furble::Camera::timesync_t last_timesync;
+  Furble::Camera *camera = target->getCamera();
+  const char *name = camera->getName().c_str();
 
-void control_update_gps(Furble::Camera::gps_t &gps, Furble::Camera::timesync_t &timesync) {
-  last_gps = gps;
-  last_timesync = timesync;
+  while (true) {
+    control_cmd_t cmd = target->getCommand();
+    switch (cmd) {
+      case CONTROL_CMD_SHUTTER_PRESS:
+        ESP_LOGI(LOG_TAG, "shutterPress(%s)", name);
+        camera->shutterPress();
+        break;
+      case CONTROL_CMD_SHUTTER_RELEASE:
+        ESP_LOGI(LOG_TAG, "shutterRelease(%s)", name);
+        camera->shutterRelease();
+        break;
+      case CONTROL_CMD_FOCUS_PRESS:
+        ESP_LOGI(LOG_TAG, "focusPress(%s)", name);
+        camera->focusPress();
+        break;
+      case CONTROL_CMD_FOCUS_RELEASE:
+        ESP_LOGI(LOG_TAG, "focusRelease(%s)", name);
+        camera->focusRelease();
+        break;
+      case CONTROL_CMD_GPS_UPDATE:
+        ESP_LOGI(LOG_TAG, "updateGeoData(%s)", name);
+        camera->updateGeoData(target->getGPS(), target->getTimesync());
+        break;
+      case CONTROL_CMD_ERROR:
+        // ignore continue
+        break;
+      case CONTROL_CMD_DISCONNECT:
+        goto task_exit;
+      default:
+        ESP_LOGE(LOG_TAG, "Invalid control command %d.", cmd);
+    }
+  }
+task_exit:
+  vTaskDelete(NULL);
+}
 
-  control_cmd_t cmd = CONTROL_CMD_GPS_UPDATE;
-  xQueueSend(queue, &cmd, sizeof(control_cmd_t));
+namespace Furble {
+Control::Target::Target(Camera *camera) {
+  m_Camera = camera;
+  m_Queue = xQueueCreate(TARGET_CMD_QUEUE_LEN, sizeof(control_cmd_t));
 }
 
-void control_task(void *param) {
-  queue = static_cast<QueueHandle_t>(param);
+Control::Target::~Target() {
+  vQueueDelete(m_Queue);
+  m_Queue = NULL;
+  m_Camera->disconnect();
+  m_Camera = NULL;
+}
+
+Camera *Control::Target::getCamera(void) {
+  return m_Camera;
+}
+
+void Control::Target::sendCommand(control_cmd_t cmd) {
+  BaseType_t ret = xQueueSend(m_Queue, &cmd, 0);
+  if (ret != pdTRUE) {
+    ESP_LOGE(LOG_TAG, "Failed to send command to target.");
+  }
+}
+
+control_cmd_t Control::Target::getCommand(void) {
+  control_cmd_t cmd = CONTROL_CMD_ERROR;
+  BaseType_t ret = xQueueReceive(m_Queue, &cmd, pdMS_TO_TICKS(50));
+  if (ret != pdTRUE) {
+    return CONTROL_CMD_ERROR;
+  }
+  return cmd;
+}
 
+const Camera::gps_t &Control::Target::getGPS(void) {
+  return m_GPS;
+}
+
+const Camera::timesync_t &Control::Target::getTimesync(void) {
+  return m_Timesync;
+}
+
+void Control::Target::updateGPS(Camera::gps_t &gps, Camera::timesync_t &timesync) {
+  m_GPS = gps;
+  m_Timesync = timesync;
+}
+
+Control::Control(void) {
+  m_Queue = xQueueCreate(CONTROL_CMD_QUEUE_LEN, sizeof(control_cmd_t));
+  if (m_Queue == NULL) {
+    ESP_LOGE(LOG_TAG, "Failed to create control queue.");
+    abort();
+  }
+}
+
+Control::~Control() {
+  vQueueDelete(m_Queue);
+  m_Queue = NULL;
+}
+
+void Control::task(void) {
   while (true) {
     control_cmd_t cmd;
-    BaseType_t ret = xQueueReceive(queue, &cmd, pdMS_TO_TICKS(50));
+    BaseType_t ret = xQueueReceive(m_Queue, &cmd, pdMS_TO_TICKS(50));
     if (ret == pdTRUE) {
-      for (size_t n = 0; n < Furble::CameraList::size(); n++) {
-        Furble::Camera *camera = Furble::CameraList::get(n);
-        if (!camera->isActive()) {
-          continue;
-        }
-
+      for (const auto &target : m_Targets) {
         switch (cmd) {
           case CONTROL_CMD_SHUTTER_PRESS:
-            camera->shutterPress();
-            ESP_LOGI(LOG_TAG, "shutterPress()");
-            break;
           case CONTROL_CMD_SHUTTER_RELEASE:
-            ESP_LOGI(LOG_TAG, "shutterRelease()");
-            camera->shutterRelease();
-            break;
           case CONTROL_CMD_FOCUS_PRESS:
-            ESP_LOGI(LOG_TAG, "focusPress()");
-            camera->focusPress();
-            break;
           case CONTROL_CMD_FOCUS_RELEASE:
-            ESP_LOGI(LOG_TAG, "focusRelease()");
-            camera->focusRelease();
-            break;
           case CONTROL_CMD_GPS_UPDATE:
-            ESP_LOGI(LOG_TAG, "updateGeoData()");
-            camera->updateGeoData(last_gps, last_timesync);
+            target->sendCommand(cmd);
             break;
           default:
             ESP_LOGE(LOG_TAG, "Invalid control command %d.", cmd);
+            break;
         }
       }
     }
   }
 }
+
+BaseType_t Control::sendCommand(control_cmd_t cmd) {
+  return xQueueSend(m_Queue, &cmd, 0);
+}
+
+BaseType_t Control::updateGPS(Camera::gps_t &gps, Camera::timesync_t &timesync) {
+  for (const auto &target : m_Targets) {
+    target->updateGPS(gps, timesync);
+  }
+
+  control_cmd_t cmd = CONTROL_CMD_GPS_UPDATE;
+  return xQueueSend(m_Queue, &cmd, 0);
+}
+
+bool Control::isConnected(void) {
+  for (const auto &target : m_Targets) {
+    if (!target->getCamera()->isConnected()) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+const std::vector<std::unique_ptr<Control::Target>> &Control::getTargets(void) {
+  return m_Targets;
+}
+
+void Control::disconnect(void) {
+  for (const auto &target : m_Targets) {
+    target->sendCommand(CONTROL_CMD_DISCONNECT);
+  }
+
+  m_Targets.clear();
+}
+
+void Control::addActive(Camera *camera) {
+  auto target = std::unique_ptr<Control::Target>(new Control::Target(camera));
+
+  // Create per-target task that will self-delete on disconnect
+  BaseType_t ret = xTaskCreatePinnedToCore(target_task, camera->getName().c_str(), 4096,
+                                           target.get(), 3, NULL, 1);
+  if (ret != pdPASS) {
+    ESP_LOGE(LOG_TAG, "Failed to create task for '%s'.", camera->getName());
+  } else {
+    m_Targets.push_back(std::move(target));
+  }
+}
+
+};  // namespace Furble
+
+void control_task(void *param) {
+  Furble::Control *control = static_cast<Furble::Control *>(param);
+
+  control->task();
+}
diff --git a/src/furble_gps.cpp b/src/furble_gps.cpp
index 2a979b7..9b2d30f 100644
--- a/src/furble_gps.cpp
+++ b/src/furble_gps.cpp
@@ -29,7 +29,7 @@ bool furble_gps_enable = false;
 /**
  * GPS serial event service handler.
  */
-static uint16_t service_grove_gps(void *private_data) {
+static uint16_t service_grove_gps(void *context) {
   if (!furble_gps_enable) {
     return GPS_SERVICE_MS;
   }
@@ -52,7 +52,7 @@ static uint16_t service_grove_gps(void *private_data) {
 /**
  * Update geotag data.
  */
-void furble_gps_update(Furble::Camera *camera) {
+void furble_gps_update(Furble::Control *control) {
   if (!furble_gps_enable) {
     return;
   }
@@ -60,13 +60,17 @@ void furble_gps_update(Furble::Camera *camera) {
   if (furble_gps.location.isUpdated() && furble_gps.location.isValid()
       && furble_gps.date.isUpdated() && furble_gps.date.isValid() && furble_gps.time.isValid()
       && furble_gps.time.isValid()) {
-    Furble::Camera::gps_t dgps = {furble_gps.location.lat(), furble_gps.location.lng(),
-                                  furble_gps.altitude.meters()};
-    Furble::Camera::timesync_t timesync = {furble_gps.date.year(),   furble_gps.date.month(),
-                                           furble_gps.date.day(),    furble_gps.time.hour(),
-                                           furble_gps.time.minute(), furble_gps.time.second()};
-
-    control_update_gps(dgps, timesync);
+    Furble::Camera::gps_t dgps = {
+        furble_gps.location.lat(),
+        furble_gps.location.lng(),
+        furble_gps.altitude.meters(),
+    };
+    Furble::Camera::timesync_t timesync = {
+        furble_gps.date.year(), furble_gps.date.month(),  furble_gps.date.day(),
+        furble_gps.time.hour(), furble_gps.time.minute(), furble_gps.time.second(),
+    };
+
+    control->updateGPS(dgps, timesync);
     ez.header.draw("gps");
   }
 }
@@ -101,12 +105,11 @@ static void current_draw_widget(uint16_t x, uint16_t y) {
   M5.Lcd.setTextDatum(TL_DATUM);
   int32_t ma = M5.Power.getBatteryCurrent();
   ESP_LOGI(LOG_TAG, "%d", ma);
-  char s[32] = {0};
-  snprintf(s, 32, "%d", ma);
-  M5.Lcd.drawString(s, x + ez.theme->header_hmargin, ez.theme->header_tmargin + 2);
+  M5.Lcd.drawString(std::to_string(ma).c_str(), x + ez.theme->header_hmargin,
+                    ez.theme->header_tmargin + 2);
 }
 
-static uint16_t current_service(void *private_data) {
+static uint16_t current_service(void *context) {
   ez.header.draw("current");
   return 1000;
 }
diff --git a/src/interval.cpp b/src/interval.cpp
index c6385a5..82508da 100644
--- a/src/interval.cpp
+++ b/src/interval.cpp
@@ -69,7 +69,7 @@ static void display_interval_msg(interval_state_t state,
 }
 
 static void do_interval(FurbleCtx *fctx, interval_t *interval) {
-  auto camera = fctx->camera;
+  auto control = fctx->control;
   const unsigned long config_delay = sv2ms(&interval->delay);
   const unsigned long config_shutter = sv2ms(&interval->shutter);
 
@@ -95,8 +95,7 @@ static void do_interval(FurbleCtx *fctx, interval_t *interval) {
     switch (state) {
       case INTERVAL_SHUTTER_OPEN:
         if ((icount < interval->count.value) || (interval->count.unit == SPIN_UNIT_INF)) {
-          // ESP_LOGI(LOG_TAG, "Shutter Open");
-          camera->shutterPress();
+          control->sendCommand(CONTROL_CMD_SHUTTER_PRESS);
           next = now + config_shutter;
           state = INTERVAL_SHUTTER_WAIT;
         } else {
@@ -110,8 +109,7 @@ static void do_interval(FurbleCtx *fctx, interval_t *interval) {
         break;
       case INTERVAL_SHUTTER_CLOSE:
         icount++;
-        // ESP_LOGI(LOG_TAG, "Shutter Release");
-        camera->shutterRelease();
+        control->sendCommand(CONTROL_CMD_SHUTTER_RELEASE);
         next = now + config_delay;
         if ((icount < interval->count.value) || (interval->count.unit == SPIN_UNIT_INF)) {
           state = INTERVAL_DELAY;
@@ -131,22 +129,25 @@ static void do_interval(FurbleCtx *fctx, interval_t *interval) {
 
     if (M5.BtnB.wasClicked()) {
       if (state == INTERVAL_SHUTTER_WAIT) {
-        // ESP_LOGI(LOG_TAG, "Shutter Release");
-        camera->shutterRelease();
+        control->sendCommand(CONTROL_CMD_SHUTTER_RELEASE);
       }
       active = false;
     }
 
     display_interval_msg(state, icount, &interval->count, now, next);
-  } while (active && camera->isConnected());
+  } while (active && control->isConnected());
   ez.backlight.inactivity(USER_SET);
 }
 
 void remote_interval(FurbleCtx *fctx) {
+  interval_t interval;
+
+  settings_load_interval(&interval);
+
   ezMenu submenu(FURBLE_STR " - Interval");
   submenu.buttons({"OK", "down"});
   submenu.addItem("Start");
-  settings_add_interval_items(&submenu);
+  settings_add_interval_items(&submenu, &interval);
   submenu.addItem("Back");
   submenu.downOnLast("first");
 
diff --git a/src/main.cpp b/src/main.cpp
index 6e8ce4d..51d058d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -17,22 +17,19 @@ void setup() {
 
   Serial.begin(115200);
 
-  QueueHandle_t queue = xQueueCreate(CONTROL_CMD_QUEUE_LEN, sizeof(control_cmd_t));
-  if (queue == NULL) {
-    ESP_LOGE(LOG_TAG, "Failed to create control queue.");
-    abort();
-  }
-
   Furble::Device::init();
   Furble::Scan::init(settings_load_esp_tx_power());
 
-  xRet = xTaskCreatePinnedToCore(control_task, "control", 8192, queue, 3, &xControlHandle, 1);
+  Furble::Control *control = new Furble::Control();
+
+  xRet = xTaskCreatePinnedToCore(control_task, "control", 8192, control, 4, &xControlHandle, 1);
   if (xRet != pdPASS) {
     ESP_LOGE(LOG_TAG, "Failed to create control task.");
     abort();
   }
 
-  xRet = xTaskCreatePinnedToCore(vUITask, "UI-M5ez", 32768, queue, 2, &xUIHandle, 1);
+  // Pin UI to same core (0) as NimBLE
+  xRet = xTaskCreatePinnedToCore(vUITask, "UI-M5ez", 32768, control, 2, &xUIHandle, 0);
   if (xRet != pdPASS) {
     ESP_LOGE(LOG_TAG, "Failed to create UI task.");
     abort();
diff --git a/src/settings.cpp b/src/settings.cpp
index 632ffb3..f9e3a74 100644
--- a/src/settings.cpp
+++ b/src/settings.cpp
@@ -13,11 +13,7 @@
 const char *PREFS_TX_POWER = "txpower";
 const char *PREFS_GPS = "gps";
 const char *PREFS_INTERVAL = "interval";
-
-/**
- * Global intervalometer configuration.
- */
-interval_t interval;
+const char *PREFS_MULTICONNECT = "multiconnect";
 
 /**
  * Save BLE transmit power to preferences.
@@ -151,10 +147,11 @@ static void settings_save_gps(bool enable) {
   prefs.end();
 }
 
-bool settings_gps_onoff(ezMenu *menu) {
-  furble_gps_enable = !furble_gps_enable;
-  menu->setCaption("onoff", std::string("GPS\t") + (furble_gps_enable ? "ON" : "OFF"));
-  settings_save_gps(furble_gps_enable);
+bool settings_gps_onoff(ezMenu *menu, void *context) {
+  bool *gps_enable = static_cast<bool *>(context);
+  *gps_enable = !*gps_enable;
+  menu->setCaption("onoff", std::string("GPS\t") + (gps_enable ? "ON" : "OFF"));
+  settings_save_gps(gps_enable);
 
   return true;
 }
@@ -167,7 +164,7 @@ void settings_menu_gps(void) {
 
   submenu.buttons({"OK", "down"});
   submenu.addItem("onoff", std::string("GPS\t") + (furble_gps_enable ? "ON" : "OFF"), NULL,
-                  settings_gps_onoff);
+                  &furble_gps_enable, settings_gps_onoff);
   submenu.addItem("GPS Data", "", show_gps_info);
   submenu.downOnLast("first");
   submenu.addItem("Back");
@@ -200,7 +197,9 @@ void settings_save_interval(interval_t *interval) {
   prefs.end();
 }
 
-static bool configure_count(ezMenu *menu) {
+static bool configure_count(ezMenu *menu, void *context) {
+  interval_t *interval = (interval_t *)context;
+
   ezMenu submenu("Count");
   submenu.buttons({"OK", "down"});
   submenu.addItem("Custom");
@@ -209,26 +208,28 @@ static bool configure_count(ezMenu *menu) {
 
   submenu.runOnce();
   if (submenu.pickName() == "Custom") {
-    interval.count.unit = SPIN_UNIT_NIL;
-    spinner_modify_value("Count", false, &interval.count);
+    interval->count.unit = SPIN_UNIT_NIL;
+    spinner_modify_value("Count", false, &interval->count);
   }
 
   if (submenu.pickName() == "Infinite") {
-    interval.count.unit = SPIN_UNIT_INF;
+    interval->count.unit = SPIN_UNIT_INF;
   }
 
-  std::string countstr = sv2str(&interval.count);
-  if (interval.count.unit == SPIN_UNIT_INF) {
+  std::string countstr = sv2str(&interval->count);
+  if (interval->count.unit == SPIN_UNIT_INF) {
     countstr = "INF";
   }
 
   menu->setCaption("interval_count", std::string("Count\t") + countstr);
-  settings_save_interval(&interval);
+  settings_save_interval(interval);
 
   return true;
 }
 
-static bool configure_delay(ezMenu *menu) {
+static bool configure_delay(ezMenu *menu, void *context) {
+  interval_t *interval = (interval_t *)context;
+
   ezMenu submenu("Delay");
   submenu.buttons({"OK", "down"});
   submenu.addItem("Custom");
@@ -245,14 +246,16 @@ static bool configure_delay(ezMenu *menu) {
     preset = true;
   }
 
-  spinner_modify_value("Delay", preset, &interval.delay);
-  menu->setCaption("interval_delay", "Delay\t" + sv2str(&interval.delay));
-  settings_save_interval(&interval);
+  spinner_modify_value("Delay", preset, &interval->delay);
+  menu->setCaption("interval_delay", "Delay\t" + sv2str(&interval->delay));
+  settings_save_interval(interval);
 
   return true;
 }
 
-static bool configure_shutter(ezMenu *menu) {
+static bool configure_shutter(ezMenu *menu, void *context) {
+  interval_t *interval = (interval_t *)context;
+
   ezMenu submenu("Shutter");
   submenu.buttons({"OK", "down"});
   submenu.addItem("Custom");
@@ -269,29 +272,49 @@ static bool configure_shutter(ezMenu *menu) {
     preset = true;
   }
 
-  spinner_modify_value("Shutter", preset, &interval.shutter);
-  menu->setCaption("interval_shutter", "Shutter\t" + sv2str(&interval.shutter));
-  settings_save_interval(&interval);
+  spinner_modify_value("Shutter", preset, &interval->shutter);
+  menu->setCaption("interval_shutter", "Shutter\t" + sv2str(&interval->shutter));
+  settings_save_interval(interval);
 
   return true;
 }
 
-void settings_add_interval_items(ezMenu *submenu) {
-  settings_load_interval(&interval);
-
-  submenu->addItem("interval_count", std::string("Count\t") + sv2str(&interval.count), NULL,
-                   configure_count);
-  submenu->addItem("interval_delay", std::string("Delay\t") + sv2str(&interval.delay), NULL,
-                   configure_delay);
-  submenu->addItem("interval_shutter", std::string("Shutter\t") + sv2str(&interval.shutter), NULL,
-                   configure_shutter);
+void settings_add_interval_items(ezMenu *submenu, interval_t *interval) {
+  submenu->addItem("interval_count", std::string("Count\t") + sv2str(&interval->count), NULL,
+                   interval, configure_count);
+  submenu->addItem("interval_delay", std::string("Delay\t") + sv2str(&interval->delay), NULL,
+                   interval, configure_delay);
+  submenu->addItem("interval_shutter", std::string("Shutter\t") + sv2str(&interval->shutter), NULL,
+                   interval, configure_shutter);
 }
 
 void settings_menu_interval(void) {
+  interval_t interval;
+
+  settings_load_interval(&interval);
+
   ezMenu submenu(FURBLE_STR " - Intervalometer settings");
   submenu.buttons({"OK", "down"});
-  settings_add_interval_items(&submenu);
+  settings_add_interval_items(&submenu, &interval);
   submenu.addItem("Back");
   submenu.downOnLast("first");
   submenu.run();
 }
+
+bool settings_load_multiconnect(void) {
+  Preferences prefs;
+
+  prefs.begin(FURBLE_STR, true);
+  bool multiconnect = prefs.getBool(PREFS_MULTICONNECT, false);
+  prefs.end();
+
+  return multiconnect;
+}
+
+void settings_save_multiconnect(bool multiconnect) {
+  Preferences prefs;
+
+  prefs.begin(FURBLE_STR, false);
+  prefs.putBool(PREFS_MULTICONNECT, multiconnect);
+  prefs.end();
+}
diff --git a/src/spinner.cpp b/src/spinner.cpp
index 7975089..6971515 100644
--- a/src/spinner.cpp
+++ b/src/spinner.cpp
@@ -3,37 +3,34 @@
 
 #include "spinner.h"
 
-static const char *unit2str[5] = {"    ",   // SPIN_UNIT_NIL
-                                  "    ",   // SPIN_UNIT_INF
-                                  "msec",   // SPIN_UNIT_MS
-                                  "secs",   // SPIN_UNIT_SEC
-                                  "mins"};  // SPIN_UNIT_MIN
+static constexpr std::array<const char *, 5> unit2str = {
+    "    ",  // SPIN_UNIT_NIL
+    "    ",  // SPIN_UNIT_INF
+    "msec",  // SPIN_UNIT_MS
+    "secs",  // SPIN_UNIT_SEC
+    "mins"   // SPIN_UNIT_MIN
+};
 
-#define PRESET_NUM 10
-static const std::array<uint16_t, PRESET_NUM> spin_preset = {1, 2, 4, 8, 15, 30, 60, 125, 250, 500};
+static constexpr std::array<uint16_t, 10> spin_preset = {1, 2, 4, 8, 15, 30, 60, 125, 250, 500};
 
-#define FMT_NONE_LEN (4)
-static const std::array<const char *, FMT_NONE_LEN> fmt_none = {
+static constexpr std::array<const char *, 4> fmt_none = {
     " %1u  %1u  %1u ",
     "[%1u] %1u  %1u ",
     " %1u [%1u] %1u ",
     " %1u  %1u [%1u]",
 };
 
-#define FMT_UNIT_LEN (5)
-static const std::array<const char *, FMT_UNIT_LEN> fmt_unit = {
+static constexpr std::array<const char *, 5> fmt_unit = {
     " %1u  %1u  %1u  %4s ", "[%1u] %1u  %1u  %4s ", " %1u [%1u] %1u  %4s ",
     " %1u  %1u [%1u] %4s ", " %1u  %1u  %1u [%4s]",
 };
 
-#define FMT_PRESET_NONE_LEN (2)
-static const std::array<const char *, FMT_PRESET_NONE_LEN> fmt_preset_none = {
+static constexpr std::array<const char *, 2> fmt_preset_none = {
     " %1u  %1u  %1u ",
     "[%1u  %1u  %1u]",
 };
 
-#define FMT_PRESET_UNIT_LEN (3)
-static const std::array<const char *, FMT_PRESET_UNIT_LEN> fmt_preset_unit = {
+static constexpr std::array<const char *, 3> fmt_preset_unit = {
     " %1u  %1u  %1u  %4s ",
     "[%1u  %1u  %1u] %4s ",
     " %1u  %1u  %1u [%4s]",
@@ -235,11 +232,11 @@ void spinner_modify_value(const char *title, bool preset, SpinValue *sv) {
   }
 }
 
-std::string sv2str(SpinValue *sv) {
+std::string sv2str(const SpinValue *sv) {
   return std::to_string(sv->value) + unit2str[sv->unit];
 }
 
-unsigned long sv2ms(SpinValue *sv) {
+unsigned long sv2ms(const SpinValue *sv) {
   switch (sv->unit) {
     case SPIN_UNIT_MIN:
       return (sv->value * 60 * 1000);