break;
case SENSOR_TYPE_GYROSCOPE:
+ case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
gyro_cal_init(&sensor_info[s]);
break;
}
compass_store_data(&sensor_info[s]);
}
+
+ /* If uncalibrated type and pair is already active don't adjust counters */
+ if (sensor_type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED &&
+ sensor_info[sensor_info[s].pair_idx].enable_count != 0)
+ return 0;
+
/* We changed the state of a sensor - adjust per iio device counters */
/* If this is a regular event-driven sensor */
case SENSOR_TYPE_ACCELEROMETER: /* m/s^2 */
case SENSOR_TYPE_MAGNETIC_FIELD: /* micro-tesla */
case SENSOR_TYPE_ORIENTATION: /* degrees */
+ case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
case SENSOR_TYPE_GYROSCOPE: /* radians/s */
return 3;
int i = sensor_info[s].catalog_index;
int is_poll_sensor = !sensor_info[s].num_channels;
+ /* If we want to activate gyro calibrated and gyro uncalibrated is activated
+ * Deactivate gyro uncalibrated - Uncalibrated releases handler
+ * Activate gyro calibrated - Calibrated has handler
+ * Reactivate gyro uncalibrated - Uncalibrated gets data from calibrated */
+
+ /* If we want to deactivate gyro calibrated and gyro uncalibrated is active
+ * Deactivate gyro uncalibrated - Uncalibrated no longer gets data from handler
+ * Deactivate gyro calibrated - Calibrated releases handler
+ * Reactivate gyro uncalibrated - Uncalibrated has handler */
+
+ if (sensor_catalog[sensor_info[s].catalog_index].type == SENSOR_TYPE_GYROSCOPE &&
+ sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enable_count != 0) {
+
+ sensor_activate(sensor_info[s].pair_idx, 0);
+ ret = sensor_activate(s, enabled);
+ sensor_activate(sensor_info[s].pair_idx, 1);
+ return ret;
+ }
+
ret = adjust_counters(s, enabled);
/* If the operation was neutral in terms of state, we're done */
if (ret <= 0)
return ret;
+
if (!is_poll_sensor) {
/* Stop sampling */
if (!num_fields)
return 0;
+
+ /* Only return uncalibrated event if also gyro active */
+ if (sensor_type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED &&
+ sensor_info[sensor_info[s].pair_idx].enable_count != 0)
+ return 0;
+
memset(data, 0, sizeof(sensors_event_t));
data->version = sizeof(sensors_event_t);
struct epoll_event ev[MAX_DEVICES];
int64_t target_ts;
int returned_events;
+ int event_count;
/* Get one or more events from our collection of sensors */
returned_events = 0;
/* Check our sensor collection for available reports */
- for (s=0; s<sensor_count && returned_events<count; s++)
+ for (s=0; s<sensor_count && returned_events < count; s++)
if (sensor_info[s].report_pending) {
-
+ event_count = 0;
/* Lower flag */
sensor_info[s].report_pending = 0;
/* Report this event if it looks OK */
- returned_events +=
- propagate_sensor_report(s, &data[returned_events]);
-
+ event_count = propagate_sensor_report(s, &data[returned_events]);
+
+ /* Duplicate only if both cal & uncal are active */
+ if (sensor_catalog[sensor_info[s].catalog_index].type == SENSOR_TYPE_GYROSCOPE &&
+ sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enable_count != 0) {
+ struct gyro_cal* gyro_data = (struct gyro_cal*) sensor_info[s].cal_data;
+
+ memcpy(&data[returned_events + event_count], &data[returned_events],
+ sizeof(struct sensors_event_t) * event_count);
+ for (i = 0; i < event_count; i++) {
+ data[returned_events + i].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
+ data[returned_events + i].sensor = sensor_info[s].pair_idx;
+
+ data[returned_events + i].data[0] = data[returned_events + i].data[0] + gyro_data->bias[0];
+ data[returned_events + i].data[1] = data[returned_events + i].data[1] + gyro_data->bias[1];
+ data[returned_events + i].data[2] = data[returned_events + i].data[2] + gyro_data->bias[2];
+
+ data[returned_events + i].uncalibrated_gyro.bias[0] = gyro_data->bias[0];
+ data[returned_events + i].uncalibrated_gyro.bias[1] = gyro_data->bias[1];
+ data[returned_events + i].uncalibrated_gyro.bias[2] = gyro_data->bias[2];
+ }
+ event_count <<= 1;
+ }
+ sensor_info[sensor_info[s].pair_idx].report_pending = 0;
+ returned_events += event_count;
/*
* If the sample was deemed invalid or unreportable,
* e.g. had the same value as the previously reported
* device number associated to a specific sensor.
*/
+ /*
+ * We duplicate entries for the uncalibrated types after their respective base
+ * sensor. This is because all sensor entries must have an associated catalog entry
+ * and also because when only the uncal sensor is active it needs to take it's data
+ * from the same iio device as the base one.
+ */
+
struct sensor_catalog_entry_t sensor_catalog[] = {
DECLARE_SENSOR3("accel", SENSOR_TYPE_ACCELEROMETER, "x", "y", "z")
DECLARE_SENSOR3("anglvel", SENSOR_TYPE_GYROSCOPE, "x", "y", "z")
"quat_x", "quat_y", "quat_z", "quat_w")
DECLARE_SENSOR0("temp", SENSOR_TYPE_AMBIENT_TEMPERATURE )
DECLARE_SENSOR0("proximity", SENSOR_TYPE_PROXIMITY )
+ DECLARE_SENSOR3("anglvel", SENSOR_TYPE_GYROSCOPE_UNCALIBRATED, "x", "y", "z")
};
#define CATALOG_SIZE ARRAY_SIZE(sensor_catalog)
strcpy(sensor_info[s].internal_name, "(null)");
}
- if (sensor_catalog[catalog_index].type == SENSOR_TYPE_GYROSCOPE) {
+ if (sensor_catalog[catalog_index].type == SENSOR_TYPE_GYROSCOPE ||
+ sensor_catalog[catalog_index].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
struct gyro_cal* calibration_data = calloc(1, sizeof(struct gyro_cal));
sensor_info[s].cal_data = calibration_data;
}
dir = opendir(base_dir);
if (!dir) {
- return;
+ return;
}
/* Enumerate entries in this iio device's base folder */
if (!strcmp(d->d_name, ".") || !strcmp(d->d_name, ".."))
continue;
- /* If the name matches a catalog entry, flag it */
- for (i = 0; i<CATALOG_SIZE; i++)
- for (c=0; c<sensor_catalog[i].num_channels; c++)
- if (!strcmp(d->d_name,
- sensor_catalog[i].channel[c].raw_path) ||
- !strcmp(d->d_name,
- sensor_catalog[i].channel[c].input_path)) {
- map[i] = 1;
- break;
- }
+ /* If the name matches a catalog entry, flag it */
+ for (i = 0; i<CATALOG_SIZE; i++) {
+ /* This will be added separately later */
+ if (sensor_catalog[i].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED)
+ continue;
+ for (c=0; c<sensor_catalog[i].num_channels; c++)
+ if (!strcmp(d->d_name,sensor_catalog[i].channel[c].raw_path) ||
+ !strcmp(d->d_name, sensor_catalog[i].channel[c].input_path)) {
+ map[i] = 1;
+ break;
+ }
+ }
}
closedir(dir);
struct dirent *d;
unsigned int i;
- memset(map, 0, CATALOG_SIZE);
+ memset(map, 0, CATALOG_SIZE);
/* Enumerate entries in this iio device's scan_elements folder */
dir = opendir(scan_elem_dir);
if (!dir) {
- return;
+ return;
}
while ((d = readdir(dir))) {
/* Compare en entry to known ones and create matching sensors */
- for (i = 0; i<CATALOG_SIZE; i++)
+ for (i = 0; i<CATALOG_SIZE; i++) {
+ if (sensor_catalog[i].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED)
+ continue;
if (!strcmp(d->d_name,
- sensor_catalog[i].channel[0].en_path)) {
- map[i] = 1;
- break;
- }
+ sensor_catalog[i].channel[0].en_path)) {
+ map[i] = 1;
+ break;
+ }
+ }
}
closedir(dir);
}
}
+static void uncalibrated_gyro_check (void)
+{
+ unsigned int has_gyr = 0;
+ unsigned int dev_num;
+ int i, c;
+ unsigned int is_poll_sensor;
+
+ int cal_idx = 0;
+ int uncal_idx = 0;
+
+ /* Checking to see if we have a gyroscope - we can only have uncal if we have the base sensor */
+ for (i=0; i < sensor_count; i++)
+ if(sensor_catalog[sensor_info[i].catalog_index].type == SENSOR_TYPE_GYROSCOPE)
+ {
+ has_gyr=1;
+ dev_num = sensor_info[i].dev_num;
+ is_poll_sensor = !sensor_info[i].num_channels;
+ cal_idx = i;
+ break;
+ }
+
+ /*
+ * If we have a gyro we can add the uncalibrated sensor of the same type and
+ * on the same dev_num. We will save indexes for easy finding and also save the
+ * channel specific information.
+ */
+ if (has_gyr)
+ for (i=0; i<CATALOG_SIZE; i++)
+ if (sensor_catalog[i].type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
+ add_sensor(dev_num, i, is_poll_sensor);
+
+ uncal_idx = sensor_count - 1; /* Just added uncalibrated sensor */
+
+ /* Similar to build_sensor_report_maps */
+ for (c = 0; c < sensor_info[uncal_idx].num_channels; c++)
+ {
+ memcpy( &(sensor_info[uncal_idx].channel[c].type_spec),
+ &(sensor_info[cal_idx].channel[c].type_spec),
+ sizeof(sensor_info[uncal_idx].channel[c].type_spec));
+ sensor_info[uncal_idx].channel[c].type_info = sensor_info[cal_idx].channel[c].type_info;
+ sensor_info[uncal_idx].channel[c].offset = sensor_info[cal_idx].channel[c].offset;
+ sensor_info[uncal_idx].channel[c].size = sensor_info[cal_idx].channel[c].size;
+ }
+ sensor_info[uncal_idx].pair_idx = cal_idx;
+ sensor_info[cal_idx].pair_idx = uncal_idx;
+ break;
+ }
+}
void enumerate_sensors (void)
{
/* Make sure Android fall backs to its own orientation sensor */
orientation_sensor_check();
+
+ /* Create the uncalibrated counterpart to the compensated gyroscope;
+ * This is is a new sensor type in Android 4.4 */
+ uncalibrated_gyro_check();
}
for (i = 0; i < sensor_count; i++)
switch (sensor_catalog[sensor_info[i].catalog_index].type) {
case SENSOR_TYPE_MAGNETIC_FIELD:
+ case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
case SENSOR_TYPE_GYROSCOPE:
if (sensor_info[i].cal_data != NULL) {
free(sensor_info[i].cal_data);
{
if (!info->calibrated) {
gyro_collect(event->data[0], event->data[1], event->data[2], info);
+ return;
}
- else {
- struct gyro_cal* gyro_data = (struct gyro_cal*) info->cal_data;
- if (gyro_data == NULL)
- return;
+ struct gyro_cal* gyro_data = (struct gyro_cal*) info->cal_data;
+
+ if (gyro_data == NULL)
+ return;
- event->data[0] = event->gyro.x = event->data[0] - gyro_data->bias[0];
- event->data[1] = event->gyro.y = event->data[1] - gyro_data->bias[1];
- event->data[2] = event->gyro.z = event->data[2] - gyro_data->bias[2];
+ switch (event->type) {
+ case SENSOR_TYPE_GYROSCOPE:
+ /* For the gyroscope apply the bias */
+ event->data[0] = event->data[0] - gyro_data->bias[0];
+ event->data[1] = event->data[1] - gyro_data->bias[1];
+ event->data[2] = event->data[2] - gyro_data->bias[2];
+ break;
+
+ case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
+ /* For the uncalibrated gyroscope don't apply the bias but save it */
+ event->uncalibrated_gyro.bias[0] = gyro_data->bias[0];
+ event->uncalibrated_gyro.bias[1] = gyro_data->bias[1];
+ event->uncalibrated_gyro.bias[2] = gyro_data->bias[2];
+ break;
+
+ default:
+ break;
}
}
\ No newline at end of file
OSDN Git Service
