Skip to content

Commit

Permalink
fixes for serial size
Browse files Browse the repository at this point in the history 
Signed-off-by: Andrey Parfenov <a1994ndrey@gmail.com>
  • Loading branch information
@Andrey1994
Andrey1994 committed Dec 21, 2022
1 parent 2c9bffc commit 61addd4
Showing 1 changed file with 47 additions and 28 deletions.
75 changes: 47 additions & 28 deletions src/board_controller/openbci/galea_serial.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -230,7 +230,7 @@ int GaleaSerial::stop_stream ()
// free kernel buffer
unsigned char b;
res = 1;
int max_attempt = 40000; // to dont get to infinite loop
int max_attempt = 400000; // to dont get to infinite loop
int current_attempt = 0;
while (res == 1)
{
Expand Down Expand Up @@ -284,13 +284,12 @@ void GaleaSerial::read_thread ()
{
int res = 0;
constexpr int package_size = 72;
constexpr int num_packages = 19;
constexpr int transaction_size = package_size * num_packages + 1; // + 1 because of end byte
unsigned char b[transaction_size];
constexpr int max_num_packages = 25;
constexpr int max_transaction_size = package_size * max_num_packages + 2;
unsigned char b[max_transaction_size];
DataBuffer time_buffer (1, 11);
double latest_times[10];
constexpr int offset_last_package = package_size * (num_packages - 1);
for (int i = 0; i < transaction_size; i++)
for (int i = 0; i < max_transaction_size; i++)
{
b[i] = 0;
}
Expand All @@ -310,33 +309,51 @@ void GaleaSerial::read_thread ()
while (keep_alive)
{
// read and check first byte
res = serial->read_from_serial_port (b, 1);
int res = serial->read_from_serial_port (b, 1);
if (res != 1)
{
safe_logger (spdlog::level::debug, "unable to read 1 byte");
continue;
}
double pc_timestamp = get_timestamp ();
if (b[0] != START_BYTE)
{
continue;
}
double pc_timestamp = get_timestamp ();
// read and check reamining bytes
int remaining_bytes = transaction_size;
int pos = 0;
while ((remaining_bytes > 0) && (keep_alive))
int pos = 1;
int num_packages = 0;
while (keep_alive)
{
res = serial->read_from_serial_port (b + pos, remaining_bytes);
remaining_bytes -= res;
pos += res;
if (pos >= 2 + package_size)
{
if ((b[0] == START_BYTE) && (b[pos] == END_BYTE) && ((pos - 2) % package_size == 0))
{
num_packages = (pos - 2) / package_size;
break;
}
}
if (pos > max_transaction_size - 1)
{
num_packages = 0;
break;
}
res = serial->read_from_serial_port (b + pos, 1);
if (res > 0)
{
pos += res;
}
}
if (!keep_alive)
{
break;
}
if (b[transaction_size - 1] != END_BYTE)
if (num_packages < 1)
{
safe_logger (spdlog::level::warn, "Wrong end byte {}", b[transaction_size - 1]);
if (pos >= 2)
safe_logger (spdlog::level::warn,
"Failed to parse some data, b[0]: {}, b[pos]: {}, size: {}", (int)b[0],
(int)b[pos], (int)pos);
continue;
}
else
Expand All @@ -355,8 +372,10 @@ void GaleaSerial::read_thread ()

// calc delta between PC timestamp and device timestamp in last 10 packages,
// use this delta later on to assign timestamps
unsigned char *package_bytes = b + 1;
int offset_last_package = package_size * (num_packages - 1);
double timestamp_last_package = 0.0;
memcpy (&timestamp_last_package, b + 64 + offset_last_package, 8);
memcpy (&timestamp_last_package, package_bytes + 64 + offset_last_package, 8);
timestamp_last_package /= 1000; // from ms to seconds
double time_delta = pc_timestamp - timestamp_last_package;
time_buffer.add_data (&time_delta);
Expand All @@ -373,16 +392,16 @@ void GaleaSerial::read_thread ()
int offset = cur_package * package_size;
// exg(default preset)
exg_package[board_descr["default"]["package_num_channel"].get<int> ()] =
(double)b[0 + offset];
(double)package_bytes[0 + offset];
for (int i = 4, tmp_counter = 0; i < 20; i++, tmp_counter++)
{
double exg_scale = (double)(4.5 / float ((pow (2, 23) - 1)) /
gain_tracker.get_gain_for_channel (tmp_counter) * 1000000.);
exg_package[i - 3] =
exg_scale * (double)cast_24bit_to_int32 (b + offset + 5 + 3 * (i - 4));
exg_package[i - 3] = exg_scale *
(double)cast_24bit_to_int32 (package_bytes + offset + 5 + 3 * (i - 4));
}
double timestamp_device = 0.0;
memcpy (&timestamp_device, b + 64 + offset, 8);
memcpy (&timestamp_device, package_bytes + 64 + offset, 8);
timestamp_device /= 1000; // from ms to seconds
exg_package[board_descr["default"]["timestamp_channel"].get<int> ()] =
timestamp_device + time_delta - half_rtt;
Expand All @@ -392,18 +411,18 @@ void GaleaSerial::read_thread ()
push_package (exg_package);

// aux, 5 times slower
if (((int)b[0 + offset]) % 5 == 0)
if (((int)package_bytes[0 + offset]) % 5 == 0)
{
aux_package[board_descr["auxiliary"]["package_num_channel"].get<int> ()] =
(double)b[0 + offset];
(double)package_bytes[0 + offset];
uint16_t temperature = 0;
int32_t ppg_ir = 0;
int32_t ppg_red = 0;
float eda;
memcpy (&temperature, b + 54 + offset, 2);
memcpy (&eda, b + 1 + offset, 4);
memcpy (&ppg_red, b + 56 + offset, 4);
memcpy (&ppg_ir, b + 60 + offset, 4);
memcpy (&temperature, package_bytes + 54 + offset, 2);
memcpy (&eda, package_bytes + 1 + offset, 4);
memcpy (&ppg_red, package_bytes + 56 + offset, 4);
memcpy (&ppg_ir, package_bytes + 60 + offset, 4);
// ppg
aux_package[board_descr["auxiliary"]["ppg_channels"][0].get<int> ()] =
(double)ppg_red;
Expand All @@ -416,7 +435,7 @@ void GaleaSerial::read_thread ()
temperature / 100.0;
// battery
aux_package[board_descr["auxiliary"]["battery_channel"].get<int> ()] =
(double)b[53 + offset];
(double)package_bytes[53 + offset];
aux_package[board_descr["auxiliary"]["timestamp_channel"].get<int> ()] =
timestamp_device + time_delta - half_rtt;
aux_package[board_descr["auxiliary"]["other_channels"][0].get<int> ()] =
Expand Down

0 comments on commit 61addd4

Please sign in to comment.