cleanup ppg for muse and add status check (#400)

* fix ppg data for muse devices

Author: Andrey1994

src/board_controller/muse/inc/muse.h

@@ -16,6 +16,7 @@ class Muse : public BLELibBoard
     bool initialized;
     bool is_streaming;
     std::mutex m;
+    std::mutex callback_lock;
     std::condition_variable cv;
     std::vector<std::pair<simpleble_uuid_t, simpleble_uuid_t>> notified_characteristics;
     std::pair<simpleble_uuid_t, simpleble_uuid_t> control_characteristics;
@@ -24,8 +25,9 @@ class Muse : public BLELibBoard
     double last_timestamp;
     int current_accel_pos;
     int current_gyro_pos;
-
-    std::mutex callback_lock;
+    int current_ppg_pos[3];
+    std::string fw_version;
+    std::string status_string;
 
 public:
     Muse (int board_id, struct BrainFlowInputParams params);
@@ -42,13 +44,11 @@ class Muse : public BLELibBoard
     void peripheral_on_eeg (simpleble_uuid_t service, simpleble_uuid_t characteristic,
         uint8_t *data, size_t size, size_t channel_num);
     void peripheral_on_ppg (simpleble_uuid_t service, simpleble_uuid_t characteristic,
-        uint8_t *data, size_t size, size_t ppg_num, int *ppg_pos);
+        uint8_t *data, size_t size, size_t ppg_num);
     void peripheral_on_accel (
         simpleble_uuid_t service, simpleble_uuid_t characteristic, uint8_t *data, size_t size);
     void peripheral_on_gyro (
         simpleble_uuid_t service, simpleble_uuid_t characteristic, uint8_t *data, size_t size);
-
-    int current_ppg_pos0;
-    int current_ppg_pos1;
-    int current_ppg_pos2;
+    void peripheral_on_status (
+        simpleble_uuid_t service, simpleble_uuid_t characteristic, uint8_t *data, size_t size);
 };

src/board_controller/muse/muse.cpp

@@ -1,4 +1,5 @@
 #include <algorithm>
+#include <regex>
 
 #include "custom_cast.h"
 #include "muse.h"
@@ -50,25 +51,25 @@ void peripheral_on_gyro (simpleble_uuid_t service, simpleble_uuid_t characterist
 void peripheral_on_ppg0 (simpleble_uuid_t service, simpleble_uuid_t characteristic, uint8_t *data,
     size_t size, void *board)
 {
-    Muse *muse_board = (Muse *)board;
-    muse_board->peripheral_on_ppg (
-        service, characteristic, data, size, 0, &muse_board->current_ppg_pos0);
+    ((Muse *)(board))->peripheral_on_ppg (service, characteristic, data, size, 0);
 }
 
 void peripheral_on_ppg1 (simpleble_uuid_t service, simpleble_uuid_t characteristic, uint8_t *data,
     size_t size, void *board)
 {
-    Muse *muse_board = (Muse *)board;
-    muse_board->peripheral_on_ppg (
-        service, characteristic, data, size, 1, &muse_board->current_ppg_pos1);
+    ((Muse *)(board))->peripheral_on_ppg (service, characteristic, data, size, 1);
 }
 
 void peripheral_on_ppg2 (simpleble_uuid_t service, simpleble_uuid_t characteristic, uint8_t *data,
     size_t size, void *board)
 {
-    Muse *muse_board = (Muse *)board;
-    muse_board->peripheral_on_ppg (
-        service, characteristic, data, size, 2, &muse_board->current_ppg_pos2);
+    ((Muse *)(board))->peripheral_on_ppg (service, characteristic, data, size, 2);
+}
+
+void peripheral_on_status (simpleble_uuid_t service, simpleble_uuid_t characteristic, uint8_t *data,
+    size_t size, void *board)
+{
+    ((Muse *)(board))->peripheral_on_status (service, characteristic, data, size);
 }
 
 
@@ -80,9 +81,9 @@ Muse::Muse (int board_id, struct BrainFlowInputParams params) : BLELibBoard (boa
     is_streaming = false;
     current_accel_pos = 0;
     current_gyro_pos = 0;
-    current_ppg_pos0 = 0;
-    current_ppg_pos1 = 0;
-    current_ppg_pos2 = 0;
+    memset (current_ppg_pos, 0, sizeof (current_ppg_pos));
+    fw_version = "";
+    status_string = "";
 }
 
 Muse::~Muse ()
@@ -184,6 +185,20 @@ int Muse::prepare_session ()
                         service.uuid, service.characteristics[j]);
                     control_characteristics_found = true;
                     safe_logger (spdlog::level::info, "found control characteristic");
