Fix ganglion firmware v2 packet ordering

brainflow-dev · Aug 7, 2023 · 7c0caff · 7c0caff
1 parent a584036
commit 7c0caff
Show file tree

Hide file tree

Showing 4 changed files with 146 additions and 78 deletions.
diff --git a/src/board_controller/openbci/ganglion.cpp b/src/board_controller/openbci/ganglion.cpp
@@ -269,11 +269,11 @@ void Ganglion::read_thread ()
             {
                 if (firmware == 3)
                 {
-                    decompress_firmware_3 (&data, last_data, acceleration);
+                    decompress_firmware_3 (&data, last_data, acceleration, package);
                 }
                 else if (firmware == 2)
                 {
-                    decompress_firmware_2 (&data, last_data, acceleration);
+                    decompress_firmware_2 (&data, last_data, acceleration, package);
                 }
             }
             else if ((data.data[0] > 200) && (data.data[0] < 206))
@@ -343,33 +343,6 @@ void Ganglion::read_thread ()
                 }
                 continue;
             }
-
-            // add first encoded package
-            package[board_descr["default"]["package_num_channel"].get<int> ()] = data.data[0];
-            package[board_descr["default"]["eeg_channels"][0].get<int> ()] =
-                eeg_scale * last_data[0];
-            package[board_descr["default"]["eeg_channels"][1].get<int> ()] =
-                eeg_scale * last_data[1];
-            package[board_descr["default"]["eeg_channels"][2].get<int> ()] =
-                eeg_scale * last_data[2];
-            package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
-                eeg_scale * last_data[3];
-            package[board_descr["default"]["accel_channels"][0].get<int> ()] = acceleration[0];
-            package[board_descr["default"]["accel_channels"][1].get<int> ()] = acceleration[1];
-            package[board_descr["default"]["accel_channels"][2].get<int> ()] = acceleration[2];
-            package[board_descr["default"]["timestamp_channel"].get<int> ()] = data.timestamp;
-            push_package (package);
-            // add second package
-            package[board_descr["default"]["eeg_channels"][0].get<int> ()] =
-                eeg_scale * last_data[4];
-            package[board_descr["default"]["eeg_channels"][1].get<int> ()] =
-                eeg_scale * last_data[5];
-            package[board_descr["default"]["eeg_channels"][2].get<int> ()] =
-                eeg_scale * last_data[6];
-            package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
-                eeg_scale * last_data[7];
-            package[board_descr["default"]["timestamp_channel"].get<int> ()] = data.timestamp;
-            push_package (package);
         }
         else
         {
@@ -398,7 +371,7 @@ void Ganglion::read_thread ()
 }
 
 void Ganglion::decompress_firmware_3 (
-    struct GanglionLib::GanglionData *data, float *last_data, double *acceleration)
+    struct GanglionLib::GanglionData *data, float *last_data, double *acceleration, double *package)
 {
     int bits_per_num = 0;
     unsigned char package_bits[160] = {0}; // 20 * 8
@@ -408,7 +381,7 @@ void Ganglion::decompress_firmware_3 (
     }
 
     // 18 bit compression, sends 17 MSBs + sign bit of 24-bit sample
-    if ((data->data[0] >= 0) && (data->data[0] < 100))
+    if (data->data[0] >= 0 && data->data[0] < 100)
     {
         int last_digit = data->data[0] % 10;
         switch (last_digit)
@@ -429,7 +402,7 @@ void Ganglion::decompress_firmware_3 (
         }
         bits_per_num = 18;
     }
-    else if ((data->data[0] >= 100) && (data->data[0] < 200))
+    else if (data->data[0] >= 100 && data->data[0] < 200)
     {
         bits_per_num = 19;
     }
@@ -446,10 +419,29 @@ void Ganglion::decompress_firmware_3 (
             last_data[counter] = (float)(cast_ganglion_bits_to_int32<19> (package_bits + i) << 5);
         }
     }
