43 add gps location and time sync support (#53)

* Initial support for sending GPS sync data.

Trial runs show the camera sends 0x5042 every minute or so to request a
geo update.

Testing shows the NimBLE stack deadlocks if we try to send from the
notification callback handler. Thus, we set a variable and send the geo
update via the menu handling loop.

Set the GPS coordinates to Montevideo to test signed lat/long handling.
(This in-lieu of an actual GPS unit).
Hardcode the GPS reference time to Christmas day, 2024 at 12:34:56.

Correct the final characteristic write to be an indication, not a
notification.

* Refactor notification handling and various loops.

This _seems_ to be more reliable, but needs more testing.

* Hook up the M5 GPS unit via the Grove UART.

Add a settings menu entry to show real-time GPS information.
GPS is active as soon as system is started.
Must be in 'Remote Control' shutter menu for GPS data to be properly
updated.

* Add GPS enable setting.
Add header widget for GPS fix/nofix.

* Tweak font sizes to work on both M5StickC and M5StickC-Plus.

Additionally, redraw the GPS header widget on update.

* Fix clang-format errors.

---------

Co-authored-by: Guo-Rong Koh <[email protected]>

lib/M5ez/src/M5ez.cpp

@@ -2873,6 +2873,7 @@ void ezMenu::_drawItem(int16_t n, String text, bool selected) {
 	uint16_t fill_color;
 	ez.setFont(_font);
 	int16_t top_item_h = ez.canvas.top() + (ez.canvas.height() % _per_item_h) / 2;   // remainder of screen left over by last item not fitting split to center menu
+        int16_t menu_text_y = top_item_h + (n * _per_item_h) + (_per_item_h / 2) + (_per_item_h % 2 ? 1 : 0);
 	m5.lcd.setTextDatum(CL_DATUM);
 	if (selected) {
 		fill_color = ez.theme->menu_sel_bgcolor;
@@ -2883,10 +2884,10 @@ void ezMenu::_drawItem(int16_t n, String text, bool selected) {
 	}
 	text = ez.clipString(text, TFT_W - ez.theme->menu_lmargin - 2 * ez.theme->menu_item_hmargin - ez.theme->menu_rmargin);
 	m5.lcd.fillRoundRect(ez.theme->menu_lmargin, top_item_h + n * _per_item_h, TFT_W - ez.theme->menu_lmargin - ez.theme->menu_rmargin, _per_item_h, ez.theme->menu_item_radius, fill_color);
-	m5.lcd.drawString(ez.leftOf(text, "\t"), ez.theme->menu_lmargin + ez.theme->menu_item_hmargin, top_item_h + _per_item_h / 2 + n * _per_item_h + 1);
+	m5.lcd.drawString(ez.leftOf(text, "\t"), ez.theme->menu_lmargin + ez.theme->menu_item_hmargin, menu_text_y);
 	if (text.indexOf("\t") != -1) {
 		m5.lcd.setTextDatum(CR_DATUM);
-		m5.lcd.drawString(ez.rightOf(text, "\t"),  TFT_W - ez.theme->menu_rmargin - ez.theme->menu_item_hmargin, top_item_h + _per_item_h / 2 + n * _per_item_h - 2);
+		m5.lcd.drawString(ez.rightOf(text, "\t"),  TFT_W - ez.theme->menu_rmargin - ez.theme->menu_item_hmargin, menu_text_y);
 	}
 }
 

lib/furble/CanonEOS.cpp

@@ -204,6 +204,11 @@ void CanonEOS::focusRelease(void) {
   return;
 }
 
+void CanonEOS::updateGeoData(gps_t &gps, timesync_t &timesync) {
+  // do nothing
+  return;
+}
+
 void CanonEOS::disconnect(void) {
   m_Client->disconnect();
 }

lib/furble/CanonEOS.h

@@ -60,6 +60,7 @@ class CanonEOS: public Device {
   void shutterRelease(void);
   void focusPress(void);
   void focusRelease(void);
+  void updateGeoData(gps_t &gps, timesync_t &timesync);
   void disconnect(void);
   size_t getSerialisedBytes(void);
   bool serialise(void *buffer, size_t bytes);

lib/furble/Device.h

@@ -13,7 +13,7 @@ namespace Furble {
 class Device {
  public:
   /**
-   * UUID type
+   * UUID type.
    */
   typedef struct _uuid128_t {
     union {
@@ -22,6 +22,27 @@ class Device {
     };
   } uuid128_t;
 
+  /**
+   * GPS data type.
+   */
+  typedef struct _gps_t {
+    double latitude;
+    double longitude;
+    double altitude;
+  } gps_t;
+
+  /**
+   * Time synchronisation type.
+   */
+  typedef struct _timesync_t {
+    unsigned int year;
+    unsigned int month;
+    unsigned int day;
+    unsigned int hour;
+    unsigned int minute;
+    unsigned int second;
+  } timesync_t;
+
   /**
    * Connect to the target camera such that it is ready for shutter control.
    *
@@ -57,6 +78,11 @@ class Device {
    */
   virtual void focusRelease(void) = 0;
 
+  /**
+   * Update geotagging data.
+   */
+  virtual void updateGeoData(gps_t &gps, timesync_t &timesync) = 0;
+
   const char *getName(void);
   void save(void);
   void remove(void);

lib/furble/Fujifilm.cpp

@@ -14,25 +14,35 @@ typedef struct _fujifilm_t {
 } fujifilm_t;
 
 /** 0x4001 */
-static const char *FUJIFILM_SVC_PAIR_UUID = "91f1de68-dff6-466e-8b65-ff13b0f16fb8";
+static const NimBLEUUID FUJIFILM_SVC_PAIR_UUID = NimBLEUUID("91f1de68-dff6-466e-8b65-ff13b0f16fb8");
 /** 0x4042 */
-static const char *FUJIFILM_CHR_PAIR_UUID = "aba356eb-9633-4e60-b73f-f52516dbd671";
-static const char *FUJIFILM_CHR_IDEN_UUID = "85b9163e-62d1-49ff-a6f5-054b4630d4a1";
+static const NimBLEUUID FUJIFILM_CHR_PAIR_UUID = NimBLEUUID("aba356eb-9633-4e60-b73f-f52516dbd671");
+static const NimBLEUUID FUJIFILM_CHR_IDEN_UUID = NimBLEUUID("85b9163e-62d1-49ff-a6f5-054b4630d4a1");
 
 // Currently unused
 // static const char *FUJIFILM_SVC_READ_UUID =
 // "4e941240-d01d-46b9-a5ea-67636806830b"; static const char
 // *FUJIFILM_CHR_READ_UUID = "bf6dc9cf-3606-4ec9-a4c8-d77576e93ea4";
 
-static const char *FUJIFILM_SVC_CONF_UUID = "4c0020fe-f3b6-40de-acc9-77d129067b14";
-static const char *FUJIFILM_CHR_IND1_UUID = "a68e3f66-0fcc-4395-8d4c-aa980b5877fa";
-static const char *FUJIFILM_CHR_IND2_UUID = "bd17ba04-b76b-4892-a545-b73ba1f74dae";
-static const char *FUJIFILM_CHR_NOT1_UUID = "f9150137-5d40-4801-a8dc-f7fc5b01da50";
-static const char *FUJIFILM_CHR_NOT2_UUID = "ad06c7b7-f41a-46f4-a29a-712055319122";
-static const char *FUJIFILM_CHR_NOT3_UUID = "049ec406-ef75-4205-a390-08fe209c51f0";
+static const NimBLEUUID FUJIFILM_SVC_CONF_UUID = NimBLEUUID("4c0020fe-f3b6-40de-acc9-77d129067b14");
+static const NimBLEUUID FUJIFILM_CHR_IND1_UUID = NimBLEUUID("a68e3f66-0fcc-4395-8d4c-aa980b5877fa");
+static const NimBLEUUID FUJIFILM_CHR_IND2_UUID = NimBLEUUID("bd17ba04-b76b-4892-a545-b73ba1f74dae");
+static const NimBLEUUID FUJIFILM_CHR_NOT1_UUID = NimBLEUUID("f9150137-5d40-4801-a8dc-f7fc5b01da50");
+static const NimBLEUUID FUJIFILM_CHR_NOT2_UUID = NimBLEUUID("ad06c7b7-f41a-46f4-a29a-712055319122");
+static const NimBLEUUID FUJIFILM_CHR_IND3_UUID = NimBLEUUID("049ec406-ef75-4205-a390-08fe209c51f0");
 
-static const char *FUJIFILM_SVC_SHUTTER_UUID = "6514eb81-4e8f-458d-aa2a-e691336cdfac";
-static const char *FUJIFILM_CHR_SHUTTER_UUID = "7fcf49c6-4ff0-4777-a03d-1a79166af7a8";
+static const NimBLEUUID FUJIFILM_SVC_SHUTTER_UUID =
+    NimBLEUUID("6514eb81-4e8f-458d-aa2a-e691336cdfac");
+static const NimBLEUUID FUJIFILM_CHR_SHUTTER_UUID =
+    NimBLEUUID("7fcf49c6-4ff0-4777-a03d-1a79166af7a8");
+
+static const NimBLEUUID FUJIFILM_SVC_GEOTAG_UUID =
+    NimBLEUUID("3b46ec2b-48ba-41fd-b1b8-ed860b60d22b");
+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 uint8_t FUJIFILM_SHUTTER_CMD[2] = {0x01, 0x00};
 static const uint8_t FUJIFILM_SHUTTER_PRESS[2] = {0x02, 0x00};
@@ -46,6 +56,34 @@ static void print_token(const uint8_t *token) {
                  + String(token[3], HEX));
 }
 
+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());
+  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;
+  }
+}
+
 Fujifilm::Fujifilm(const void *data, size_t len) {
   if (len != sizeof(fujifilm_t))
     throw;
@@ -101,6 +139,8 @@ bool Fujifilm::matches(NimBLEAdvertisedDevice *pDevice) {
  * re-pairing.
  */
 bool Fujifilm::connect(NimBLEClient *pClient, ezProgressBar &progress_bar) {
+  using namespace std::placeholders;
+
   m_Client = pClient;
 
   progress_bar.value(10.0f);
@@ -143,17 +183,44 @@ bool Fujifilm::connect(NimBLEClient *pClient, ezProgressBar &progress_bar) {
   Serial.println("Configuring");
   pSvc = m_Client->getService(FUJIFILM_SVC_CONF_UUID);
   // indications
-  pSvc->getCharacteristic(FUJIFILM_CHR_IND1_UUID)->subscribe(false, nullptr, true);
+  pSvc->getCharacteristic(FUJIFILM_CHR_IND1_UUID)
+      ->subscribe(false, std::bind(&Fujifilm::notify, this, _1, _2, _3, _4), true);
   progress_bar.value(50.0f);
-  pSvc->getCharacteristic(FUJIFILM_CHR_IND2_UUID)->subscribe(false, nullptr, true);
+
+  pSvc->getCharacteristic(FUJIFILM_CHR_IND2_UUID)
+      ->subscribe(false, std::bind(&Fujifilm::notify, this, _1, _2, _3, _4), true);
+
+  // wait for up to 5000ms callback
+  for (unsigned int i = 0; i < 5000; i += 100) {
+    if (m_Configured) {
+      break;
+    }
+    delay(100);
+  }
+
   progress_bar.value(60.0f);
   // notifications
-  pSvc->getCharacteristic(FUJIFILM_CHR_NOT1_UUID)->subscribe(true, nullptr, true);
+  pSvc->getCharacteristic(FUJIFILM_CHR_NOT1_UUID)
+      ->subscribe(true, std::bind(&Fujifilm::notify, this, _1, _2, _3, _4), true);
+
   progress_bar.value(70.0f);
-  pSvc->getCharacteristic(FUJIFILM_CHR_NOT2_UUID)->subscribe(true, nullptr, true);
+  pSvc->getCharacteristic(FUJIFILM_CHR_NOT2_UUID)
+      ->subscribe(true, std::bind(&Fujifilm::notify, this, _1, _2, _3, _4), true);
+
   progress_bar.value(80.0f);
-  pSvc->getCharacteristic(FUJIFILM_CHR_NOT3_UUID)->subscribe(true, nullptr, true);
+  pSvc->getCharacteristic(FUJIFILM_CHR_IND3_UUID)
+      ->subscribe(false, std::bind(&Fujifilm::notify, this, _1, _2, _3, _4), true);
+
   progress_bar.value(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;
+    }
+    delay(100);
+  }
 
   Serial.println("Configured");
 
@@ -187,6 +254,45 @@ void Fujifilm::focusRelease(void) {
   shutterRelease();
 }
 
+void Fujifilm::sendGeoData(void) {
+  NimBLERemoteService *pSvc = m_Client->getService(FUJIFILM_SVC_GEOTAG_UUID);
+  NimBLERemoteCharacteristic *pChr = pSvc->getCharacteristic(FUJIFILM_CHR_GEOTAG_UUID);
+
+  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,
+                     }};
+
+  if (pChr->canWrite()) {
+    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);
+
+    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();
+    m_GeoRequested = false;
+  }
+}
+
 void Fujifilm::print(void) {
   Serial.print("Name: ");
   Serial.println(m_Name.c_str());

lib/furble/Fujifilm.h

@@ -1,6 +1,8 @@
 #ifndef FUJIFILM_H
 #define FUJIFILM_H
 
+#include <NimBLERemoteCharacteristic.h>
+
 #include "Device.h"
 
 #define FUJIFILM_TOKEN_LEN (4)
@@ -25,15 +27,49 @@ class Fujifilm: public Device {
   void shutterRelease(void);
   void focusPress(void);
   void focusRelease(void);
+  void updateGeoData(gps_t &gps, timesync_t &timesync);
   void disconnect(void);
   void print(void);
 
  private:
+  /**
+   * Time synchronisation.
+   */
+  typedef struct __attribute__((packed)) _fujifilm_time_t {
+    uint16_t year;
+    uint8_t day;
+    uint8_t month;
+    uint8_t hour;
+    uint8_t minute;
+    uint8_t second;
+  } fujifilm_time_t;
+
+  /**
+   * Location and time packet.
+   */
+  typedef struct __attribute__((packed)) _fujigeotag_t {
+    int32_t latitude;
+    int32_t longitude;
+    int32_t altitude;
+    uint8_t pad[4];
+    fujifilm_time_t gps_time;
+  } geotag_t;
+
   device_type_t getDeviceType(void);
   size_t getSerialisedBytes(void);
   bool serialise(void *buffer, size_t bytes);
+  void notify(NimBLERemoteCharacteristic *, uint8_t *, size_t, bool);
+  void sendGeoData();
 
   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;
 };
 
 }  // namespace Furble

platformio.ini

@@ -3,6 +3,7 @@ build_flags = -D FURBLE_VERSION=\"${sysenv.FURBLE_VERSION}\"
 lib_deps =
   Battery Sense
   [email protected]
+  mikalhart/[email protected]
 
 [env]
 platform = espressif32