diff options
Diffstat (limited to 'libsensors_iio/src/SWAccelMagnFusion6X.cpp')
-rw-r--r-- | libsensors_iio/src/SWAccelMagnFusion6X.cpp | 166 |
1 files changed, 166 insertions, 0 deletions
diff --git a/libsensors_iio/src/SWAccelMagnFusion6X.cpp b/libsensors_iio/src/SWAccelMagnFusion6X.cpp new file mode 100644 index 0000000..4822256 --- /dev/null +++ b/libsensors_iio/src/SWAccelMagnFusion6X.cpp @@ -0,0 +1,166 @@ +/* + * 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); +} |