+                    if (simpleble_peripheral_notify (muse_peripheral, service.uuid,
+                            service.characteristics[j], ::peripheral_on_status,
+                            (void *)this) == SIMPLEBLE_SUCCESS)
+                    {
+                        notified_characteristics.push_back (
+                            std::pair<simpleble_uuid_t, simpleble_uuid_t> (
+                                service.uuid, service.characteristics[j]));
+                    }
+                    else
+                    {
+                        safe_logger (spdlog::level::err, "Failed to notify for {} {}",
+                            service.uuid.value, service.characteristics[j].value);
+                        res = (int)BrainFlowExitCodes::GENERAL_ERROR;
+                    }
                 }
                 if (strcmp (service.characteristics[j].value, MUSE_GATT_ATTR_TP9) == 0)
                 {
@@ -349,9 +364,7 @@ int Muse::prepare_session ()
         new_eeg_data.resize (4); // 4 eeg channels total
         current_gyro_pos = 0;
         current_accel_pos = 0;
-        current_ppg_pos0 = 0;
-        current_ppg_pos1 = 0;
-        current_ppg_pos2 = 0;
+        memset (current_ppg_pos, 0, sizeof (current_ppg_pos));
         for (int i = 0; i < 12; i++)
         {
             current_buf[i].resize (buffer_size);
@@ -362,6 +375,10 @@ int Muse::prepare_session ()
         initialized = true;
     }
 
+    if (res == (int)BrainFlowExitCodes::STATUS_OK)
+    {
+        res = config_board ("v1");
+    }
     if (res == (int)BrainFlowExitCodes::STATUS_OK)
     {
         res = config_board ("p21");
@@ -458,9 +475,7 @@ int Muse::release_session ()
     new_eeg_data.clear ();
     current_gyro_pos = 0;
     current_accel_pos = 0;
-    current_ppg_pos0 = 0;
-    current_ppg_pos1 = 0;
-    current_ppg_pos2 = 0;
+    memset (current_ppg_pos, 0, sizeof (current_ppg_pos));
 
     return (int)BrainFlowExitCodes::STATUS_OK;
 }
@@ -662,7 +677,7 @@ void Muse::peripheral_on_gyro (
 }
 
 void Muse::peripheral_on_ppg (simpleble_uuid_t service, simpleble_uuid_t characteristic,
-    uint8_t *data, size_t size, size_t ppg_num, int *ppg_pos)
+    uint8_t *data, size_t size, size_t ppg_num)
 {
     std::lock_guard<std::mutex> lock (callback_lock);
     if (size != 20)
@@ -678,9 +693,39 @@ void Muse::peripheral_on_ppg (simpleble_uuid_t service, simpleble_uuid_t charact
         double ppg_val = (double)cast_24bit_to_int32 ((unsigned char *)&data[2 + i * 3]);
         for (int j = 0; j < 2; j++)
         {
-            int pos = (*ppg_pos + i * 2 + j) % 12;
+            int pos = (current_ppg_pos[ppg_num] + i * 2 + j) % 12;
             current_buf[pos][ppg_channels[ppg_num]] = ppg_val;
         }
     }
-    *ppg_pos += 2;
+    current_ppg_pos[ppg_num] += 2;
+}
+
+void Muse::peripheral_on_status (
+    simpleble_uuid_t service, simpleble_uuid_t characteristic, uint8_t *data, size_t size)
+{
+    std::lock_guard<std::mutex> lock (callback_lock);
+    if (size != 20)
+    {
+        safe_logger (spdlog::level::warn, "unknown size for status callback: {}", size);
+        return;
+    }
+
+    int len = (int)data[0]; // first byte is a len of non garbage data
+    std::string incom_string (((char *)data) + 1, len);
+    status_string += incom_string;
+    safe_logger (spdlog::level::trace, "status string is {}", status_string);
+    std::regex rgx ("fw\":\"([0-9]+\\.[0-9]+\\.[0-9]+)");
+    std::smatch matches;
+    if (std::regex_search (status_string, matches, rgx) == true)
+    {
+        if (matches.size () == 2)
+        {
+            fw_version = matches.str (1);
+            safe_logger (spdlog::level::trace, "Determined fw version: {}", fw_version.c_str ());
+        }
+        else
+        {
+            safe_logger (spdlog::level::warn, "invalid number of groups found");
+        }
+    }
 }