+
+    // add first encoded package
+    package[board_descr["default"]["package_num_channel"].get<int> ()] = data->data[0];
+    package[board_descr["default"]["eeg_channels"][0].get<int> ()] = eeg_scale * last_data[0];
+    package[board_descr["default"]["eeg_channels"][1].get<int> ()] = eeg_scale * last_data[1];
+    package[board_descr["default"]["eeg_channels"][2].get<int> ()] = eeg_scale * last_data[2];
+    package[board_descr["default"]["eeg_channels"][3].get<int> ()] = eeg_scale * last_data[3];
+    package[board_descr["default"]["accel_channels"][0].get<int> ()] = acceleration[0];
+    package[board_descr["default"]["accel_channels"][1].get<int> ()] = acceleration[1];
+    package[board_descr["default"]["accel_channels"][2].get<int> ()] = acceleration[2];
+    package[board_descr["default"]["timestamp_channel"].get<int> ()] = data->timestamp;
+    push_package (package);
+    // add second package
+    package[board_descr["default"]["eeg_channels"][0].get<int> ()] = eeg_scale * last_data[4];
+    package[board_descr["default"]["eeg_channels"][1].get<int> ()] = eeg_scale * last_data[5];
+    package[board_descr["default"]["eeg_channels"][2].get<int> ()] = eeg_scale * last_data[6];
+    package[board_descr["default"]["eeg_channels"][3].get<int> ()] = eeg_scale * last_data[7];
+    package[board_descr["default"]["timestamp_channel"].get<int> ()] = data->timestamp;
+    push_package (package);
 }
 
 void Ganglion::decompress_firmware_2 (
-    struct GanglionLib::GanglionData *data, float *last_data, double *acceleration)
+    struct GanglionLib::GanglionData *data, float *last_data, double *acceleration, double *package)
 {
     int bits_per_num = 0;
     unsigned char package_bits[160] = {0}; // 20 * 8
@@ -474,9 +466,22 @@ void Ganglion::decompress_firmware_2 (
         last_data[5] = (float)cast_24bit_to_int32 (data->data + 4);
         last_data[6] = (float)cast_24bit_to_int32 (data->data + 7);
         last_data[7] = (float)cast_24bit_to_int32 (data->data + 10);
+
+        // scale new packet and insert into result
+        package[board_descr["default"]["package_num_channel"].get<int> ()] = 0.;
+        package[board_descr["default"]["eeg_channels"][0].get<int> ()] = eeg_scale * data->data[4];
+        package[board_descr["default"]["eeg_channels"][1].get<int> ()] = eeg_scale * data->data[5];
+        package[board_descr["default"]["eeg_channels"][2].get<int> ()] = eeg_scale * data->data[6];
+        package[board_descr["default"]["eeg_channels"][3].get<int> ()] = eeg_scale * data->data[7];
+        package[board_descr["default"]["accel_channels"][0].get<int> ()] = acceleration[0];
+        package[board_descr["default"]["accel_channels"][1].get<int> ()] = acceleration[1];
+        package[board_descr["default"]["accel_channels"][2].get<int> ()] = acceleration[2];
+        package[board_descr["default"]["timestamp_channel"].get<int> ()] = data->timestamp;
+        push_package (package);
+        return;
     }
     // 18 bit compression, sends delta from previous value instead of real value!
-    else if ((data->data[0] >= 1) && (data->data[0] <= 100))
+    else if (data->data[0] >= 1 && data->data[0] <= 100)
     {
         int last_digit = data->data[0] % 10;
         switch (last_digit)
@@ -497,7 +502,7 @@ void Ganglion::decompress_firmware_2 (
         }
         bits_per_num = 18;
     }
-    else if ((data->data[0] >= 101) && (data->data[0] <= 200))
+    else if (data->data[0] >= 101 && data->data[0] <= 200)
     {
         bits_per_num = 19;
     }
@@ -526,6 +531,25 @@ void Ganglion::decompress_firmware_2 (
     {
         last_data[i] = last_data[i - 4] - delta[i];
     }
+
+    // add first encoded package
+    package[board_descr["default"]["package_num_channel"].get<int> ()] = data->data[0];
+    package[board_descr["default"]["eeg_channels"][0].get<int> ()] = eeg_scale * last_data[0];
+    package[board_descr["default"]["eeg_channels"][1].get<int> ()] = eeg_scale * last_data[1];
+    package[board_descr["default"]["eeg_channels"][2].get<int> ()] = eeg_scale * last_data[2];
+    package[board_descr["default"]["eeg_channels"][3].get<int> ()] = eeg_scale * last_data[3];
+    package[board_descr["default"]["accel_channels"][0].get<int> ()] = acceleration[0];
+    package[board_descr["default"]["accel_channels"][1].get<int> ()] = acceleration[1];
+    package[board_descr["default"]["accel_channels"][2].get<int> ()] = acceleration[2];
+    package[board_descr["default"]["timestamp_channel"].get<int> ()] = data->timestamp;
+    push_package (package);
+    // add second package
+    package[board_descr["default"]["eeg_channels"][0].get<int> ()] = eeg_scale * last_data[4];
+    package[board_descr["default"]["eeg_channels"][1].get<int> ()] = eeg_scale * last_data[5];
+    package[board_descr["default"]["eeg_channels"][2].get<int> ()] = eeg_scale * last_data[6];
+    package[board_descr["default"]["eeg_channels"][3].get<int> ()] = eeg_scale * last_data[7];
+    package[board_descr["default"]["timestamp_channel"].get<int> ()] = data->timestamp;
+    push_package (package);
 }
 
 int Ganglion::config_board (std::string config, std::string &response)

diff --git a/src/board_controller/openbci/ganglion_native.cpp b/src/board_controller/openbci/ganglion_native.cpp
@@ -474,16 +474,17 @@ void GanglionNative::read_data (
         package[i] = 0.0;
     }
 
-    if (data[0] <= 200)
+    if (data[0] <= 200 && size == 20)
     {
         if (firmware == 3)
         {
-            decompress_firmware_3 (data);
+            decompress_firmware_3 (data, package);
         }
         else if (firmware == 2)
         {
-            decompress_firmware_2 (data);
+            decompress_firmware_2 (data, package);
         }
+        delete package;
     }
     else if ((data[0] > 200) && (data[0] < 206))
     {
@@ -556,37 +557,9 @@ void GanglionNative::read_data (
         delete[] package;
         return;
     }
-
-    // add first encoded package
-    package[board_descr["default"]["package_num_channel"].get<int> ()] = data[0];
-    package[board_descr["default"]["eeg_channels"][0].get<int> ()] =
-        eeg_scale * temp_data.last_data[0];
-    package[board_descr["default"]["eeg_channels"][1].get<int> ()] =
-        eeg_scale * temp_data.last_data[1];
-    package[board_descr["default"]["eeg_channels"][2].get<int> ()] =
-        eeg_scale * temp_data.last_data[2];
-    package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
-        eeg_scale * temp_data.last_data[3];
-    package[board_descr["default"]["accel_channels"][0].get<int> ()] = temp_data.accel_x;
-    package[board_descr["default"]["accel_channels"][1].get<int> ()] = temp_data.accel_y;
-    package[board_descr["default"]["accel_channels"][2].get<int> ()] = temp_data.accel_z;
-    package[board_descr["default"]["timestamp_channel"].get<int> ()] = get_timestamp ();
-    push_package (package);
-    // add second package
-    package[board_descr["default"]["eeg_channels"][0].get<int> ()] =
-        eeg_scale * temp_data.last_data[4];
-    package[board_descr["default"]["eeg_channels"][1].get<int> ()] =
-        eeg_scale * temp_data.last_data[5];
-    package[board_descr["default"]["eeg_channels"][2].get<int> ()] =
-        eeg_scale * temp_data.last_data[6];
-    package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
-        eeg_scale * temp_data.last_data[7];
-    package[board_descr["default"]["timestamp_channel"].get<int> ()] = get_timestamp ();
-    push_package (package);
-    delete[] package;
 }
 
-void GanglionNative::decompress_firmware_3 (uint8_t *data)
+void GanglionNative::decompress_firmware_3 (uint8_t *data, double *package)
 {
     int bits_per_num = 0;
     unsigned char package_bits[160] = {0}; // 20 * 8
@@ -596,7 +569,7 @@ void GanglionNative::decompress_firmware_3 (uint8_t *data)
     }
 
     // 18 bit compression, sends 17 MSBs + sign bit of 24-bit sample
-    if ((data[0] >= 0) && (data[0] < 100))
+    if (data[0] >= 0 && data[0] < 100)
     {
         int last_digit = data[0] % 10;
         switch (last_digit)
@@ -617,7 +590,7 @@ void GanglionNative::decompress_firmware_3 (uint8_t *data)
         }
         bits_per_num = 18;
     }
