From a0468d6884be1193ad3d5c056e284fc642564f7d Mon Sep 17 00:00:00 2001 From: Vignesh Date: Wed, 29 Apr 2026 14:56:31 +0100 Subject: [PATCH 1/6] Add Shimmer3 board support (C++ API adapted from pyshimmer) Shimmer3 Bluetooth SPP driver for BrainFlow, ported from the semoo-lab/pyshimmer Python library (GNU General Public License v3.0). Includes protocol constants, channel definitions, calibration parsing, and the full Board interface implementation (prepare/start/stop/release/config). --- src/board_controller/shimmer3/inc/shimmer3.h | 84 ++ .../shimmer3/inc/shimmer3_defines.h | 412 +++++++++ src/board_controller/shimmer3/shimmer3.cpp | 858 ++++++++++++++++++ 3 files changed, 1354 insertions(+) create mode 100644 src/board_controller/shimmer3/inc/shimmer3.h create mode 100644 src/board_controller/shimmer3/inc/shimmer3_defines.h create mode 100644 src/board_controller/shimmer3/shimmer3.cpp 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; + } +} From ecc6814d9d68c559f7b5d3652d77da504a2190d0 Mon Sep 17 00:00:00 2001 From: Vignesh Date: Wed, 29 Apr 2026 15:11:49 +0100 Subject: [PATCH 2/6] Add Shimmer3 board (ID 68) to BrainFlow Register Shimmer3 as a new board: board description JSON, board ID constant, CMakeLists integration and other necessary changes. --- .../brainflow/board_controller_library.cs | 3 +- .../src/main/java/brainflow/BoardIds.java | 3 +- julia_package/brainflow/src/board_shim.jl | 1 + matlab_package/brainflow/BoardIds.m | 1 + nodejs_package/brainflow/brainflow.types.ts | 3 +- python_package/brainflow/board_shim.py | 1 + rust_package/brainflow/src/ffi/constants.rs | 1 + src/board_controller/board_controller.cpp | 4 ++ src/board_controller/brainflow_boards.cpp | 44 ++++++++++++++++++- src/board_controller/build.cmake | 2 + src/utils/inc/brainflow_constants.h | 1 + 11 files changed, 60 insertions(+), 4 deletions(-) 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..92967ea72 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 = 67 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 754e1501a..0c7250375 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 67) + // 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 578a9465d..3edce58a9 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/utils/inc/brainflow_constants.h b/src/utils/inc/brainflow_constants.h index 4a7f11e47..cf425ebbc 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_ATHENA_BOARD = 67, + SHIMMER3_BOARD = 68, // use it to iterate FIRST = PLAYBACK_FILE_BOARD, LAST = MUSE_S_ATHENA_BOARD From 9eea0b9de34f44371740591ea29654a71f573adf Mon Sep 17 00:00:00 2001 From: Vignesh Date: Wed, 29 Apr 2026 15:33:57 +0100 Subject: [PATCH 3/6] Fix more Shimmer3 Board ID in more spots --- julia_package/brainflow/src/board_shim.jl | 2 +- src/board_controller/brainflow_boards.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/julia_package/brainflow/src/board_shim.jl b/julia_package/brainflow/src/board_shim.jl index 92967ea72..b1e05f60c 100644 --- a/julia_package/brainflow/src/board_shim.jl +++ b/julia_package/brainflow/src/board_shim.jl @@ -69,7 +69,7 @@ export BrainFlowInputParams IRONBCI_32_BOARD = 65 NEUROPAWN_KNIGHT_BOARD_IMU = 66 MUSE_S_ATHENA_BOARD = 67 - SHIMMER3_BOARD = 67 + SHIMMER3_BOARD = 68 end diff --git a/src/board_controller/brainflow_boards.cpp b/src/board_controller/brainflow_boards.cpp index 0c7250375..34c547429 100644 --- a/src/board_controller/brainflow_boards.cpp +++ b/src/board_controller/brainflow_boards.cpp @@ -1199,7 +1199,7 @@ 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 67) + // Shimmer3 (ID 68) // Default preset: IMU data (accel, gyro, mag) brainflow_boards_json["boards"]["68"]["default"] = { From 3d0710121cad85b3b570ca3c5de353a4d1c4cb9b Mon Sep 17 00:00:00 2001 From: Vignesh Date: Fri, 29 May 2026 12:48:09 +0100 Subject: [PATCH 4/6] Fully reworked and refined Shimmer3 board support --- src/board_controller/brainflow_boards.cpp | 44 +- src/board_controller/build.cmake | 4 +- src/board_controller/shimmer/inc/shimmer3.h | 77 ++ .../shimmer/inc/shimmer3_defines.h | 300 ++++++ src/board_controller/shimmer/shimmer3.cpp | 694 ++++++++++++++ src/board_controller/shimmer3/inc/shimmer3.h | 84 -- .../shimmer3/inc/shimmer3_defines.h | 412 --------- src/board_controller/shimmer3/shimmer3.cpp | 858 ------------------ 8 files changed, 1083 insertions(+), 1390 deletions(-) create mode 100644 src/board_controller/shimmer/inc/shimmer3.h create mode 100644 src/board_controller/shimmer/inc/shimmer3_defines.h create mode 100644 src/board_controller/shimmer/shimmer3.cpp delete mode 100644 src/board_controller/shimmer3/inc/shimmer3.h delete mode 100644 src/board_controller/shimmer3/inc/shimmer3_defines.h delete mode 100644 src/board_controller/shimmer3/shimmer3.cpp diff --git a/src/board_controller/brainflow_boards.cpp b/src/board_controller/brainflow_boards.cpp index 34c547429..38dbc5e46 100644 --- a/src/board_controller/brainflow_boards.cpp +++ b/src/board_controller/brainflow_boards.cpp @@ -1199,46 +1199,22 @@ 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}, + {"sampling_rate", 512}, // configurable via config_board("sampling_rate:") {"package_num_channel", 0}, - {"timestamp_channel", 10}, - {"marker_channel", 11}, - {"num_rows", 12}, + {"timestamp_channel", 18}, + {"marker_channel", 19}, + {"num_rows", 20}, {"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 + {"magnetometer_channels", {7, 8, 9}}, + {"ecg_channels", {10, 11, 12, 13}}, + {"eda_channels", {14}}, + {"temperature_channels", {15}}, + {"battery_channel", 16}, + {"other_channels", {17}} }; } diff --git a/src/board_controller/build.cmake b/src/board_controller/build.cmake index 3edce58a9..d83a3d3b9 100644 --- a/src/board_controller/build.cmake +++ b/src/board_controller/build.cmake @@ -88,7 +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 + ${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/shimmer/shimmer3.cpp ) include (${CMAKE_CURRENT_SOURCE_DIR}/src/board_controller/ant_neuro/build.cmake) @@ -157,7 +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 + ${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..39a6a1aeb --- /dev/null +++ b/src/board_controller/shimmer/inc/shimmer3.h @@ -0,0 +1,77 @@ +#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 +{ +public: + Shimmer3 (struct BrainFlowInputParams params); + ~Shimmer3 (); + + 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; + +private: + // Describes one field inside the streamed data packet, in transmit order. + struct PacketField + { + shimmer3::Signal signal; + shimmer3::FieldFormat format; + }; + + volatile bool keep_alive; + volatile bool initialized; + + Serial *serial_port; + std::string port_name; + + std::thread streaming_thread; + + // First-data handshake: start_stream blocks until + // read_thread confirms real data packets are flowing, or times out. + std::mutex sync_mutex; + std::condition_variable sync_cv; + bool first_data_received; + + double sampling_rate; + double package_num; // local sequence counter (Shimmer3 + // packets carry no sequence number) + std::vector packet_layout; // leading timestamp + active signals + int packet_data_size; // bytes after the 0x00 header + + // -- low-level serial helpers -- + int write_bytes (const uint8_t *data, int len); + int read_exact (uint8_t *buf, int len); + int read_byte (uint8_t &out); + 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 -- + void build_packet_layout (const std::vector &signals); + void read_thread (); + int route_field (shimmer3::Signal s, int32_t raw, double *package, int &accel_axis, + int &gyro_axis, int &mag_axis, int &exg_idx, int &other_idx); +}; 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..ff0a54375 --- /dev/null +++ b/src/board_controller/shimmer/inc/shimmer3_defines.h @@ -0,0 +1,300 @@ +#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_A1 = 0x000002; // external ADC + constexpr uint32_t EXT_A0 = 0x000001; // external ADC + + constexpr uint32_t INT_A1 = 0x000100; // internal ADC + constexpr uint32_t INT_A0 = 0x000200; // internal ADC + constexpr uint32_t INT_A3 = 0x000400; // internal ADC + constexpr uint32_t EXT_A2 = 0x000800; // external ADC + constexpr uint32_t D_ACCEL = 0x001000; // wide-range (digital) accel + constexpr uint32_t VBATT = 0x002000; + constexpr uint32_t STRAIN = 0x008000; + + constexpr uint32_t TEMP = 0x020000; + constexpr uint32_t PRESSURE = 0x040000; + constexpr uint32_t EXG2_16BIT = 0x080000; + constexpr uint32_t EXG1_16BIT = 0x100000; + constexpr uint32_t MAG_WR = 0x200000; + constexpr uint32_t HIGH_G_ACCEL = 0x400000; + constexpr uint32_t INT_A2 = 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_A0 = 0x0D, + EXT_ADC_A1 = 0x0E, + EXT_ADC_A2 = 0x0F, + INT_ADC_A3 = 0x10, + INT_ADC_A0 = 0x11, + INT_ADC_A1 = 0x12, + INT_ADC_A2 = 0x13, + HIGH_G_ACCEL_X = 0x14, + HIGH_G_ACCEL_Y = 0x15, + HIGH_G_ACCEL_Z = 0x16, + MAG_WR_X = 0x17, + MAG_WR_Y = 0x18, + MAG_WR_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: the 3-byte timestamp prepended to every packet. Not a real + // signal ID, so we use a value outside the on-wire 8-bit range conceptually + // (kept inside uint8_t but never sent by the device). + 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 given signal. found is set false for signals + // that exist in the protocol but are not produced by Shimmer3 hardware. + 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_A0: + case Signal::EXT_ADC_A1: + case Signal::EXT_ADC_A2: + case Signal::INT_ADC_A3: + case Signal::INT_ADC_A0: + case Signal::INT_ADC_A1: + case Signal::INT_ADC_A2: + case Signal::GSR: + case Signal::STRAIN_HIGH: + case Signal::STRAIN_LOW: + return {2, false, 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}; + + // Not present on Shimmer3 (high-g accel, wide-range mag). + 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 (hz <= 0.0) + return 0; + return static_cast (CLOCK_HZ / hz + 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) + { + 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; + } + +} // namespace shimmer3 diff --git a/src/board_controller/shimmer/shimmer3.cpp b/src/board_controller/shimmer/shimmer3.cpp new file mode 100644 index 000000000..1c13296bc --- /dev/null +++ b/src/board_controller/shimmer/shimmer3.cpp @@ -0,0 +1,694 @@ +#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; +} + + +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; +} + +// Block until len bytes are read or we give up. +int Shimmer3::read_exact (uint8_t *buf, int len) +{ + if (serial_port == nullptr) + return (int)BrainFlowExitCodes::BOARD_NOT_CREATED_ERROR; + + int got = 0; + int idle = 0; + const int max_idle = 2000; // ~2s of empty reads before timing out + while (got < len) + { + int r = + serial_port->read_from_serial_port (reinterpret_cast (buf + got), len - got); + if (r > 0) + { + got += r; + idle = 0; + } + else if (r == 0) + { + if (++idle > max_idle) + { + safe_logger (spdlog::level::err, "serial read timeout {}/{}", got, len); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + std::this_thread::sleep_for (std::chrono::milliseconds (1)); + } + else + { + safe_logger (spdlog::level::err, "serial read error"); + return (int)BrainFlowExitCodes::GENERAL_ERROR; + } + } + return (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::read_byte (uint8_t &out) +{ + return read_exact (&out, 1); +} + +// Read bytes until ACK (0xFF). In-stream command responses (0x8A) may arrive +// first; consume their following opcode byte and keep looking. +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; + read_byte (discard); + } + } + 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; + + // 6 payload bytes: fw_id(2), major(2), minor(1), internal(1). + 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); +} + +// Ask the firmware not to prefix in-stream responses with an ACK byte, so the +// data stream stays clean. Older firmware may reject it; caller tolerates that. +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 (); +} + +// SET_SENSORS_COMMAND + 3 little-endian bitfield bytes ("0x08, 0x80, 0x00, 0x00"). +int Shimmer3::cmd_set_sensors (uint32_t bitfield) +{ + uint8_t buf[4] = {Opcode::SET_SENSORS_COMMAND, static_cast (bitfield & 0xFF), + static_cast ((bitfield >> 8) & 0xFF), + static_cast ((bitfield >> 16) & 0xFF)}; + int res = write_bytes (buf, 4); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return wait_for_ack (); +} + +// SET_SAMPLING_RATE_COMMAND + 2 little-endian divider bytes. +int Shimmer3::cmd_set_sampling_rate (double hz) +{ + uint16_t div = hz_to_divider (hz); + 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: learn the active sampling rate and the ordered list of channels so +// we can parse packets. Response layout (per BtStream manual, Table 5-1): +// 0x02 | rate(2) | config(4) | num_channels(1) | buffer_size(1) | chan 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)); + sampling_rate = divider_to_hz (div); + uint8_t num_channels = hdr[6]; + uint8_t buffer_size = hdr[7]; + + 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)); + build_packet_layout (signals); + + safe_logger (spdlog::level::info, + "inquiry: {} Hz, {} channels, buffer_size {}, packet data {} bytes", sampling_rate, + (int)num_channels, (int)buffer_size, 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); // ACK consumed opportunistically by reader +} + +// --------------------------------------------------------------------------- +// Packet layout +// --------------------------------------------------------------------------- + +// Every packet is: 0x00 header, 3-byte timestamp, then the active channels in +// inquiry order. We prepend a synthetic TIMESTAMP field so parsing is uniform. +void Shimmer3::build_packet_layout (const std::vector &signals) +{ + packet_layout.clear (); + bool found = false; + + packet_layout.push_back ({Signal::TIMESTAMP, format_for (Signal::TIMESTAMP, found)}); + + for (Signal s : signals) + { + FieldFormat fmt = format_for (s, found); + if (!found || fmt.width == 0) + { + safe_logger (spdlog::level::warn, "skipping unsupported signal 0x{:02X}", (int)s); + continue; + } + packet_layout.push_back ({s, fmt}); + } + + packet_data_size = 0; + for (const auto &f : packet_layout) + packet_data_size += f.format.width; +} + +// --------------------------------------------------------------------------- +// 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->open_serial_port () < 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_port->set_serial_port_settings (1000, false); + std::this_thread::sleep_for (std::chrono::milliseconds (500)); + + // Confirm we are actually talking to a Shimmer3 (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; + } + + cmd_get_fw_version (); // informational only + + // Best-effort: silence the in-stream ACK prefix. + 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) + 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; // reset sequence counter for the new stream + first_data_received = false; // reset handshake flag + + res = cmd_start_streaming (); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + safe_logger (spdlog::level::err, "failed to start streaming"); + return res; + } + + keep_alive = true; + streaming_thread = std::thread ([this] { this->read_thread (); }); + + // Wait until the read thread confirms real 0x00 data packets are flowing. + // The device already ACKed START_STREAMING above; this additionally guards + // against a paired-but-dead Bluetooth link that ACKs but never streams. + 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 (); + 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 (); + + cmd_stop_streaming (); + // Drain whatever is left in the buffer (trailing data / 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 (int)BrainFlowExitCodes::STATUS_OK; +} + +int Shimmer3::release_session () +{ + if (keep_alive) + stop_stream (); + + 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; + sampling_rate = 0.0; + return (int)BrainFlowExitCodes::STATUS_OK; +} + +// Supported config strings: +// "sampling_rate:" -> change sampling rate (re-runs inquiry) +// "sensors:" -> change enabled-sensor bitfield (re-runs inquiry) +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; + }; + + if (config.rfind ("sampling_rate:", 0) == 0) + { + double hz = std::stod (config.substr (14)); + int res = cmd_set_sampling_rate (hz); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return reinquire (response); + } + if (config.rfind ("sensors:", 0) == 0) + { + uint32_t bits = static_cast (std::stoul (config.substr (8), nullptr, 16)); + int res = cmd_set_sensors (bits); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + return res; + return reinquire (response); + } + + safe_logger (spdlog::level::warn, "unknown config '{}'", config); + response = "UNKNOWN_COMMAND"; + return (int)BrainFlowExitCodes::INVALID_ARGUMENTS_ERROR; +} + +// --------------------------------------------------------------------------- +// Routing decoded values to BrainFlow rows +// --------------------------------------------------------------------------- + +// Returns the row arrays from board_descr by key, or an empty vector. +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; +} + +int Shimmer3::route_field (Signal s, int32_t raw, double *package, int &accel_axis, int &gyro_axis, + int &mag_axis, int &exg_idx, int &other_idx) +{ + const json &d = board_descr["default"]; + + auto put_axis = [&] (const char *key, int axis) + { + auto rows = rows_by_key (d, key); + if (axis >= 0 && axis < (int)rows.size ()) + package[rows[axis]] = static_cast (raw); + }; + + switch (s) + { + case Signal::TIMESTAMP: + // Device timestamp converted to seconds; only stored if a generic + // "device timestamp" style row exists, otherwise ignored here. + break; + + case Signal::ACCEL_LN_X: + case Signal::ACCEL_LN_Y: + case Signal::ACCEL_LN_Z: + case Signal::ACCEL_WR_X: + case Signal::ACCEL_WR_Y: + case Signal::ACCEL_WR_Z: + put_axis ("accel_channels", accel_axis++ % 3); + break; + + case Signal::GYRO_X: + case Signal::GYRO_Y: + case Signal::GYRO_Z: + put_axis ("gyro_channels", gyro_axis++ % 3); + break; + + case Signal::MAG_X: + case Signal::MAG_Y: + case Signal::MAG_Z: + put_axis ("magnetometer_channels", mag_axis++ % 3); + break; + + case Signal::GSR: + { + auto rows = rows_by_key (d, "eda_channels"); + if (!rows.empty ()) + package[rows[0]] = static_cast (raw); + break; + } + + case Signal::TEMPERATURE: + { + auto rows = rows_by_key (d, "temperature_channels"); + if (!rows.empty ()) + package[rows[0]] = static_cast (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_CH2_24BIT: + case Signal::EXG2_CH1_24BIT: + case Signal::EXG2_CH2_24BIT: + case Signal::EXG1_CH1_16BIT: + case Signal::EXG1_CH2_16BIT: + case Signal::EXG2_CH1_16BIT: + case Signal::EXG2_CH2_16BIT: + { + auto rows = rows_by_key (d, "ecg_channels"); + if (exg_idx < (int)rows.size ()) + package[rows[exg_idx]] = static_cast (raw); + exg_idx++; + break; + } + + default: + { + // ADC channels, pressure, strain, ExG status, etc. + auto rows = rows_by_key (d, "other_channels"); + if (other_idx < (int)rows.size ()) + package[rows[other_idx]] = static_cast (raw); + other_idx++; + break; + } + } + return 0; +} + +// --------------------------------------------------------------------------- +// Streaming thread: resync on 0x00 header, read one packet, decode, push. +// --------------------------------------------------------------------------- + +void Shimmer3::read_thread () +{ + int num_rows = board_descr["default"]["num_rows"]; + std::vector buf (packet_data_size); + + int ts_row = -1; + if (board_descr["default"].contains ("timestamp_channel")) + ts_row = board_descr["default"]["timestamp_channel"]; + + int package_num_row = -1; + if (board_descr["default"].contains ("package_num_channel")) + package_num_row = board_descr["default"]["package_num_channel"]; + + while (keep_alive) + { + // Resync: every data packet starts with a 0x00 header byte. + uint8_t header = 0xFF; + if (read_byte (header) != (int)BrainFlowExitCodes::STATUS_OK) + continue; + if (header != Opcode::DATA_PACKET) + { + if (header == Opcode::INSTREAM_CMD_RESPONSE) + { + uint8_t discard; + read_byte (discard); + } + continue; + } + + if (read_exact (buf.data (), packet_data_size) != (int)BrainFlowExitCodes::STATUS_OK) + continue; + + double *package = new double[num_rows]; + for (int i = 0; i < num_rows; ++i) + package[i] = 0.0; + + int offset = 0; + int accel_axis = 0, gyro_axis = 0, mag_axis = 0, exg_idx = 0, other_idx = 0; + bool ok = true; + + for (const auto &field : packet_layout) + { + if (offset + field.format.width > packet_data_size) + { + 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, accel_axis, gyro_axis, mag_axis, exg_idx, other_idx); + } + + if (!ok) + { + delete[] package; + continue; + } + + if (package_num_row >= 0) + package[package_num_row] = package_num; + package_num += 1.0; + + if (ts_row >= 0) + package[ts_row] = get_timestamp (); + + push_package (package); + delete[] package; + + // Signal start_stream that data is flowing (first valid packet only). + if (!first_data_received) + { + { + std::lock_guard lk (sync_mutex); + first_data_received = true; + } + sync_cv.notify_one (); + } + } +} diff --git a/src/board_controller/shimmer3/inc/shimmer3.h b/src/board_controller/shimmer3/inc/shimmer3.h deleted file mode 100644 index 322ca7392..000000000 --- a/src/board_controller/shimmer3/inc/shimmer3.h +++ /dev/null @@ -1,84 +0,0 @@ -/* - * 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 deleted file mode 100644 index 4b8b016f5..000000000 --- a/src/board_controller/shimmer3/inc/shimmer3_defines.h +++ /dev/null @@ -1,412 +0,0 @@ -/* - * 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 deleted file mode 100644 index 8563c9589..000000000 --- a/src/board_controller/shimmer3/shimmer3.cpp +++ /dev/null @@ -1,858 +0,0 @@ -/* - * 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; - } -} From 43d29b725438a7aecf39816456a5138ad94d5d16 Mon Sep 17 00:00:00 2001 From: Vignesh Date: Tue, 28 Jul 2026 12:10:49 +0100 Subject: [PATCH 5/6] Fix: change last board id to Shimmer3 --- src/utils/inc/brainflow_constants.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/utils/inc/brainflow_constants.h b/src/utils/inc/brainflow_constants.h index cf425ebbc..8b31b0df2 100644 --- a/src/utils/inc/brainflow_constants.h +++ b/src/utils/inc/brainflow_constants.h @@ -99,7 +99,7 @@ enum class BoardIds : int SHIMMER3_BOARD = 68, // use it to iterate FIRST = PLAYBACK_FILE_BOARD, - LAST = MUSE_S_ATHENA_BOARD + LAST = SHIMMER3_BOARD }; enum class IpProtocolTypes : int From 0415e0393c3a7329c890cb0ee87b06bb1eb998e1 Mon Sep 17 00:00:00 2001 From: Vignesh Date: Tue, 28 Jul 2026 12:14:41 +0100 Subject: [PATCH 6/6] Fix (shimmer3): address runtime config and packet parsing feedback Make Shimmer3 runtime configuration exception-safe and validate sampling-rate and sensor-mask inputs before sending commands to the device. Check serial port configuration errors and expose the active device rate through get_board_sampling_rate(). Parse the complete inquiry response and consume every sample contained in a buffered Bluetooth packet. Route IMU axes by explicit signal ID, reject conflicting physical IMU sources, and fail safely on unsupported inquiry channels to preserve packet framing. Convert GSR ADC samples to conductance in microSiemens using the configured fixed range or the per-sample range bits in automatic mode. Add cancellable streaming reads, atomic thread control, and first-packet synchronization for reliable startup and shutdown. Correct Shimmer3 sensor masks, inquiry channel identifiers, wire formats, sampling-divider validation, and packet decoding helpers. --- src/board_controller/shimmer/inc/shimmer3.h | 32 +- .../shimmer/inc/shimmer3_defines.h | 103 +-- src/board_controller/shimmer/shimmer3.cpp | 607 +++++++++++++----- 3 files changed, 534 insertions(+), 208 deletions(-) diff --git a/src/board_controller/shimmer/inc/shimmer3.h b/src/board_controller/shimmer/inc/shimmer3.h index 39a6a1aeb..a13436c98 100644 --- a/src/board_controller/shimmer/inc/shimmer3.h +++ b/src/board_controller/shimmer/inc/shimmer3.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -17,46 +18,47 @@ class Shimmer3 : public Board { public: Shimmer3 (struct BrainFlowInputParams params); - ~Shimmer3 (); + ~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 the streamed data packet, in transmit order. + // Describes one field inside one streamed sample, in transmit order. struct PacketField { shimmer3::Signal signal; shimmer3::FieldFormat format; }; - volatile bool keep_alive; - volatile bool initialized; + std::atomic keep_alive; + bool initialized; Serial *serial_port; std::string port_name; std::thread streaming_thread; - // First-data handshake: start_stream blocks until - // read_thread confirms real data packets are flowing, or times out. + // 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; // local sequence counter (Shimmer3 - // packets carry no sequence number) - std::vector packet_layout; // leading timestamp + active signals - int packet_data_size; // bytes after the 0x00 header + 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); - int read_byte (uint8_t &out); + 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 -- @@ -70,8 +72,8 @@ class Shimmer3 : public Board int cmd_stop_streaming (); // -- helpers -- - void build_packet_layout (const std::vector &signals); + 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 (); - int route_field (shimmer3::Signal s, int32_t raw, double *package, int &accel_axis, - int &gyro_axis, int &mag_axis, int &exg_idx, int &other_idx); }; diff --git a/src/board_controller/shimmer/inc/shimmer3_defines.h b/src/board_controller/shimmer/inc/shimmer3_defines.h index ff0a54375..ec2c8ad38 100644 --- a/src/board_controller/shimmer/inc/shimmer3_defines.h +++ b/src/board_controller/shimmer/inc/shimmer3_defines.h @@ -1,7 +1,7 @@ #pragma once +#include #include -#include namespace shimmer3 { @@ -78,24 +78,23 @@ namespace shimmer3 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_A1 = 0x000002; // external ADC - constexpr uint32_t EXT_A0 = 0x000001; // external ADC + constexpr uint32_t EXT_A7 = 0x000002; // external ADC + constexpr uint32_t EXT_A6 = 0x000001; // external ADC - constexpr uint32_t INT_A1 = 0x000100; // internal ADC - constexpr uint32_t INT_A0 = 0x000200; // internal ADC - constexpr uint32_t INT_A3 = 0x000400; // internal ADC - constexpr uint32_t EXT_A2 = 0x000800; // 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 STRAIN = 0x008000; + constexpr uint32_t BRIDGE_AMP = 0x008000; - constexpr uint32_t TEMP = 0x020000; constexpr uint32_t PRESSURE = 0x040000; constexpr uint32_t EXG2_16BIT = 0x080000; constexpr uint32_t EXG1_16BIT = 0x100000; - constexpr uint32_t MAG_WR = 0x200000; - constexpr uint32_t HIGH_G_ACCEL = 0x400000; - constexpr uint32_t INT_A2 = 0x800000; + 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. @@ -115,19 +114,19 @@ namespace shimmer3 GYRO_X = 0x0A, GYRO_Y = 0x0B, GYRO_Z = 0x0C, - EXT_ADC_A0 = 0x0D, - EXT_ADC_A1 = 0x0E, - EXT_ADC_A2 = 0x0F, - INT_ADC_A3 = 0x10, - INT_ADC_A0 = 0x11, - INT_ADC_A1 = 0x12, - INT_ADC_A2 = 0x13, - HIGH_G_ACCEL_X = 0x14, - HIGH_G_ACCEL_Y = 0x15, - HIGH_G_ACCEL_Z = 0x16, - MAG_WR_X = 0x17, - MAG_WR_Y = 0x18, - MAG_WR_Z = 0x19, + 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, @@ -144,9 +143,7 @@ namespace shimmer3 STRAIN_HIGH = 0x27, STRAIN_LOW = 0x28, - // Synthetic: the 3-byte timestamp prepended to every packet. Not a real - // signal ID, so we use a value outside the on-wire 8-bit range conceptually - // (kept inside uint8_t but never sent by the device). + // Synthetic timestamp field used only by the host-side packet layout. TIMESTAMP = 0xFE, }; @@ -182,8 +179,7 @@ namespace shimmer3 return static_cast (raw); } - // Return the wire format for a given signal. found is set false for signals - // that exist in the protocol but are not produced by Shimmer3 hardware. + // Return the wire format for a signal. found is false for unsupported IDs. inline FieldFormat format_for (Signal s, bool &found) { found = true; @@ -215,18 +211,30 @@ namespace shimmer3 return {2, true, false}; // ADC channels: unsigned 16-bit little-endian. - case Signal::EXT_ADC_A0: - case Signal::EXT_ADC_A1: - case Signal::EXT_ADC_A2: - case Signal::INT_ADC_A3: - case Signal::INT_ADC_A0: + case Signal::EXT_ADC_A7: + case Signal::EXT_ADC_A6: + case Signal::EXT_ADC_A15: case Signal::INT_ADC_A1: - case Signal::INT_ADC_A2: + 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}; @@ -251,7 +259,6 @@ namespace shimmer3 case Signal::TIMESTAMP: return {TIMESTAMP_BYTES, false, true}; - // Not present on Shimmer3 (high-g accel, wide-range mag). default: found = false; return {0, false, false}; @@ -267,9 +274,14 @@ namespace shimmer3 inline uint16_t hz_to_divider (double hz) { - if (hz <= 0.0) + if (!std::isfinite (hz) || (hz <= 0.0)) return 0; - return static_cast (CLOCK_HZ / hz + 0.5); + + double divider = CLOCK_HZ / hz; + if ((divider < 1.0) || (divider > 65535.0)) + return 0; + + return static_cast (divider + 0.5); } // -------------------------- CRC -------------------------- @@ -289,6 +301,9 @@ namespace shimmer3 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]); @@ -297,4 +312,14 @@ namespace shimmer3 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 index 1c13296bc..d71e1584b 100644 --- a/src/board_controller/shimmer/shimmer3.cpp +++ b/src/board_controller/shimmer/shimmer3.cpp @@ -1,6 +1,9 @@ #include -#include +#include +#include +#include #include +#include #include "shimmer3.h" @@ -19,9 +22,10 @@ Shimmer3::Shimmer3 (struct BrainFlowInputParams params) 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; @@ -46,49 +50,59 @@ int Shimmer3::write_bytes (const uint8_t *data, int len) return (int)BrainFlowExitCodes::STATUS_OK; } -// Block until len bytes are read or we give up. -int Shimmer3::read_exact (uint8_t *buf, int len) +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 = 2000; // ~2s of empty reads before timing out + const int max_idle = 2; + while (got < len) { - int r = + 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 (r > 0) + + if (cancellable && !keep_alive) + return (int)BrainFlowExitCodes::STREAM_THREAD_IS_NOT_RUNNING; + + if (res > 0) { - got += r; + got += res; idle = 0; } - else if (r == 0) + else if (res == 0) { if (++idle > max_idle) { safe_logger (spdlog::level::err, "serial read timeout {}/{}", got, len); return (int)BrainFlowExitCodes::GENERAL_ERROR; } - std::this_thread::sleep_for (std::chrono::milliseconds (1)); } else { - safe_logger (spdlog::level::err, "serial read error"); + 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) +int Shimmer3::read_byte (uint8_t &out, bool cancellable) { - return read_exact (&out, 1); + return read_exact (&out, 1, cancellable); } -// Read bytes until ACK (0xFF). In-stream command responses (0x8A) may arrive -// first; consume their following opcode byte and keep looking. +// Read bytes until ACK. In-stream responses may arrive first. int Shimmer3::wait_for_ack () { for (int tries = 0; tries < 512; ++tries) @@ -101,10 +115,13 @@ int Shimmer3::wait_for_ack () return (int)BrainFlowExitCodes::STATUS_OK; if (b == Opcode::INSTREAM_CMD_RESPONSE) { - uint8_t discard; - read_byte (discard); + 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; } @@ -119,6 +136,7 @@ int Shimmer3::cmd_get_fw_version () 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; @@ -130,7 +148,6 @@ int Shimmer3::cmd_get_fw_version () if (resp != Opcode::FW_VERSION_RESPONSE) return (int)BrainFlowExitCodes::GENERAL_ERROR; - // 6 payload bytes: fw_id(2), major(2), minor(1), internal(1). uint8_t b[6]; res = read_exact (b, 6); if (res != (int)BrainFlowExitCodes::STATUS_OK) @@ -148,6 +165,7 @@ int Shimmer3::cmd_get_hw_version (uint8_t &hw_version) 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; @@ -162,8 +180,7 @@ int Shimmer3::cmd_get_hw_version (uint8_t &hw_version) return read_byte (hw_version); } -// Ask the firmware not to prefix in-stream responses with an ACK byte, so the -// data stream stays clean. Older firmware may reject it; caller tolerates that. +// 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}; @@ -173,42 +190,52 @@ int Shimmer3::cmd_disable_instream_ack_prefix () return wait_for_ack (); } -// SET_SENSORS_COMMAND + 3 little-endian bitfield bytes ("0x08, 0x80, 0x00, 0x00"). int Shimmer3::cmd_set_sensors (uint32_t bitfield) { - uint8_t buf[4] = {Opcode::SET_SENSORS_COMMAND, static_cast (bitfield & 0xFF), - static_cast ((bitfield >> 8) & 0xFF), - static_cast ((bitfield >> 16) & 0xFF)}; + 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 (); } -// SET_SAMPLING_RATE_COMMAND + 2 little-endian divider bytes. int Shimmer3::cmd_set_sampling_rate (double hz) { - uint16_t div = hz_to_divider (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: learn the active sampling rate and the ordered list of channels so -// we can parse packets. Response layout (per BtStream manual, Table 5-1): -// 0x02 | rate(2) | config(4) | num_channels(1) | buffer_size(1) | chan IDs... +// 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; @@ -229,9 +256,23 @@ int Shimmer3::cmd_inquiry () 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]; - uint8_t buffer_size = hdr[7]; + 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) @@ -244,11 +285,16 @@ int Shimmer3::cmd_inquiry () std::vector signals; for (uint8_t id : ids) signals.push_back (static_cast (id)); - build_packet_layout (signals); + + 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 {}, packet data {} bytes", sampling_rate, - (int)num_channels, (int)buffer_size, packet_data_size); + "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; } @@ -264,36 +310,99 @@ int Shimmer3::cmd_start_streaming () int Shimmer3::cmd_stop_streaming () { uint8_t cmd = Opcode::STOP_STREAMING_COMMAND; - return write_bytes (&cmd, 1); // ACK consumed opportunistically by reader + return write_bytes (&cmd, 1); } // --------------------------------------------------------------------------- // Packet layout // --------------------------------------------------------------------------- -// Every packet is: 0x00 header, 3-byte timestamp, then the active channels in -// inquiry order. We prepend a synthetic TIMESTAMP field so parsing is uniform. -void Shimmer3::build_packet_layout (const std::vector &signals) +// 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, format_for (Signal::TIMESTAMP, found)}); + packet_layout.push_back ({Signal::TIMESTAMP, fmt}); + packet_data_size += fmt.width; for (Signal s : signals) { - FieldFormat fmt = format_for (s, found); - if (!found || fmt.width == 0) + fmt = format_for (s, found); + if (!found || (fmt.width == 0)) { - safe_logger (spdlog::level::warn, "skipping unsupported signal 0x{:02X}", (int)s); - continue; + 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; } - packet_data_size = 0; - for (const auto &f : packet_layout) - packet_data_size += f.format.width; + return true; } // --------------------------------------------------------------------------- @@ -313,17 +422,32 @@ int Shimmer3::prepare_session () port_name = params.serial_port; serial_port = Serial::create (port_name.c_str (), this); - if (serial_port->open_serial_port () < 0) + 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_port->set_serial_port_settings (1000, false); + + 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 we are actually talking to a Shimmer3 (reject Shimmer3R). + // 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) @@ -340,9 +464,9 @@ int Shimmer3::prepare_session () return (int)BrainFlowExitCodes::BOARD_NOT_READY_ERROR; } - cmd_get_fw_version (); // informational only + if (cmd_get_fw_version () != (int)BrainFlowExitCodes::STATUS_OK) + safe_logger (spdlog::level::warn, "could not read firmware version"); - // Best-effort: silence the in-stream ACK prefix. if (cmd_disable_instream_ack_prefix () != (int)BrainFlowExitCodes::STATUS_OK) safe_logger (spdlog::level::warn, "could not disable in-stream ACK prefix (old firmware?)"); @@ -363,7 +487,7 @@ int Shimmer3::start_stream (int buffer_size, const char *streamer_params) return (int)BrainFlowExitCodes::BOARD_NOT_CREATED_ERROR; if (keep_alive) return (int)BrainFlowExitCodes::STREAM_ALREADY_RUN_ERROR; - if (packet_data_size <= 0) + 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; @@ -372,22 +496,23 @@ int Shimmer3::start_stream (int buffer_size, const char *streamer_params) if (res != (int)BrainFlowExitCodes::STATUS_OK) return res; - package_num = 0.0; // reset sequence counter for the new stream - first_data_received = false; // reset handshake flag + 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 (); }); - // Wait until the read thread confirms real 0x00 data packets are flowing. - // The device already ACKed START_STREAMING above; this additionally guards - // against a paired-but-dead Bluetooth link that ACKs but never streams. int timeout = params.timeout > 0 ? params.timeout : 5; std::unique_lock lk (sync_mutex); bool got_data = sync_cv.wait_for ( @@ -401,13 +526,13 @@ int Shimmer3::start_stream (int buffer_size, const char *streamer_params) 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) @@ -417,8 +542,9 @@ int Shimmer3::stop_stream () if (streaming_thread.joinable ()) streaming_thread.join (); - cmd_stop_streaming (); - // Drain whatever is left in the buffer (trailing data / ACK). + int res = cmd_stop_streaming (); + + // Drain trailing data and ACK. if (serial_port != nullptr) { uint8_t junk[256]; @@ -430,13 +556,16 @@ int Shimmer3::stop_stream () break; } } - return (int)BrainFlowExitCodes::STATUS_OK; + + return res; } int Shimmer3::release_session () { if (keep_alive) stop_stream (); + else if (streaming_thread.joinable ()) + streaming_thread.join (); if (initialized) free_packages (); @@ -451,13 +580,31 @@ int Shimmer3::release_session () 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:" -> change sampling rate (re-runs inquiry) -// "sensors:" -> change enabled-sensor bitfield (re-runs inquiry) +// "sampling_rate:" +// "sensors:" int Shimmer3::config_board (std::string config, std::string &response) { if (!initialized) @@ -476,21 +623,81 @@ int Shimmer3::config_board (std::string config, std::string &response) return r; }; - if (config.rfind ("sampling_rate:", 0) == 0) + try { - double hz = std::stod (config.substr (14)); - int res = cmd_set_sampling_rate (hz); - if (res != (int)BrainFlowExitCodes::STATUS_OK) - return res; - return reinquire (response); + 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); + } } - if (config.rfind ("sensors:", 0) == 0) + catch (const std::exception &e) { - uint32_t bits = static_cast (std::stoul (config.substr (8), nullptr, 16)); - int res = cmd_set_sensors (bits); - if (res != (int)BrainFlowExitCodes::STATUS_OK) - return res; - return reinquire (response); + 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); @@ -498,11 +705,44 @@ int Shimmer3::config_board (std::string config, std::string &response) 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 // --------------------------------------------------------------------------- -// Returns the row arrays from board_descr by key, or an empty vector. static std::vector rows_by_key (const json &descr, const char *key) { std::vector out; @@ -514,61 +754,74 @@ static std::vector rows_by_key (const json &descr, const char *key) return out; } -int Shimmer3::route_field (Signal s, int32_t raw, double *package, int &accel_axis, int &gyro_axis, - int &mag_axis, int &exg_idx, int &other_idx) +void Shimmer3::route_field (Signal s, int32_t raw, double *package, int &other_idx) { const json &d = board_descr["default"]; - auto put_axis = [&] (const char *key, int axis) + auto put_row = [&] (const char *key, int index, double value) { auto rows = rows_by_key (d, key); - if (axis >= 0 && axis < (int)rows.size ()) - package[rows[axis]] = static_cast (raw); + if ((index >= 0) && (index < (int)rows.size ())) + package[rows[index]] = value; }; switch (s) { case Signal::TIMESTAMP: - // Device timestamp converted to seconds; only stored if a generic - // "device timestamp" style row exists, otherwise ignored here. break; case Signal::ACCEL_LN_X: - case Signal::ACCEL_LN_Y: - case Signal::ACCEL_LN_Z: 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_axis ("accel_channels", accel_axis++ % 3); + 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_axis ("gyro_channels", gyro_axis++ % 3); + 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_axis ("magnetometer_channels", mag_axis++ % 3); + put_row ("magnetometer_channels", 2, raw); break; case Signal::GSR: { - auto rows = rows_by_key (d, "eda_channels"); - if (!rows.empty ()) - package[rows[0]] = static_cast (raw); + 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: - { - auto rows = rows_by_key (d, "temperature_channels"); - if (!rows.empty ()) - package[rows[0]] = static_cast (raw); + put_row ("temperature_channels", 0, raw); break; - } case Signal::VBATT: if (d.contains ("battery_channel")) @@ -576,24 +829,51 @@ int Shimmer3::route_field (Signal s, int32_t raw, double *package, int &accel_ax break; case Signal::EXG1_CH1_24BIT: - case Signal::EXG1_CH2_24BIT: - case Signal::EXG2_CH1_24BIT: - case Signal::EXG2_CH2_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: - { - auto rows = rows_by_key (d, "ecg_channels"); - if (exg_idx < (int)rows.size ()) - package[rows[exg_idx]] = static_cast (raw); - exg_idx++; + 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: { - // ADC channels, pressure, strain, ExG status, etc. auto rows = rows_by_key (d, "other_channels"); if (other_idx < (int)rows.size ()) package[rows[other_idx]] = static_cast (raw); @@ -601,94 +881,113 @@ int Shimmer3::route_field (Signal s, int32_t raw, double *package, int &accel_ax break; } } - return 0; } // --------------------------------------------------------------------------- -// Streaming thread: resync on 0x00 header, read one packet, decode, push. +// Streaming thread // --------------------------------------------------------------------------- void Shimmer3::read_thread () { - int num_rows = board_descr["default"]["num_rows"]; - std::vector buf (packet_data_size); + 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 (board_descr["default"].contains ("timestamp_channel")) - ts_row = board_descr["default"]["timestamp_channel"]; + if (d.contains ("timestamp_channel")) + ts_row = d["timestamp_channel"]; int package_num_row = -1; - if (board_descr["default"].contains ("package_num_channel")) - package_num_row = board_descr["default"]["package_num_channel"]; + if (d.contains ("package_num_channel")) + package_num_row = d["package_num_channel"]; while (keep_alive) { - // Resync: every data packet starts with a 0x00 header byte. uint8_t header = 0xFF; - if (read_byte (header) != (int)BrainFlowExitCodes::STATUS_OK) + 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; - read_byte (discard); + uint8_t discard = 0; + read_byte (discard, true); } continue; } - if (read_exact (buf.data (), packet_data_size) != (int)BrainFlowExitCodes::STATUS_OK) + res = read_exact (buf.data (), payload_size, true); + if (res != (int)BrainFlowExitCodes::STATUS_OK) + { + if (!keep_alive) + break; continue; + } - double *package = new double[num_rows]; - for (int i = 0; i < num_rows; ++i) - package[i] = 0.0; - - int offset = 0; - int accel_axis = 0, gyro_axis = 0, mag_axis = 0, exg_idx = 0, other_idx = 0; - bool ok = true; + double packet_timestamp = get_timestamp (); - for (const auto &field : packet_layout) + for (int sample = 0; sample < (int)samples_per_packet; ++sample) { - if (offset + field.format.width > packet_data_size) + 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) { - ok = false; - break; - } - int32_t raw = decode_field (field.format, buf.data () + offset); - offset += field.format.width; + if ((field.format.width <= 0) || (offset + field.format.width > sample_end)) + { + ok = false; + break; + } - if (field.signal == Signal::TIMESTAMP) - continue; + int32_t raw = decode_field (field.format, buf.data () + offset); + offset += field.format.width; - route_field ( - field.signal, raw, package, accel_axis, gyro_axis, mag_axis, exg_idx, other_idx); - } + if (field.signal == Signal::TIMESTAMP) + continue; - if (!ok) - { - delete[] package; - continue; - } + route_field (field.signal, raw, package.data (), other_idx); + } - if (package_num_row >= 0) - package[package_num_row] = package_num; - package_num += 1.0; + 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) - package[ts_row] = get_timestamp (); + 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); - delete[] package; + push_package (package.data ()); - // Signal start_stream that data is flowing (first valid packet only). - if (!first_data_received) - { + bool notify = false; { std::lock_guard lk (sync_mutex); - first_data_received = true; + if (!first_data_received) + { + first_data_received = true; + notify = true; + } } - sync_cv.notify_one (); + if (notify) + sync_cv.notify_one (); } } }