X-Git-Url: http://git.osdn.net/view?a=blobdiff_plain;ds=sidebyside;f=transform.c;h=cc53f3945399076eaf4ba310f59f46a76b63464d;hb=389edefcad440dc0344e6cebe11ddef1f8464b08;hp=bce192bf2dae7771efb1981c25a2d889b46d1762;hpb=0d063b71dd3c9b4588fd1f989928dcaffa61eb9a;p=android-x86%2Fhardware-intel-libsensors.git diff --git a/transform.c b/transform.c index bce192b..cc53f39 100644 --- a/transform.c +++ b/transform.c @@ -7,10 +7,12 @@ #include #include #include +#include "calibration.h" #include "common.h" +#include "description.h" #include "transform.h" #include "utils.h" -#include "calibration.h" +#include "filtering.h" /*----------------------------------------------------------------------------*/ @@ -187,26 +189,122 @@ static void reorder_fields(float* data, unsigned char map[MAX_CHANNELS]) } -static int finalize_sample_default(int s, struct sensors_event_t* data) +static void clamp_gyro_readings_to_zero (int s, struct sensors_event_t* data) { - int i = sensor_info[s].catalog_index; - int sensor_type = sensor_catalog[i].type; + float x, y, z; + float near_zero; + + switch (sensor_info[s].type) { + case SENSOR_TYPE_GYROSCOPE: + x = data->data[0]; + y = data->data[1]; + z = data->data[2]; + break; + + case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: + x = data->data[0] - data->uncalibrated_gyro.bias[0]; + y = data->data[1] - data->uncalibrated_gyro.bias[1]; + z = data->data[2] - data->uncalibrated_gyro.bias[2]; + break; + + default: + return; + } + + /* If we're calibrated, don't filter out as much */ + if (sensor_info[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. + */ + switch (sensor_info[s].type) { + case SENSOR_TYPE_GYROSCOPE: + data->data[0] *= 0.000001; + data->data[1] *= 0.000001; + data->data[2] *= 0.000001; + break; + + case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: + data->data[0]= data->uncalibrated_gyro.bias[0] + + 0.000001 * x; + data->data[1]= data->uncalibrated_gyro.bias[1] + + 0.000001 * y; + data->data[2]= data->uncalibrated_gyro.bias[2] + + 0.000001 * z; + break; + } + } +} + +static int finalize_sample_default (int s, struct sensors_event_t* data) +{ /* Swap fields if we have a custom channel ordering on this sensor */ - if (sensor_info[s].flags & FLAG_FIELD_ORDERING) + if (sensor_info[s].quirks & QUIRK_FIELD_ORDERING) reorder_fields(data->data, sensor_info[s].order); - switch (sensor_type) { + sensor_info[s].event_count++; + switch (sensor_info[s].type) { case SENSOR_TYPE_ACCELEROMETER: + /* Always consider the accelerometer accurate */ + data->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; + if (sensor_info[s].quirks & QUIRK_NOISY) + denoise(s, data); break; case SENSOR_TYPE_MAGNETIC_FIELD: calibrate_compass (data, &sensor_info[s], get_timestamp()); + if (sensor_info[s].quirks & QUIRK_NOISY) + denoise(s, data); break; case SENSOR_TYPE_GYROSCOPE: + + /* + * Report medium accuracy by default ; higher accuracy + * levels will be reported once, and if, we achieve + * calibration. + */ + data->gyro.status = SENSOR_STATUS_ACCURACY_MEDIUM; + + /* ... fall through */ + case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: - calibrate_gyro(data, &sensor_info[s]); + + /* + * We're only trying to calibrate data from continuously + * firing gyroscope drivers, as motion based ones use + * movement thresholds that may lead us to incorrectly + * estimate bias. + */ + if (sensor_info[s].selected_trigger != + sensor_info[s].motion_trigger_name) + calibrate_gyro(data, &sensor_info[s]); + + /* For noisy sensors we'll drop a very few number + * of samples to make sure we have at least MIN_SAMPLES events + * in the filtering queue. This is to make sure we are not sending + * events that can disturb our mean or stddev. + */ + if (sensor_info[s].quirks & QUIRK_NOISY) { + if((sensor_info[s].selected_trigger != + sensor_info[s].motion_trigger_name) && + sensor_info[s].event_count < MIN_SAMPLES) + return 0; + + denoise(s, data); + } + + /* Clamp near zero moves to (0,0,0) if appropriate */ + clamp_gyro_readings_to_zero(s, data); break; case SENSOR_TYPE_LIGHT: @@ -248,17 +346,15 @@ static float transform_sample_default(int s, int c, unsigned char* sample_data) } -static int finalize_sample_ISH(int s, struct sensors_event_t* data) +static int finalize_sample_ISH (int s, struct sensors_event_t* data) { - int i = sensor_info[s].catalog_index; - int sensor_type = sensor_catalog[i].type; float pitch, roll, yaw; /* Swap fields if we have a custom channel ordering on this sensor */ - if (sensor_info[s].flags & FLAG_FIELD_ORDERING) + if (sensor_info[s].quirks & QUIRK_FIELD_ORDERING) reorder_fields(data->data, sensor_info[s].order); - if (sensor_type == SENSOR_TYPE_ORIENTATION) { + if (sensor_info[s].type == SENSOR_TYPE_ORIENTATION) { pitch = data->data[0]; roll = data->data[1]; @@ -269,16 +365,17 @@ static int finalize_sample_ISH(int s, struct sensors_event_t* data) data->data[2] = -roll; } + /* Add this event to our global records, for filtering purposes */ + record_sample(s, data); + return 1; /* Return sample to Android */ } -static float transform_sample_ISH(int s, int c, unsigned char* sample_data) +static float transform_sample_ISH (int s, int c, unsigned char* sample_data) { struct datum_info_t* sample_type = &sensor_info[s].channel[c].type_info; int val = (int) sample_as_int64(sample_data, sample_type); - int i = sensor_info[s].catalog_index; - int sensor_type = sensor_catalog[i].type; float correction; int data_bytes = (sample_type->realbits)/8; int exponent = sensor_info[s].offset; @@ -286,7 +383,7 @@ static float transform_sample_ISH(int s, int c, unsigned char* sample_data) /* In case correction has been requested using properties, apply it */ correction = sensor_info[s].channel[c].opt_scale; - switch (sensor_type) { + switch (sensor_info[s].type) { case SENSOR_TYPE_ACCELEROMETER: switch (c) { case 0: