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(RV "OFF")
SET(ORIENTATION "OFF")
ENDIF()
SET(MOTION "OFF")
INCLUDE_DIRECTORIES(
SET(MOTION "OFF")
INCLUDE_DIRECTORIES(
SET(SENSOR_DEFINITIONS ${SENSOR_DEFINITIONS} "-DENABLE_ORIENTATION")
ENDIF()
IF("${RV}" 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")
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)
ENDIF()
IF("${MOTION}" STREQUAL "ON")
add_subdirectory(motion)
#include <sensor_base.h>
#include <cmath>
#include "fusion_base.h"
#include <sensor_base.h>
#include <cmath>
#include "fusion_base.h"
-#include "orientation_filter.h"
#define ACCEL_COMPENSATION -1
#define GYRO_COMPENSATION 1
#define ACCEL_COMPENSATION -1
#define GYRO_COMPENSATION 1
#define __FUSION_BASE_H__
#include <fusion.h>
#define __FUSION_BASE_H__
#include <fusion.h>
-#include <orientation_filter.h>
+#include "fusion_utils/orientation_filter.h"
class fusion_base : public virtual fusion {
public:
class fusion_base : public virtual fusion {
public:
#include <sensor_common.h>
#include <virtual_sensor.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"
#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_mag_sensor(NULL)
, m_x(-1)
, m_y(-1)
, m_z(-1)
, m_w(-1)
, m_time(0)
, m_mag_sensor(NULL)
, m_x(-1)
, m_y(-1)
, m_z(-1)
, m_w(-1)
, m_time(0)
+, m_accuracy(SENSOR_ACCURACY_UNDEFINED)
-rotation_vector_sensor::~rotation_vector_sensor()
{
_I("%s is destroyed!", SENSOR_NAME);
}
{
_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_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);
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;
}
_E("cannot load sensors[%s]", SENSOR_NAME);
return false;
}
-sensor_type_t rotation_vector_sensor::get_type(void)
+sensor_type_t rv_sensor::get_type(void)
{
return ROTATION_VECTOR_SENSOR;
}
{
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);
}
{
return CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR);
}
-const char* rotation_vector_sensor::get_name(void)
+const char* rv_sensor::get_name(void)
-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());
{
info.set_type(get_type());
info.set_id(get_id());
-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;
{
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 &&
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)
- 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))
- 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) {
rotation_vector_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
if (!rotation_vector_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->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->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);
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);
}
_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));
{
sensor_data_t *sensor_data;
sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
-bool rotation_vector_sensor::set_interval(unsigned long interval)
+bool rv_sensor::set_interval(unsigned long interval)
{
m_interval = interval;
return true;
}
{
m_interval = interval;
return true;
}
-bool rotation_vector_sensor::set_batch_latency(unsigned long latency)
+bool rv_sensor::set_batch_latency(unsigned long latency)
-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_accuracy = SENSOR_ACCURACY_UNDEFINED;
-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_accuracy = SENSOR_ACCURACY_UNDEFINED;
-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);
}
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);
}
return sensor_base::delete_interval(client_id, is_processor);
}
#include <virtual_sensor.h>
#include <sensor_types.h>
#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 {
- rotation_vector_sensor();
- virtual ~rotation_vector_sensor();
+ rv_sensor();
+ virtual ~rv_sensor();
/* initialize sensor */
bool init(void);
/* initialize sensor */
bool init(void);
private:
sensor_base *m_accel_sensor;
sensor_base *m_mag_sensor;
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;
float m_x;
float m_y;
float m_z;
float m_w;
unsigned long long m_time;
unsigned long m_interval;
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 set_interval(unsigned long interval);
virtual bool set_batch_latency(unsigned long latency);
#include <orientation_sensor.h>
#endif
#ifdef ENABLE_ROTATION_VECTOR
#include <orientation_sensor.h>
#endif
#ifdef ENABLE_ROTATION_VECTOR
-#include <rotation_vector_sensor.h>
#endif
using std::vector;
#endif
using std::vector;
create_virtual_sensors<auto_rotation_sensor>("Auto Rotation");
#endif
#ifdef ENABLE_ROTATION_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");
#endif
#ifdef ENABLE_GRAVITY
create_virtual_sensors<gravity_sensor>("Gravity");