#include <utils/Log.h>
#include "calibration.h"
#include "matrix-ops.h"
-
+#include "description.h"
#ifdef DBG_RAW_DATA
#define MAX_RAW_DATA_COUNT 2000
int file_no = 0;
#endif
+#define CAL_STEPS 5
+
+/* We'll have multiple calibration levels
+* so that we can provide an estimation as fast as possible
+*/
+static const float min_diffs[CAL_STEPS] = {0.15, 0.2, 0.4, 0.6, 1.0 };
+static const float max_sqr_errs[CAL_STEPS] = {10.0, 10.0, 8.0, 5.0, 3.5 };
+static const unsigned int lookback_counts[CAL_STEPS] = {2, 3, 4, 5, 6 };
/* reset calibration algorithm */
static void reset_sample (struct compass_cal* data)
static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3][3], double* bfield)
{
- int i,j;
+ int i;
double h[DS_SIZE][9];
double w[DS_SIZE][1];
double h_trans[9][DS_SIZE];
int data_count = 14;
reset_sample(cal_data);
- if (!info->calibrated && data_file != NULL) {
+ if (!info->cal_level && data_file != NULL) {
int ret = fscanf(data_file, "%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
- &info->calibrated, &cal_data->offset[0][0], &cal_data->offset[1][0], &cal_data->offset[2][0],
+ &info->cal_level, &cal_data->offset[0][0], &cal_data->offset[1][0], &cal_data->offset[2][0],
&cal_data->w_invert[0][0], &cal_data->w_invert[0][1], &cal_data->w_invert[0][2],
&cal_data->w_invert[1][0], &cal_data->w_invert[1][1], &cal_data->w_invert[1][2],
&cal_data->w_invert[2][0], &cal_data->w_invert[2][1], &cal_data->w_invert[2][2],
&cal_data->bfield);
if (ret != data_count) {
- info->calibrated = 0;
+ info->cal_level = 0;
}
}
- if (info->calibrated) {
+ if (info->cal_level) {
ALOGV("CompassCalibration: load old data, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f",
cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0],
cal_data->w_invert[0][0], cal_data->w_invert[0][1], cal_data->w_invert[0][2], cal_data->w_invert[1][0],
return;
int ret = fprintf(data_file, "%d %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
- info->calibrated, cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0],
+ info->cal_level, cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0],
cal_data->w_invert[0][0], cal_data->w_invert[0][1], cal_data->w_invert[0][2],
cal_data->w_invert[1][0], cal_data->w_invert[1][1], cal_data->w_invert[1][2],
cal_data->w_invert[2][0], cal_data->w_invert[2][1], cal_data->w_invert[2][2],
if (cal_data == NULL)
return -1;
+ /* Discard the point if not valid */
+ if (data[0] == 0 || data[1] == 0 || data[2] == 0)
+ return -1;
+
#ifdef DBG_RAW_DATA
if (raw_data && raw_data_count < MAX_RAW_DATA_COUNT) {
fprintf(raw_data, "%f %f %f\n", (double)data[0], (double)data[1],
}
#endif
- lookback_count = info->calibrated ? LOOKBACK_COUNT : FIRST_LOOKBACK_COUNT;
- min_diff = info->calibrated ? MIN_DIFF : FIRST_MIN_DIFF;
+ lookback_count = lookback_counts[info->cal_level];
+ min_diff = min_diffs[info->cal_level];
// For the current point to be accepted, each x/y/z value must be different enough
// to the last several collected points
return 1;
}
+static void scale_event (struct sensors_event_t* event)
+{
+ float sqr_norm = 0;
+ float sanity_norm = 0;
+ float scale = 1;
+
+ sqr_norm = (event->magnetic.x * event->magnetic.x +
+ event->magnetic.y * event->magnetic.y +
+ event->magnetic.z * event->magnetic.z);
+
+ sanity_norm = (sqr_norm < MAGNETIC_LOW) ? MAGNETIC_LOW : sanity_norm;
+ sanity_norm = (sqr_norm > MAGNETIC_HIGH) ? MAGNETIC_HIGH : sanity_norm;
+
+ if (sanity_norm && sqr_norm) {
+ scale = sanity_norm / sqr_norm;
+ scale = sqrt(scale);
+ event->magnetic.x = event->magnetic.x * scale;
+ event->magnetic.y = event->magnetic.y * scale;
+ event->magnetic.z = event->magnetic.z * scale;
+
+ }
+}
+
static void compass_compute_cal (struct sensors_event_t* event, struct sensor_info_t* info)
{
struct compass_cal* cal_data = (struct compass_cal*) info->cal_data;
+ double result[3][1], raw[3][1], diff[3][1];
- if (!info->calibrated || cal_data == NULL)
+ if (!info->cal_level || cal_data == NULL)
return;
- double result[3][1], raw[3][1], diff[3][1];
-
raw[0][0] = event->magnetic.x;
raw[1][0] = event->magnetic.y;
raw[2][0] = event->magnetic.z;
event->magnetic.x = event->data[0] = result[0][0];
event->magnetic.y = event->data[1] = result[1][0];
event->magnetic.z = event->data[2] = result[2][0];
+
+ scale_event(event);
}
+
static int compass_ready (struct sensor_info_t* info)
{
mat_input_t mat;
struct compass_cal* cal_data = (struct compass_cal*) info->cal_data;
if (cal_data->sample_count < DS_SIZE)
- return info->calibrated;
+ return info->cal_level;
- max_sqr_err = info->calibrated ? MAX_SQR_ERR : FIRST_MAX_SQR_ERR;
+ max_sqr_err = max_sqr_errs[info->cal_level];
/* enough points have been collected, do the ellipsoid calibration */
for (i = 0; i < DS_SIZE; i++) {
memcpy(cal_data->offset, new_cal_data.offset, sizeof(cal_data->offset));
memcpy(cal_data->w_invert, new_cal_data.w_invert, sizeof(cal_data->w_invert));
cal_data->bfield = new_cal_data.bfield;
- info->calibrated = 1;
+ if (info->cal_level < (CAL_STEPS - 1))
+ info->cal_level++;
ALOGV("CompassCalibration: ready check success, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f, err %f",
cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0], cal_data->w_invert[0][0],
cal_data->w_invert[0][1], cal_data->w_invert[0][2], cal_data->w_invert[1][0], cal_data->w_invert[1][1],
}
}
reset_sample(cal_data);
- return info->calibrated;
+ return info->cal_level;
}
void calibrate_compass (struct sensors_event_t* event, struct sensor_info_t* info, int64_t current_time)
{
long current_time_ms = current_time / 1000000;
+ int cal_level;
+
+ /* Calibration is continuous */
compass_collect (event, info, current_time_ms);
- if (compass_ready(info)) {
- compass_compute_cal (event, info);
- event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
- } else {
- event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW;
+
+ cal_level = compass_ready(info);
+
+ switch (cal_level) {
+
+ case 0:
+ scale_event(event);
+ event->magnetic.status = SENSOR_STATUS_UNRELIABLE;
+ break;
+
+ case 1:
+ compass_compute_cal (event, info);
+ event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW;
+ break;
+
+ case 2:
+ compass_compute_cal (event, info);
+ event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM;
+ break;
+
+ default:
+ compass_compute_cal (event, info);
+ event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
+ break;
}
}
OSDN Git Service
