Created a rv_sensor using Sensor Fusion.
Change-Id: Ie7b3f2585621d724f0f3bd2e956e1bab4d3bfdfc
Signed-off-by: akhilkedia94 <akhil.kedia@samsung.com>
SET(RV "OFF")
SET(ORIENTATION "OFF")
ENDIF()
-SET(FUSION "ON")
SET(MOTION "OFF")
INCLUDE_DIRECTORIES(
SET(SENSOR_DEFINITIONS ${SENSOR_DEFINITIONS} "-DENABLE_ORIENTATION")
ENDIF()
IF("${RV}" STREQUAL "ON")
- FILE(GLOB_RECURSE SENSOR_SRCS ${SENSOR_SRCS} ${CMAKE_CURRENT_SOURCE_DIR}/rotation_vector/*.cpp)
+ FILE(GLOB SENSOR_SRCS ${SENSOR_SRCS} ${CMAKE_CURRENT_SOURCE_DIR}/rotation_vector/*.cpp)
SET(SENSOR_HEADERS ${SENSOR_HEADERS} ${CMAKE_CURRENT_SOURCE_DIR}/rotation_vector)
SET(SENSOR_DEFINITIONS ${SENSOR_DEFINITIONS} "-DENABLE_ROTATION_VECTOR")
-ENDIF()
-IF("${FUSION}" STREQUAL "ON")
-FILE(GLOB SENSOR_SRCS ${SENSOR_SRCS} ${CMAKE_CURRENT_SOURCE_DIR}/sensor_fusion/*.cpp)
-SET(SENSOR_HEADERS ${SENSOR_HEADERS} ${CMAKE_CURRENT_SOURCE_DIR}/sensor_fusion)
+ FILE(GLOB SENSOR_SRCS ${SENSOR_SRCS} ${CMAKE_CURRENT_SOURCE_DIR}/rotation_vector/fusion_utils/*.cpp)
ENDIF()
IF("${MOTION}" STREQUAL "ON")
add_subdirectory(motion)
#include <sensor_base.h>
#include <cmath>
#include "fusion_base.h"
-#include "orientation_filter.h"
#define ACCEL_COMPENSATION -1
#define GYRO_COMPENSATION 1
#define __FUSION_BASE_H__
#include <fusion.h>
-#include <orientation_filter.h>
+#include "fusion_utils/orientation_filter.h"
class fusion_base : public virtual fusion {
public:
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2016 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <errno.h>
-#include <math.h>
-#include <time.h>
-#include <sys/types.h>
-#include <dlfcn.h>
-
-#include <sensor_log.h>
-#include <sensor_types.h>
-
-#include <sensor_common.h>
-#include <virtual_sensor.h>
-#include <rotation_vector_sensor.h>
-#include <sensor_loader.h>
-#include <fusion_util.h>
-
-#define SENSOR_NAME "SENSOR_ROTATION_VECTOR"
-
-#define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z))
-
-#define STATE_ACCEL 0x1
-#define STATE_MAGNETIC 0x2
-
-rotation_vector_sensor::rotation_vector_sensor()
-: m_accel_sensor(NULL)
-, m_mag_sensor(NULL)
-, m_x(-1)
-, m_y(-1)
-, m_z(-1)
-, m_w(-1)
-, m_time(0)
-, m_state(0)
-{
-}
-
-rotation_vector_sensor::~rotation_vector_sensor()
-{
- _I("%s is destroyed!", SENSOR_NAME);
-}
-
-bool rotation_vector_sensor::init(void)
-{
- m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
- m_mag_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
-
- if (!m_accel_sensor || !m_mag_sensor) {
- _E("cannot load sensors[%s]", SENSOR_NAME);
- return false;
- }
-
- _I("%s is created!", SENSOR_NAME);
- return true;
-}
-
-sensor_type_t rotation_vector_sensor::get_type(void)
-{
- return ROTATION_VECTOR_SENSOR;
-}
-
-unsigned int rotation_vector_sensor::get_event_type(void)
-{
- return CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR);
-}
-
-const char* rotation_vector_sensor::get_name(void)
-{
- return SENSOR_NAME;
-}
-
-bool rotation_vector_sensor::get_sensor_info(sensor_info &info)
-{
- info.set_type(get_type());
- info.set_id(get_id());
- info.set_privilege(SENSOR_PRIVILEGE_PUBLIC);
- info.set_name(get_name());
- info.set_vendor("Samsung Electronics");
- info.set_min_range(0);
- info.set_max_range(1);
- info.set_resolution(1);
- info.set_min_interval(1);
- info.set_fifo_count(0);
- info.set_max_batch_count(0);
- info.set_supported_event(get_event_type());
- info.set_wakeup_supported(false);
-
- return true;
-}
-
-void rotation_vector_sensor::synthesize(const sensor_event_t& event)
-{
- sensor_event_t *rotation_vector_event;
- float R[9];
- float I[9];
- float quat[4];
- int error;
-
- if (event.event_type != GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME &&
- event.event_type != ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME)
- return;
-
- if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
- m_mag[0] = event.data->values[0];
- m_mag[1] = event.data->values[1];
- m_mag[2] = event.data->values[2];
- m_accuracy = event.data->accuracy;
-
- m_state |= STATE_MAGNETIC;
- }
-
- if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
- m_acc[0] = event.data->values[0];
- m_acc[1] = event.data->values[1];
- m_acc[2] = event.data->values[2];
-
- m_state |= STATE_ACCEL;
- }
-
- if (m_state != (STATE_ACCEL | STATE_MAGNETIC))
- return;
-
- m_state = 0;
-
- unsigned long long timestamp = event.data->timestamp;
-
- error = calculate_rotation_matrix(m_acc, m_mag, R, I);
- ret_if(error < 0);
-
- error = matrix_to_quat(R, quat);
- ret_if(error < 0);
-
- rotation_vector_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
- if (!rotation_vector_event) {
- _E("Failed to allocate memory");
- return;
- }
- rotation_vector_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
- if (!rotation_vector_event->data) {
- _E("Failed to allocate memory");
- free(rotation_vector_event);
- return;
- }
-
- rotation_vector_event->sensor_id = get_id();
- rotation_vector_event->event_type = CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR);
- rotation_vector_event->data_length = sizeof(sensor_data_t);
- rotation_vector_event->data->accuracy = m_accuracy;
- rotation_vector_event->data->timestamp = timestamp;
- rotation_vector_event->data->value_count = 4;
- rotation_vector_event->data->values[0] = quat[0];
- rotation_vector_event->data->values[1] = quat[1];
- rotation_vector_event->data->values[2] = quat[2];
- rotation_vector_event->data->values[3] = quat[3];
- push(rotation_vector_event);
-
- m_time = timestamp;
- m_x = quat[0];
- m_y = quat[1];
- m_z = quat[2];
- m_w = quat[3];
- m_accuracy = event.data->accuracy;
-
- _D("[rotation_vector] : [%10f] [%10f] [%10f] [%10f]", m_x, m_y, m_z, m_w);
-}
-
-int rotation_vector_sensor::get_data(sensor_data_t **data, int *length)
-{
- sensor_data_t *sensor_data;
- sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
-
- sensor_data->accuracy = m_accuracy;
- sensor_data->timestamp = m_time;
- sensor_data->value_count = 3;
- sensor_data->values[0] = m_x;
- sensor_data->values[1] = m_y;
- sensor_data->values[2] = m_z;
-
- *data = sensor_data;
- *length = sizeof(sensor_data_t);
-
- return 0;
-}
-
-bool rotation_vector_sensor::set_interval(unsigned long interval)
-{
- m_interval = interval;
- return true;
-}
-
-bool rotation_vector_sensor::set_batch_latency(unsigned long latency)
-{
- return false;
-}
-
-bool rotation_vector_sensor::on_start(void)
-{
- if (m_accel_sensor)
- m_accel_sensor->start();
-
- if (m_mag_sensor)
- m_mag_sensor->start();
-
- m_time = 0;
- return activate();
-}
-
-bool rotation_vector_sensor::on_stop(void)
-{
- if (m_accel_sensor)
- m_accel_sensor->stop();
-
- if (m_mag_sensor)
- m_mag_sensor->stop();
-
- m_time = 0;
- m_state = 0;
-
- return deactivate();
-}
-
-bool rotation_vector_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
-{
- if (m_accel_sensor)
- m_accel_sensor->add_interval(client_id, interval, true);
-
- if (m_mag_sensor)
- m_mag_sensor->add_interval(client_id, interval, true);
-
- return sensor_base::add_interval(client_id, interval, is_processor);
-}
-
-bool rotation_vector_sensor::delete_interval(int client_id, bool is_processor)
-{
- if (m_accel_sensor)
- m_accel_sensor->delete_interval(client_id, true);
-
- if (m_mag_sensor)
- m_mag_sensor->delete_interval(client_id, true);
-
- return sensor_base::delete_interval(client_id, is_processor);
-}
+++ /dev/null
-/*
- * sensord
- *
- * Copyright (c) 2016 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- */
-
-#ifndef _ROTATION_VECTOR_SENSOR_H_
-#define _ROTATION_VECTOR_SENSOR_H_
-
-#include <virtual_sensor.h>
-#include <sensor_types.h>
-
-class rotation_vector_sensor : public virtual_sensor {
-public:
- rotation_vector_sensor();
- virtual ~rotation_vector_sensor();
-
- /* initialize sensor */
- bool init(void);
-
- /* sensor info */
- virtual sensor_type_t get_type(void);
- virtual unsigned int get_event_type(void);
- virtual const char* get_name(void);
-
- virtual bool get_sensor_info(sensor_info &info);
-
- /* synthesize event */
- virtual void synthesize(const sensor_event_t& event);
-
- bool add_interval(int client_id, unsigned int interval, bool is_processor);
- bool delete_interval(int client_id, bool is_processor);
-
- /* get data */
- virtual int get_data(sensor_data_t **data, int *length);
-private:
- sensor_base *m_accel_sensor;
- sensor_base *m_mag_sensor;
-
- float m_x;
- float m_y;
- float m_z;
- float m_w;
- int m_accuracy;
- unsigned long long m_time;
- unsigned long m_interval;
-
- float m_acc[3];
- float m_mag[3];
- int m_state;
-
- virtual bool set_interval(unsigned long interval);
- virtual bool set_batch_latency(unsigned long latency);
-
- virtual bool on_start(void);
- virtual bool on_stop(void);
-};
-
-#endif /* _ROTATION_VECTOR_SENSOR_H_ */
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <time.h>
+#include <sys/types.h>
+#include <dlfcn.h>
+
+#include <sensor_log.h>
+#include <sensor_types.h>
+
+#include <sensor_common.h>
+#include <virtual_sensor.h>
+#include <rv_sensor.h>
+#include <sensor_loader.h>
+#include <fusion_util.h>
+
+#define SENSOR_NAME "SENSOR_ROTATION_VECTOR"
+
+
+rv_sensor::rv_sensor()
+: m_accel_sensor(NULL)
+, m_gyro_sensor(NULL)
+, m_mag_sensor(NULL)
+, m_x(-1)
+, m_y(-1)
+, m_z(-1)
+, m_w(-1)
+, m_time(0)
+, m_accuracy(SENSOR_ACCURACY_UNDEFINED)
+{
+}
+
+rv_sensor::~rv_sensor()
+{
+ _I("%s is destroyed!", SENSOR_NAME);
+}
+
+bool rv_sensor::init(void)
+{
+ m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
+ m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
+ m_mag_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
+
+ if (!m_accel_sensor || !m_gyro_sensor|| !m_mag_sensor) {
+ _E("cannot load sensors[%s]", SENSOR_NAME);
+ return false;
+ }
+
+ _I("%s is created!", SENSOR_NAME);
+ return true;
+}
+
+sensor_type_t rv_sensor::get_type(void)
+{
+ return ROTATION_VECTOR_SENSOR;
+}
+
+unsigned int rv_sensor::get_event_type(void)
+{
+ return CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR);
+}
+
+const char* rv_sensor::get_name(void)
+{
+ return SENSOR_NAME;
+}
+
+bool rv_sensor::get_sensor_info(sensor_info &info)
+{
+ info.set_type(get_type());
+ info.set_id(get_id());
+ info.set_privilege(SENSOR_PRIVILEGE_PUBLIC);
+ info.set_name(get_name());
+ info.set_vendor("Samsung Electronics");
+ info.set_min_range(0);
+ info.set_max_range(1);
+ info.set_resolution(1);
+ info.set_min_interval(1);
+ info.set_fifo_count(0);
+ info.set_max_batch_count(0);
+ info.set_supported_event(get_event_type());
+ info.set_wakeup_supported(false);
+
+ return true;
+}
+
+void rv_sensor::synthesize(const sensor_event_t& event)
+{
+ sensor_event_t *rotation_vector_event;
+
+ if (event.event_type != GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME &&
+ event.event_type != ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME &&
+ event.event_type != GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME)
+ return;
+
+ if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME)
+ m_fusion.push_accel(*(event.data));
+ else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME)
+ m_fusion.push_mag(*(event.data));
+ else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME)
+ m_fusion.push_gyro(*(event.data));
+
+ if (m_accuracy == SENSOR_ACCURACY_UNDEFINED)
+ m_accuracy = event.data->accuracy;
+ else if (m_accuracy > event.data->accuracy)
+ m_accuracy = event.data->accuracy;
+
+ unsigned long long timestamp;
+ if (!m_fusion.get_rv(timestamp, m_w, m_x, m_y, m_z))
+ return;
+
+ if (timestamp == m_time)
+ return;
+ m_time = timestamp;
+
+ rotation_vector_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
+ if (!rotation_vector_event) {
+ _E("Failed to allocate memory");
+ return;
+ }
+ rotation_vector_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
+ if (!rotation_vector_event->data) {
+ _E("Failed to allocate memory");
+ free(rotation_vector_event);
+ return;
+ }
+
+ rotation_vector_event->sensor_id = get_id();
+ rotation_vector_event->event_type = CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR);
+ rotation_vector_event->data_length = sizeof(sensor_data_t);
+ rotation_vector_event->data->accuracy = m_accuracy;
+ rotation_vector_event->data->timestamp = m_time;
+ rotation_vector_event->data->value_count = 4;
+ rotation_vector_event->data->values[0] = m_w;
+ rotation_vector_event->data->values[1] = m_x;
+ rotation_vector_event->data->values[2] = m_y;
+ rotation_vector_event->data->values[3] = m_z;
+ push(rotation_vector_event);
+ m_accuracy = SENSOR_ACCURACY_UNDEFINED;
+
+ _D("[rotation_vector] : [%10f] [%10f] [%10f] [%10f]", m_x, m_y, m_z, m_w);
+}
+
+int rv_sensor::get_data(sensor_data_t **data, int *length)
+{
+ sensor_data_t *sensor_data;
+ sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
+
+ sensor_data->accuracy = m_accuracy;
+ sensor_data->timestamp = m_time;
+ sensor_data->value_count = 3;
+ sensor_data->values[0] = m_x;
+ sensor_data->values[1] = m_y;
+ sensor_data->values[2] = m_z;
+
+ *data = sensor_data;
+ *length = sizeof(sensor_data_t);
+
+ return 0;
+}
+
+bool rv_sensor::set_interval(unsigned long interval)
+{
+ m_interval = interval;
+ return true;
+}
+
+bool rv_sensor::set_batch_latency(unsigned long latency)
+{
+ return false;
+}
+
+bool rv_sensor::on_start(void)
+{
+ m_accel_sensor->start();
+ m_gyro_sensor->start();
+ m_mag_sensor->start();
+ m_time = 0;
+ m_accuracy = SENSOR_ACCURACY_UNDEFINED;
+ return activate();
+}
+
+bool rv_sensor::on_stop(void)
+{
+ m_accel_sensor->stop();
+ m_gyro_sensor->stop();
+ m_mag_sensor->stop();
+ m_time = 0;
+ m_accuracy = SENSOR_ACCURACY_UNDEFINED;
+ return deactivate();
+}
+
+bool rv_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
+{
+ m_accel_sensor->add_interval(client_id, interval, true);
+ m_gyro_sensor->add_interval(client_id, interval, true);
+ m_mag_sensor->add_interval(client_id, interval, true);
+
+ return sensor_base::add_interval(client_id, interval, is_processor);
+}
+
+bool rv_sensor::delete_interval(int client_id, bool is_processor)
+{
+ m_accel_sensor->delete_interval(client_id, true);
+ m_gyro_sensor->delete_interval(client_id, true);
+ m_mag_sensor->delete_interval(client_id, true);
+
+ return sensor_base::delete_interval(client_id, is_processor);
+}
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2016 Samsung Electronics Co., Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef _ROTATION_VECTOR_SENSOR_H_
+#define _ROTATION_VECTOR_SENSOR_H_
+
+#include <virtual_sensor.h>
+#include <sensor_types.h>
+#include <gyro_magnetic_fusion.h>
+
+class rv_sensor : public virtual_sensor {
+public:
+ rv_sensor();
+ virtual ~rv_sensor();
+
+ /* initialize sensor */
+ bool init(void);
+
+ /* sensor info */
+ virtual sensor_type_t get_type(void);
+ virtual unsigned int get_event_type(void);
+ virtual const char* get_name(void);
+
+ virtual bool get_sensor_info(sensor_info &info);
+
+ /* synthesize event */
+ virtual void synthesize(const sensor_event_t& event);
+
+ bool add_interval(int client_id, unsigned int interval, bool is_processor);
+ bool delete_interval(int client_id, bool is_processor);
+
+ /* get data */
+ virtual int get_data(sensor_data_t **data, int *length);
+private:
+ sensor_base *m_accel_sensor;
+ sensor_base *m_mag_sensor;
+ sensor_base *m_gyro_sensor;
+ gyro_magnetic_fusion m_fusion;
+
+ float m_x;
+ float m_y;
+ float m_z;
+ float m_w;
+ unsigned long long m_time;
+ unsigned long m_interval;
+ int m_accuracy;
+
+ virtual bool set_interval(unsigned long interval);
+ virtual bool set_batch_latency(unsigned long latency);
+
+ virtual bool on_start(void);
+ virtual bool on_stop(void);
+};
+
+#endif /* _ROTATION_VECTOR_SENSOR_H_ */
#include <orientation_sensor.h>
#endif
#ifdef ENABLE_ROTATION_VECTOR
-#include <rotation_vector_sensor.h>
+#include <rv_sensor.h>
#endif
using std::vector;
create_virtual_sensors<auto_rotation_sensor>("Auto Rotation");
#endif
#ifdef ENABLE_ROTATION_VECTOR
- create_virtual_sensors<rotation_vector_sensor>("Rotation Vector");
+ create_virtual_sensors<rv_sensor>("Rotation Vector");
#endif
#ifdef ENABLE_GRAVITY
create_virtual_sensors<gravity_sensor>("Gravity");
projects / platform / core / system / sensord.git / commitdiff