-    else if ((data[0] >= 100) && (data[0] < 200))
+    else if (data[0] >= 100 && data[0] < 200)
     {
         bits_per_num = 19;
     }
@@ -636,9 +609,36 @@ void GanglionNative::decompress_firmware_3 (uint8_t *data)
                 (float)(cast_ganglion_bits_to_int32<19> (package_bits + i) << 5);
         }
     }
+
+    // add first encoded package
+    package[board_descr["default"]["package_num_channel"].get<int> ()] = data[0];
+    package[board_descr["default"]["eeg_channels"][0].get<int> ()] =
+        eeg_scale * temp_data.last_data[0];
+    package[board_descr["default"]["eeg_channels"][1].get<int> ()] =
+        eeg_scale * temp_data.last_data[1];
+    package[board_descr["default"]["eeg_channels"][2].get<int> ()] =
+        eeg_scale * temp_data.last_data[2];
+    package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
+        eeg_scale * temp_data.last_data[3];
+    package[board_descr["default"]["accel_channels"][0].get<int> ()] = temp_data.accel_x;
+    package[board_descr["default"]["accel_channels"][1].get<int> ()] = temp_data.accel_y;
+    package[board_descr["default"]["accel_channels"][2].get<int> ()] = temp_data.accel_z;
+    package[board_descr["default"]["timestamp_channel"].get<int> ()] = get_timestamp ();
+    push_package (package);
+    // add second package
+    package[board_descr["default"]["eeg_channels"][0].get<int> ()] =
+        eeg_scale * temp_data.last_data[4];
+    package[board_descr["default"]["eeg_channels"][1].get<int> ()] =
+        eeg_scale * temp_data.last_data[5];
+    package[board_descr["default"]["eeg_channels"][2].get<int> ()] =
+        eeg_scale * temp_data.last_data[6];
+    package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
+        eeg_scale * temp_data.last_data[7];
+    package[board_descr["default"]["timestamp_channel"].get<int> ()] = get_timestamp ();
+    push_package (package);
 }
 
