Fix GPS geotag support for different firmware versions. (#96)

* Use consistent UUID for geotag service identifiers.

Reading the spec, the 16-bit handle is only consistent per device
instance.
Thus, it can change between devices and firmware versions.
The full UUID is guaranteed to be consistent, so use that instead.

* Remove useless m_GeoDataValid checks.

* Refactor Fujifilm geotag handling.

* Add basic error handling in Fujifilm geotagging.

* Migrate from std::bind to lambda for notification callbacks.

* Minor refactor of GPS data loop.

* Remove redundant M5.update() in shutter loop.

ez.yield() already calls M5.update().
Remove the delay(50), this buys us nothing (except UI latency).

* Remove remaining String references in lib/furble.

lib/furble/CameraList.cpp

@@ -41,12 +41,12 @@ static std::vector<index_entry_t> load_index(void) {
   if (bytes > 0 && (bytes % sizeof(index_entry_t) == 0)) {
     uint8_t buffer[bytes] = {0};
     size_t count = bytes / sizeof(index_entry_t);
-    Serial.println("Index entries: " + String(count));
+    Serial.printf("Index entries: %d\r\n", count);
     m_Prefs.getBytes(FURBLE_PREF_INDEX, buffer, bytes);
     index_entry_t *entry = (index_entry_t *)buffer;
 
     for (int i = 0; i < count; i++) {
-      Serial.println("Loading index entry: " + String(entry[i].name));
+      Serial.printf("Loading index entry: %s\r\n", entry[i].name);
       index.push_back(entry[i]);
     }
   }
@@ -57,7 +57,7 @@ static std::vector<index_entry_t> load_index(void) {
 static void add_index(std::vector<index_entry_t> &index, index_entry_t &entry) {
   bool exists = false;
   for (size_t i = 0; i < index.size(); i++) {
-    Serial.println("[" + String(i) + "] " + String(index[i].name) + " : " + String(entry.name));
+    Serial.printf("[%d] %s : %s\r\n", i, index[i].name, entry.name);
     if (strcmp(index[i].name, entry.name) == 0) {
       Serial.println("Overwriting existing entry");
       index[i] = entry;
@@ -87,10 +87,9 @@ void CameraList::save(Camera *pCamera) {
   if (pCamera->serialise(dbuffer, dbytes)) {
     // Store the entry and the index if serialisation succeeds
     m_Prefs.putBytes(entry.name, dbuffer, dbytes);
-    Serial.println("Saved " + String(entry.name));
+    Serial.printf("Saved %s\r\n", entry.name);
     save_index(index);
-    Serial.print("Index entries: ");
-    Serial.println(index.size());
+    Serial.printf("Index entries: %d\r\n", index.size());
   }
 
   m_Prefs.end();

lib/furble/CanonEOS.cpp

@@ -22,8 +22,8 @@ CanonEOS::CanonEOS(const void *data, size_t len) {
 CanonEOS::CanonEOS(NimBLEAdvertisedDevice *pDevice) {
   m_Name = pDevice->getName();
   m_Address = pDevice->getAddress();
-  Serial.println("Name = " + String(m_Name.c_str()));
-  Serial.println("Address = " + String(m_Address.toString().c_str()));
+  Serial.printf("Name = %s\r\n", m_Name.c_str());
+  Serial.printf("Address = %s\r\n", m_Address.toString().c_str());
   Device::getUUID128(&m_Uuid);
 }
 

lib/furble/Fujifilm.cpp

@@ -47,8 +47,7 @@ static const NimBLEUUID FUJIFILM_SVC_GEOTAG_UUID =
 static const NimBLEUUID FUJIFILM_CHR_GEOTAG_UUID =
     NimBLEUUID("0f36ec14-29e5-411a-a1b6-64ee8383f090");
 
-static const uint16_t FUJIFILM_CHR_CONFIGURE = 0x5022;
-static const uint16_t FUJIFILM_GEOTAG_UPDATE = 0x5042;
+static const NimBLEUUID FUJIFILM_GEOTAG_UPDATE = NimBLEUUID("ad06c7b7-f41a-46f4-a29a-712055319122");
 
 static const uint8_t FUJIFILM_SHUTTER_CMD[2] = {0x01, 0x00};
 static const uint8_t FUJIFILM_SHUTTER_PRESS[2] = {0x02, 0x00};
@@ -58,35 +57,28 @@ static const uint8_t FUJIFILM_SHUTTER_FOCUS[2] = {0x03, 0x00};
 namespace Furble {
 
 static void print_token(const uint8_t *token) {
-  Serial.println("Token = " + String(token[0], HEX) + String(token[1], HEX) + String(token[2], HEX)
-                 + String(token[3], HEX));
+  Serial.printf("Token = %02x%02x%02x%02x\r\n", token[0], token[1], token[2], token[3]);
 }
 
 void Fujifilm::notify(BLERemoteCharacteristic *pChr, uint8_t *pData, size_t length, bool isNotify) {
-  Serial.printf("Got %s callback: %u bytes from 0x%04x\r\n",
-                isNotify ? "notification" : "indication", length, pChr->getHandle());
+  Serial.printf("Got %s callback: %u bytes from %s\r\n", isNotify ? "notification" : "indication",
+                length, pChr->getUUID().toString().c_str());
   if (length > 0) {
     for (int i = 0; i < length; i++) {
       Serial.printf("  [%d] 0x%02x\r\n", i, pData[i]);
     }
   }
 
-  switch (pChr->getHandle()) {
-    case FUJIFILM_CHR_CONFIGURE:
-      if ((length >= 2) && (pData[0] == 0x02) && (pData[1] == 0x00)) {
-        m_Configured = true;
-      }
-      break;
-
-    case FUJIFILM_GEOTAG_UPDATE:
-      if ((length >= 2) && (pData[0] == 0x01) && (pData[1] == 0x00) && m_GeoDataValid) {
-        m_GeoRequested = true;
-      }
-      break;
-
-    default:
-      Serial.println("Unhandled notification handle.");
-      break;
+  if (pChr->getUUID() == FUJIFILM_CHR_NOT1_UUID) {
+    if ((length >= 2) && (pData[0] == 0x02) && (pData[1] == 0x00)) {
+      m_Configured = true;
+    }
+  } else if (pChr->getUUID() == FUJIFILM_GEOTAG_UPDATE) {
+    if ((length >= 2) && (pData[0] == 0x01) && (pData[1] == 0x00)) {
+      m_GeoRequested = true;
+    }
+  } else {
+    Serial.println("Unhandled notification handle.");
   }
 }
 
@@ -108,8 +100,8 @@ Fujifilm::Fujifilm(NimBLEAdvertisedDevice *pDevice) {
   m_Token[1] = data[4];
   m_Token[2] = data[5];
   m_Token[3] = data[6];
-  Serial.println("Name = " + String(m_Name.c_str()));
-  Serial.println("Address = " + String(m_Address.toString().c_str()));
+  Serial.printf("Name = %s\r\n", m_Name.c_str());
+  Serial.printf("Address = %s\r\n", m_Address.toString().c_str());
   print_token(m_Token);
 }
 
@@ -188,11 +180,21 @@ bool Fujifilm::connect(progressFunc pFunc, void *pCtx) {
   pSvc = m_Client->getService(FUJIFILM_SVC_CONF_UUID);
   // indications
   pSvc->getCharacteristic(FUJIFILM_CHR_IND1_UUID)
-      ->subscribe(false, std::bind(&Fujifilm::notify, this, _1, _2, _3, _4), true);
+      ->subscribe(
+          false,
+          [this](BLERemoteCharacteristic *pChr, uint8_t *pData, size_t length, bool isNotify) {
+            this->notify(pChr, pData, length, isNotify);
+          },
+          true);
   updateProgress(pFunc, pCtx, 50.0f);
 
   pSvc->getCharacteristic(FUJIFILM_CHR_IND2_UUID)
-      ->subscribe(false, std::bind(&Fujifilm::notify, this, _1, _2, _3, _4), true);
+      ->subscribe(
+          false,
+          [this](BLERemoteCharacteristic *pChr, uint8_t *pData, size_t length, bool isNotify) {
+            this->notify(pChr, pData, length, isNotify);
+          },
+          true);
 
   // wait for up to 5000ms callback
   for (unsigned int i = 0; i < 5000; i += 100) {
@@ -206,28 +208,30 @@ bool Fujifilm::connect(progressFunc pFunc, void *pCtx) {
   updateProgress(pFunc, pCtx, 60.0f);
   // notifications
   pSvc->getCharacteristic(FUJIFILM_CHR_NOT1_UUID)
-      ->subscribe(true, std::bind(&Fujifilm::notify, this, _1, _2, _3, _4), true);
+      ->subscribe(
+          true,
+          [this](BLERemoteCharacteristic *pChr, uint8_t *pData, size_t length, bool isNotify) {
+            this->notify(pChr, pData, length, isNotify);
+          },
+          true);
 
   updateProgress(pFunc, pCtx, 70.0f);
   pSvc->getCharacteristic(FUJIFILM_CHR_NOT2_UUID)
-      ->subscribe(true, std::bind(&Fujifilm::notify, this, _1, _2, _3, _4), true);
+      ->subscribe(
+          true,
+          [this](BLERemoteCharacteristic *pChr, uint8_t *pData, size_t length, bool isNotify) {
+            this->notify(pChr, pData, length, isNotify);
+          },
+          true);
 
   updateProgress(pFunc, pCtx, 80.0f);
   pSvc->getCharacteristic(FUJIFILM_CHR_IND3_UUID)
-      ->subscribe(false, std::bind(&Fujifilm::notify, this, _1, _2, _3, _4), true);
-
-  updateProgress(pFunc, pCtx, 90.0f);
-  // wait for up to 5000ms for geotag request
-  for (unsigned int i = 0; i < 5000; i += 100) {
-    if (m_GeoRequested) {
-      sendGeoData();
-      m_GeoRequested = false;
-      break;
-    }
-    updateProgress(pFunc, pCtx, 90.0f + (((float)i / 5000.0f) * 10.0f));
-    delay(100);
-  }
-
+      ->subscribe(
+          false,
+          [this](BLERemoteCharacteristic *pChr, uint8_t *pData, size_t length, bool isNotify) {
+            this->notify(pChr, pData, length, isNotify);
+          },
+          true);
   Serial.println("Configured");
 
   updateProgress(pFunc, pCtx, 100.0f);
@@ -260,52 +264,52 @@ void Fujifilm::focusRelease(void) {
   shutterRelease();
 }
 
-void Fujifilm::sendGeoData(void) {
+void Fujifilm::sendGeoData(gps_t &gps, timesync_t &timesync) {
   NimBLERemoteService *pSvc = m_Client->getService(FUJIFILM_SVC_GEOTAG_UUID);
-  NimBLERemoteCharacteristic *pChr = pSvc->getCharacteristic(FUJIFILM_CHR_GEOTAG_UUID);
+  if (pSvc == nullptr) {
+    return;
+  }
 
-  geotag_t geotag = {.latitude = (int32_t)(m_GPS.latitude * 10000000),
-                     .longitude = (int32_t)(m_GPS.longitude * 10000000),
-                     .altitude = (int32_t)m_GPS.altitude,
-                     .pad = {0},
-                     .gps_time = {
-                         .year = (uint16_t)m_TimeSync.year,
-                         .day = (uint8_t)m_TimeSync.day,
-                         .month = (uint8_t)m_TimeSync.month,
-                         .hour = (uint8_t)m_TimeSync.hour,
-                         .minute = (uint8_t)m_TimeSync.minute,
-                         .second = (uint8_t)m_TimeSync.second,
-                     }};
+  NimBLERemoteCharacteristic *pChr = pSvc->getCharacteristic(FUJIFILM_CHR_GEOTAG_UUID);
+  if (pChr == nullptr) {
+    return;
+  }
 
   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,
+                       }};
+
     Serial.printf("Sending geotag data (%u bytes) to 0x%04x\r\n", sizeof(geotag),
                   pChr->getHandle());
-    Serial.printf("  lat: %f, %d\r\n", m_GPS.latitude, geotag.latitude);
-    Serial.printf("  lon: %f, %d\r\n", m_GPS.longitude, geotag.longitude);
-    Serial.printf("  alt: %f, %d\r\n", m_GPS.altitude, geotag.altitude);
+    Serial.printf("  lat: %f, %d\r\n", gps.latitude, geotag.latitude);
+    Serial.printf("  lon: %f, %d\r\n", gps.longitude, geotag.longitude);
+    Serial.printf("  alt: %f, %d\r\n", gps.altitude, geotag.altitude);
 
     pChr->writeValue((uint8_t *)&geotag, sizeof(geotag), true);
   }
 }
 
 void Fujifilm::updateGeoData(gps_t &gps, timesync_t &timesync) {
-  m_GPS = gps;
-  m_TimeSync = timesync;
-  m_GeoDataValid = true;
-
   if (m_GeoRequested) {
-    sendGeoData();
+    sendGeoData(gps, timesync);
     m_GeoRequested = false;
   }
 }
 
 void Fujifilm::print(void) {
-  Serial.print("Name: ");
-  Serial.println(m_Name.c_str());
-  Serial.print("Address: ");
-  Serial.println(m_Address.toString().c_str());
-  Serial.print("Type: ");
-  Serial.println(m_Address.getType());
+  Serial.printf("Name: %s\r\n", m_Name.c_str());
+  Serial.printf("Address: %s\r\n", m_Address.toString().c_str());
+  Serial.printf("Type: %d\r\n", m_Address.getType());
 }
 
 void Fujifilm::disconnect(void) {

lib/furble/Fujifilm.h

@@ -59,16 +59,12 @@ class Fujifilm: public Camera {
   size_t getSerialisedBytes(void);
   bool serialise(void *buffer, size_t bytes);
   void notify(NimBLERemoteCharacteristic *, uint8_t *, size_t, bool);
-  void sendGeoData();
+  void sendGeoData(gps_t &gps, timesync_t &timesync);
 
   uint8_t m_Token[FUJIFILM_TOKEN_LEN] = {0};
 
   bool m_Configured = false;
 
-  bool m_GeoDataValid = false;
-  gps_t m_GPS = {0};
-  timesync_t m_TimeSync = {0};
-
   volatile bool m_GeoRequested = false;
 };
 

src/furble.cpp

@@ -76,7 +76,7 @@ static void remote_control(FurbleCtx *fctx) {
   show_shutter_control(false, 0);
 
   do {
-    M5.update();
+    ez.yield();
 
     furble_gps_update_geodata(camera);
 
@@ -137,9 +137,6 @@ static void remote_control(FurbleCtx *fctx) {
         continue;
       }
     }
-
-    ez.yield();
-    delay(50);
   } while (camera->isConnected());
 }
 

src/interval.cpp

@@ -1,6 +1,7 @@
 #include <Furble.h>
 #include <M5ez.h>
 
+#include "furble_gps.h"
 #include "furble_ui.h"
 #include "interval.h"
 #include "settings.h"
@@ -78,8 +79,9 @@ static void do_interval(FurbleCtx *fctx, interval_t *interval) {
   ez.backlight.inactivity(NEVER);
 
   do {
+    ez.yield();
+    furble_gps_update_geodata(camera);
     now = millis();
-    M5.update();
 
     if (fctx->reconnected) {
       fctx->reconnected = false;
@@ -130,9 +132,7 @@ static void do_interval(FurbleCtx *fctx, interval_t *interval) {
       active = false;
     }
 
-    ez.yield();
     display_interval_msg(state, icount, &interval->count, now, next);
-    delay(10);
   } while (active && camera->isConnected());
   ez.backlight.inactivity(USER_SET);
 }

src/settings.cpp

@@ -96,6 +96,10 @@ static void show_gps_info(void) {
   bool first = true;
 
   do {
+    if (M5.BtnB.wasReleased()) {
+      break;
+    }
+
     bool updated = furble_gps.location.isUpdated() || furble_gps.date.isUpdated()
                    || furble_gps.time.isUpdated();
 
@@ -114,14 +118,7 @@ static void show_gps_info(void) {
       ez.msgBox("GPS Data", buffer, "Back", false);
     }
 
-    M5.update();
-
-    if (M5.BtnB.wasPressed()) {
-      break;
-    }
-
     ez.yield();
-    delay(100);
   } while (true);
 }