/*
- * Copyright (C) 2014 Intel Corporation.
+ * Copyright (C) 2014-2015 Intel Corporation.
*/
#include <stdio.h>
#include "common.h"
#include "calibration.h"
- /* gyro defines */
-#define GYRO_MAX_ERR 0.05f
+
+/* Gyro defines */
+#define GYRO_MAX_ERR 0.05
#define GYRO_DS_SIZE 100
#define GYRO_CALIBRATION_PATH "/data/gyro.conf"
+#define GYRO_CAL_VERSION 1.0
+
-static void reset (struct gyro_cal* cal_data)
+static void reset (gyro_cal_t* cal_data)
{
cal_data->count = 0;
}
-void gyro_cal_init (struct sensor_info_t* info)
+void gyro_cal_init (int s)
{
int ret;
- struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
+ gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
FILE* data_file;
+ float version;
- info->cal_level = 0;
+ sensor[s].cal_level = 0;
if (cal_data == NULL)
return;
if (data_file == NULL)
return;
- ret = fscanf(data_file, "%f %f %f",
+ ret = fscanf(data_file, "%f %f %f %f", &version,
&cal_data->bias_x, &cal_data->bias_y, &cal_data->bias_z);
+
+ if (ret != 4 || version != GYRO_CAL_VERSION) {
+ reset(cal_data);
+ ALOGE("Gyro calibration - init failed!\n");
+ }
+
fclose(data_file);
}
-void gyro_store_data (struct sensor_info_t* info)
+
+void gyro_store_data (int s)
{
int ret;
- struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
+ gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
FILE* data_file;
if (cal_data == NULL)
if (data_file == NULL)
return;
- ret = fprintf(data_file, "%f %f %f",
+ ret = fprintf(data_file, "%f %f %f %f", GYRO_CAL_VERSION,
cal_data->bias_x, cal_data->bias_y, cal_data->bias_z);
if (ret < 0)
}
-static int gyro_collect (float x, float y, float z, struct gyro_cal* cal_data)
+static int gyro_collect (float x, float y, float z, gyro_cal_t* cal_data)
{
/* Analyze gyroscope data */
return 1; /* Calibrated! */
}
-void calibrate_gyro(struct sensors_event_t* event, struct sensor_info_t* info)
+
+void calibrate_gyro (int s, sensors_event_t* event)
{
- struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
+ gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
if (cal_data == NULL)
return;
/* Attempt gyroscope calibration if we have not reached this state */
- if (info->cal_level == 0)
- info->cal_level = gyro_collect(event->data[0], event->data[1],
+ if (sensor[s].cal_level == 0)
+ sensor[s].cal_level = gyro_collect(event->data[0], event->data[1],
event->data[2], cal_data);
- switch (event->type) {
- case SENSOR_TYPE_GYROSCOPE:
- /* For the gyroscope apply the bias */
- event->data[0] = event->data[0] - cal_data->bias_x;
- event->data[1] = event->data[1] - cal_data->bias_y;
- event->data[2] = event->data[2] - cal_data->bias_z;
-
- if (info->cal_level)
- event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
- break;
-
- case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
- /*
- * For the uncalibrated gyroscope don't apply the bias,
- * but tell he Android framework what we think it is.
- */
- event->uncalibrated_gyro.bias[0] = cal_data->bias_x;
- event->uncalibrated_gyro.bias[1] = cal_data->bias_y;
- event->uncalibrated_gyro.bias[2] = cal_data->bias_z;
- break;
- }
+
+ event->data[0] = event->data[0] - cal_data->bias_x;
+ event->data[1] = event->data[1] - cal_data->bias_y;
+ event->data[2] = event->data[2] - cal_data->bias_z;
+
+ if (sensor[s].cal_level)
+ event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
}