Restore Muse Anthena data parsing

Author: Andrey1994

src/board_controller/brainflow_boards.cpp

@@ -1164,23 +1164,35 @@ BrainFlowBoards::BrainFlowBoards()
     {
         {"name", "MuseAnthena"},
         {"sampling_rate", 256},
-        {"timestamp_channel", 6},
-        {"marker_channel", 7},
+        {"timestamp_channel", 9},
+        {"marker_channel", 10},
         {"package_num_channel", 0},
-        {"num_rows", 8},
-        {"eeg_channels", {1, 2, 3, 4}},
-        {"eeg_names", "TP9,AF7,AF8,TP10"},
-        {"other_channels", {5}}
+        {"num_rows", 11},
+        {"eeg_channels", {1, 2, 3, 4, 5, 6, 7, 8}},
+        {"eeg_names", "TP9,AF7,AF8,TP10,AUX1,AUX2,AUX3,AUX4"}
     };
     brainflow_boards_json["boards"]["67"]["auxiliary"] =
     {
         {"name", "MuseAnthenaAux"},
+        {"sampling_rate", 52},
+        {"timestamp_channel", 7},
+        {"marker_channel", 8},
+        {"package_num_channel", 0},
+        {"num_rows", 9},
+        {"accel_channels", {1, 2, 3}},
+        {"gyro_channels", {4, 5, 6}}
+    };
+    brainflow_boards_json["boards"]["67"]["ancillary"] =
+    {
+        {"name", "MuseAnthenaAnc"},
         {"sampling_rate", 64},
-        {"timestamp_channel", 4},
-        {"marker_channel", 5},
+        {"timestamp_channel", 21},
+        {"marker_channel", 22},
         {"package_num_channel", 0},
-        {"num_rows", 6},
+        {"num_rows", 23},
         {"ppg_channels", {1, 2, 3}},
+        {"optical_channels", {4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19}},
+        {"battery_channel", 20}
     };
 }
 

src/board_controller/muse/inc/muse_anthena.h

@@ -1,49 +1,83 @@
 #pragma once
 
+#include <array>
+#include <cstddef>
+#include <cstdint>
 #include <condition_variable>
 #include <mutex>
+#include <string>
 #include <utility>
 #include <vector>
 
 #include "ble_lib_board.h"
