diff --git a/src/board_controller/openbci/galea_v4.cpp b/src/board_controller/openbci/galea_v4.cpp index 105c7f0d7..e1ed5ef26 100644 --- a/src/board_controller/openbci/galea_v4.cpp +++ b/src/board_controller/openbci/galea_v4.cpp @@ -466,9 +466,12 @@ void GaleaV4::read_thread () // aux, 5 times smaller sampling rate if (((int)b[0 + offset]) % 5 == 0) { - double accel_scale = (double)(0.002 / (pow (2, 4))); - double gyro_scale = (double)(0.002 / (pow (2, 4))); // to be confirmed - double magnetometer_scale = (double)(0.002 / (pow (2, 4))); // to be confirmed + double accel_scale = (double)(8.0 / static_cast (pow (2, 16) - 1)); + double gyro_scale = (double)(1000.0 / static_cast (pow (2, 16) - 1)); + double magnetometer_scale_xy = + (double)(2.6 / static_cast (pow (2, 13) - 1)); + double magnetometer_scale_z = + (double)(5.0 / static_cast (pow (2, 15) - 1)); aux_package[board_descr["auxiliary"]["package_num_channel"].get ()] = (double)b[0 + offset]; uint16_t temperature = 0; @@ -501,25 +504,28 @@ void GaleaV4::read_thread () timestamp_device; // accel aux_package[board_descr["auxiliary"]["accel_channels"][0].get ()] = - accel_scale * cast_16bit_to_int32 (b + 96 + offset); + accel_scale * (double)cast_16bit_to_int32_swap_order (b + 96 + offset); aux_package[board_descr["auxiliary"]["accel_channels"][1].get ()] = - accel_scale * cast_16bit_to_int32 (b + 98 + offset); + accel_scale * (double)cast_16bit_to_int32_swap_order (b + 98 + offset); aux_package[board_descr["auxiliary"]["accel_channels"][2].get ()] = - accel_scale * cast_16bit_to_int32 (b + 100 + offset); + accel_scale * (double)cast_16bit_to_int32_swap_order (b + 100 + offset); // gyro aux_package[board_descr["auxiliary"]["gyro_channels"][0].get ()] = - gyro_scale * cast_16bit_to_int32 (b + 102 + offset); + gyro_scale * (double)cast_16bit_to_int32_swap_order (b + 102 + offset); aux_package[board_descr["auxiliary"]["gyro_channels"][1].get ()] = - gyro_scale * cast_16bit_to_int32 (b + 104 + offset); + gyro_scale * (double)cast_16bit_to_int32_swap_order (b + 104 + offset); aux_package[board_descr["auxiliary"]["gyro_channels"][2].get ()] = - gyro_scale * cast_16bit_to_int32 (b + 106 + offset); + gyro_scale * (double)cast_16bit_to_int32_swap_order (b + 106 + offset); // magnetometer aux_package[board_descr["auxiliary"]["magnetometer_channels"][0].get ()] = - magnetometer_scale * cast_16bit_to_int32 (b + 108 + offset); + magnetometer_scale_xy * + (double)cast_13bit_to_int32_swap_order (b + 108 + offset); aux_package[board_descr["auxiliary"]["magnetometer_channels"][1].get ()] = - magnetometer_scale * cast_16bit_to_int32 (b + 110 + offset); + magnetometer_scale_xy * + (double)cast_13bit_to_int32_swap_order (b + 110 + offset); aux_package[board_descr["auxiliary"]["magnetometer_channels"][2].get ()] = - magnetometer_scale * cast_16bit_to_int32 (b + 112 + offset); + magnetometer_scale_z * + (double)cast_15bit_to_int32_swap_order (b + 112 + offset); push_package (aux_package, (int)BrainFlowPresets::AUXILIARY_PRESET); } diff --git a/src/utils/inc/custom_cast.h b/src/utils/inc/custom_cast.h index 28d82c8de..16dc99140 100644 --- a/src/utils/inc/custom_cast.h +++ b/src/utils/inc/custom_cast.h @@ -46,6 +46,42 @@ inline int32_t cast_16bit_to_int32_swap_order (unsigned char *byte_array) return cast_16bit_to_int32 (swapped); } +inline int32_t cast_13bit_to_int32 (unsigned char *byte_array) +{ + int prefix = 0; + if (byte_array[0] > 0x0F) + { + prefix = 0xFFFFE000; + } + return prefix | (byte_array[0] << 8) | byte_array[1]; +} + +inline int32_t cast_13bit_to_int32_swap_order (unsigned char *byte_array) +{ + unsigned char swapped[2]; + swapped[0] = byte_array[1]; + swapped[1] = byte_array[0]; + return cast_13bit_to_int32 (swapped); +} + +inline int32_t cast_15bit_to_int32 (unsigned char *byte_array) +{ + int prefix = 0; + if (byte_array[0] > 0x3F) + { + prefix = 0xFFFF8000; + } + return prefix | (byte_array[0] << 8) | byte_array[1]; +} + +inline int32_t cast_15bit_to_int32_swap_order (unsigned char *byte_array) +{ + unsigned char swapped[2]; + swapped[0] = byte_array[1]; + swapped[1] = byte_array[0]; + return cast_15bit_to_int32 (swapped); +} + inline void uchar_to_bits (unsigned char c, unsigned char *bits) { for (int i = sizeof (unsigned char) * 8; i; c >>= 1)