From 707e40b6ca3d5e8a2c3eb8fdcf7dc308e7330f10 Mon Sep 17 00:00:00 2001 From: akhilkedia94 Date: Thu, 18 Aug 2016 14:48:33 +0900 Subject: [PATCH] Rotation Vector Sensor Created a rv_sensor using Sensor Fusion. Change-Id: Ie7b3f2585621d724f0f3bd2e956e1bab4d3bfdfc Signed-off-by: akhilkedia94 --- src/sensor/CMakeLists.txt | 8 +- src/sensor/rotation_vector/fusion_base.cpp | 1 - src/sensor/rotation_vector/fusion_base.h | 2 +- .../{rotation_vector_sensor.cpp => rv_sensor.cpp} | 145 ++++++++------------- .../{rotation_vector_sensor.h => rv_sensor.h} | 15 +-- src/server/sensor_loader.cpp | 4 +- 6 files changed, 69 insertions(+), 106 deletions(-) rename src/sensor/rotation_vector/{rotation_vector_sensor.cpp => rv_sensor.cpp} (58%) rename src/sensor/rotation_vector/{rotation_vector_sensor.h => rv_sensor.h} (90%) diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt index 0547bc8..3cbd1a7 100644 --- 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 --- a/src/sensor/rotation_vector/fusion_base.cpp +++ b/src/sensor/rotation_vector/fusion_base.cpp @@ -26,7 +26,6 @@ #include #include #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 --- 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 -#include +#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 --- a/src/sensor/rotation_vector/rotation_vector_sensor.cpp +++ b/src/sensor/rotation_vector/rv_sensor.cpp @@ -31,40 +31,38 @@ #include #include -#include +#include #include #include #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 --- a/src/sensor/rotation_vector/rotation_vector_sensor.h +++ b/src/sensor/rotation_vector/rv_sensor.h @@ -22,11 +22,12 @@ #include #include +#include -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 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -46,7 +46,7 @@ #include #endif #ifdef ENABLE_ROTATION_VECTOR -#include +#include #endif using std::vector; @@ -171,7 +171,7 @@ void sensor_loader::create_sensors(void) create_virtual_sensors("Auto Rotation"); #endif #ifdef ENABLE_ROTATION_VECTOR - create_virtual_sensors("Rotation Vector"); + create_virtual_sensors("Rotation Vector"); #endif #ifdef ENABLE_GRAVITY create_virtual_sensors("Gravity"); -- 2.7.4