diff --git a/csharp_package/brainflow/brainflow/board_controller_library.cs b/csharp_package/brainflow/brainflow/board_controller_library.cs index 7f87b8ce9..83d134a4c 100644 --- a/csharp_package/brainflow/brainflow/board_controller_library.cs +++ b/csharp_package/brainflow/brainflow/board_controller_library.cs @@ -123,7 +123,8 @@ public enum BoardIds BIOLISTENER_BOARD = 64, IRONBCI_32_BOARD = 65, NEUROPAWN_KNIGHT_BOARD_IMU = 66, - MUSE_S_ATHENA_BOARD = 67 + MUSE_S_ATHENA_BOARD = 67, + SHIMMER3_BOARD = 68 }; diff --git a/java_package/brainflow/src/main/java/brainflow/BoardIds.java b/java_package/brainflow/src/main/java/brainflow/BoardIds.java index bd3d3df2a..e6b24ab11 100644 --- a/java_package/brainflow/src/main/java/brainflow/BoardIds.java +++ b/java_package/brainflow/src/main/java/brainflow/BoardIds.java @@ -73,7 +73,8 @@ public enum BoardIds BIOLISTENER_BOARD(64), IRONBCI_32_BOARD(65), NEUROPAWN_KNIGHT_BOARD_IMU(66), - MUSE_S_ATHENA_BOARD(67); + MUSE_S_ATHENA_BOARD(67), + SHIMMER3_BOARD(68); private final int board_id; private static final Map bi_map = new HashMap (); diff --git a/julia_package/brainflow/src/board_shim.jl b/julia_package/brainflow/src/board_shim.jl index 0583a1629..b1e05f60c 100644 --- a/julia_package/brainflow/src/board_shim.jl +++ b/julia_package/brainflow/src/board_shim.jl @@ -69,6 +69,7 @@ export BrainFlowInputParams IRONBCI_32_BOARD = 65 NEUROPAWN_KNIGHT_BOARD_IMU = 66 MUSE_S_ATHENA_BOARD = 67 + SHIMMER3_BOARD = 68 end diff --git a/matlab_package/brainflow/BoardIds.m b/matlab_package/brainflow/BoardIds.m index 9b5db2d2f..a30390909 100644 --- a/matlab_package/brainflow/BoardIds.m +++ b/matlab_package/brainflow/BoardIds.m @@ -67,5 +67,6 @@ IRONBCI_32_BOARD(65) NEUROPAWN_KNIGHT_BOARD_IMU(66) MUSE_S_ATHENA_BOARD(67) + SHIMMER3_BOARD(68) end end diff --git a/nodejs_package/brainflow/brainflow.types.ts b/nodejs_package/brainflow/brainflow.types.ts index 7eb1cf54f..5aabeaed5 100644 --- a/nodejs_package/brainflow/brainflow.types.ts +++ b/nodejs_package/brainflow/brainflow.types.ts @@ -76,7 +76,8 @@ export enum BoardIds { BIOLISTENER_BOARD = 64, IRONBCI_32_BOARD = 65, NEUROPAWN_KNIGHT_BOARD_IMU = 66, - MUSE_S_ATHENA_BOARD = 67 + MUSE_S_ATHENA_BOARD = 67, + SHIMMER3_BOARD = 68 } export enum IpProtocolTypes { diff --git a/python_package/brainflow/board_shim.py b/python_package/brainflow/board_shim.py index 657dc173c..178221ede 100644 --- a/python_package/brainflow/board_shim.py +++ b/python_package/brainflow/board_shim.py @@ -82,6 +82,7 @@ class BoardIds(enum.IntEnum): IRONBCI_32_BOARD = 65 #: NEUROPAWN_KNIGHT_BOARD_IMU = 66 #: MUSE_S_ATHENA_BOARD = 67 #: + SHIMMER3_BOARD = 68 #: class IpProtocolTypes(enum.IntEnum): diff --git a/rust_package/brainflow/src/ffi/constants.rs b/rust_package/brainflow/src/ffi/constants.rs index 7e11cf125..37d78ddfa 100644 --- a/rust_package/brainflow/src/ffi/constants.rs +++ b/rust_package/brainflow/src/ffi/constants.rs @@ -103,6 +103,7 @@ pub enum BoardIds { Ironbci32Board = 65, NeuropawnKnightBoardImu = 66, MuseSAthenaBoard = 67, + Shimmer3Board = 68, } #[repr(i32)] #[derive(FromPrimitive, ToPrimitive, Debug, Copy, Clone, Hash, PartialEq, Eq)] diff --git a/src/board_controller/board_controller.cpp b/src/board_controller/board_controller.cpp index 8c59fac41..612965994 100644 --- a/src/board_controller/board_controller.cpp +++ b/src/board_controller/board_controller.cpp @@ -53,6 +53,7 @@ #include "ntl_wifi.h" #include "pieeg_board.h" #include "playback_file_board.h" +#include "shimmer3.h" #include "streaming_board.h" #include "synchroni_board.h" #include "synthetic_board.h" @@ -314,6 +315,9 @@ int prepare_session (int board_id, const char *json_brainflow_input_params) board = std::shared_ptr ( new KnightIMU ((int)BoardIds::NEUROPAWN_KNIGHT_BOARD_IMU, params)); break; + case BoardIds::SHIMMER3_BOARD: + board = std::shared_ptr (new Shimmer3 (params)); + break; default: return (int)BrainFlowExitCodes::UNSUPPORTED_BOARD_ERROR; } diff --git a/src/board_controller/brainflow_boards.cpp b/src/board_controller/brainflow_boards.cpp index a320cca69..9493e3f9a 100644 --- a/src/board_controller/brainflow_boards.cpp +++ b/src/board_controller/brainflow_boards.cpp @@ -85,7 +85,8 @@ BrainFlowBoards::BrainFlowBoards() {"64", json::object()}, {"65", json::object()}, {"66", json::object()}, - {"67", json::object()} + {"67", json::object()}, + {"68", json::object()} } }}; @@ -1199,6 +1200,23 @@ BrainFlowBoards::BrainFlowBoards() {"optical_channels", {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}, {"battery_channel", 17} }; + brainflow_boards_json["boards"]["68"]["default"] = + { + {"name", "Shimmer3"}, + {"sampling_rate", 512}, // configurable via config_board("sampling_rate:") + {"package_num_channel", 0}, + {"timestamp_channel", 18}, + {"marker_channel", 19}, + {"num_rows", 20}, + {"accel_channels", {1, 2, 3}}, + {"gyro_channels", {4, 5, 6}}, + {"magnetometer_channels", {7, 8, 9}}, + {"ecg_channels", {10, 11, 12, 13}}, + {"eda_channels", {14}}, + {"temperature_channels", {15}}, + {"battery_channel", 16}, + {"other_channels", {17}} + }; } BrainFlowBoards boards_struct; diff --git a/src/board_controller/build.cmake b/src/board_controller/build.cmake index 006d68752..f3fb0d790 100644 --- a/src/board_controller/build.cmake +++ b/src/board_controller/build.cmake @@ -88,6 +88,7 @@ SET (BOARD_CONTROLLER_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/neuropawn/knight_base.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/neuropawn/knight_imu.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/biolistener/biolistener.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/shimmer/shimmer3.cpp ) include (${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/ant_neuro/build.cmake) @@ -156,6 +157,7 @@ target_include_directories ( ${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/synchroni/inc ${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/neuropawn/inc ${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/biolistener/inc + ${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/shimmer/inc ) target_compile_definitions(${BOARD_CONTROLLER_NAME} PRIVATE NOMINMAX BRAINFLOW_VERSION=${BRAINFLOW_VERSION}) diff --git a/src/board_controller/shimmer/inc/shimmer3.h b/src/board_controller/shimmer/inc/shimmer3.h new file mode 100644 index 000000000..a13436c98 --- /dev/null +++ b/src/board_controller/shimmer/inc/shimmer3.h @@ -0,0 +1,79 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "board.h" +#include "board_controller.h" +#include "serial.h" + +#include "shimmer3_defines.h" + +class Shimmer3 : public Board +{ +public: + Shimmer3 (struct BrainFlowInputParams params); + ~Shimmer3 () override; + + 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 get_board_sampling_rate (int preset) override; + +private: + // Describes one field inside one streamed sample, in transmit order. + struct PacketField + { + shimmer3::Signal signal; + shimmer3::FieldFormat format; + }; + + std::atomic keep_alive; + bool initialized; + + Serial *serial_port; + std::string port_name; + + std::thread streaming_thread; + + // First-data handshake between start_stream and read_thread. + std::mutex sync_mutex; + std::condition_variable sync_cv; + bool first_data_received; + + double sampling_rate; + double package_num; + std::vector packet_layout; + int packet_data_size; + uint8_t samples_per_packet; + uint8_t configured_gsr_range; + + // -- low-level serial helpers -- + int write_bytes (const uint8_t *data, int len); + int read_exact (uint8_t *buf, int len, bool cancellable = false); + int read_byte (uint8_t &out, bool cancellable = false); + int wait_for_ack (); + + // -- device commands -- + int cmd_get_fw_version (); + int cmd_get_hw_version (uint8_t &hw_version); + int cmd_disable_instream_ack_prefix (); + int cmd_set_sensors (uint32_t bitfield); + int cmd_set_sampling_rate (double hz); + int cmd_inquiry (); + int cmd_start_streaming (); + int cmd_stop_streaming (); + + // -- helpers -- + bool build_packet_layout (const std::vector &signals); + bool convert_gsr_to_microsiemens (int32_t raw, double &eda_us) const; + void route_field (shimmer3::Signal s, int32_t raw, double *package, int &other_idx); + void read_thread (); +}; diff --git a/src/board_controller/shimmer/inc/shimmer3_defines.h b/src/board_controller/shimmer/inc/shimmer3_defines.h new file mode 100644 index 000000000..ec2c8ad38 --- /dev/null +++ b/src/board_controller/shimmer/inc/shimmer3_defines.h @@ -0,0 +1,325 @@ +#pragma once + +#include +#include + +namespace shimmer3 +{ + + // Hardware version reported by GET_SHIMMER_VERSION_COMMAND. We only accept 3. + constexpr uint8_t HW_VERSION_SHIMMER3 = 3; + constexpr uint8_t HW_VERSION_SHIMMER3R = 10; // listed only so we can reject it + + // The Shimmer3's low-frequency crystal runs at 32768 Hz. Sampling rate is + // stored on the device as a clock divider: f = 32768 / divider. + constexpr double CLOCK_HZ = 32768.0; + + // Width of the on-wire timestamp that leads every data packet. + constexpr int TIMESTAMP_BYTES = 3; + + // Bluetooth command / response opcodes (subset that this driver uses). + enum Opcode : uint8_t + { + DATA_PACKET = 0x00, + + INQUIRY_COMMAND = 0x01, + INQUIRY_RESPONSE = 0x02, + + GET_SAMPLING_RATE_COMMAND = 0x03, + SAMPLING_RATE_RESPONSE = 0x04, + SET_SAMPLING_RATE_COMMAND = 0x05, + + START_STREAMING_COMMAND = 0x07, + SET_SENSORS_COMMAND = 0x08, + + STOP_STREAMING_COMMAND = 0x20, + + DEVICE_VERSION_RESPONSE = 0x25, + + GET_ALL_CALIBRATION_COMMAND = 0x2C, + ALL_CALIBRATION_RESPONSE = 0x2D, + + GET_FW_VERSION_COMMAND = 0x2E, + FW_VERSION_RESPONSE = 0x2F, + + GET_SHIMMER_VERSION_COMMAND = 0x3F, + + SET_EXG_REGS_COMMAND = 0x61, + EXG_REGS_RESPONSE = 0x62, + GET_EXG_REGS_COMMAND = 0x63, + + START_SDBT_COMMAND = 0x70, // start streaming AND logging to SD + STATUS_RESPONSE = 0x71, + GET_STATUS_COMMAND = 0x72, + + SET_SHIMMERNAME_COMMAND = 0x79, + SHIMMERNAME_RESPONSE = 0x7A, + GET_SHIMMERNAME_COMMAND = 0x7B, + + INSTREAM_CMD_RESPONSE = 0x8A, + SET_CRC_COMMAND = 0x8B, + + VBATT_RESPONSE = 0x94, + GET_VBATT_COMMAND = 0x95, + TEST_CONNECTION_COMMAND = 0x96, + + SET_INSTREAM_RESPONSE_ACK_PREFIX_STATE = 0xA3, + + ACK_COMMAND_PROCESSED = 0xFF, + }; + + // 24-bit "enabled sensors" bitfield used by SET_SENSORS_COMMAND. + // The field is transmitted little-endian: byte0 = (field & 0xFF), etc. + namespace sensor + { + constexpr uint32_t A_ACCEL = 0x000080; // low-noise (analog) accel + constexpr uint32_t GYRO = 0x000040; // MPU9150 gyroscope + constexpr uint32_t MAG = 0x000020; // LSM303 magnetometer + constexpr uint32_t EXG1_24BIT = 0x000010; // ADS1292R chip 1, 24-bit + constexpr uint32_t EXG2_24BIT = 0x000008; // ADS1292R chip 2, 24-bit + constexpr uint32_t GSR = 0x000004; + constexpr uint32_t EXT_A7 = 0x000002; // external ADC + constexpr uint32_t EXT_A6 = 0x000001; // external ADC + + constexpr uint32_t INT_A13 = 0x000100; // internal ADC + constexpr uint32_t INT_A12 = 0x000200; // internal ADC + constexpr uint32_t INT_A1 = 0x000400; // internal ADC + constexpr uint32_t EXT_A15 = 0x000800; // external ADC + constexpr uint32_t D_ACCEL = 0x001000; // wide-range (digital) accel + constexpr uint32_t VBATT = 0x002000; + constexpr uint32_t BRIDGE_AMP = 0x008000; + + constexpr uint32_t PRESSURE = 0x040000; + constexpr uint32_t EXG2_16BIT = 0x080000; + constexpr uint32_t EXG1_16BIT = 0x100000; + constexpr uint32_t MPU_MAG = 0x200000; + constexpr uint32_t MPU_ACCEL = 0x400000; + constexpr uint32_t INT_A14 = 0x800000; + } + + // Channel (signal) identifiers as reported, in order, by the inquiry response. + // These tell us how to lay out the bytes that follow the timestamp. + enum class Signal : uint8_t + { + ACCEL_LN_X = 0x00, + ACCEL_LN_Y = 0x01, + ACCEL_LN_Z = 0x02, + VBATT = 0x03, + ACCEL_WR_X = 0x04, + ACCEL_WR_Y = 0x05, + ACCEL_WR_Z = 0x06, + MAG_X = 0x07, + MAG_Y = 0x08, + MAG_Z = 0x09, + GYRO_X = 0x0A, + GYRO_Y = 0x0B, + GYRO_Z = 0x0C, + EXT_ADC_A7 = 0x0D, + EXT_ADC_A6 = 0x0E, + EXT_ADC_A15 = 0x0F, + INT_ADC_A1 = 0x10, + INT_ADC_A12 = 0x11, + INT_ADC_A13 = 0x12, + INT_ADC_A14 = 0x13, + MPU_ACCEL_X = 0x14, + MPU_ACCEL_Y = 0x15, + MPU_ACCEL_Z = 0x16, + MPU_MAG_X = 0x17, + MPU_MAG_Y = 0x18, + MPU_MAG_Z = 0x19, + TEMPERATURE = 0x1A, + PRESSURE = 0x1B, + GSR = 0x1C, + EXG1_STATUS = 0x1D, + EXG1_CH1_24BIT = 0x1E, + EXG1_CH2_24BIT = 0x1F, + EXG2_STATUS = 0x20, + EXG2_CH1_24BIT = 0x21, + EXG2_CH2_24BIT = 0x22, + EXG1_CH1_16BIT = 0x23, + EXG1_CH2_16BIT = 0x24, + EXG2_CH1_16BIT = 0x25, + EXG2_CH2_16BIT = 0x26, + STRAIN_HIGH = 0x27, + STRAIN_LOW = 0x28, + + // Synthetic timestamp field used only by the host-side packet layout. + TIMESTAMP = 0xFE, + }; + + // Wire format of a single field inside a data packet. + struct FieldFormat + { + uint8_t width; // 1, 2 or 3 bytes + bool is_signed; // sign-extend when decoding + bool little_endian; + }; + + // Decode a field's raw bytes into a signed 32-bit integer. + inline int32_t decode_field (const FieldFormat &fmt, const uint8_t *p) + { + uint32_t raw = 0; + if (fmt.little_endian) + { + for (int i = fmt.width - 1; i >= 0; --i) + raw = (raw << 8) | p[i]; + } + else + { + for (int i = 0; i < fmt.width; ++i) + raw = (raw << 8) | p[i]; + } + + if (fmt.is_signed && fmt.width < 4) + { + const uint32_t sign_bit = 1u << (fmt.width * 8 - 1); + if (raw & sign_bit) + raw |= ~((1u << (fmt.width * 8)) - 1); + } + return static_cast (raw); + } + + // Return the wire format for a signal. found is false for unsupported IDs. + inline FieldFormat format_for (Signal s, bool &found) + { + found = true; + switch (s) + { + // Low-noise analog accel + battery: unsigned 16-bit little-endian ADC. + case Signal::ACCEL_LN_X: + case Signal::ACCEL_LN_Y: + case Signal::ACCEL_LN_Z: + case Signal::VBATT: + return {2, false, true}; + + // Wide-range (LSM303) accel: signed 16-bit little-endian. + case Signal::ACCEL_WR_X: + case Signal::ACCEL_WR_Y: + case Signal::ACCEL_WR_Z: + return {2, true, true}; + + // LSM303 magnetometer: signed 16-bit big-endian. + case Signal::MAG_X: + case Signal::MAG_Y: + case Signal::MAG_Z: + return {2, true, false}; + + // MPU9150 gyroscope: signed 16-bit big-endian. + case Signal::GYRO_X: + case Signal::GYRO_Y: + case Signal::GYRO_Z: + return {2, true, false}; + + // ADC channels: unsigned 16-bit little-endian. + case Signal::EXT_ADC_A7: + case Signal::EXT_ADC_A6: + case Signal::EXT_ADC_A15: + case Signal::INT_ADC_A1: + case Signal::INT_ADC_A12: + case Signal::INT_ADC_A13: + case Signal::INT_ADC_A14: + case Signal::GSR: + case Signal::STRAIN_HIGH: + case Signal::STRAIN_LOW: + return {2, false, true}; + + // MPU9150 accel: signed 16-bit big-endian. + case Signal::MPU_ACCEL_X: + case Signal::MPU_ACCEL_Y: + case Signal::MPU_ACCEL_Z: + return {2, true, false}; + + // MPU9150 magnetometer: signed 16-bit little-endian. + case Signal::MPU_MAG_X: + case Signal::MPU_MAG_Y: + case Signal::MPU_MAG_Z: + return {2, true, true}; + + // BMP180/280 temperature & pressure: unsigned big-endian. + case Signal::TEMPERATURE: + return {2, false, false}; + case Signal::PRESSURE: + return {3, false, false}; + + // ExG (ADS1292R): 1-byte status, 24-/16-bit signed big-endian data. + case Signal::EXG1_STATUS: + case Signal::EXG2_STATUS: + return {1, false, true}; + case Signal::EXG1_CH1_24BIT: + case Signal::EXG1_CH2_24BIT: + case Signal::EXG2_CH1_24BIT: + case Signal::EXG2_CH2_24BIT: + return {3, true, false}; + case Signal::EXG1_CH1_16BIT: + case Signal::EXG1_CH2_16BIT: + case Signal::EXG2_CH1_16BIT: + case Signal::EXG2_CH2_16BIT: + return {2, true, false}; + + case Signal::TIMESTAMP: + return {TIMESTAMP_BYTES, false, true}; + + default: + found = false; + return {0, false, false}; + } + } + + // ---- Sampling-rate conversion (f = 32768 / divider) ---- + + inline double divider_to_hz (uint16_t divider) + { + return divider == 0 ? 0.0 : CLOCK_HZ / static_cast (divider); + } + + inline uint16_t hz_to_divider (double hz) + { + if (!std::isfinite (hz) || (hz <= 0.0)) + return 0; + + double divider = CLOCK_HZ / hz; + if ((divider < 1.0) || (divider > 65535.0)) + return 0; + + return static_cast (divider + 0.5); + } + + // -------------------------- CRC -------------------------- + // Optional: only needed if SET_CRC_COMMAND is used to enable BT-packet CRC. + + constexpr uint16_t CRC_INIT = 0xB0CA; + + inline uint16_t crc_byte (uint16_t crc, uint8_t b) + { + crc = static_cast (((crc >> 8) & 0xFFFF) | ((crc << 8) & 0xFFFF)); + crc ^= b; + crc ^= (crc & 0xFF) >> 4; + crc = static_cast (crc ^ ((crc << 12) & 0xFFFF)); + crc = static_cast (crc ^ ((crc & 0xFF) << 5)); + return crc; + } + + inline uint16_t calc_crc (int length, const uint8_t *msg) + { + if ((length <= 0) || (msg == nullptr)) + return CRC_INIT; + + uint16_t crc = crc_byte (CRC_INIT, msg[0]); + for (int i = 1; i < length; ++i) + crc = crc_byte (crc, msg[i]); + if (length % 2 == 1) + crc = crc_byte (crc, 0x00); + return crc; + } + + constexpr uint32_t SUPPORTED_SENSORS = sensor::A_ACCEL | sensor::GYRO | sensor::MAG | + sensor::EXG1_24BIT | sensor::EXG2_24BIT | sensor::GSR | sensor::EXT_A7 | sensor::EXT_A6 | + sensor::INT_A13 | sensor::INT_A12 | sensor::INT_A1 | sensor::EXT_A15 | sensor::D_ACCEL | + sensor::VBATT | sensor::BRIDGE_AMP | sensor::PRESSURE | sensor::EXG2_16BIT | + sensor::EXG1_16BIT | sensor::MPU_MAG | sensor::MPU_ACCEL | sensor::INT_A14; + constexpr uint32_t ACCEL_SENSORS = sensor::A_ACCEL | sensor::D_ACCEL | sensor::MPU_ACCEL; + constexpr uint32_t MAG_SENSORS = sensor::MAG | sensor::MPU_MAG; + constexpr uint32_t EXG1_SENSORS = sensor::EXG1_24BIT | sensor::EXG1_16BIT; + constexpr uint32_t EXG2_SENSORS = sensor::EXG2_24BIT | sensor::EXG2_16BIT; + +} // namespace shimmer3 diff --git a/src/board_controller/shimmer/shimmer3.cpp b/src/board_controller/shimmer/shimmer3.cpp new file mode 100644 index 000000000..d71e1584b --- /dev/null +++ b/src/board_controller/shimmer/shimmer3.cpp @@ -0,0 +1,993 @@ +#include +#include +#include +#include +#include +#include + +#include "shimmer3.h" + +#include "brainflow_constants.h" +#include "timestamp.h" + +using namespace shimmer3; + +Shimmer3::Shimmer3 (struct BrainFlowInputParams params) + : Board ((int)BoardIds::SHIMMER3_BOARD, params) +{ + keep_alive = false; + initialized = false; + first_data_received = false; + serial_port = nullptr; + sampling_rate = 0.0; + package_num = 0.0; + packet_data_size = 0; + samples_per_packet = 1; + configured_gsr_range = 0xFF; +} + +Shimmer3::~Shimmer3 () +{ + skip_logs = true; + release_session (); +} + +// --------------------------------------------------------------------------- +// Serial helpers +// --------------------------------------------------------------------------- + +int Shimmer3::write_bytes (const uint8_t *data, int len) +{ + if (serial_port == nullptr) + return (int)BrainFlowExitCodes::BOARD_NOT_CREATED_ERROR; + + int res = serial_port->send_to_serial_port (reinterpret_cast (data), len); + if (res != len) + { + safe_logger (spdlog::level::err, "failed to write {} bytes", len); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::read_exact (uint8_t *buf, int len, bool cancellable) +{ + if (serial_port == nullptr) + return (int)BrainFlowExitCodes::BOARD_NOT_CREATED_ERROR; + if ((buf == nullptr) || (len < 0)) + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + if (len == 0) + return (int)BrainFlowExitCodes::STATUS_OK; + + int got = 0; + int idle = 0; + const int max_idle = 2; + + while (got < len) + { + if (cancellable && !keep_alive) + return (int)BrainFlowExitCodes::STREAM_THREAD_IS_NOT_RUNNING; + + int res = + serial_port->read_from_serial_port (reinterpret_cast (buf + got), len - got); + + if (cancellable && !keep_alive) + return (int)BrainFlowExitCodes::STREAM_THREAD_IS_NOT_RUNNING; + + if (res > 0) + { + got += res; + idle = 0; + } + else if (res == 0) + { + if (++idle > max_idle) + { + safe_logger (spdlog::level::err, "serial read timeout {}/{}", got, len); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + } + else + { + safe_logger (spdlog::level::err, "serial read error {}", res); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + } + + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::read_byte (uint8_t &out, bool cancellable) +{ + return read_exact (&out, 1, cancellable); +} + +// Read bytes until ACK. In-stream responses may arrive first. +int Shimmer3::wait_for_ack () +{ + for (int tries = 0; tries < 512; ++tries) + { + uint8_t b = 0; + int res = read_byte (b); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + if (b == Opcode::ACK_COMMAND_PROCESSED) + return (int)BrainFlowExitCodes::STATUS_OK; + if (b == Opcode::INSTREAM_CMD_RESPONSE) + { + uint8_t discard = 0; + res = read_byte (discard); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + } + } + + safe_logger (spdlog::level::err, "no ACK received"); + return (int)BrainFlowExitCodes::BOARD_NOT_READY_ERROR; +} + +// --------------------------------------------------------------------------- +// Device commands +// --------------------------------------------------------------------------- + +int Shimmer3::cmd_get_fw_version () +{ + uint8_t cmd = Opcode::GET_FW_VERSION_COMMAND; + int res = write_bytes (&cmd, 1); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + res = wait_for_ack (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint8_t resp = 0; + res = read_byte (resp); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + if (resp != Opcode::FW_VERSION_RESPONSE) + return (int)BrainFlowExitCodes::GENERAL_ERROR; + + uint8_t b[6]; + res = read_exact (b, 6); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + int fw_id = b[0] | (b[1] << 8); + int major = b[2] | (b[3] << 8); + safe_logger (spdlog::level::info, "Shimmer FW id={} v{}.{}.{}", fw_id, major, b[4], b[5]); + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::cmd_get_hw_version (uint8_t &hw_version) +{ + uint8_t cmd = Opcode::GET_SHIMMER_VERSION_COMMAND; + int res = write_bytes (&cmd, 1); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + res = wait_for_ack (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint8_t resp = 0; + res = read_byte (resp); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + if (resp != Opcode::DEVICE_VERSION_RESPONSE) + return (int)BrainFlowExitCodes::GENERAL_ERROR; + + return read_byte (hw_version); +} + +// Disable the ACK prefix for in-stream command responses. +int Shimmer3::cmd_disable_instream_ack_prefix () +{ + uint8_t buf[2] = {Opcode::SET_INSTREAM_RESPONSE_ACK_PREFIX_STATE, 0x00}; + int res = write_bytes (buf, 2); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return wait_for_ack (); +} + +int Shimmer3::cmd_set_sensors (uint32_t bitfield) +{ + if (bitfield > 0x00FFFFFFU) + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + + uint8_t buf[4] = {Opcode::SET_SENSORS_COMMAND, static_cast (bitfield & 0xFFU), + static_cast ((bitfield >> 8) & 0xFFU), + static_cast ((bitfield >> 16) & 0xFFU)}; + + int res = write_bytes (buf, 4); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return wait_for_ack (); +} + +int Shimmer3::cmd_set_sampling_rate (double hz) +{ + if (!std::isfinite (hz) || (hz <= 0.0)) + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + + double divider_value = CLOCK_HZ / hz; + if ((divider_value < 1.0) || (divider_value > 65535.0)) + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + + uint16_t div = static_cast (std::lround (divider_value)); + uint8_t buf[3] = {Opcode::SET_SAMPLING_RATE_COMMAND, static_cast (div & 0xFF), + static_cast ((div >> 8) & 0xFF)}; + + int res = write_bytes (buf, 3); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + res = wait_for_ack (); + if (res == (int)BrainFlowExitCodes::STATUS_OK) + sampling_rate = divider_to_hz (div); + return res; +} + +// INQUIRY returns the rate, configuration, packet buffer size and channel IDs. +int Shimmer3::cmd_inquiry () +{ + uint8_t cmd = Opcode::INQUIRY_COMMAND; + int res = write_bytes (&cmd, 1); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + res = wait_for_ack (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint8_t resp = 0; + res = read_byte (resp); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + if (resp != Opcode::INQUIRY_RESPONSE) + { + safe_logger (spdlog::level::err, "expected inquiry response, got 0x{:02X}", resp); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + + uint8_t hdr[8]; + res = read_exact (hdr, 8); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint16_t div = static_cast (hdr[0] | (hdr[1] << 8)); + if (div == 0) + return (int)BrainFlowExitCodes::BOARD_NOT_READY_ERROR; + + sampling_rate = divider_to_hz (div); + + uint32_t config_setup = static_cast (hdr[2]) | (static_cast (hdr[3]) << 8) | + (static_cast (hdr[4]) << 16) | (static_cast (hdr[5]) << 24); + + configured_gsr_range = static_cast ((config_setup >> 25) & 0x07U); + + uint8_t num_channels = hdr[6]; + samples_per_packet = hdr[7]; + if (samples_per_packet == 0) + { + safe_logger (spdlog::level::err, "inquiry returned buffer size 0"); + return (int)BrainFlowExitCodes::BOARD_NOT_READY_ERROR; + } + + std::vector ids (num_channels); + if (num_channels > 0) + { + res = read_exact (ids.data (), num_channels); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + } + + std::vector signals; + for (uint8_t id : ids) + signals.push_back (static_cast (id)); + + if (!build_packet_layout (signals)) + return (int)BrainFlowExitCodes::BOARD_NOT_READY_ERROR; + + if (packet_data_size <= 0) + return (int)BrainFlowExitCodes::BOARD_NOT_READY_ERROR; + + safe_logger (spdlog::level::info, + "inquiry: {} Hz, {} channels, buffer_size {}, sample data {} bytes", sampling_rate, + (int)num_channels, (int)samples_per_packet, packet_data_size); + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::cmd_start_streaming () +{ + uint8_t cmd = Opcode::START_STREAMING_COMMAND; + int res = write_bytes (&cmd, 1); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return wait_for_ack (); +} + +int Shimmer3::cmd_stop_streaming () +{ + uint8_t cmd = Opcode::STOP_STREAMING_COMMAND; + return write_bytes (&cmd, 1); +} + +// --------------------------------------------------------------------------- +// Packet layout +// --------------------------------------------------------------------------- + +// Each sample contains a timestamp followed by the active channels. +bool Shimmer3::build_packet_layout (const std::vector &signals) +{ + packet_layout.clear (); + packet_data_size = 0; + + bool analog_accel = false; + bool digital_accel = false; + bool mpu_accel = false; + bool lsm_mag = false; + bool mpu_mag = false; + + for (Signal s : signals) + { + switch (s) + { + case Signal::ACCEL_LN_X: + case Signal::ACCEL_LN_Y: + case Signal::ACCEL_LN_Z: + analog_accel = true; + break; + + case Signal::ACCEL_WR_X: + case Signal::ACCEL_WR_Y: + case Signal::ACCEL_WR_Z: + digital_accel = true; + break; + + case Signal::MPU_ACCEL_X: + case Signal::MPU_ACCEL_Y: + case Signal::MPU_ACCEL_Z: + mpu_accel = true; + break; + + case Signal::MAG_X: + case Signal::MAG_Y: + case Signal::MAG_Z: + lsm_mag = true; + break; + + case Signal::MPU_MAG_X: + case Signal::MPU_MAG_Y: + case Signal::MPU_MAG_Z: + mpu_mag = true; + break; + + default: + break; + } + } + + int accel_sources = static_cast (analog_accel) + static_cast (digital_accel) + + static_cast (mpu_accel); + + int mag_sources = static_cast (lsm_mag) + static_cast (mpu_mag); + + if ((accel_sources > 1) || (mag_sources > 1)) + { + safe_logger (spdlog::level::err, "multiple IMU sources use the same BrainFlow rows"); + return false; + } + + bool found = false; + FieldFormat fmt = format_for (Signal::TIMESTAMP, found); + if (!found || (fmt.width == 0)) + return false; + + packet_layout.push_back ({Signal::TIMESTAMP, fmt}); + packet_data_size += fmt.width; + + for (Signal s : signals) + { + fmt = format_for (s, found); + if (!found || (fmt.width == 0)) + { + safe_logger (spdlog::level::err, "unsupported signal 0x{:02X}", (int)s); + packet_layout.clear (); + packet_data_size = 0; + return false; + } + + packet_layout.push_back ({s, fmt}); + packet_data_size += fmt.width; + } + + return true; +} + +// --------------------------------------------------------------------------- +// Board interface +// --------------------------------------------------------------------------- + +int Shimmer3::prepare_session () +{ + if (initialized) + return (int)BrainFlowExitCodes::STATUS_OK; + + if (params.serial_port.empty ()) + { + safe_logger (spdlog::level::err, "serial_port (Bluetooth SPP) must be provided"); + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + } + port_name = params.serial_port; + + serial_port = Serial::create (port_name.c_str (), this); + if (serial_port == nullptr) + return (int)BrainFlowExitCodes::UNABLE_TO_OPEN_PORT_ERROR; + + int serial_res = serial_port->open_serial_port (); + if (serial_res < 0) + { + safe_logger (spdlog::level::err, "failed to open {}", port_name); + delete serial_port; + serial_port = nullptr; + return (int)BrainFlowExitCodes::UNABLE_TO_OPEN_PORT_ERROR; + } + + serial_res = serial_port->set_serial_port_settings (1000, false); + if (serial_res != (int)SerialExitCodes::OK) + { + safe_logger ( + spdlog::level::err, "failed to configure {}, serial error {}", port_name, serial_res); + serial_port->close_serial_port (); + delete serial_port; + serial_port = nullptr; + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + + std::this_thread::sleep_for (std::chrono::milliseconds (500)); + + // Confirm this is a Shimmer3 and reject Shimmer3R. + uint8_t hw = 0; + int res = cmd_get_hw_version (hw); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + safe_logger (spdlog::level::err, "could not read hardware version"); + release_session (); + return res; + } + if (hw != HW_VERSION_SHIMMER3) + { + safe_logger (spdlog::level::err, + "unsupported hardware version {} (this driver supports Shimmer3 only)", (int)hw); + release_session (); + return (int)BrainFlowExitCodes::BOARD_NOT_READY_ERROR; + } + + if (cmd_get_fw_version () != (int)BrainFlowExitCodes::STATUS_OK) + safe_logger (spdlog::level::warn, "could not read firmware version"); + + if (cmd_disable_instream_ack_prefix () != (int)BrainFlowExitCodes::STATUS_OK) + safe_logger (spdlog::level::warn, "could not disable in-stream ACK prefix (old firmware?)"); + + res = cmd_inquiry (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + release_session (); + return res; + } + + initialized = true; + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::start_stream (int buffer_size, const char *streamer_params) +{ + if (!initialized) + return (int)BrainFlowExitCodes::BOARD_NOT_CREATED_ERROR; + if (keep_alive) + return (int)BrainFlowExitCodes::STREAM_ALREADY_RUN_ERROR; + if ((packet_data_size <= 0) || packet_layout.empty () || (samples_per_packet == 0)) + return (int)BrainFlowExitCodes::BOARD_NOT_READY_ERROR; + if (buffer_size <= 0) + return (int)BrainFlowExitCodes::INVALID_BUFFER_SIZE_ERROR; + + int res = prepare_for_acquisition (buffer_size, streamer_params); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + package_num = 0.0; + { + std::lock_guard lk (sync_mutex); + first_data_received = false; + } + + res = cmd_start_streaming (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + safe_logger (spdlog::level::err, "failed to start streaming"); + free_packages (); + return res; + } + + keep_alive = true; + streaming_thread = std::thread ([this] { this->read_thread (); }); + + int timeout = params.timeout > 0 ? params.timeout : 5; + std::unique_lock lk (sync_mutex); + bool got_data = sync_cv.wait_for ( + lk, std::chrono::seconds (timeout), [this] { return first_data_received; }); + if (!got_data) + { + lk.unlock (); + safe_logger ( + spdlog::level::err, "no data received within {} sec of starting stream", timeout); + keep_alive = false; + if (streaming_thread.joinable ()) + streaming_thread.join (); + cmd_stop_streaming (); + free_packages (); + return (int)BrainFlowExitCodes::SYNC_TIMEOUT_ERROR; + } + + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::stop_stream () +{ + if (!keep_alive) + return (int)BrainFlowExitCodes::STREAM_THREAD_IS_NOT_RUNNING; + + keep_alive = false; + if (streaming_thread.joinable ()) + streaming_thread.join (); + + int res = cmd_stop_streaming (); + + // Drain trailing data and ACK. + if (serial_port != nullptr) + { + uint8_t junk[256]; + for (int i = 0; i < 8; ++i) + { + int r = + serial_port->read_from_serial_port (reinterpret_cast (junk), sizeof (junk)); + if (r <= 0) + break; + } + } + + return res; +} + +int Shimmer3::release_session () +{ + if (keep_alive) + stop_stream (); + else if (streaming_thread.joinable ()) + streaming_thread.join (); + + if (initialized) + free_packages (); + initialized = false; + + if (serial_port != nullptr) + { + serial_port->close_serial_port (); + delete serial_port; + serial_port = nullptr; + } + + packet_layout.clear (); + packet_data_size = 0; + samples_per_packet = 1; + configured_gsr_range = 0xFF; + sampling_rate = 0.0; + package_num = 0.0; + + { + std::lock_guard lk (sync_mutex); + first_data_received = false; + } + + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::get_board_sampling_rate (int preset) +{ + if (preset != (int)BrainFlowPresets::DEFAULT_PRESET) + return Board::get_board_sampling_rate (preset); + if (!std::isfinite (sampling_rate) || (sampling_rate <= 0.0)) + return Board::get_board_sampling_rate (preset); + return static_cast (std::lround (sampling_rate)); +} + +// Supported config strings: +// "sampling_rate:" +// "sensors:" +int Shimmer3::config_board (std::string config, std::string &response) +{ + if (!initialized) + return (int)BrainFlowExitCodes::BOARD_NOT_CREATED_ERROR; + if (keep_alive) + { + safe_logger (spdlog::level::err, "cannot configure while streaming"); + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + } + + auto reinquire = [this] (std::string &resp) + { + int r = cmd_inquiry (); + if (r == (int)BrainFlowExitCodes::STATUS_OK) + resp = "OK"; + return r; + }; + + try + { + if (config.rfind ("sampling_rate:", 0) == 0) + { + std::string value = config.substr (14); + if (value.empty ()) + { + response = "INVALID_SAMPLING_RATE"; + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + } + + size_t parsed = 0; + double hz = std::stod (value, &parsed); + if ((parsed != value.size ()) || !std::isfinite (hz) || (hz <= 0.0)) + { + response = "INVALID_SAMPLING_RATE"; + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + } + + double divider_value = CLOCK_HZ / hz; + if ((divider_value < 1.0) || (divider_value > 65535.0)) + { + response = "INVALID_SAMPLING_RATE"; + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + } + + int res = cmd_set_sampling_rate (hz); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return reinquire (response); + } + + if (config.rfind ("sensors:", 0) == 0) + { + std::string value = config.substr (8); + if (value.empty ()) + { + response = "INVALID_SENSORS"; + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + } + + size_t parsed = 0; + unsigned long bits = std::stoul (value, &parsed, 16); + + if ((parsed != value.size ()) || (bits > 0x00FFFFFFUL)) + { + response = "INVALID_SENSORS"; + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + } + + uint32_t sensor_bits = static_cast (bits); + uint32_t accel_bits = sensor_bits & ACCEL_SENSORS; + uint32_t mag_bits = sensor_bits & MAG_SENSORS; + uint32_t exg1_bits = sensor_bits & EXG1_SENSORS; + uint32_t exg2_bits = sensor_bits & EXG2_SENSORS; + + if (((sensor_bits & ~SUPPORTED_SENSORS) != 0) || + ((accel_bits & (accel_bits - 1)) != 0) || ((mag_bits & (mag_bits - 1)) != 0) || + ((exg1_bits & (exg1_bits - 1)) != 0) || ((exg2_bits & (exg2_bits - 1)) != 0)) + { + response = "INVALID_SENSORS"; + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + } + + int res = cmd_set_sensors (sensor_bits); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return reinquire (response); + } + } + catch (const std::exception &e) + { + safe_logger (spdlog::level::err, "invalid config '{}': {}", config, e.what ()); + response = "INVALID_CONFIG"; + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + } + + safe_logger (spdlog::level::warn, "unknown config '{}'", config); + response = "UNKNOWN_COMMAND"; + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; +} + +// --------------------------------------------------------------------------- +// GSR conversion +// --------------------------------------------------------------------------- + +bool Shimmer3::convert_gsr_to_microsiemens (int32_t raw_value, double &eda_us) const +{ + uint16_t raw = static_cast (raw_value & 0xFFFF); + uint16_t adc = raw & 0x0FFF; + uint8_t range = configured_gsr_range; + + if (range == 4) + range = static_cast ((raw >> 14) & 0x03); + + if (range > 3) + { + eda_us = 0.0; + return false; + } + + struct Coeff + { + double p1; + double p2; + }; + + static constexpr Coeff coeffs[4] = { + {0.0373, -24.9915}, {0.0054, -3.5194}, {0.0015, -1.0163}, {0.0004558, -0.3014}}; + + eda_us = coeffs[range].p1 * adc + coeffs[range].p2; + if (!std::isfinite (eda_us) || (eda_us <= 0.0)) + eda_us = 0.0; + return true; +} + +// --------------------------------------------------------------------------- +// Routing decoded values to BrainFlow rows +// --------------------------------------------------------------------------- + +static std::vector rows_by_key (const json &descr, const char *key) +{ + std::vector out; + if (descr.contains (key)) + { + for (const auto &v : descr[key]) + out.push_back (v.get ()); + } + return out; +} + +void Shimmer3::route_field (Signal s, int32_t raw, double *package, int &other_idx) +{ + const json &d = board_descr["default"]; + + auto put_row = [&] (const char *key, int index, double value) + { + auto rows = rows_by_key (d, key); + if ((index >= 0) && (index < (int)rows.size ())) + package[rows[index]] = value; + }; + + switch (s) + { + case Signal::TIMESTAMP: + break; + + case Signal::ACCEL_LN_X: + case Signal::ACCEL_WR_X: + put_row ("accel_channels", 0, raw); + break; + + case Signal::ACCEL_LN_Y: + case Signal::ACCEL_WR_Y: + put_row ("accel_channels", 1, raw); + break; + + case Signal::ACCEL_LN_Z: + case Signal::ACCEL_WR_Z: + put_row ("accel_channels", 2, raw); + break; + + case Signal::GYRO_X: + put_row ("gyro_channels", 0, raw); + break; + + case Signal::GYRO_Y: + put_row ("gyro_channels", 1, raw); + break; + + case Signal::GYRO_Z: + put_row ("gyro_channels", 2, raw); + break; + + case Signal::MAG_X: + put_row ("magnetometer_channels", 0, raw); + break; + + case Signal::MAG_Y: + put_row ("magnetometer_channels", 1, raw); + break; + + case Signal::MAG_Z: + put_row ("magnetometer_channels", 2, raw); + break; + + case Signal::GSR: + { + double eda_us = 0.0; + if (!convert_gsr_to_microsiemens (raw, eda_us)) + safe_logger (spdlog::level::warn, "invalid configured GSR range {}", + (int)configured_gsr_range); + put_row ("eda_channels", 0, eda_us); + break; + } + + case Signal::TEMPERATURE: + put_row ("temperature_channels", 0, raw); + break; + + case Signal::VBATT: + if (d.contains ("battery_channel")) + package[d["battery_channel"].get ()] = static_cast (raw); + break; + + case Signal::EXG1_CH1_24BIT: + case Signal::EXG1_CH1_16BIT: + put_row ("ecg_channels", 0, raw); + break; + + case Signal::EXG1_CH2_24BIT: + case Signal::EXG1_CH2_16BIT: + put_row ("ecg_channels", 1, raw); + break; + + case Signal::EXG2_CH1_24BIT: + case Signal::EXG2_CH1_16BIT: + put_row ("ecg_channels", 2, raw); + break; + + case Signal::EXG2_CH2_24BIT: + case Signal::EXG2_CH2_16BIT: + put_row ("ecg_channels", 3, raw); + break; + + case Signal::MPU_ACCEL_X: + put_row ("accel_channels", 0, raw); + break; + + case Signal::MPU_ACCEL_Y: + put_row ("accel_channels", 1, raw); + break; + + case Signal::MPU_ACCEL_Z: + put_row ("accel_channels", 2, raw); + break; + + case Signal::MPU_MAG_X: + put_row ("magnetometer_channels", 0, raw); + break; + + case Signal::MPU_MAG_Y: + put_row ("magnetometer_channels", 1, raw); + break; + + case Signal::MPU_MAG_Z: + put_row ("magnetometer_channels", 2, raw); + break; + + default: + { + auto rows = rows_by_key (d, "other_channels"); + if (other_idx < (int)rows.size ()) + package[rows[other_idx]] = static_cast (raw); + other_idx++; + break; + } + } +} + +// --------------------------------------------------------------------------- +// Streaming thread +// --------------------------------------------------------------------------- + +void Shimmer3::read_thread () +{ + const json &d = board_descr["default"]; + int num_rows = d["num_rows"]; + int payload_size = packet_data_size * (int)samples_per_packet; + std::vector buf (payload_size); + + int ts_row = -1; + if (d.contains ("timestamp_channel")) + ts_row = d["timestamp_channel"]; + + int package_num_row = -1; + if (d.contains ("package_num_channel")) + package_num_row = d["package_num_channel"]; + + while (keep_alive) + { + uint8_t header = 0xFF; + int res = read_byte (header, true); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + if (!keep_alive) + break; + continue; + } + + if (header != Opcode::DATA_PACKET) + { + if (header == Opcode::INSTREAM_CMD_RESPONSE) + { + uint8_t discard = 0; + read_byte (discard, true); + } + continue; + } + + res = read_exact (buf.data (), payload_size, true); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + if (!keep_alive) + break; + continue; + } + + double packet_timestamp = get_timestamp (); + + for (int sample = 0; sample < (int)samples_per_packet; ++sample) + { + std::vector package (num_rows, 0.0); + + int offset = sample * packet_data_size; + int sample_end = offset + packet_data_size; + int other_idx = 0; + bool ok = true; + + for (const auto &field : packet_layout) + { + if ((field.format.width <= 0) || (offset + field.format.width > sample_end)) + { + ok = false; + break; + } + + int32_t raw = decode_field (field.format, buf.data () + offset); + offset += field.format.width; + + if (field.signal == Signal::TIMESTAMP) + continue; + + route_field (field.signal, raw, package.data (), other_idx); + } + + if (!ok || (offset != sample_end)) + { + continue; + } + + if (package_num_row >= 0) + package[package_num_row] = package_num; + package_num += 1.0; + + if (ts_row >= 0) + { + int samples_after = (int)samples_per_packet - sample - 1; + package[ts_row] = + packet_timestamp - static_cast (samples_after) / sampling_rate; + } + + push_package (package.data ()); + + bool notify = false; + { + std::lock_guard lk (sync_mutex); + if (!first_data_received) + { + first_data_received = true; + notify = true; + } + } + if (notify) + sync_cv.notify_one (); + } + } +} diff --git a/src/utils/inc/brainflow_constants.h b/src/utils/inc/brainflow_constants.h index 4a7f11e47..8b31b0df2 100644 --- a/src/utils/inc/brainflow_constants.h +++ b/src/utils/inc/brainflow_constants.h @@ -96,9 +96,10 @@ enum class BoardIds : int IRONBCI_32_BOARD = 65, NEUROPAWN_KNIGHT_BOARD_IMU = 66, MUSE_S_ATHENA_BOARD = 67, + SHIMMER3_BOARD = 68, // use it to iterate FIRST = PLAYBACK_FILE_BOARD, - LAST = MUSE_S_ATHENA_BOARD + LAST = SHIMMER3_BOARD }; enum class IpProtocolTypes : int