diff --git a/csharp_package/brainflow/brainflow/board_controller_library.cs b/csharp_package/brainflow/brainflow/board_controller_library.cs index 9f4257f3b..11e5bd0cf 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_ANTHENA_BOARD = 67 + MUSE_S_ANTHENA_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 e5de61fdc..b751c1b9a 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_ANTHENA_BOARD(67); + MUSE_S_ANTHENA_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 c0e3d43f8..5f7c2f8a1 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_ANTHENA_BOARD = 67 + SHIMMER3_BOARD = 68 end diff --git a/matlab_package/brainflow/BoardIds.m b/matlab_package/brainflow/BoardIds.m index 79c3988c4..fcbc90f80 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_ANTHENA_BOARD(67) + SHIMMER3_BOARD(68) end end diff --git a/nodejs_package/brainflow/brainflow.types.ts b/nodejs_package/brainflow/brainflow.types.ts index e41c3e275..4301e4782 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_ANTHENA_BOARD = 67 + MUSE_S_ANTHENA_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 82113be56..fa2aa9ef8 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_ANTHENA_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 070c6f9bc..ac7f1e066 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, MuseSAnthenaBoard = 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 be4ccc41f..13e912fa2 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" @@ -310,6 +311,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 388a9a175..c37f39a0f 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()} } }}; @@ -1198,6 +1199,47 @@ BrainFlowBoards::BrainFlowBoards() {"optical_channels", {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}, {"battery_channel", 17} }; + // Shimmer3 (ID 68) + // Default preset: IMU data (accel, gyro, mag) + brainflow_boards_json["boards"]["68"]["default"] = + { + {"name", "Shimmer3"}, + {"sampling_rate", 512}, + {"package_num_channel", 0}, + {"timestamp_channel", 10}, + {"marker_channel", 11}, + {"num_rows", 12}, + {"accel_channels", {1, 2, 3}}, + {"gyro_channels", {4, 5, 6}}, + {"magnetometer_channels", {7, 8, 9}} + }; + // Auxiliary preset: ExG data (ECG/EMG via ADS1292R chips) + brainflow_boards_json["boards"]["68"]["auxiliary"] = + { + {"name", "Shimmer3"}, + {"sampling_rate", 512}, + {"package_num_channel", 0}, + {"timestamp_channel", 7}, + {"marker_channel", 8}, + {"num_rows", 9}, + {"ecg_channels", {1, 2}}, + {"emg_channels", {3, 4}}, + {"other_channels", {5, 6}} // ExG1 status, ExG2 status + }; + // Ancillary preset: GSR, temperature, battery, pressure + brainflow_boards_json["boards"]["68"]["ancillary"] = + { + {"name", "Shimmer3"}, + {"sampling_rate", 64}, + {"package_num_channel", 0}, + {"timestamp_channel", 7}, + {"marker_channel", 8}, + {"num_rows", 9}, + {"eda_channels", {1}}, + {"temperature_channels", {2}}, + {"battery_channel", 3}, + {"other_channels", {4, 5, 6}} // pressure, internal ADC, external ADC + }; } BrainFlowBoards boards_struct; diff --git a/src/board_controller/build.cmake b/src/board_controller/build.cmake index 9a4206f0c..c549f5721 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/shimmer3/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/shimmer3/inc ) target_compile_definitions(${BOARD_CONTROLLER_NAME} PRIVATE NOMINMAX BRAINFLOW_VERSION=${BRAINFLOW_VERSION}) diff --git a/src/board_controller/shimmer3/inc/shimmer3.h b/src/board_controller/shimmer3/inc/shimmer3.h new file mode 100644 index 000000000..322ca7392 --- /dev/null +++ b/src/board_controller/shimmer3/inc/shimmer3.h @@ -0,0 +1,84 @@ +/* + * Shimmer3 board driver for BrainFlow. + * + * Adapted from the pyshimmer Python library by semoo-lab: + * https://github.com/seemoo-lab/pyshimmer + * + * Original work licensed under the GNU General Public License v3.0. + * See https://www.gnu.org/licenses/gpl-3.0.html for details. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "board.h" +#include "board_controller.h" +#include "serial.h" + +#include "shimmer3_defines.h" + + +class Shimmer3 : public Board +{ + +private: + volatile bool keep_alive; + volatile bool initialized; + + Serial *serial_port; + std::string port_name; + + std::thread streaming_thread; + std::mutex m; + + // Populated during prepare_session by querying the device + double sampling_rate; + std::vector active_channels; + std::vector active_dtypes; + int packet_size; + ShimmerAllCalibration calibration; + + // Serial helpers + int serial_write (const uint8_t *data, int len); + int serial_read (uint8_t *buf, int len); + int serial_read_byte (uint8_t &out); + int send_command (uint8_t cmd); + int send_command_with_args (uint8_t cmd, const uint8_t *args, int args_len); + int wait_for_ack (); + int read_response (uint8_t expected_code, uint8_t *buf, int buf_len, int &out_len); + + // Device commands + int get_firmware_version (uint16_t &fw_type, uint16_t &major, uint8_t &minor, uint8_t &rel); + int get_sampling_rate (double &sr); + int set_sampling_rate (double sr); + int get_shimmer_version (uint8_t &hw_ver); + int send_inquiry (double &sr, int &buf_size, std::vector &channels); + int set_sensors (uint32_t sensor_bitfield); + int start_streaming_cmd (); + int stop_streaming_cmd (); + int disable_status_ack (); + int get_all_calibration (ShimmerAllCalibration &cal); + + // Packet parsing + bool lookup_channel_dtype (EChannelType ch, ShimmerChannelDType &out); + void build_active_channel_list (const std::vector &inquiry_channels); + int compute_packet_size (); + + void read_thread (); + +public: + Shimmer3 (struct BrainFlowInputParams params); + ~Shimmer3 (); + + 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); +}; diff --git a/src/board_controller/shimmer3/inc/shimmer3_defines.h b/src/board_controller/shimmer3/inc/shimmer3_defines.h new file mode 100644 index 000000000..4b8b016f5 --- /dev/null +++ b/src/board_controller/shimmer3/inc/shimmer3_defines.h @@ -0,0 +1,412 @@ +/* + * Shimmer3 protocol definitions for BrainFlow. + * + * Adapted from the pyshimmer Python library by semoo-lab: + * https://github.com/seemoo-lab/pyshimmer + * + * Original work licensed under the GNU General Public License v3.0. + * See https://www.gnu.org/licenses/gpl-3.0.html for details. + */ + +#pragma once + +#include +#include +#include +#include + + +// Shimmer3 Bluetooth command/response byte constants. +// These are the opcodes used in the binary serial protocol between +// the host and the Shimmer3 over Bluetooth SPP. +namespace ShimmerBT +{ + // Acknowledgment and framing + constexpr uint8_t ACK_COMMAND_PROCESSED = 0xFF; + constexpr uint8_t INSTREAM_CMD_RESPONSE = 0x8A; + constexpr uint8_t DATA_PACKET = 0x00; + + // Inquiry + constexpr uint8_t INQUIRY_COMMAND = 0x01; + constexpr uint8_t INQUIRY_RESPONSE = 0x02; + + // Sampling rate + constexpr uint8_t GET_SAMPLING_RATE_COMMAND = 0x03; + constexpr uint8_t SAMPLING_RATE_RESPONSE = 0x04; + constexpr uint8_t SET_SAMPLING_RATE_COMMAND = 0x05; + + // Battery + constexpr uint8_t GET_BATTERY_COMMAND = 0x95; + constexpr uint8_t BATTERY_RESPONSE = 0x94; + + // Streaming control + constexpr uint8_t START_STREAMING_COMMAND = 0x07; + constexpr uint8_t STOP_STREAMING_COMMAND = 0x20; + + // Sensor selection + constexpr uint8_t SET_SENSORS_COMMAND = 0x08; + + // Hardware version + constexpr uint8_t GET_SHIMMER_VERSION_COMMAND = 0x3F; + constexpr uint8_t SHIMMER_VERSION_RESPONSE = 0x25; + + // Config time + constexpr uint8_t GET_CONFIGTIME_COMMAND = 0x87; + constexpr uint8_t CONFIGTIME_RESPONSE = 0x86; + constexpr uint8_t SET_CONFIGTIME_COMMAND = 0x85; + + // Real-time clock + constexpr uint8_t GET_RWC_COMMAND = 0x91; + constexpr uint8_t RWC_RESPONSE = 0x90; + constexpr uint8_t SET_RWC_COMMAND = 0x8F; + + // Device status + constexpr uint8_t GET_STATUS_COMMAND = 0x72; + constexpr uint8_t STATUS_RESPONSE = 0x71; + + // Firmware version + constexpr uint8_t GET_FW_VERSION_COMMAND = 0x2E; + constexpr uint8_t FW_VERSION_RESPONSE = 0x2F; + + // ExG (ADS1292R) register access + constexpr uint8_t GET_EXG_REGS_COMMAND = 0x63; + constexpr uint8_t EXG_REGS_RESPONSE = 0x62; + constexpr uint8_t SET_EXG_REGS_COMMAND = 0x61; + + // Experiment ID + constexpr uint8_t GET_EXPID_COMMAND = 0x7E; + constexpr uint8_t EXPID_RESPONSE = 0x7D; + constexpr uint8_t SET_EXPID_COMMAND = 0x7C; + + // Device name + constexpr uint8_t GET_SHIMMERNAME_COMMAND = 0x7B; + constexpr uint8_t SHIMMERNAME_RESPONSE = 0x7A; + constexpr uint8_t SET_SHIMMERNAME_COMMAND = 0x79; + + // Miscellaneous + constexpr uint8_t DUMMY_COMMAND = 0x96; + constexpr uint8_t START_LOGGING_COMMAND = 0x92; + constexpr uint8_t STOP_LOGGING_COMMAND = 0x93; + constexpr uint8_t ENABLE_STATUS_ACK_COMMAND = 0xA3; + + // Calibration + constexpr uint8_t GET_ALL_CALIBRATION_COMMAND = 0x2C; + constexpr uint8_t ALL_CALIBRATION_RESPONSE = 0x2D; + constexpr int ALL_CALIBRATION_LEN = 84; // 4 sensors, 21 bytes each +} + + +// Describes how a single channel's raw bytes should be decoded. +// Each channel in a Shimmer3 data packet has a fixed width, signedness, +// and byte order that we need to know at parse time. +struct ShimmerChannelDType +{ + int size; // width in bytes (1, 2, or 3) + bool is_signed; + bool little_endian; + + // Unpack raw bytes into a 32-bit signed integer, handling + // byte order and sign extension. + int32_t decode (const uint8_t *buf) const + { + uint32_t raw = 0; + if (little_endian) + { + for (int i = size - 1; i >= 0; i--) + raw = (raw << 8) | buf[i]; + } + else + { + for (int i = 0; i < size; i++) + raw = (raw << 8) | buf[i]; + } + + if (is_signed && (size < 4)) + { + uint32_t sign_bit = 1u << (size * 8 - 1); + if (raw & sign_bit) + raw |= ~((1u << (size * 8)) - 1); + } + return static_cast (raw); + } +}; + + +// Identifies a data channel within a Shimmer3 data packet. +// The numeric values match the byte codes the device sends +// in its inquiry response to describe the active channel layout. +// TIMESTAMP is synthetic and it's always present as the first +// field in every packet but isn't listed in the inquiry. +enum class EChannelType : uint16_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_REG_X = 0x07, + MAG_REG_Y = 0x08, + MAG_REG_Z = 0x09, + GYRO_X = 0x0A, + GYRO_Y = 0x0B, + GYRO_Z = 0x0C, + EXTERNAL_ADC_A0 = 0x0D, + EXTERNAL_ADC_A1 = 0x0E, + EXTERNAL_ADC_A2 = 0x0F, + INTERNAL_ADC_A3 = 0x10, + INTERNAL_ADC_A0 = 0x11, + INTERNAL_ADC_A1 = 0x12, + INTERNAL_ADC_A2 = 0x13, + ACCEL_HG_X = 0x14, + ACCEL_HG_Y = 0x15, + ACCEL_HG_Z = 0x16, + MAG_WR_X = 0x17, + MAG_WR_Y = 0x18, + MAG_WR_Z = 0x19, + TEMPERATURE = 0x1A, + PRESSURE = 0x1B, + GSR_RAW = 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, + + // Not a real on-wire channel — used internally to represent + // the 3-byte timestamp that leads every data packet. + TIMESTAMP = 0x100, +}; + + +// Groups of related sensors that can be enabled or disabled together +// via the 3-byte sensor bitfield. +enum class ESensorGroup : int +{ + ACCEL_LN = 0, + BATTERY, + EXT_CH_A0, + EXT_CH_A1, + EXT_CH_A2, + INT_CH_A0, + INT_CH_A1, + INT_CH_A2, + STRAIN, + INT_CH_A3, + GSR, + GYRO, + ACCEL_WR, + MAG_REG, + ACCEL_HG, + MAG_WR, + TEMP, + PRESSURE, + EXG1_24BIT, + EXG1_16BIT, + EXG2_24BIT, + EXG2_16BIT, + SENSOR_GROUP_COUNT +}; + + +// Shimmer3-specific numeric constants used for clock and timing math. +namespace Shimmer3Const +{ + // The Shimmer3's internal clock runs at 32768 Hz. + constexpr double DEV_CLOCK_RATE = 32768.0; + constexpr int ENABLED_SENSORS_LEN = 3; // sensor bitfield is 3 bytes wide + constexpr int TIMESTAMP_SIZE = 3; // on-wire timestamp is 3 bytes + constexpr uint32_t TIMESTAMP_MAX = (1u << 24); // 24-bit counter wraps here + + // The device stores sampling rate as a clock divider register. + // Actual Hz = 32768 / divider. + inline double dr2sr (uint16_t divider) + { + if (divider == 0) + return 0.0; + return DEV_CLOCK_RATE / static_cast (divider); + } + + inline uint16_t sr2dr (double hz) + { + if (hz <= 0.0) + return 0; + return static_cast (DEV_CLOCK_RATE / hz); + } + + inline double ticks2sec (uint64_t ticks) + { + return static_cast (ticks) / DEV_CLOCK_RATE; + } +} + + +// Maps a sensor group to its bit position in the 3-byte sensor +// enable/disable bitfield that the device accepts. +struct SensorBitEntry +{ + ESensorGroup group; + int bit_position; +}; + +static const SensorBitEntry SENSOR_BIT_ASSIGNMENT[] = { + {ESensorGroup::EXT_CH_A1, 0}, + {ESensorGroup::EXT_CH_A0, 1}, + {ESensorGroup::GSR, 2}, + {ESensorGroup::EXG2_24BIT, 3}, + {ESensorGroup::EXG1_24BIT, 4}, + {ESensorGroup::MAG_REG, 5}, + {ESensorGroup::GYRO, 6}, + {ESensorGroup::ACCEL_LN, 7}, + {ESensorGroup::INT_CH_A1, 8}, + {ESensorGroup::INT_CH_A0, 9}, + {ESensorGroup::INT_CH_A3, 10}, + {ESensorGroup::EXT_CH_A2, 11}, + {ESensorGroup::ACCEL_WR, 12}, + {ESensorGroup::BATTERY, 13}, + // bit 14 is unused + {ESensorGroup::STRAIN, 15}, + // bit 16 is unused + {ESensorGroup::TEMP, 17}, + {ESensorGroup::PRESSURE, 18}, + {ESensorGroup::EXG2_16BIT, 19}, + {ESensorGroup::EXG1_16BIT, 20}, + {ESensorGroup::MAG_WR, 21}, + {ESensorGroup::ACCEL_HG, 22}, + {ESensorGroup::INT_CH_A2, 23}, +}; +static constexpr int SENSOR_BIT_ASSIGNMENT_COUNT = + sizeof (SENSOR_BIT_ASSIGNMENT) / sizeof (SENSOR_BIT_ASSIGNMENT[0]); + + +// Pairs each channel type with its wire format. +// Channels marked valid=false are defined in the protocol but not +// available on the Shimmer3 hardware (e.g. high-g accel). +struct ChannelDTypeEntry +{ + EChannelType channel; + ShimmerChannelDType dtype; + bool valid; +}; + +static const ChannelDTypeEntry CH_DTYPE_TABLE[] = { + // Low-noise accelerometer: 2 bytes, signed, little-endian + {EChannelType::ACCEL_LN_X, {2, true, true}, true}, + {EChannelType::ACCEL_LN_Y, {2, true, true}, true}, + {EChannelType::ACCEL_LN_Z, {2, true, true}, true}, + // Battery voltage + {EChannelType::VBATT, {2, true, true}, true}, + // Wide-range accelerometer + {EChannelType::ACCEL_WR_X, {2, true, true}, true}, + {EChannelType::ACCEL_WR_Y, {2, true, true}, true}, + {EChannelType::ACCEL_WR_Z, {2, true, true}, true}, + // Magnetometer + {EChannelType::MAG_REG_X, {2, true, true}, true}, + {EChannelType::MAG_REG_Y, {2, true, true}, true}, + {EChannelType::MAG_REG_Z, {2, true, true}, true}, + // Gyroscope: 2 bytes, signed, big-endian + {EChannelType::GYRO_X, {2, true, false}, true}, + {EChannelType::GYRO_Y, {2, true, false}, true}, + {EChannelType::GYRO_Z, {2, true, false}, true}, + // External ADC channels + {EChannelType::EXTERNAL_ADC_A0, {2, false, true}, true}, + {EChannelType::EXTERNAL_ADC_A1, {2, false, true}, true}, + {EChannelType::EXTERNAL_ADC_A2, {2, false, true}, true}, + // Internal ADC channels + {EChannelType::INTERNAL_ADC_A3, {2, false, true}, true}, + {EChannelType::INTERNAL_ADC_A0, {2, false, true}, true}, + {EChannelType::INTERNAL_ADC_A1, {2, false, true}, true}, + {EChannelType::INTERNAL_ADC_A2, {2, false, true}, true}, + // High-g accel + {EChannelType::ACCEL_HG_X, {0, false, false}, false}, + {EChannelType::ACCEL_HG_Y, {0, false, false}, false}, + {EChannelType::ACCEL_HG_Z, {0, false, false}, false}, + // Wide-range mag + {EChannelType::MAG_WR_X, {0, false, false}, false}, + {EChannelType::MAG_WR_Y, {0, false, false}, false}, + {EChannelType::MAG_WR_Z, {0, false, false}, false}, + // Temperature and pressure + {EChannelType::TEMPERATURE, {2, false, false}, true}, + {EChannelType::PRESSURE, {3, false, false}, true}, + // GSR (galvanic skin response) + {EChannelType::GSR_RAW, {2, false, true}, true}, + // ExG chip 1 (ADS1292R): status + two data channels + {EChannelType::EXG1_STATUS, {1, false, true}, true}, + {EChannelType::EXG1_CH1_24BIT, {3, true, false}, true}, + {EChannelType::EXG1_CH2_24BIT, {3, true, false}, true}, + // ExG chip 2 + {EChannelType::EXG2_STATUS, {1, false, true}, true}, + {EChannelType::EXG2_CH1_24BIT, {3, true, false}, true}, + {EChannelType::EXG2_CH2_24BIT, {3, true, false}, true}, + // 16-bit ExG variants + {EChannelType::EXG1_CH1_16BIT, {2, true, false}, true}, + {EChannelType::EXG1_CH2_16BIT, {2, true, false}, true}, + {EChannelType::EXG2_CH1_16BIT, {2, true, false}, true}, + {EChannelType::EXG2_CH2_16BIT, {2, true, false}, true}, + // Strain gauge + {EChannelType::STRAIN_HIGH, {2, false, true}, true}, + {EChannelType::STRAIN_LOW, {2, false, true}, true}, + // Timestamp (3 bytes, unsigned, little-endian) + {EChannelType::TIMESTAMP, {3, false, true}, true}, +}; +static constexpr int CH_DTYPE_TABLE_COUNT = sizeof (CH_DTYPE_TABLE) / sizeof (CH_DTYPE_TABLE[0]); + + +// Per-sensor calibration parameters. +// The Shimmer3 stores offset, sensitivity, and a 3×3 alignment +// matrix for each of its four calibrated sensor groups. +struct ShimmerCalibrationSensor +{ + int16_t offset_bias[3]; // 3-axis offset, big-endian on wire + int16_t sensitivity[3]; // 3-axis sensitivity, big-endian on wire + int8_t alignment[9]; // 3×3 alignment matrix, row-major +}; + +// Holds calibration data for all four sensor groups: +// low-noise accel, gyro, magnetometer, wide-range accel. +// The device returns all 84 bytes in one response. +struct ShimmerAllCalibration +{ + ShimmerCalibrationSensor sensors[4]; // 0=ACCEL_LN, 1=GYRO, 2=MAG, 3=ACCEL_WR + bool valid; + + ShimmerAllCalibration () : valid (false) + { + memset (sensors, 0, sizeof (sensors)); + } + + bool parse (const uint8_t *data, int len) + { + if (len < 84) + return false; + + for (int s = 0; s < 4; s++) + { + const uint8_t *p = data + s * 21; + + for (int i = 0; i < 3; i++) + { + sensors[s].offset_bias[i] = static_cast ((p[i * 2] << 8) | p[i * 2 + 1]); + } + for (int i = 0; i < 3; i++) + { + sensors[s].sensitivity[i] = + static_cast ((p[6 + i * 2] << 8) | p[6 + i * 2 + 1]); + } + for (int i = 0; i < 9; i++) + { + sensors[s].alignment[i] = static_cast (p[12 + i]); + } + } + valid = true; + return true; + } +}; diff --git a/src/board_controller/shimmer3/shimmer3.cpp b/src/board_controller/shimmer3/shimmer3.cpp new file mode 100644 index 000000000..8563c9589 --- /dev/null +++ b/src/board_controller/shimmer3/shimmer3.cpp @@ -0,0 +1,858 @@ +/* + * Shimmer3 board driver for BrainFlow. + * + * Adapted from the pyshimmer Python library by semoo-lab: + * https://github.com/seemoo-lab/pyshimmer + * + * Original work licensed under the GNU General Public License v3.0. + * See https://www.gnu.org/licenses/gpl-3.0.html for details. + */ + +#include +#include +#include + +#include "shimmer3.h" +#include "shimmer3_defines.h" + +#include "board_controller.h" +#include "brainflow_constants.h" +#include "custom_cast.h" +#include "get_dll_dir.h" +#include "timestamp.h" + + +Shimmer3::Shimmer3 (struct BrainFlowInputParams params) + : Board ((int)BoardIds::SHIMMER3_BOARD, params) +{ + keep_alive = false; + initialized = false; + serial_port = NULL; + sampling_rate = 0.0; + packet_size = 0; +} + +Shimmer3::~Shimmer3 () +{ + skip_logs = true; + release_session (); +} + + +// --------------------------------------------------------------------------- +// Serial helpers +// --------------------------------------------------------------------------- + +int Shimmer3::serial_write (const uint8_t *data, int len) +{ + if (serial_port == NULL) + 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 to serial", len); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::serial_read (uint8_t *buf, int len) +{ + if (serial_port == NULL) + return (int)BrainFlowExitCodes::BOARD_NOT_CREATED_ERROR; + + int total_read = 0; + int max_attempts = len * 10; + while (total_read < len && max_attempts-- > 0) + { + int r = serial_port->read_from_serial_port ( + reinterpret_cast (buf + total_read), len - total_read); + if (r > 0) + { + total_read += r; + } + else if (r == 0) + { + std::this_thread::sleep_for (std::chrono::milliseconds (1)); + } + else + { + safe_logger (spdlog::level::err, "Serial read error"); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + } + if (total_read < len) + { + safe_logger (spdlog::level::err, "Serial read timeout: got {}/{}", total_read, len); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::serial_read_byte (uint8_t &out) +{ + return serial_read (&out, 1); +} + +int Shimmer3::send_command (uint8_t cmd) +{ + return serial_write (&cmd, 1); +} + +int Shimmer3::send_command_with_args (uint8_t cmd, const uint8_t *args, int args_len) +{ + int res = send_command (cmd); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + if (args != NULL && args_len > 0) + return serial_write (args, args_len); + return (int)BrainFlowExitCodes::STATUS_OK; +} + +// Keep reading bytes until we see the ACK byte. The device sometimes +// sends stale status responses before the ACK, so we skip those. +int Shimmer3::wait_for_ack () +{ + uint8_t byte = 0; + int max_tries = 256; + while (max_tries-- > 0) + { + int res = serial_read_byte (byte); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + if (byte == ShimmerBT::ACK_COMMAND_PROCESSED) + return (int)BrainFlowExitCodes::STATUS_OK; + if (byte == ShimmerBT::INSTREAM_CMD_RESPONSE) + { + uint8_t discard; + serial_read_byte (discard); + } + } + safe_logger (spdlog::level::err, "Timed out waiting for ACK"); + return (int)BrainFlowExitCodes::BOARD_NOT_READY_ERROR; +} + +int Shimmer3::read_response (uint8_t expected_code, uint8_t *buf, int buf_len, int &out_len) +{ + uint8_t code = 0; + int res = serial_read_byte (code); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + if (code != expected_code) + { + safe_logger ( + spdlog::level::err, "Expected response 0x{:02X}, got 0x{:02X}", expected_code, code); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + if (buf != NULL && buf_len > 0) + { + res = serial_read (buf, buf_len); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + out_len = buf_len; + } + else + { + out_len = 0; + } + return (int)BrainFlowExitCodes::STATUS_OK; +} + + +// --------------------------------------------------------------------------- +// Device commands +// --------------------------------------------------------------------------- + +int Shimmer3::get_firmware_version ( + uint16_t &fw_type, uint16_t &major, uint8_t &minor, uint8_t &rel) +{ + int res = send_command (ShimmerBT::GET_FW_VERSION_COMMAND); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + res = wait_for_ack (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint8_t buf[6]; + int out_len = 0; + res = read_response (ShimmerBT::FW_VERSION_RESPONSE, buf, 6, out_len); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + fw_type = static_cast (buf[0] | (buf[1] << 8)); + major = static_cast (buf[2] | (buf[3] << 8)); + minor = buf[4]; + rel = buf[5]; + + safe_logger ( + spdlog::level::info, "Shimmer FW: type={}, version={}.{}.{}", fw_type, major, minor, rel); + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::get_shimmer_version (uint8_t &hw_ver) +{ + int res = send_command (ShimmerBT::GET_SHIMMER_VERSION_COMMAND); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + res = wait_for_ack (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint8_t buf[1]; + int out_len = 0; + res = read_response (ShimmerBT::SHIMMER_VERSION_RESPONSE, buf, 1, out_len); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + hw_ver = buf[0]; + safe_logger (spdlog::level::info, "Shimmer HW version: {}", hw_ver); + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::get_sampling_rate (double &sr) +{ + int res = send_command (ShimmerBT::GET_SAMPLING_RATE_COMMAND); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + res = wait_for_ack (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint8_t buf[2]; + int out_len = 0; + res = read_response (ShimmerBT::SAMPLING_RATE_RESPONSE, buf, 2, out_len); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint16_t dr = static_cast (buf[0] | (buf[1] << 8)); + sr = Shimmer3Const::dr2sr (dr); + safe_logger (spdlog::level::info, "Shimmer sampling rate: {} Hz", sr); + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::set_sampling_rate (double sr) +{ + uint16_t dr = Shimmer3Const::sr2dr (sr); + uint8_t args[2] = {static_cast (dr & 0xFF), static_cast ((dr >> 8) & 0xFF)}; + int res = send_command_with_args (ShimmerBT::SET_SAMPLING_RATE_COMMAND, args, 2); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return wait_for_ack (); +} + +int Shimmer3::set_sensors (uint32_t sensor_bitfield) +{ + uint8_t args[3] = {static_cast (sensor_bitfield & 0xFF), + static_cast ((sensor_bitfield >> 8) & 0xFF), + static_cast ((sensor_bitfield >> 16) & 0xFF)}; + int res = send_command_with_args (ShimmerBT::SET_SENSORS_COMMAND, args, 3); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return wait_for_ack (); +} + +int Shimmer3::send_inquiry (double &sr, int &buf_size, std::vector &channels) +{ + int res = send_command (ShimmerBT::INQUIRY_COMMAND); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + res = wait_for_ack (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint8_t hdr_code = 0; + res = serial_read_byte (hdr_code); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + if (hdr_code != ShimmerBT::INQUIRY_RESPONSE) + { + safe_logger (spdlog::level::err, "Expected INQUIRY_RESPONSE 0x{:02X}, got 0x{:02X}", + ShimmerBT::INQUIRY_RESPONSE, hdr_code); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + + // Inquiry header: sampling rate (2) + sensor bitfield (4) + n_ch (1) + buf_size (1) + uint8_t hdr_buf[8]; + res = serial_read (hdr_buf, 8); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint16_t sr_val = static_cast (hdr_buf[0] | (hdr_buf[1] << 8)); + uint8_t n_ch = hdr_buf[6]; + buf_size = hdr_buf[7]; + sr = Shimmer3Const::dr2sr (sr_val); + + // Each following byte identifies one active channel + std::vector ch_bytes (n_ch); + res = serial_read (ch_bytes.data (), n_ch); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + channels.clear (); + for (int i = 0; i < n_ch; i++) + channels.push_back (static_cast (ch_bytes[i])); + + safe_logger ( + spdlog::level::info, "Inquiry: sr={} Hz, buf_size={}, channels={}", sr, buf_size, n_ch); + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::start_streaming_cmd () +{ + int res = send_command (ShimmerBT::START_STREAMING_COMMAND); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return wait_for_ack (); +} + +int Shimmer3::stop_streaming_cmd () +{ + int res = send_command (ShimmerBT::STOP_STREAMING_COMMAND); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return wait_for_ack (); +} + +int Shimmer3::disable_status_ack () +{ + uint8_t args[1] = {0x00}; + int res = send_command_with_args (ShimmerBT::ENABLE_STATUS_ACK_COMMAND, args, 1); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return wait_for_ack (); +} + +int Shimmer3::get_all_calibration (ShimmerAllCalibration &cal) +{ + int res = send_command (ShimmerBT::GET_ALL_CALIBRATION_COMMAND); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + res = wait_for_ack (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + uint8_t resp_code = 0; + res = serial_read_byte (resp_code); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + if (resp_code != ShimmerBT::ALL_CALIBRATION_RESPONSE) + { + safe_logger (spdlog::level::err, "Unexpected calibration response code"); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + + uint8_t cal_data[ShimmerBT::ALL_CALIBRATION_LEN]; + res = serial_read (cal_data, ShimmerBT::ALL_CALIBRATION_LEN); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + if (!cal.parse (cal_data, ShimmerBT::ALL_CALIBRATION_LEN)) + { + safe_logger (spdlog::level::warn, "Failed to parse calibration data"); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + + safe_logger (spdlog::level::info, "Calibration data retrieved"); + return (int)BrainFlowExitCodes::STATUS_OK; +} + + +// --------------------------------------------------------------------------- +// Packet parsing helpers +// --------------------------------------------------------------------------- + +bool Shimmer3::lookup_channel_dtype (EChannelType ch, ShimmerChannelDType &out) +{ + for (int i = 0; i < CH_DTYPE_TABLE_COUNT; i++) + { + if (CH_DTYPE_TABLE[i].channel == ch && CH_DTYPE_TABLE[i].valid) + { + out = CH_DTYPE_TABLE[i].dtype; + return true; + } + } + return false; +} + +// The device always sends a 3-byte timestamp at the start of each +// data packet, followed by the channels reported in the inquiry. +// We prepend a TIMESTAMP entry so the parsing loop can handle +// everything uniformly. +void Shimmer3::build_active_channel_list (const std::vector &inquiry_channels) +{ + active_channels.clear (); + active_dtypes.clear (); + + ShimmerChannelDType ts_dtype = {Shimmer3Const::TIMESTAMP_SIZE, false, true}; + active_channels.push_back (EChannelType::TIMESTAMP); + active_dtypes.push_back (ts_dtype); + + for (auto ch : inquiry_channels) + { + ShimmerChannelDType dtype; + if (lookup_channel_dtype (ch, dtype)) + { + active_channels.push_back (ch); + active_dtypes.push_back (dtype); + } + else + { + safe_logger (spdlog::level::warn, "No dtype for channel 0x{:02X}, skipping", + static_cast (ch)); + } + } +} + +int Shimmer3::compute_packet_size () +{ + int total = 0; + for (auto &dt : active_dtypes) + total += dt.size; + return total; +} + + +// --------------------------------------------------------------------------- +// Board interface: prepare_session +// --------------------------------------------------------------------------- + +int Shimmer3::prepare_session () +{ + if (initialized) + { + safe_logger (spdlog::level::info, "Session already prepared"); + return (int)BrainFlowExitCodes::STATUS_OK; + } + + if (params.serial_port.empty ()) + { + safe_logger (spdlog::level::err, "A serial port path is required (Bluetooth SPP)"); + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; + } + + port_name = params.serial_port; + + serial_port = Serial::create (port_name.c_str (), this); + int res = serial_port->open_serial_port (); + if (res < 0) + { + safe_logger (spdlog::level::err, "Could not open serial port {}", port_name); + delete serial_port; + serial_port = NULL; + return (int)BrainFlowExitCodes::UNABLE_TO_OPEN_PORT_ERROR; + } + + serial_port->set_serial_port_settings (1000, false); + + // Give the Bluetooth link a moment to settle + std::this_thread::sleep_for (std::chrono::milliseconds (500)); + + // Check that we can talk to the device + uint16_t fw_type = 0, fw_major = 0; + uint8_t fw_minor = 0, fw_rel = 0; + res = get_firmware_version (fw_type, fw_major, fw_minor, fw_rel); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + safe_logger (spdlog::level::err, "Could not read firmware version"); + serial_port->close_serial_port (); + delete serial_port; + serial_port = NULL; + return res; + } + + // Turn off periodic status ACKs so they don't clutter the stream. + // Older firmware may not support this, which is fine. + res = disable_status_ack (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + safe_logger ( + spdlog::level::warn, "Could not disable status ACK — old firmware? Continuing anyway"); + } + + // Ask the device which channels are currently enabled + double sr = 0.0; + int buf_sz = 0; + std::vector inquiry_channels; + res = send_inquiry (sr, buf_sz, inquiry_channels); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + safe_logger (spdlog::level::err, "Inquiry failed"); + serial_port->close_serial_port (); + delete serial_port; + serial_port = NULL; + return res; + } + + sampling_rate = sr; + build_active_channel_list (inquiry_channels); + packet_size = compute_packet_size (); + + safe_logger (spdlog::level::info, "Active channels: {}, packet size: {} bytes", + active_channels.size (), packet_size); + + // Try to grab calibration data. If it fails we'll just + // pass through raw values. It's not ideal but still usable. + res = get_all_calibration (calibration); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + safe_logger ( + spdlog::level::warn, "Could not retrieve calibration data; raw values will be used"); + } + + initialized = true; + return (int)BrainFlowExitCodes::STATUS_OK; +} + + +// --------------------------------------------------------------------------- +// Board interface: start_stream / stop_stream / release_session +// --------------------------------------------------------------------------- + +int Shimmer3::start_stream (int buffer_size, const char *streamer_params) +{ + if (!initialized) + { + safe_logger (spdlog::level::err, "Call prepare_session first"); + return (int)BrainFlowExitCodes::BOARD_NOT_CREATED_ERROR; + } + if (keep_alive) + { + safe_logger (spdlog::level::err, "Streaming thread is already running"); + return (int)BrainFlowExitCodes::STREAM_ALREADY_RUN_ERROR; + } + + int res = prepare_for_acquisition (buffer_size, streamer_params); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + + res = start_streaming_cmd (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + safe_logger (spdlog::level::err, "Start-streaming command failed"); + return res; + } + + keep_alive = true; + streaming_thread = std::thread ([this] { this->read_thread (); }); + + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::stop_stream () +{ + if (keep_alive) + { + keep_alive = false; + streaming_thread.join (); + + int res = stop_streaming_cmd (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + safe_logger (spdlog::level::warn, "Stop-streaming command failed"); + + return (int)BrainFlowExitCodes::STATUS_OK; + } + return (int)BrainFlowExitCodes::STREAM_THREAD_IS_NOT_RUNNING; +} + +int Shimmer3::release_session () +{ + if (initialized) + { + if (keep_alive) + stop_stream (); + + free_packages (); + initialized = false; + + if (serial_port != NULL) + { + serial_port->close_serial_port (); + delete serial_port; + serial_port = NULL; + } + + active_channels.clear (); + active_dtypes.clear (); + packet_size = 0; + sampling_rate = 0.0; + } + return (int)BrainFlowExitCodes::STATUS_OK; +} + + +// --------------------------------------------------------------------------- +// Board interface: config_board +// +// Accepted commands: +// "set_sampling_rate:" — change the sampling rate +// "set_sensors:" — change which sensors are enabled +// --------------------------------------------------------------------------- + +int Shimmer3::config_board (std::string config, std::string &response) +{ + if (!initialized) + return (int)BrainFlowExitCodes::BOARD_NOT_CREATED_ERROR; + + if (config.rfind ("set_sampling_rate:", 0) == 0) + { + double sr = std::stod (config.substr (18)); + int res = set_sampling_rate (sr); + if (res == (int)BrainFlowExitCodes::STATUS_OK) + { + sampling_rate = sr; + response = "OK"; + } + return res; + } + else if (config.rfind ("set_sensors:", 0) == 0) + { + uint32_t bitfield = static_cast (std::stoul (config.substr (12), nullptr, 16)); + int res = set_sensors (bitfield); + if (res == (int)BrainFlowExitCodes::STATUS_OK) + { + // The channel layout may have changed, so re-query + double sr = 0.0; + int buf_sz = 0; + std::vector inquiry_channels; + res = send_inquiry (sr, buf_sz, inquiry_channels); + if (res == (int)BrainFlowExitCodes::STATUS_OK) + { + sampling_rate = sr; + build_active_channel_list (inquiry_channels); + packet_size = compute_packet_size (); + response = "OK"; + } + } + return res; + } + + safe_logger (spdlog::level::warn, "Unrecognised config command: {}", config); + response = "UNKNOWN_COMMAND"; + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; +} + + +// --------------------------------------------------------------------------- +// Streaming thread +// +// Runs in the background after start_stream(). Reads one data packet +// at a time from the serial port, decodes each channel, and pushes +// the result into BrainFlow's ring buffer. +// --------------------------------------------------------------------------- + +void Shimmer3::read_thread () +{ + int num_rows = board_descr["default"]["num_rows"]; + std::vector pkt_buf (packet_size); + + while (keep_alive) + { + // Every data packet starts with a 0x00 header byte + uint8_t header = 0xFF; + int res = serial_read_byte (header); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + safe_logger (spdlog::level::warn, "Read error in streaming thread"); + continue; + } + + if (header != ShimmerBT::DATA_PACKET) + { + // Stale in-stream status response (consume and discard it) + if (header == ShimmerBT::INSTREAM_CMD_RESPONSE) + { + uint8_t discard; + serial_read_byte (discard); + } + continue; + } + + res = serial_read (pkt_buf.data (), packet_size); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + safe_logger (spdlog::level::warn, "Incomplete data packet"); + continue; + } + + // Decode each channel and place it in the right row + double *package = new double[num_rows]; + for (int i = 0; i < num_rows; i++) + package[i] = 0.0; + + int offset = 0; + for (size_t ch_idx = 0; ch_idx < active_channels.size (); ch_idx++) + { + if (offset + active_dtypes[ch_idx].size > packet_size) + { + safe_logger (spdlog::level::err, "Packet overrun while parsing"); + break; + } + + int32_t raw_val = active_dtypes[ch_idx].decode (pkt_buf.data () + offset); + offset += active_dtypes[ch_idx].size; + + EChannelType ch = active_channels[ch_idx]; + + switch (ch) + { + case EChannelType::TIMESTAMP: + { + double ts_sec = static_cast (static_cast (raw_val)) / + Shimmer3Const::DEV_CLOCK_RATE; + if (board_descr["default"].contains ("timestamp_channel")) + { + int ts_row = board_descr["default"]["timestamp_channel"]; + package[ts_row] = ts_sec; + } + break; + } + + case EChannelType::ACCEL_LN_X: + case EChannelType::ACCEL_LN_Y: + case EChannelType::ACCEL_LN_Z: + case EChannelType::ACCEL_WR_X: + case EChannelType::ACCEL_WR_Y: + case EChannelType::ACCEL_WR_Z: + { + if (board_descr["default"].contains ("accel_channels")) + { + auto &rows = board_descr["default"]["accel_channels"]; + int axis = -1; + if (ch == EChannelType::ACCEL_LN_X || ch == EChannelType::ACCEL_WR_X) + axis = 0; + else if (ch == EChannelType::ACCEL_LN_Y || ch == EChannelType::ACCEL_WR_Y) + axis = 1; + else if (ch == EChannelType::ACCEL_LN_Z || ch == EChannelType::ACCEL_WR_Z) + axis = 2; + if (axis >= 0 && axis < (int)rows.size ()) + package[rows[axis].get ()] = static_cast (raw_val); + } + break; + } + + case EChannelType::GYRO_X: + case EChannelType::GYRO_Y: + case EChannelType::GYRO_Z: + { + if (board_descr["default"].contains ("gyro_channels")) + { + auto &rows = board_descr["default"]["gyro_channels"]; + int axis = static_cast (ch) - static_cast (EChannelType::GYRO_X); + if (axis >= 0 && axis < (int)rows.size ()) + package[rows[axis].get ()] = static_cast (raw_val); + } + break; + } + + case EChannelType::MAG_REG_X: + case EChannelType::MAG_REG_Y: + case EChannelType::MAG_REG_Z: + { + if (board_descr["default"].contains ("magnetometer_channels")) + { + auto &rows = board_descr["default"]["magnetometer_channels"]; + int axis = + static_cast (ch) - static_cast (EChannelType::MAG_REG_X); + if (axis >= 0 && axis < (int)rows.size ()) + package[rows[axis].get ()] = static_cast (raw_val); + } + break; + } + + case EChannelType::GSR_RAW: + { + if (board_descr["default"].contains ("eda_channels")) + { + auto &rows = board_descr["default"]["eda_channels"]; + if (!rows.empty ()) + package[rows[0].get ()] = static_cast (raw_val); + } + break; + } + + case EChannelType::EXG1_CH1_24BIT: + case EChannelType::EXG1_CH2_24BIT: + case EChannelType::EXG1_CH1_16BIT: + case EChannelType::EXG1_CH2_16BIT: + case EChannelType::EXG2_CH1_24BIT: + case EChannelType::EXG2_CH2_24BIT: + case EChannelType::EXG2_CH1_16BIT: + case EChannelType::EXG2_CH2_16BIT: + { + if (board_descr["default"].contains ("ecg_channels")) + { + auto &rows = board_descr["default"]["ecg_channels"]; + // Figure out which ExG data channel this is by + // counting how many we've already seen. + int exg_idx = 0; + for (size_t k = 0; k < ch_idx; k++) + { + auto prev = active_channels[k]; + if (prev == EChannelType::EXG1_CH1_24BIT || + prev == EChannelType::EXG1_CH2_24BIT || + prev == EChannelType::EXG1_CH1_16BIT || + prev == EChannelType::EXG1_CH2_16BIT || + prev == EChannelType::EXG2_CH1_24BIT || + prev == EChannelType::EXG2_CH2_24BIT || + prev == EChannelType::EXG2_CH1_16BIT || + prev == EChannelType::EXG2_CH2_16BIT) + exg_idx++; + } + if (exg_idx < (int)rows.size ()) + package[rows[exg_idx].get ()] = static_cast (raw_val); + } + break; + } + + case EChannelType::TEMPERATURE: + { + if (board_descr["default"].contains ("temperature_channels")) + { + auto &rows = board_descr["default"]["temperature_channels"]; + if (!rows.empty ()) + package[rows[0].get ()] = static_cast (raw_val); + } + break; + } + + case EChannelType::VBATT: + { + if (board_descr["default"].contains ("battery_channel")) + { + int row = board_descr["default"]["battery_channel"]; + package[row] = static_cast (raw_val); + } + break; + } + + default: + { + // Anything else (ADC, pressure, strain, ExG status, …) + // goes into other_channels if the board description has them. + if (board_descr["default"].contains ("other_channels")) + { + auto &rows = board_descr["default"]["other_channels"]; + static int other_idx = 0; + if (other_idx < (int)rows.size ()) + { + package[rows[other_idx].get ()] = static_cast (raw_val); + other_idx++; + } + } + break; + } + } + } + + // Host-side wall-clock timestamp in the last row + if (board_descr["default"].contains ("timestamp_channel")) + { + int last_row = num_rows - 1; + package[last_row] = get_timestamp (); + } + + push_package (package); + delete[] package; + } +} diff --git a/src/utils/inc/brainflow_constants.h b/src/utils/inc/brainflow_constants.h index 93aecb3ca..3c78ab804 100644 --- a/src/utils/inc/brainflow_constants.h +++ b/src/utils/inc/brainflow_constants.h @@ -96,6 +96,7 @@ enum class BoardIds : int IRONBCI_32_BOARD = 65, NEUROPAWN_KNIGHT_BOARD_IMU = 66, MUSE_S_ANTHENA_BOARD = 67, + SHIMMER3_BOARD = 68, // use it to iterate FIRST = PLAYBACK_FILE_BOARD, LAST = MUSE_S_ANTHENA_BOARD