1 files changed, 141 insertions, 0 deletions
diff --git a/libsensors_iio/src/SWAccelGyroFusion6X.cpp b/libsensors_iio/src/SWAccelGyroFusion6X.cpp
new file mode 100644
index 0000000..f03c950
--- /dev/null
+++ b/libsensors_iio/src/SWAccelGyroFusion6X.cpp
@@ -0,0 +1,141 @@
+/*
+ * 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 "iNemoEngineAPI.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;
+
+	iNemoEngine_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;
+		iNemoEngine_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;
+			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)
+			iNemoEngine_API_Run_6X(accel_data.raw, gyro_data.processed, gyro_data.timestamp);
+
+		sensor_event.timestamp = gyro_data.timestamp;
+
+		err = iNemoEngine_API_Get_Quaternion_6X(outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].processed);
+		if (err < 0)
+			return;
+
+		outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].timestamp = sensor_event.timestamp;
+
+		SplitAndProcessData(outdata);
+	} while (data_remaining_gyro > 0);
+}