From 5934d9ea1345ca495e2d9b9f42c7fc2cbf0a0eac Mon Sep 17 00:00:00 2001 From: akhilkedia94 Date: Tue, 16 Aug 2016 20:33:00 +0900 Subject: [PATCH] Magnetic RV Sensor Change-Id: I7a0586952f69404573d5ae974d6dc3c8cf1b7055 Signed-off-by: akhilkedia94 --- src/sensor/rotation_vector/magnetic_rv_sensor.cpp | 220 ++++++++++++++++++++++ src/sensor/rotation_vector/magnetic_rv_sensor.h | 70 +++++++ src/server/sensor_loader.cpp | 2 + 3 files changed, 292 insertions(+) create mode 100644 src/sensor/rotation_vector/magnetic_rv_sensor.cpp create mode 100644 src/sensor/rotation_vector/magnetic_rv_sensor.h diff --git a/src/sensor/rotation_vector/magnetic_rv_sensor.cpp b/src/sensor/rotation_vector/magnetic_rv_sensor.cpp new file mode 100644 index 0000000..6700230 --- /dev/null +++ b/src/sensor/rotation_vector/magnetic_rv_sensor.cpp @@ -0,0 +1,220 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#define SENSOR_NAME "SENSOR_MAGNETIC_ROTATION_VECTOR" + + +magnetic_rv_sensor::magnetic_rv_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_accuracy(SENSOR_ACCURACY_UNDEFINED) +{ +} + +magnetic_rv_sensor::~magnetic_rv_sensor() +{ + _I("%s is destroyed!", SENSOR_NAME); +} + +bool magnetic_rv_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 magnetic_rv_sensor::get_type(void) +{ + return GEOMAGNETIC_RV_SENSOR; +} + +unsigned int magnetic_rv_sensor::get_event_type(void) +{ + return CONVERT_TYPE_EVENT(GEOMAGNETIC_RV_SENSOR); +} + +const char* magnetic_rv_sensor::get_name(void) +{ + return SENSOR_NAME; +} + +bool magnetic_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 magnetic_rv_sensor::synthesize(const sensor_event_t& event) +{ + sensor_event_t *rotation_vector_event; + + if (event.event_type != ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME && + event.event_type != GEOMAGNETIC_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)); + + 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(GEOMAGNETIC_RV_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 magnetic_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 magnetic_rv_sensor::set_interval(unsigned long interval) +{ + m_interval = interval; + return true; +} + +bool magnetic_rv_sensor::set_batch_latency(unsigned long latency) +{ + return false; +} + +bool magnetic_rv_sensor::on_start(void) +{ + m_accel_sensor->start(); + m_mag_sensor->start(); + m_time = 0; + m_accuracy = SENSOR_ACCURACY_UNDEFINED; + return activate(); +} + +bool magnetic_rv_sensor::on_stop(void) +{ + m_accel_sensor->stop(); + m_mag_sensor->stop(); + m_time = 0; + m_accuracy = SENSOR_ACCURACY_UNDEFINED; + return deactivate(); +} + +bool magnetic_rv_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) +{ + m_accel_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 magnetic_rv_sensor::delete_interval(int client_id, bool is_processor) +{ + m_accel_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/magnetic_rv_sensor.h b/src/sensor/rotation_vector/magnetic_rv_sensor.h new file mode 100644 index 0000000..34b5d49 --- /dev/null +++ b/src/sensor/rotation_vector/magnetic_rv_sensor.h @@ -0,0 +1,70 @@ +/* + * 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 __MAGNETIC_RV_SENSOR_H__ +#define __MAGNETIC_RV_SENSOR_H__ + +#include +#include +#include + +class magnetic_rv_sensor : public virtual_sensor { +public: + magnetic_rv_sensor(); + virtual ~magnetic_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; + 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 /* __MAGNETIC_SENSOR_H__ */ diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index 5b69da0..5c44f95 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -47,6 +47,7 @@ #endif #ifdef ENABLE_ROTATION_VECTOR #include +#include #endif using std::vector; @@ -172,6 +173,7 @@ void sensor_loader::create_sensors(void) #endif #ifdef ENABLE_ROTATION_VECTOR create_virtual_sensors("Rotation Vector"); + create_virtual_sensors("Magnetic Rotation Vector"); #endif #ifdef ENABLE_GRAVITY create_virtual_sensors("Gravity"); -- 2.7.4