Skip to content

Commit

Permalink
Update ganglion decompression (#655)
Browse files Browse the repository at this point in the history 
* Support Ganglion firmware v3
  • Loading branch information
@philippitts
philippitts authored Aug 4, 2023
1 parent 7b23b19 commit 0dbd69d
Show file tree
Hide file tree
Showing 7 changed files with 359 additions and 194 deletions.
251 changes: 156 additions & 95 deletions src/board_controller/openbci/ganglion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -226,9 +226,7 @@ void Ganglion::read_thread ()
int max_attempts = params.timeout * 1000 / sleep_time;
float last_data[8] = {0};

double accel_x = 0.;
double accel_y = 0.;
double accel_z = 0.;
double acceleration[3] = {0.};

double resist_ref = 0.0;
double resist_first = 0.0;
Expand Down Expand Up @@ -267,72 +265,16 @@ void Ganglion::read_thread ()
safe_logger (spdlog::level::debug, "start streaming");
}

// delta holds 8 nums (4 by each package)
float delta[8] = {0.f};
int bits_per_num = 0;
unsigned char package_bits[160] = {0}; // 20 * 8
for (int i = 0; i < 20; i++)
if (data.data[0] <= 200)
{
uchar_to_bits (data.data[i], package_bits + i * 8);
}

// no compression, used to init variable
if (data.data[0] == 0)
{
// shift the last data packet to make room for a newer one
last_data[0] = last_data[4];
last_data[1] = last_data[5];
last_data[2] = last_data[6];
last_data[3] = last_data[7];

// add new packet
last_data[4] = (float)cast_24bit_to_int32 (data.data + 1);
last_data[5] = (float)cast_24bit_to_int32 (data.data + 4);
last_data[6] = (float)cast_24bit_to_int32 (data.data + 7);
last_data[7] = (float)cast_24bit_to_int32 (data.data + 10);

// scale new packet and insert into result
package[board_descr["default"]["package_num_channel"].get<int> ()] = 0.;
package[board_descr["default"]["eeg_channels"][0].get<int> ()] =
eeg_scale * last_data[4];
package[board_descr["default"]["eeg_channels"][1].get<int> ()] =
eeg_scale * last_data[5];
package[board_descr["default"]["eeg_channels"][2].get<int> ()] =
eeg_scale * last_data[6];
package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
eeg_scale * last_data[7];
package[board_descr["default"]["accel_channels"][0].get<int> ()] = accel_x;
package[board_descr["default"]["accel_channels"][1].get<int> ()] = accel_y;
package[board_descr["default"]["accel_channels"][2].get<int> ()] = accel_z;
package[board_descr["default"]["timestamp_channel"].get<int> ()] = data.timestamp;
push_package (package);
continue;
}
// 18 bit compression, sends delta from previous value instead of real value!
else if ((data.data[0] >= 1) && (data.data[0] <= 100))
{
int last_digit = data.data[0] % 10;
switch (last_digit)
if (firmware == 3)
{
// accel data is signed, so we must cast it to signed char
// due to a known bug in ganglion firmware, we must swap x and z, and invert z.
case 0:
accel_z = -accel_scale * (char)data.data[19];
break;
case 1:
accel_y = accel_scale * (char)data.data[19];
break;
case 2:
accel_x = accel_scale * (char)data.data[19];
break;
default:
break;
decompress_firmware_3 (&data, last_data, acceleration);
}
else if (firmware == 2)
{
decompress_firmware_2 (&data, last_data, acceleration);
}
bits_per_num = 18;
}
else if ((data.data[0] >= 101) && (data.data[0] <= 200))
{
bits_per_num = 19;
}
else if ((data.data[0] > 200) && (data.data[0] < 206))
{
Expand Down Expand Up @@ -401,30 +343,6 @@ void Ganglion::read_thread ()
}
continue;
}
// handle compressed data for 18 or 19 bits
for (int i = 8, counter = 0; i < bits_per_num * 8; i += bits_per_num, counter++)
{
if (bits_per_num == 18)
{
delta[counter] = (float)cast_ganglion_bits_to_int32<18> (package_bits + i);
}
else
{
delta[counter] = (float)cast_ganglion_bits_to_int32<19> (package_bits + i);
}
}

// apply the first delta to the last data we got in the previous iteration
for (int i = 0; i < 4; i++)
{
last_data[i] = last_data[i + 4] - delta[i];
}

// apply the second delta to the previous packet which we just decompressed above
for (int i = 4; i < 8; i++)
{
last_data[i] = last_data[i - 4] - delta[i];
}

