+
+static void reorder_fields (float* data, unsigned char map[MAX_CHANNELS])
+{
+ int i;
+ float temp[MAX_CHANNELS];
+
+ for (i=0; i<MAX_CHANNELS; i++)
+ temp[i] = data[map[i]];
+
+ for (i=0; i<MAX_CHANNELS; i++)
+ data[i] = temp[i];
+}
+
+static void mount_correction (float* data, float mm[9])
+{
+ int i;
+ float temp[3];
+
+ for (i=0; i<3; i++)
+ temp[i] = data[0] * mm[i * 3] + data[1] * mm[i * 3 + 1] + data[2] * mm[i * 3 + 2];
+
+ for (i=0; i<3; i++)
+ data[i] = temp[i];
+}
+
+static void clamp_gyro_readings_to_zero (int s, sensors_event_t* data)
+{
+ float x, y, z;
+ float near_zero;
+
+ x = data->data[0];
+ y = data->data[1];
+ z = data->data[2];
+
+ /* If we're calibrated, don't filter out as much */
+ if (sensor[s].cal_level > 0)
+ near_zero = 0.02; /* rad/s */
+ else
+ near_zero = 0.1;
+
+ /* If motion on all axes is small enough */
+ if (fabs(x) < near_zero && fabs(y) < near_zero && fabs(z) < near_zero) {
+
+ /*
+ * Report that we're not moving at all... but not exactly zero as composite sensors (orientation, rotation vector) don't
+ * seem to react very well to it.
+ */
+
+ data->data[0] *= 0.000001;
+ data->data[1] *= 0.000001;
+ data->data[2] *= 0.000001;
+ }
+}
+
+
+static void process_event_gyro_uncal (int s, int i, sensors_event_t* data)
+{
+ gyro_cal_t* gyro_data;
+
+ if (sensor[s].type == SENSOR_TYPE_GYROSCOPE) {
+ gyro_data = (gyro_cal_t*) sensor[s].cal_data;
+
+ memcpy(&sensor[i].sample, data, sizeof(sensors_event_t));
+
+ sensor[i].sample.type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
+ sensor[i].sample.sensor = s;
+
+ sensor[i].sample.data[0] = data->data[0] + gyro_data->bias_x;
+ sensor[i].sample.data[1] = data->data[1] + gyro_data->bias_y;
+ sensor[i].sample.data[2] = data->data[2] + gyro_data->bias_z;
+
+ sensor[i].sample.uncalibrated_gyro.bias[0] = gyro_data->bias_x;
+ sensor[i].sample.uncalibrated_gyro.bias[1] = gyro_data->bias_y;
+ sensor[i].sample.uncalibrated_gyro.bias[2] = gyro_data->bias_z;
+
+ sensor[i].report_pending = 1;
+ }
+}
+
+static void process_event_magn_uncal (int s, int i, sensors_event_t* data)
+{
+ compass_cal_t* magn_data;
+
+ if (sensor[s].type == SENSOR_TYPE_MAGNETIC_FIELD) {
+ magn_data = (compass_cal_t*) sensor[s].cal_data;
+
+ memcpy(&sensor[i].sample, data, sizeof(sensors_event_t));
+
+ sensor[i].sample.type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED;
+ sensor[i].sample.sensor = s;
+
+ sensor[i].sample.data[0] = data->data[0] + magn_data->offset[0][0];
+ sensor[i].sample.data[1] = data->data[1] + magn_data->offset[1][0];
+ sensor[i].sample.data[2] = data->data[2] + magn_data->offset[2][0];
+
+ sensor[i].sample.uncalibrated_magnetic.bias[0] = magn_data->offset[0][0];
+ sensor[i].sample.uncalibrated_magnetic.bias[1] = magn_data->offset[1][0];
+ sensor[i].sample.uncalibrated_magnetic.bias[2] = magn_data->offset[2][0];
+
+ sensor[i].report_pending = 1;
+ }