-void GanglionNative::decompress_firmware_2 (uint8_t *data)
+void GanglionNative::decompress_firmware_2 (uint8_t *data, double *package)
 {
     int bits_per_num = 0;
     unsigned char package_bits[160] = {0}; // 20 * 8
@@ -663,9 +663,26 @@ void GanglionNative::decompress_firmware_2 (uint8_t *data)
         temp_data.last_data[5] = (float)cast_24bit_to_int32 (data + 4);
         temp_data.last_data[6] = (float)cast_24bit_to_int32 (data + 7);
         temp_data.last_data[7] = (float)cast_24bit_to_int32 (data + 10);
+
+        // scale new packet and insert into result
+        package[board_descr["default"]["package_num_channel"].get<int> ()] = 0.;
+        package[board_descr["default"]["eeg_channels"][0].get<int> ()] =
+            eeg_scale * temp_data.last_data[4];
+        package[board_descr["default"]["eeg_channels"][1].get<int> ()] =
+            eeg_scale * temp_data.last_data[5];
+        package[board_descr["default"]["eeg_channels"][2].get<int> ()] =
+            eeg_scale * temp_data.last_data[6];
+        package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
+            eeg_scale * temp_data.last_data[7];
+        package[board_descr["default"]["accel_channels"][0].get<int> ()] = temp_data.accel_x;
+        package[board_descr["default"]["accel_channels"][1].get<int> ()] = temp_data.accel_y;
+        package[board_descr["default"]["accel_channels"][2].get<int> ()] = temp_data.accel_z;
+        package[board_descr["default"]["timestamp_channel"].get<int> ()] = get_timestamp ();
+        push_package (package);
+        return;
     }
     // 18 bit compression, sends delta from previous value instead of real value!