// add first encoded package
package[board_descr["default"]["package_num_channel"].get<int> ()] = data.data[0];
Expand All @@ -436,9 +354,9 @@ void Ganglion::read_thread ()
eeg_scale * last_data[2];
package[board_descr["default"]["eeg_channels"][3].get<int> ()] =
eeg_scale * last_data[3];
package[board_descr["default"]["accel_channels"][0].get<int> ()] = accel_x;
package[board_descr["default"]["accel_channels"][1].get<int> ()] = accel_y;
package[board_descr["default"]["accel_channels"][2].get<int> ()] = accel_z;
package[board_descr["default"]["accel_channels"][0].get<int> ()] = acceleration[0];
package[board_descr["default"]["accel_channels"][1].get<int> ()] = acceleration[1];
package[board_descr["default"]["accel_channels"][2].get<int> ()] = acceleration[2];
package[board_descr["default"]["timestamp_channel"].get<int> ()] = data.timestamp;
push_package (package);
// add second package
Expand Down Expand Up @@ -479,6 +397,137 @@ void Ganglion::read_thread ()
delete[] package;
}

void Ganglion::decompress_firmware_3 (
struct GanglionLib::GanglionData *data, float *last_data, double *acceleration)
{
int bits_per_num = 0;
unsigned char package_bits[160] = {0}; // 20 * 8
for (int i = 0; i < 20; i++)
{
uchar_to_bits (data->data[i], package_bits + i * 8);
}

// 18 bit compression, sends 17 MSBs + sign bit of 24-bit sample
if ((data->data[0] >= 0) && (data->data[0] < 100))
{
int last_digit = data->data[0] % 10;
switch (last_digit)
{
// accel data is signed, so we must cast it to signed char
// swap x and z, and invert z to convert to standard coordinate space.
case 0:
acceleration[2] = -accel_scale * (char)data->data[19];
break;
case 1:
acceleration[1] = accel_scale * (char)data->data[19];
break;
case 2:
acceleration[0] = accel_scale * (char)data->data[19];
break;
default:
break;
}
bits_per_num = 18;
}
else if ((data->data[0] >= 100) && (data->data[0] < 200))
{
bits_per_num = 19;
}

// handle compressed data for 18 or 19 bits
for (int i = 8, counter = 0; i < bits_per_num * 8; i += bits_per_num, counter++)
{
if (bits_per_num == 18)
{
last_data[counter] = (float)(cast_ganglion_bits_to_int32<18> (package_bits + i) << 6);
}
else
{
last_data[counter] = (float)(cast_ganglion_bits_to_int32<19> (package_bits + i) << 5);
}
}
}

void Ganglion::decompress_firmware_2 (
struct GanglionLib::GanglionData *data, float *last_data, double *acceleration)
{
int bits_per_num = 0;
unsigned char package_bits[160] = {0}; // 20 * 8
for (int i = 0; i < 20; i++)
{
uchar_to_bits (data->data[i], package_bits + i * 8);
}

float delta[8] = {0.f}; // delta holds 8 nums (4 by each package)

// no compression, used to init variable
if (data->data[0] == 0)
{
// shift the last data packet to make room for a newer one
last_data[0] = last_data[4];
last_data[1] = last_data[5];
last_data[2] = last_data[6];
last_data[3] = last_data[7];

// add new packet
last_data[4] = (float)cast_24bit_to_int32 (data->data + 1);
last_data[5] = (float)cast_24bit_to_int32 (data->data + 4);
last_data[6] = (float)cast_24bit_to_int32 (data->data + 7);
last_data[7] = (float)cast_24bit_to_int32 (data->data + 10);
}
// 18 bit compression, sends delta from previous value instead of real value!
else if ((data->data[0] >= 1) && (data->data[0] <= 100))
{
int last_digit = data->data[0] % 10;
switch (last_digit)
{
// accel data is signed, so we must cast it to signed char
// swap x and z, and invert z to convert to standard coordinate space.
case 0:
acceleration[2] = -accel_scale * (char)data->data[19];
break;
case 1:
acceleration[1] = accel_scale * (char)data->data[19];
break;
case 2:
acceleration[0] = accel_scale * (char)data->data[19];
break;
default:
break;
}
bits_per_num = 18;
}
else if ((data->data[0] >= 101) && (data->data[0] <= 200))
{
bits_per_num = 19;
}

// handle compressed data for 18 or 19 bits
for (int i = 8, counter = 0; i < bits_per_num * 8; i += bits_per_num, counter++)
{
if (bits_per_num == 18)
{
delta[counter] = (float)cast_ganglion_bits_to_int32<18> (package_bits + i);
}
else
{
delta[counter] = (float)cast_ganglion_bits_to_int32<19> (package_bits + i);
}
}

// apply the first delta to the last data we got in the previous iteration
for (int i = 0; i < 4; i++)
{
last_data[i] = last_data[i + 4] - delta[i];
}

// apply the second delta to the previous packet which we just decompressed above
for (int i = 4; i < 8; i++)
{
last_data[i] = last_data[i - 4] - delta[i];
}
}

