Fix scaling based on FW defaults

BMM150 x,y axes are 13-bit and z axis is 15-bit two's complement

src/board_controller/openbci/galea_v4.cpp

@@ -466,12 +466,12 @@ void GaleaV4::read_thread ()
                 // aux, 5 times smaller sampling rate
                 if (((int)b[0 + offset]) % 5 == 0)
                 {
-                    double accel_scale = (double)(4.0 / static_cast<double> (pow (2, 16) - 1));
-                    double gyro_scale = (double)(4000.0 / static_cast<double> (pow (2, 16) - 1));
+                    double accel_scale = (double)(8.0 / static_cast<double> (pow (2, 16) - 1));
+                    double gyro_scale = (double)(1000.0 / static_cast<double> (pow (2, 16) - 1));
                     double magnetometer_scale_xy =
-                        (double)(2.6 / static_cast<double> (pow (2, 16) - 1));
+                        (double)(2.6 / static_cast<double> (pow (2, 13) - 1));
                     double magnetometer_scale_z =
-                        (double)(5.0 / static_cast<double> (pow (2, 16) - 1));
+                        (double)(5.0 / static_cast<double> (pow (2, 15) - 1));
                     aux_package[board_descr["auxiliary"]["package_num_channel"].get<int> ()] =
                         (double)b[0 + offset];
                     uint16_t temperature = 0;
@@ -519,13 +519,13 @@ void GaleaV4::read_thread ()
                     // magnetometer
                     aux_package[board_descr["auxiliary"]["magnetometer_channels"][0].get<int> ()] =
                         magnetometer_scale_xy *
-                        (double)cast_16bit_to_int32_swap_order (b + 108 + offset);
+                        (double)cast_13bit_to_int32_swap_order (b + 108 + offset);
                     aux_package[board_descr["auxiliary"]["magnetometer_channels"][1].get<int> ()] =
                         magnetometer_scale_xy *
-                        (double)cast_16bit_to_int32_swap_order (b + 110 + offset);
+                        (double)cast_13bit_to_int32_swap_order (b + 110 + offset);
                     aux_package[board_descr["auxiliary"]["magnetometer_channels"][2].get<int> ()] =
                         magnetometer_scale_z *
-                        (double)cast_16bit_to_int32_swap_order (b + 112 + offset);
+                        (double)cast_15bit_to_int32_swap_order (b + 112 + offset);
 
                     push_package (aux_package, (int)BrainFlowPresets::AUXILIARY_PRESET);
                 }

src/utils/inc/custom_cast.h

@@ -46,6 +46,40 @@ 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)