-    else if ((data[0] >= 1) && (data[0] <= 100))
+    if (data[0] >= 1 && data[0] <= 100)
     {
         int last_digit = data[0] % 10;
         switch (last_digit)
@@ -686,7 +703,7 @@ void GanglionNative::decompress_firmware_2 (uint8_t *data)
         }
         bits_per_num = 18;
     }
-    else if ((data[0] >= 101) && (data[0] <= 200))
+    else if (data[0] >= 101 && data[0] <= 200)
     {
         bits_per_num = 19;
     }
@@ -715,4 +732,31 @@ void GanglionNative::decompress_firmware_2 (uint8_t *data)
     {
         temp_data.last_data[i] = temp_data.last_data[i - 4] - delta[i];
     }
+
+    // add first encoded package
+    package[board_descr["default"]["package_num_channel"].get<int> ()] = data[0];
+    package[board_descr["default"]["eeg_channels"][0].get<int> ()] =
+        eeg_scale * temp_data.last_data[0];
+    package[board_descr["default"]["eeg_channels"][1].get<int> ()] =
+        eeg_scale * temp_data.last_data[1];
+    package[board_descr["default"]["eeg_channels"][2].get<int> ()] =
+        eeg_scale * temp_data.last_data[2];
+    package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
+        eeg_scale * temp_data.last_data[3];
+    package[board_descr["default"]["accel_channels"][0].get<int> ()] = temp_data.accel_x;
+    package[board_descr["default"]["accel_channels"][1].get<int> ()] = temp_data.accel_y;
+    package[board_descr["default"]["accel_channels"][2].get<int> ()] = temp_data.accel_z;
+    package[board_descr["default"]["timestamp_channel"].get<int> ()] = get_timestamp ();
+    push_package (package);
+    // add second package
+    package[board_descr["default"]["eeg_channels"][0].get<int> ()] =
+        eeg_scale * temp_data.last_data[4];
+    package[board_descr["default"]["eeg_channels"][1].get<int> ()] =
+        eeg_scale * temp_data.last_data[5];
+    package[board_descr["default"]["eeg_channels"][2].get<int> ()] =
+        eeg_scale * temp_data.last_data[6];
+    package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
+        eeg_scale * temp_data.last_data[7];
+    package[board_descr["default"]["timestamp_channel"].get<int> ()] = get_timestamp ();
+    push_package (package);
 }
diff --git a/src/board_controller/openbci/inc/ganglion.h b/src/board_controller/openbci/inc/ganglion.h
@@ -63,8 +63,8 @@ class Ganglion : public Board
     int release_session ();
     int config_board (std::string config, std::string &response);
 
-    void decompress_firmware_3 (
-        struct GanglionLib::GanglionData *data, float *last_data, double *acceleration);
-    void decompress_firmware_2 (
-        struct GanglionLib::GanglionData *data, float *last_data, double *acceleration);
+    void decompress_firmware_3 (struct GanglionLib::GanglionData *data, float *last_data,
+        double *acceleration, double *package);
+    void decompress_firmware_2 (struct GanglionLib::GanglionData *data, float *last_data,
+        double *acceleration, double *package);
 };
diff --git a/src/board_controller/openbci/inc/ganglion_native.h b/src/board_controller/openbci/inc/ganglion_native.h
@@ -115,6 +115,6 @@ class GanglionNative : public BLELibBoard
     double const accel_scale = 0.016f;
     double const eeg_scale = (1.2f * 1000000) / (8388607.0f * 1.5f * 51.0f);
 
-    void decompress_firmware_3 (uint8_t *data);
-    void decompress_firmware_2 (uint8_t *data);
+    void decompress_firmware_3 (uint8_t *data, double *package);
+    void decompress_firmware_2 (uint8_t *data, double *package);
 };