int Ganglion::config_board (std::string config, std::string &response)
{
const char *conf = config.c_str ();
Expand Down Expand Up @@ -592,7 +641,16 @@ int Ganglion::call_open ()
}

safe_logger (spdlog::level::info, "search for {}", params.mac_address.c_str ());
res = func (const_cast<char *> (params.mac_address.c_str ()));

GanglionLib::ConnectionParameters connection_params;
connection_params.mac_address = const_cast<char *> (params.mac_address.c_str ());
connection_params.firmware = 2;

res = func ((void *)&connection_params);

safe_logger (
spdlog::level::info, "detected firmware version {}", connection_params.firmware);
firmware = connection_params.firmware;
}
else
{
Expand All @@ -605,7 +663,10 @@ int Ganglion::call_open ()

safe_logger (
spdlog::level::info, "mac address is not specified, try to find ganglion without it");
res = func (NULL);

res = func ((void *)&firmware);

safe_logger (spdlog::level::info, "detected firmware version {}", firmware);
}
if (res != GanglionLib::CustomExitCodes::STATUS_OK)
{
Expand Down
16 changes: 13 additions & 3 deletions src/board_controller/openbci/ganglion_bglib/callbacks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,11 @@ namespace GanglionLib
{
extern volatile int exit_code;
extern volatile bd_addr connect_addr;
extern volatile uint8 firmware;
extern volatile uint8 connection;
extern volatile uint16 ganglion_handle_start;
extern volatile uint16 ganglion_handle_end;

// recv and send chars handles
extern volatile uint16 ganglion_handle_recv;
extern volatile uint16 ganglion_handle_send;
Expand All @@ -44,6 +46,7 @@ void ble_evt_gap_scan_response (const struct ble_msg_gap_scan_response_evt_t *ms
{
char name[512];
bool name_found_in_response = false;

for (int i = 0; i < msg->data.len;)
{
int8 len = msg->data.data[i++];
Expand All @@ -56,7 +59,7 @@ void ble_evt_gap_scan_response (const struct ble_msg_gap_scan_response_evt_t *ms
break; // not enough data
}
uint8 type = msg->data.data[i++];
if (type == 0x09) // no idea what is 0x09
if (type == 0x09) // Ensure that the device is advertising with a complete name
{
name_found_in_response = true;
memcpy (name, msg->data.data + i, len - 1);
Expand All @@ -68,10 +71,17 @@ void ble_evt_gap_scan_response (const struct ble_msg_gap_scan_response_evt_t *ms

if (name_found_in_response)
{
// strcasestr is unavailable for windows
if (strstr (name, "anglion") != NULL)
// Check if the found device is the one specified by the serial_number parameter
if (memcmp (msg->sender.addr, (uint8 *)GanglionLib::connect_addr.addr, sizeof (bd_addr)) ==
0)
{
GanglionLib::firmware = msg->data.data[0] == 20 && msg->data.data[13] == '3' ? 3 : 2;
GanglionLib::exit_code = (int)GanglionLib::STATUS_OK;
}
else if (strstr (name, "anglion") != NULL) // strcasestr is unavailable for windows
{
memcpy ((void *)GanglionLib::connect_addr.addr, msg->sender.addr, sizeof (bd_addr));
GanglionLib::firmware = msg->data.data[0] == 20 && msg->data.data[13] == '3' ? 3 : 2;
GanglionLib::exit_code = (int)GanglionLib::STATUS_OK;
}
}
Expand Down
7 changes: 7 additions & 0 deletions src/board_controller/openbci/ganglion_bglib/inc/ganglion_types.h
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#pragma once

#include "shared_export.h"
#include <cstdint>
#include <string.h>

namespace GanglionLib
Expand Down Expand Up @@ -86,4 +87,10 @@ namespace GanglionLib
INVALID_MAC_ADDR_ERROR = 17,
PORT_OPEN_ERROR = 18
};

struct ConnectionParameters
{
char *mac_address;
uint8_t firmware;
};
}
Loading

0 comments on commit 0dbd69d

Please sign in to comment.