diff options
Diffstat (limited to 'libsensors_iio/src/SWAccelMagnFusion6X.cpp')
| -rw-r--r-- | libsensors_iio/src/SWAccelMagnFusion6X.cpp | 166 |
1 files changed, 0 insertions, 166 deletions
diff --git a/libsensors_iio/src/SWAccelMagnFusion6X.cpp b/libsensors_iio/src/SWAccelMagnFusion6X.cpp deleted file mode 100644 index 4822256..0000000 --- a/libsensors_iio/src/SWAccelMagnFusion6X.cpp +++ /dev/null @@ -1,166 +0,0 @@ -/* - * STMicroelectronics Accel-Magn 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 "SWAccelMagnFusion6X.h" - -extern "C" { - #include "iNemoEngineGeoMagAPI.h" -} - -SWAccelMagnFusion6X::SWAccelMagnFusion6X(const char *name, int handle, int pipe_data_fd) : - SWSensorBaseWithPollrate(name, handle, SENSOR_TYPE_ST_ACCEL_MAGN_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_MAGNETIC_FIELD; - type_sensor_need_trigger = SENSOR_TYPE_MAGNETIC_FIELD; -} - -SWAccelMagnFusion6X::~SWAccelMagnFusion6X() -{ - -} - -int SWAccelMagnFusion6X::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) { - sensor_event.timestamp = 0; - iNemoEngine_GeoMag_API_Initialization(); - } - - return 0; -} - -int SWAccelMagnFusion6X::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 SWAccelMagnFusion6X::SplitAndProcessData(SensorBaseData data[ST_GEOMAG_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_GEOMAGNETIC_ROTATION_VECTOR: - id = ST_GEOMAG_ROTATION_VECTOR_OUT_ID; - break; - case SENSOR_TYPE_ORIENTATION: - id = ST_GEOMAG_ORIENTATION_OUT_ID; - break; - case SENSOR_TYPE_LINEAR_ACCELERATION: - id = ST_GEOMAG_LINEAR_ACCEL_OUT_ID; - break; - case SENSOR_TYPE_GRAVITY: - id = ST_GEOMAG_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 SWAccelMagnFusion6X::TriggerEventReceived() -{ - int deltatime; - int64_t time_diff = 0; - SensorBaseData accel_data, magn_data; - int err, data_remaining_magn, nomaxdata = 10; - - do { - data_remaining_magn = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_1, &magn_data); - if (data_remaining_magn < 0) - return; - - do { - err = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data); - if (err < 0) { - nomaxdata--; - usleep(200); - continue; - } - - time_diff = magn_data.timestamp - accel_data.timestamp; - - } while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0)); - - if ((err >= 0) && (sensor_event.timestamp > 0)) { - deltatime = (int)NS_TO_MS((uint64_t)(magn_data.timestamp - sensor_event.timestamp)); - iNemoEngine_GeoMag_API_Run(accel_data.raw, magn_data.processed, deltatime); - } - - sensor_event.timestamp = magn_data.timestamp; - - err = iNemoEngine_GeoMag_API_Get_Quaternion(outdata[ST_GEOMAG_ROTATION_VECTOR_OUT_ID].processed); - if (err < 0) - return; - - err = iNemoEngine_GeoMag_API_Get_Hpr(outdata[ST_GEOMAG_ORIENTATION_OUT_ID].processed); - if (err < 0) - return; - - err = iNemoEngine_GeoMag_API_Get_LinAcc(outdata[ST_GEOMAG_LINEAR_ACCEL_OUT_ID].processed); - if (err < 0) - return; - - err = iNemoEngine_GeoMag_API_Get_Gravity(outdata[ST_GEOMAG_GRAVITY_OUT_ID].processed); - if (err < 0) - return; - - outdata[ST_GEOMAG_ROTATION_VECTOR_OUT_ID].timestamp = sensor_event.timestamp; - outdata[ST_GEOMAG_ORIENTATION_OUT_ID].timestamp = sensor_event.timestamp; - outdata[ST_GEOMAG_LINEAR_ACCEL_OUT_ID].timestamp = sensor_event.timestamp; - outdata[ST_GEOMAG_GRAVITY_OUT_ID].timestamp = sensor_event.timestamp; - - SplitAndProcessData(outdata); - } while (data_remaining_magn > 0); -} |