src/board_controller/muse/muse_bglib/inc/muse_bglib_helper.h

@@ -54,9 +54,7 @@ class MuseBGLibHelper
     json board_descr;
     int current_accel_pos;
     int current_gyro_pos;
-    int current_ppg_pos0;
-    int current_ppg_pos1;
-    int current_ppg_pos2;
+    int current_ppg_pos[3];
 
     std::string preset;
 
@@ -80,9 +78,7 @@ class MuseBGLibHelper
         board_descr = descr;
         current_accel_pos = 0;
         current_gyro_pos = 0;
-        current_ppg_pos0 = 0;
-        current_ppg_pos1 = 0;
-        current_ppg_pos2 = 0;
+        memset (current_ppg_pos, 0, sizeof (current_ppg_pos));
     }
 
     virtual ~MuseBGLibHelper ()

src/board_controller/muse/muse_bglib/muse_bglib_helper.cpp

@@ -34,9 +34,7 @@ int MuseBGLibHelper::initialize (struct BrainFlowInputParams params)
         new_eeg_data.resize (4); // 4 eeg channels total
         current_accel_pos = 0;
         current_gyro_pos = 0;
-        current_ppg_pos0 = 0;
-        current_ppg_pos1 = 0;
-        current_ppg_pos2 = 0;
+        memset (current_ppg_pos, 0, sizeof (current_ppg_pos));
         for (int i = 0; i < 12; i++)
         {
             current_buf[i].resize (buffer_size);
@@ -186,9 +184,7 @@ int MuseBGLibHelper::release ()
     }
     current_buf.clear ();
     current_accel_pos = 0;
-    current_ppg_pos0 = 0;
-    current_ppg_pos1 = 0;
-    current_ppg_pos2 = 0;
+    memset (current_ppg_pos, 0, sizeof (current_ppg_pos));
     current_gyro_pos = 0;
     last_timestamp = -1.0;
 
@@ -419,48 +415,32 @@ void MuseBGLibHelper::ble_evt_attclient_attribute_value (
     else if ((uuid == MUSE_GATT_ATTR_PPG0) || (uuid == MUSE_GATT_ATTR_PPG1) ||
         (uuid == MUSE_GATT_ATTR_PPG2))
     {
-        int current_ppg_pos = 0;
         int ppg_chann_num = 0;
         if (uuid == MUSE_GATT_ATTR_PPG0)
         {
             ppg_chann_num = 0;
-            current_ppg_pos = current_ppg_pos0;
         }
         if (uuid == MUSE_GATT_ATTR_PPG1)
         {
             ppg_chann_num = 1;
-            current_ppg_pos = current_ppg_pos1;
         }
         if (uuid == MUSE_GATT_ATTR_PPG2)
         {
             ppg_chann_num = 2;
-            current_ppg_pos = current_ppg_pos2;
         }
 
-
         std::vector<int> ppg_channels = board_descr["ppg_channels"];
         for (int i = 0; i < 6; i++)
         {
             double ppg_val =
                 (double)cast_24bit_to_int32 ((unsigned char *)&msg->value.data[2 + i * 3]);
             for (int j = 0; j < 2; j++)
             {
-                int pos = (current_ppg_pos + i * 2 + j) % 12;
+                int pos = (current_ppg_pos[ppg_chann_num] + i * 2 + j) % 12;
                 current_buf[pos][ppg_channels[ppg_chann_num]] = ppg_val;
             }
         }
-        if (uuid == MUSE_GATT_ATTR_PPG0)
-        {
-            current_ppg_pos0 += 2;
-        }
-        if (uuid == MUSE_GATT_ATTR_PPG1)
-        {
-            current_ppg_pos1 += 2;
-        }
-        if (uuid == MUSE_GATT_ATTR_PPG2)
-        {
-            current_ppg_pos2 += 2;
-        }
+        current_ppg_pos[ppg_chann_num] += 2;
     }
     else
     {