Rotation Vector Sensor

Created a rv_sensor using Sensor Fusion.

Change-Id: Ie7b3f2585621d724f0f3bd2e956e1bab4d3bfdfc
Signed-off-by: akhilkedia94 <akhil.kedia@samsung.com>

diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt
index 0547bc8..3cbd1a7 100644 (file)
--- a/src/sensor/CMakeLists.txt
+++ b/src/sensor/CMakeLists.txt
@@ -14,7 +14,6 @@ ELSE()
 SET(RV "OFF")
 SET(ORIENTATION "OFF")
 ENDIF()
-SET(FUSION "ON")
 SET(MOTION "OFF")
 
 INCLUDE_DIRECTORIES(
@@ -59,13 +58,10 @@ IF("${ORIENTATION}" STREQUAL "ON")
        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)

diff --git a/src/sensor/rotation_vector/fusion_base.cpp b/src/sensor/rotation_vector/fusion_base.cpp
index 355a551..51e45d3 100644 (file)
--- a/src/sensor/rotation_vector/fusion_base.cpp
+++ b/src/sensor/rotation_vector/fusion_base.cpp
@@ -26,7 +26,6 @@
 #include <sensor_base.h>
 #include <cmath>
 #include "fusion_base.h"
-#include "orientation_filter.h"
 
 #define ACCEL_COMPENSATION -1
 #define GYRO_COMPENSATION 1

diff --git a/src/sensor/rotation_vector/fusion_base.h b/src/sensor/rotation_vector/fusion_base.h
index 1479023..3af7c91 100644 (file)
--- a/src/sensor/rotation_vector/fusion_base.h
+++ b/src/sensor/rotation_vector/fusion_base.h
@@ -20,7 +20,7 @@
 #define __FUSION_BASE_H__
 
 #include <fusion.h>
-#include <orientation_filter.h>
+#include "fusion_utils/orientation_filter.h"
 
 class fusion_base : public virtual fusion {
 public:

diff --git a/src/sensor/rotation_vector/rotation_vector_sensor.cpp b/src/sensor/rotation_vector/rv_sensor.cpp
similarity index 58%
rename from src/sensor/rotation_vector/rotation_vector_sensor.cpp
rename to src/sensor/rotation_vector/rv_sensor.cpp
index 93c03db..0e9db8e 100644 (file)
--- a/src/sensor/rotation_vector/rotation_vector_sensor.cpp
+++ b/src/sensor/rotation_vector/rv_sensor.cpp
@@ -31,40 +31,38 @@
 
 #include <sensor_common.h>
 #include <virtual_sensor.h>
-#include <rotation_vector_sensor.h>
+#include <rv_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()
+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_state(0)
+, m_accuracy(SENSOR_ACCURACY_UNDEFINED)
 {
 }
 
-rotation_vector_sensor::~rotation_vector_sensor()
+rv_sensor::~rv_sensor()
 {
        _I("%s is destroyed!", SENSOR_NAME);
 }
 
-bool rotation_vector_sensor::init(void)
+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_mag_sensor) {
+       if (!m_accel_sensor || !m_gyro_sensor|| !m_mag_sensor) {
                _E("cannot load sensors[%s]", SENSOR_NAME);
                return false;
        }
@@ -73,22 +71,22 @@ bool rotation_vector_sensor::init(void)
        return true;
 }
 
-sensor_type_t rotation_vector_sensor::get_type(void)
+sensor_type_t rv_sensor::get_type(void)
 {
        return ROTATION_VECTOR_SENSOR;
 }
 
-unsigned int rotation_vector_sensor::get_event_type(void)
+unsigned int rv_sensor::get_event_type(void)
 {
        return CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR);
 }
 
-const char* rotation_vector_sensor::get_name(void)
+const char* rv_sensor::get_name(void)
 {
        return SENSOR_NAME;
 }
 
-bool rotation_vector_sensor::get_sensor_info(sensor_info &info)
+bool rv_sensor::get_sensor_info(sensor_info &info)
 {
        info.set_type(get_type());
        info.set_id(get_id());
@@ -107,47 +105,34 @@ bool rotation_vector_sensor::get_sensor_info(sensor_info &info)
        return true;
 }
 
-void rotation_vector_sensor::synthesize(const sensor_event_t& event)
+void rv_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)
+               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 == 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];
+       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));
 
-               m_state |= STATE_ACCEL;
-       }
+       if (m_accuracy == SENSOR_ACCURACY_UNDEFINED)
+               m_accuracy = event.data->accuracy;
+       else if (m_accuracy > event.data->accuracy)
+               m_accuracy = event.data->accuracy;
 