+#include "muse_anthena_types.h"
 
 
 class MuseAnthena : public BLELibBoard
 {
 
 protected:
+    static const size_t PACKET_HEADER_SIZE = 14;
+    static const size_t SUBPACKET_HEADER_SIZE = 5;
+
+    struct SensorConfig
+    {
+        SensorType type;
+        int n_channels;
+        int n_samples;
+        double sampling_rate;
+        size_t data_len;
+        bool variable_length;
+
+        SensorConfig ();
+        SensorConfig (SensorType type, int n_channels, int n_samples, double sampling_rate,
+            size_t data_len, bool variable_length = false);
+    };
+
     volatile simpleble_adapter_t muse_adapter;
     volatile simpleble_peripheral_t muse_peripheral;
     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;
-    int last_sensor_packet_id;
-    int last_status_packet_id;
-    int last_traditional_packet_id;
+    bool timestamp_initialized;
+    uint32_t first_device_tick;
+    double first_host_timestamp;
+    double last_battery;
+    std::string muse_preset;
+    bool enable_low_latency;
 
+    static std::string trim_string (const std::string &value);
+    static std::string to_lower (const std::string &value);
+    static bool is_valid_muse_preset (const std::string &preset);
+    static bool parse_bool_option (const std::string &value, bool &parsed);
+    int parse_muse_options ();
     std::string bytes_to_string (const uint8_t *data, size_t size);
-
-    void parse_main_packet (const uint8_t *data, size_t start_pos, size_t size);
-    void parse_sensor_packet (const uint8_t *data, size_t start_pos, size_t size);
+    void handle_data_notification (const uint8_t *data, size_t size);
+    void parse_sensor_payload (
+        uint8_t tag, uint8_t sequence_num, uint32_t device_tick, const uint8_t *data, size_t size);
+    bool get_sensor_config (uint8_t tag, SensorConfig &config);
+    int get_optics_canonical_index (uint8_t tag, int channel);
+    double average_present (const std::array<double, 16> &values,
+        const std::array<bool, 16> &present, const int *indexes, int len);
+    double get_sample_timestamp (uint32_t device_tick, int sample_index, double sampling_rate);
 
 public:
     MuseAnthena (int board_id, struct BrainFlowInputParams params);
-    ~MuseAnthena ();
+    ~MuseAnthena () override;
 
-    int prepare_session ();
-    int start_stream (int buffer_size, const char *streamer_params);
-    int stop_stream ();
-    int release_session ();
-    int config_board (std::string config, std::string &response);
+    int prepare_session () override;
+    int start_stream (int buffer_size, const char *streamer_params) override;
+    int stop_stream () override;
+    int release_session () override;
+    int config_board (std::string config, std::string &response) override;
     int config_board (std::string config);
 
     void adapter_on_scan_found (simpleble_adapter_t adapter, simpleble_peripheral_t peripheral);
-    void peripheral_on_main (simpleble_peripheral_t peripheral, simpleble_uuid_t service,
-        simpleble_uuid_t characteristic, const uint8_t *data, size_t size);
-    void peripheral_on_aux (simpleble_peripheral_t peripheral, simpleble_uuid_t service,
+    void peripheral_on_data (simpleble_peripheral_t peripheral, simpleble_uuid_t service,
         simpleble_uuid_t characteristic, const uint8_t *data, size_t size);
     void peripheral_on_status (simpleble_peripheral_t peripheral, simpleble_uuid_t service,
         simpleble_uuid_t characteristic, const uint8_t *data, size_t size);

src/board_controller/muse/inc/muse_anthena_constants.h

@@ -5,25 +5,12 @@
 #define MUSE_ANTHENA_SERVICE_UUID 0xFE8D
 
 #define MUSE_ANTHENA_GATT_ATTR_STREAM_TOGGLE "273e0001-4c4d-454d-96be-f03bac821358"
-#define MUSE_ANTHENA_GATT_EEG "273e0013-4c4d-454d-96be-f03bac821358"
-#define MUSE_ANTHENA_GATT_AUX "273e0014-4c4d-454d-96be-f03bac821358"
+#define MUSE_ANTHENA_GATT_DATA_1 "273e0013-4c4d-454d-96be-f03bac821358"
+#define MUSE_ANTHENA_GATT_DATA_2 "273e0014-4c4d-454d-96be-f03bac821358"
 
-// info about packet type
-#define MUSE_S_ANTHENA_EEG_FOUR_PACKET_TYPE 0x11
-#define MUSE_S_ANTHENA_EEG_EIGHT_PACKET_TYPE 0x12
-#define MUSE_S_ANTHENA_OPTICS_FOUR_PACKET_TYPE 0x34
-#define MUSE_S_ANTHENA_OPTICS_EIGHT_PACKET_TYPE 0x35
-#define MUSE_S_ANTHENA_OPTICS_SIXTEEN_PACKET_TYPE 0x36
-#define MUSE_S_ANTHENA_SENSOR_PACKET_TYPE 0x47
-#define MUSE_S_ANTHENA_STATUS_PACKET_TYPE 0x53
-#define MUSE_S_ANTHENA_BATTERY_PACKET_TYPE 0x98
-
-#define MUSE_S_ANTHENA_EEG_FOUR_PACKET_SIZE 32
-#define MUSE_S_ANTHENA_EEG_EIGHT_PACKET_SIZE 32
-#define MUSE_S_ANTHENA_MAIN_PACKET_SIZE MUSE_S_ANTHENA_EEG_FOUR_PACKET_SIZE
-#define MUSE_S_ANTHENA_TRADITIONAL_PACKET_SIZE MUSE_S_ANTHENA_EEG_EIGHT_PACKET_SIZE
-#define MUSE_S_ANTHENA_OPTICS_FOUR_PACKET_SIZE 34
-#define MUSE_S_ANTHENA_OPTICS_EIGHT_PACKET_SIZE 44
-#define MUSE_S_ANTHENA_OPTICS_SIXTEEN_PACKET_SIZE 44
-#define MUSE_S_ANTHENA_SENSOR_PACKET_SIZE 40
-#define MUSE_S_ANTHENA_STATUS_PACKET_SIZE 28
+// info for equations
+#define MUSE_ANTHENA_ACCELEROMETER_SCALE_FACTOR 0.0000610352
+#define MUSE_ANTHENA_GYRO_SCALE_FACTOR -0.0074768
+#define MUSE_ANTHENA_DEVICE_CLOCK_HZ 256000.0
+#define MUSE_ANTHENA_EEG_SCALE_FACTOR (1450.0 / 16383.0)
+#define MUSE_ANTHENA_OPTICS_SCALE_FACTOR (1.0 / 32768.0)

src/board_controller/muse/inc/muse_anthena_types.h

@@ -0,0 +1,11 @@
+#pragma once
+
+
+enum class SensorType
+{
+    EEG,
+    ACC_GYRO,
+    OPTICS,
+    BATTERY,
+    UNKNOWN
+};