diff options
Diffstat (limited to 'libsensors_iio/src/SWAccelGyroFusion6X.cpp')
-rw-r--r-- | libsensors_iio/src/SWAccelGyroFusion6X.cpp | 157 |
1 files changed, 0 insertions, 157 deletions
diff --git a/libsensors_iio/src/SWAccelGyroFusion6X.cpp b/libsensors_iio/src/SWAccelGyroFusion6X.cpp deleted file mode 100644 index 83f8bd5..0000000 --- a/libsensors_iio/src/SWAccelGyroFusion6X.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/* - * STMicroelectronics Accel-Gyro Fusion 6X Sensor Class - * - * Copyright 2013-2015 STMicroelectronics Inc. - * Author: Denis Ciocca - <denis.ciocca@st.com> - * - * Licensed under the Apache License, Version 2.0 (the "License"). - */ - -#include <fcntl.h> -#include <assert.h> -#include <signal.h> - -#include "SWAccelGyroFusion6X.h" - -extern "C" { - #include "vSensorAPI.h" -} - -SWAccelGyroFusion6X::SWAccelGyroFusion6X(const char *name, int handle, int pipe_data_fd) : - SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_ST_ACCEL_GYRO_FUSION6X, - pipe_data_fd, false, false, true, false) -{ - sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE; - - sensor_t_data.resolution = ST_SENSOR_FUSION_RESOLUTION(1.0f); - sensor_t_data.maxRange = 1.0f; - - type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ACCELEROMETER; - type_dependencies[SENSOR_BASE_DEPENDENCY_1] = SENSOR_TYPE_GYROSCOPE; - type_sensor_need_trigger = SENSOR_TYPE_GYROSCOPE; - - vSensor_API_Initialization_6X(NULL); -} - -SWAccelGyroFusion6X::~SWAccelGyroFusion6X() -{ - -} - -int SWAccelGyroFusion6X::Enable(int handle, bool enable) -{ - int err; - bool old_status; - - old_status = GetStatus(); - - err = SWSensorBaseWithPollrate::Enable(handle, enable); - if (err < 0) - return err; - - if ((GetStatus() && !old_status) || (!GetStatus() && old_status)) { - sensor_event.timestamp = 0; - vSensor_API_enable_6X(enable); - } - - return 0; -} - -int SWAccelGyroFusion6X::SetDelay(int handle, int64_t period_ns, int64_t timeout) -{ - int err; - - if ((period_ns > FREQUENCY_TO_NS(CONFIG_ST_HAL_MIN_FUSION_POLLRATE) && period_ns != INT64_MAX)) - period_ns = FREQUENCY_TO_NS(CONFIG_ST_HAL_MIN_FUSION_POLLRATE); - - err = SWSensorBaseWithPollrate::SetDelay(handle, period_ns, timeout); - if (err < 0) - return err; - - real_pollrate = dependencies[SENSOR_BASE_DEPENDENCY_1]->GetRealPollrate(); - - return 0; -} - -void SWAccelGyroFusion6X::SplitAndProcessData(SensorBaseData data[ST_ACCEL_GYRO_MAX_OUT_ID]) -{ - int i, id, sensor_type; - trigger_mutex *dep_mutex; - - for (i = 0; i < (int)sensors_to_push_data_num; i++) { - if (sensors_to_push_data[i]->GetStatus()) { - switch (sensors_to_push_data_type[i]) { - case SENSOR_TYPE_GAME_ROTATION_VECTOR: - id = ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID; - break; - case SENSOR_TYPE_LINEAR_ACCELERATION: - id = ST_ACCEL_GYRO_LINEAR_ACCEL_OUT_ID; - break; - case SENSOR_TYPE_GRAVITY: - id = ST_ACCEL_GYRO_GRAVITY_OUT_ID; - break; - default: - continue; - } - - sensors_to_push_data[i]->ReceiveDataFromDependency(sensor_t_data.handle, &data[id]); - } - } - - for (i = 0; i < (int)sensors_to_trigger_num; i++) { - if (sensors_to_trigger[i]->GetStatus()) { - dep_mutex = sensors_to_trigger[i]->GetMutexForTrigger(); - pthread_mutex_lock(&dep_mutex->trigger_mutex); - pthread_cond_signal(&dep_mutex->trigger_data_cond); - pthread_mutex_unlock(&dep_mutex->trigger_mutex); - } - } -} - -void SWAccelGyroFusion6X::TriggerEventReceived() -{ - int64_t time_diff = 0; - SensorBaseData accel_data, gyro_data; - int err, data_remaining_gyro, nomaxdata = 10; - - do { - data_remaining_gyro = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_1, &gyro_data); - if (data_remaining_gyro < 0) - return; - - do { - err = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data); - if (err < 0) { - nomaxdata--; - usleep(200); - continue; - } - - time_diff = gyro_data.timestamp - accel_data.timestamp; - - } while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0)); - - if (err >= 0) - vSensor_API_Run_6X(accel_data.raw, gyro_data.processed, gyro_data.timestamp); - - sensor_event.timestamp = gyro_data.timestamp; - - err = vSensor_API_Get_Quaternion_6X(outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].processed); - if (err < 0) - return; - - err = vSensor_API_Get_LinAcc_6X(outdata[ST_ACCEL_GYRO_LINEAR_ACCEL_OUT_ID].processed); - if (err < 0) - return; - - err = vSensor_API_Get_Gravity_6X(outdata[ST_ACCEL_GYRO_GRAVITY_OUT_ID].processed); - if (err < 0) - return; - - outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].timestamp = sensor_event.timestamp; - outdata[ST_ACCEL_GYRO_LINEAR_ACCEL_OUT_ID].timestamp = sensor_event.timestamp; - outdata[ST_ACCEL_GYRO_GRAVITY_OUT_ID].timestamp = sensor_event.timestamp; - - SplitAndProcessData(outdata); - } while (data_remaining_gyro > 0); -} |