-       if (m_state != (STATE_ACCEL | STATE_MAGNETIC))
+       unsigned long long timestamp;
+       if (!m_fusion.get_rv(timestamp, m_w, m_x, m_y, m_z))
                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);
+       if (timestamp == m_time)
+               return;
+       m_time = timestamp;
 
        rotation_vector_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
        if (!rotation_vector_event) {
@@ -165,25 +150,19 @@ void rotation_vector_sensor::synthesize(const sensor_event_t& event)
        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->timestamp = m_time;
        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];
+       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_time = timestamp;
-       m_x = quat[0];
-       m_y = quat[1];
-       m_z = quat[2];
-       m_w = quat[3];
-       m_accuracy = event.data->accuracy;
+       m_accuracy = SENSOR_ACCURACY_UNDEFINED;
 
        _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)
+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));
@@ -201,61 +180,51 @@ int rotation_vector_sensor::get_data(sensor_data_t **data, int *length)
        return 0;
 }
 
-bool rotation_vector_sensor::set_interval(unsigned long interval)
+bool rv_sensor::set_interval(unsigned long interval)
 {
        m_interval = interval;
        return true;
 }
 
-bool rotation_vector_sensor::set_batch_latency(unsigned long latency)
+bool rv_sensor::set_batch_latency(unsigned long latency)
 {
        return false;
 }
 
-bool rotation_vector_sensor::on_start(void)
+bool rv_sensor::on_start(void)
 {
-       if (m_accel_sensor)
-               m_accel_sensor->start();
-
-       if (m_mag_sensor)
-               m_mag_sensor->start();
-
+       m_accel_sensor->start();
+       m_gyro_sensor->start();
+       m_mag_sensor->start();
        m_time = 0;
+       m_accuracy = SENSOR_ACCURACY_UNDEFINED;
        return activate();
 }
 
-bool rotation_vector_sensor::on_stop(void)
+bool rv_sensor::on_stop(void)
 {
-       if (m_accel_sensor)
-               m_accel_sensor->stop();
-
-       if (m_mag_sensor)
-               m_mag_sensor->stop();
-
+       m_accel_sensor->stop();
+       m_gyro_sensor->stop();
+       m_mag_sensor->stop();
        m_time = 0;
-       m_state = 0;
-
+       m_accuracy = SENSOR_ACCURACY_UNDEFINED;
        return deactivate();
 }
 
-bool rotation_vector_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
+bool rv_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);
+       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 rotation_vector_sensor::delete_interval(int client_id, bool is_processor)
+bool rv_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);
+       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);
 }

diff --git a/src/sensor/rotation_vector/rotation_vector_sensor.h b/src/sensor/rotation_vector/rv_sensor.h
similarity index 90%
rename from src/sensor/rotation_vector/rotation_vector_sensor.h
rename to src/sensor/rotation_vector/rv_sensor.h
index fcd4a4b..2a984c7 100644 (file)
--- a/src/sensor/rotation_vector/rotation_vector_sensor.h
+++ b/src/sensor/rotation_vector/rv_sensor.h
@@ -22,11 +22,12 @@
 
 #include <virtual_sensor.h>
 #include <sensor_types.h>
+#include <gyro_magnetic_fusion.h>
 
-class rotation_vector_sensor : public virtual_sensor {
+class rv_sensor : public virtual_sensor {
 public:
-       rotation_vector_sensor();
-       virtual ~rotation_vector_sensor();
+       rv_sensor();
+       virtual ~rv_sensor();
 
        /* initialize sensor */
        bool init(void);
@@ -49,18 +50,16 @@ public:
 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;
-       int m_accuracy;
        unsigned long long m_time;
        unsigned long m_interval;
-
-       float m_acc[3];
-       float m_mag[3];
-       int m_state;
+       int m_accuracy;
 
        virtual bool set_interval(unsigned long interval);
        virtual bool set_batch_latency(unsigned long latency);

diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp
index 0d12915..5b69da0 100644 (file)
--- a/src/server/sensor_loader.cpp
+++ b/src/server/sensor_loader.cpp
@@ -46,7 +46,7 @@
 #include <orientation_sensor.h>
 #endif
 #ifdef ENABLE_ROTATION_VECTOR
-#include <rotation_vector_sensor.h>
+#include <rv_sensor.h>
 #endif
 
 using std::vector;
@@ -171,7 +171,7 @@ void sensor_loader::create_sensors(void)
        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");