PROJECT(sensors CXX)
INCLUDE(GNUInstallDirs)
-SET(HRM "OFF")
+SET(HRM "ON")
SET(HRM_VIRT "OFF")
SET(AUTO_ROTATION "ON")
-SET(GRAVITY "OFF")
-SET(LINEAR_ACCEL "OFF")
+SET(GRAVITY "ON")
+SET(LINEAR_ACCEL "ON")
SET(ORIENTATION "OFF")
SET(FUSION "OFF")
SET(MOTION "OFF")
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2014 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 <fusion_util.h>
+#include <math.h>
+#include <stdlib.h>
+
+int quat_to_matrix(const float *quat, float *R)
+{
+ if(quat == NULL || R == NULL)
+ return -1;
+
+ float q0 = quat[3];
+ float q1 = quat[0];
+ float q2 = quat[1];
+ float q3 = quat[2];
+
+ float sq_q1 = 2 * q1 * q1;
+ float sq_q2 = 2 * q2 * q2;
+ float sq_q3 = 2 * q3 * q3;
+ float q1_q2 = 2 * q1 * q2;
+ float q3_q0 = 2 * q3 * q0;
+ float q1_q3 = 2 * q1 * q3;
+ float q2_q0 = 2 * q2 * q0;
+ float q2_q3 = 2 * q2 * q3;
+ float q1_q0 = 2 * q1 * q0;
+
+ R[0] = 1 - sq_q2 - sq_q3;
+ R[1] = q1_q2 - q3_q0;
+ R[2] = q1_q3 + q2_q0;
+ R[3] = q1_q2 + q3_q0;
+ R[4] = 1 - sq_q1 - sq_q3;
+ R[5] = q2_q3 - q1_q0;
+ R[6] = q1_q3 - q2_q0;
+ R[7] = q2_q3 + q1_q0;
+ R[8] = 1 - sq_q1 - sq_q2;
+
+ return 0;
+}
+
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2014 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 _FUSION_UTIL_H_
+#define _FUSION_UTIL_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+int quat_to_matrix(const float *quat, float *R);
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* _FUSION_UTIL_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 <gravity_sensor.h>
#include <sensor_loader.h>
-#include <virtual_sensor_config.h>
+#include <fusion_util.h>
-using std::string;
-using std::vector;
+#define SENSOR_NAME "GRAVITY_SENSOR"
-#define INITIAL_VALUE -1
#define GRAVITY 9.80665
-#define DEVIATION 0.1
-
-#define PI 3.141593
-#define AZIMUTH_OFFSET_DEGREES 360
-#define AZIMUTH_OFFSET_RADIANS (2 * PI)
+#define PHASE_ACCEL_READY 0x01
+#define PHASE_GYRO_READY 0x02
+#define PHASE_FUSION_READY 0x03
+#define US_PER_SEC 1000000
+#define MS_PER_SEC 1000
+#define INV_ANGLE -1000
+#define TAU_LOW 0.4
+#define TAU_MID 0.75
+#define TAU_HIGH 0.99
-#define SENSOR_NAME "GRAVITY_SENSOR"
-#define SENSOR_TYPE_GRAVITY "GRAVITY"
-#define SENSOR_TYPE_ORIENTATION "ORIENTATION"
-
-#define MS_TO_US 1000
-#define MIN_DELIVERY_DIFF_FACTOR 0.75f
-
-#define ELEMENT_NAME "NAME"
-#define ELEMENT_VENDOR "VENDOR"
-#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT"
-#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME"
-#define ELEMENT_GRAVITY_SIGN_COMPENSATION "GRAVITY_SIGN_COMPENSATION"
-#define ELEMENT_ORIENTATION_DATA_UNIT "RAW_DATA_UNIT"
-#define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION"
-#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION"
-#define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION"
+#define DEG2RAD(x) ((x) * M_PI / 180.0)
+#define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z))
+#define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x),(y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0)
gravity_sensor::gravity_sensor()
-: m_accel_sensor(NULL)
+: m_fusion(NULL)
+, m_accel_sensor(NULL)
, m_gyro_sensor(NULL)
-, m_magnetic_sensor(NULL)
-, m_fusion_sensor(NULL)
+, m_fusion_phase(0)
+, m_x(-1)
+, m_y(-1)
+, m_z(-1)
+, m_accuracy(-1)
, m_time(0)
{
- virtual_sensor_config &config = virtual_sensor_config::get_instance();
+}
- sensor_hal *fusion_sensor_hal = sensor_loader::get_instance().get_sensor_hal(SENSOR_HAL_TYPE_FUSION);
- if (!fusion_sensor_hal)
- m_hardware_fusion = false;
- else
- m_hardware_fusion = true;
+gravity_sensor::~gravity_sensor()
+{
+ _I("gravity_sensor is destroyed!\n");
+}
- m_name = std::string(SENSOR_NAME);
- register_supported_event(GRAVITY_RAW_DATA_EVENT);
+bool gravity_sensor::init()
+{
+ /* Acc (+ Gyro) fusion */
+ m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
- if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_VENDOR, m_vendor)) {
- _E("[VENDOR] is empty\n");
- throw ENXIO;
+ if (!m_accel_sensor) {
+ _E("cannot load accelerometer sensor_hal[%s]", get_name());
+ return false;
}
- _I("m_vendor = %s", m_vendor.c_str());
+ m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
+
+ _I("%s (%s) is created!\n", get_name(), m_gyro_sensor ? "Acc+Gyro Fusion" : "LowPass Acc");
+ return true;
+}
- if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ORIENTATION_DATA_UNIT, m_orientation_data_unit)) {
- _E("[ORIENTATION_DATA_UNIT] is empty\n");
- throw ENXIO;
- }
+sensor_type_t gravity_sensor::get_type(void)
+{
+ return GRAVITY_SENSOR;
+}
- _I("m_orientation_data_unit = %s", m_orientation_data_unit.c_str());
+unsigned int gravity_sensor::get_event_type(void)
+{
+ return GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
+}
- if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
- _E("[RAW_DATA_UNIT] is empty\n");
- throw ENXIO;
- }
+const char* gravity_sensor::get_name(void)
+{
+ return SENSOR_NAME;
+}
- _I("m_raw_data_unit = %s", m_raw_data_unit.c_str());
+bool gravity_sensor::get_sensor_info(sensor_info &info)
+{
+ info.set_type(get_type());
+ info.set_id(get_id());
+ info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); // FIXME
+ info.set_name("Gravity Sensor");
+ info.set_vendor("Samsung Electronics");
+ info.set_min_range(-19.6);
+ info.set_max_range(19.6);
+ info.set_resolution(0.01);
+ 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);
- if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
- _E("[DEFAULT_SAMPLING_TIME] is empty\n");
- throw ENXIO;
- }
+ return true;
+}
- _I("m_default_sampling_time = %d", m_default_sampling_time);
+void gravity_sensor::synthesize(const sensor_event_t& event)
+{
+ /* If the rotation vector sensor is available */
+ if (m_fusion) {
+ synthesize_rv(event);
+ return;
+ }
- if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) {
- _E("[GRAVITY_SIGN_COMPENSATION] is empty\n");
- throw ENXIO;
+ /* If both Acc & Gyro are available */
+ if (m_gyro_sensor) {
+ synthesize_fusion(event);
+ return;
}
- _I("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]);
+ /* If only Acc is available */
+ synthesize_lowpass(event);
+}
+
+void gravity_sensor::synthesize_rv(const sensor_event_t& event)
+{
+ if (!m_fusion->is_data_ready())
+ return;
+
+ sensor_event_t *gravity_event;
+ float gravity[3];
+ float x, y, z, w, heading_accuracy;
+ int accuracy;
- if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) {
- _E("[AZIMUTH_ROTATION_COMPENSATION] is empty\n");
- throw ENXIO;
+ if (!m_fusion->get_rotation_vector(x, y, z, w, heading_accuracy, accuracy)) {
+ _E("Failed to get rotation vector");
+ return;
}
- _I("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation);
+ unsigned long long timestamp = m_fusion->get_data_timestamp();
+
+ if (!check_sampling_time(timestamp))
+ return;
+
+ float rotation[4] = {x, y, z, w};
- if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) {
- _E("[PITCH_ROTATION_COMPENSATION] is empty\n");
- throw ENXIO;
+ rotation_to_gravity(rotation, gravity);
+
+ gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
+ if (!gravity_event) {
+ _E("Failed to allocate memory");
+ return;
+ }
+ gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
+ if (!gravity_event->data) {
+ _E("Failed to allocate memory");
+ return;
}
- _I("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation);
+ gravity_event->sensor_id = get_id();
+ gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
+ gravity_event->data_length = sizeof(sensor_data_t);
+ gravity_event->data->accuracy = accuracy;
+ gravity_event->data->timestamp = m_fusion->get_data_timestamp();
+ gravity_event->data->value_count = 3;
+ gravity_event->data->values[0] = gravity[0];
+ gravity_event->data->values[1] = gravity[1];
+ gravity_event->data->values[2] = gravity[2];
+ push(gravity_event);
+
+ m_time = event.data->timestamp;
+ m_x = gravity[0];
+ m_y = gravity[1];
+ m_z = gravity[2];
+ m_accuracy = accuracy;
+}
- if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) {
- _E("[ROLL_ROTATION_COMPENSATION] is empty\n");
- throw ENXIO;
+void gravity_sensor::synthesize_lowpass(const sensor_event_t& event)
+{
+ sensor_event_t *gravity_event;
+ float x, y, z, norm, alpha, tau, err;
+
+ norm = NORM(event.data->values[0], event.data->values[1], event.data->values[2]);
+ x = event.data->values[0] / norm * GRAVITY;
+ y = event.data->values[1] / norm * GRAVITY;
+ z = event.data->values[2] / norm * GRAVITY;
+
+ if (m_time > 0) {
+ err = fabs(norm - GRAVITY) / GRAVITY;
+ tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID);
+ alpha = tau / (tau + (float)(event.data->timestamp - m_time) / US_PER_SEC);
+ x = alpha * m_x + (1 - alpha) * x;
+ y = alpha * m_y + (1 - alpha) * y;
+ z = alpha * m_z + (1 - alpha) * z;
+ norm = NORM(x, y, z);
+ x = x / norm * GRAVITY;
+ y = y / norm * GRAVITY;
+ z = z / norm * GRAVITY;
}
- _I("m_roll_rotation_compensation = %d", m_roll_rotation_compensation);
+ gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
+ if (!gravity_event) {
+ _E("Failed to allocate memory");
+ return;
+ }
+ gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
+ if (!gravity_event->data) {
+ _E("Failed to allocate memory");
+ return;
+ }
- m_interval = m_default_sampling_time * MS_TO_US;
+ gravity_event->sensor_id = get_id();
+ gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
+ gravity_event->data_length = sizeof(sensor_data_t);
+ gravity_event->data->accuracy = event.data->accuracy;
+ gravity_event->data->timestamp = event.data->timestamp;
+ gravity_event->data->value_count = 3;
+ gravity_event->data->values[0] = x;
+ gravity_event->data->values[1] = y;
+ gravity_event->data->values[2] = z;
+ push(gravity_event);
+
+ m_time = event.data->timestamp;
+ m_x = x;
+ m_y = y;
+ m_z = z;
+ m_accuracy = event.data->accuracy;
}
-gravity_sensor::~gravity_sensor()
+void gravity_sensor::synthesize_fusion(const sensor_event_t& event)
{
- _I("gravity_sensor is destroyed!\n");
+ sensor_event_t *gravity_event;
+
+ if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ fusion_set_accel(event);
+ m_fusion_phase |= PHASE_ACCEL_READY;
+ } else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ fusion_set_gyro(event);
+ m_fusion_phase |= PHASE_GYRO_READY;
+ }
+
+ if (m_fusion_phase != PHASE_FUSION_READY)
+ return;
+
+ m_fusion_phase = 0;
+
+ fusion_update_angle();
+ fusion_get_gravity();
+
+ gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
+ if (!gravity_event) {
+ _E("Failed to allocate memory");
+ return;
+ }
+ gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
+ if (!gravity_event->data) {
+ _E("Failed to allocate memory");
+ return;
+ }
+
+ gravity_event->sensor_id = get_id();
+ gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
+ gravity_event->data_length = sizeof(sensor_data_t);
+ gravity_event->data->accuracy = m_accuracy;
+ gravity_event->data->timestamp = m_time;
+ gravity_event->data->value_count = 3;
+ gravity_event->data->values[0] = m_x;
+ gravity_event->data->values[1] = m_y;
+ gravity_event->data->values[2] = m_z;
+ push(gravity_event);
}
-bool gravity_sensor::init()
+void gravity_sensor::fusion_set_accel(const sensor_event_t& event)
{
- m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
- m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
- m_magnetic_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
+ double x = event.data->values[0];
+ double y = event.data->values[1];
+ double z = event.data->values[2];
- m_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR);
+ m_accel_mag = NORM(x, y, z);
- if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) {
- _E("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x",
- m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor);
- return false;
- }
+ m_angle_n[0] = ARCTAN(z, y);
+ m_angle_n[1] = ARCTAN(x, z);
+ m_angle_n[2] = ARCTAN(y, x);
- _I("%s is created!", sensor_base::get_name());
- return true;
+ m_time_new = event.data->timestamp;
+
+ _D("AccIn: (%f, %f, %f)", x/m_accel_mag, y/m_accel_mag, z/m_accel_mag);
}
-void gravity_sensor::get_types(vector<sensor_type_t> &types)
+void gravity_sensor::fusion_set_gyro(const sensor_event_t& event)
{
- types.push_back(GRAVITY_SENSOR);
+ m_velocity[0] = -DEG2RAD(event.data->values[0]);
+ m_velocity[1] = -DEG2RAD(event.data->values[1]);
+ m_velocity[2] = -DEG2RAD(event.data->values[2]);
+
+ m_time_new = event.data->timestamp;
}
-bool gravity_sensor::on_start(void)
+void gravity_sensor::fusion_update_angle()
{
- AUTOLOCK(m_mutex);
+ _D("AngleIn: (%f, %f, %f)", m_angle_n[0], m_angle_n[1], m_angle_n[2]);
+ _D("AngAccl: (%f, %f, %f)", m_velocity[0], m_velocity[1], m_velocity[2]);
+ _D("Angle : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]);
+
+ if (m_angle[0] == INV_ANGLE) {
+ /* 1st iteration */
+ m_angle[0] = m_angle_n[0];
+ m_angle[1] = m_angle_n[1];
+ m_angle[2] = m_angle_n[2];
+ } else {
+ complementary(m_time_new - m_time);
+ }
- if (!m_hardware_fusion) {
- m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT);
- m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
- m_accel_sensor->start();
- m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT);
- m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
- m_gyro_sensor->start();
- m_magnetic_sensor->add_client(GEOMAGNETIC_RAW_DATA_EVENT);
- m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
- m_magnetic_sensor->start();
+ _D("Angle' : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]);
+}
+
+void gravity_sensor::fusion_get_gravity()
+{
+ double x = 0, y = 0, z = 0;
+ double norm;
+ double vec[3][3];
+
+ /* Rotating along y-axis then z-axis */
+ vec[0][2] = cos(m_angle[1]);
+ vec[0][0] = sin(m_angle[1]);
+ vec[0][1] = vec[0][0] * tan(m_angle[2]);
+
+ /* Rotating along z-axis then x-axis */
+ vec[1][0] = cos(m_angle[2]);
+ vec[1][1] = sin(m_angle[2]);
+ vec[1][2] = vec[1][1] * tan(m_angle[0]);
+
+ /* Rotating along x-axis then y-axis */
+ vec[2][1] = cos(m_angle[0]);
+ vec[2][2] = sin(m_angle[0]);
+ vec[2][0] = vec[2][2] * tan(m_angle[1]);
+
+ /* Normalize */
+ for (int i = 0; i < 3; ++i) {
+ norm = NORM(vec[i][0], vec[i][1], vec[i][2]);
+ vec[i][0] /= norm;
+ vec[i][1] /= norm;
+ vec[i][2] /= norm;
+ x += vec[i][0];
+ y += vec[i][1];
+ z += vec[i][2];
}
- m_fusion_sensor->register_supported_event(FUSION_EVENT);
- m_fusion_sensor->register_supported_event(FUSION_ORIENTATION_ENABLED);
- m_fusion_sensor->add_client(FUSION_EVENT);
- m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
- m_fusion_sensor->start();
+ norm = NORM(x, y, z);
- activate();
- return true;
+ m_x = x / norm * GRAVITY;
+ m_y = y / norm * GRAVITY;
+ m_z = z / norm * GRAVITY;
+ m_time = m_time_new;
}
-bool gravity_sensor::on_stop(void)
+void gravity_sensor::complementary(unsigned long long time_diff)
{
- AUTOLOCK(m_mutex);
+ double err = fabs(m_accel_mag - GRAVITY) / GRAVITY;
+ double tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID);
+ double delta_t = (double)time_diff/ US_PER_SEC;
+ double alpha = tau / (tau + delta_t);
- if (!m_hardware_fusion) {
- m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
- m_accel_sensor->delete_interval((intptr_t)this, false);
- m_accel_sensor->stop();
- m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT);
- m_gyro_sensor->delete_interval((intptr_t)this, false);
- m_gyro_sensor->stop();
- m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT);
- m_magnetic_sensor->delete_interval((intptr_t)this, false);
- m_magnetic_sensor->stop();
- }
+ _D("mag, err, tau, dt, alpha = %f, %f, %f, %f, %f", m_accel_mag, err, tau, delta_t, alpha);
- m_fusion_sensor->delete_client(FUSION_EVENT);
- m_fusion_sensor->delete_interval((intptr_t)this, false);
- m_fusion_sensor->unregister_supported_event(FUSION_EVENT);
- m_fusion_sensor->unregister_supported_event(FUSION_ORIENTATION_ENABLED);
- m_fusion_sensor->stop();
+ m_angle[0] = complementary(m_angle[0], m_angle_n[0], m_velocity[0], delta_t, alpha);
+ m_angle[1] = complementary(m_angle[1], m_angle_n[1], m_velocity[1], delta_t, alpha);
+ m_angle[2] = complementary(m_angle[2], m_angle_n[2], m_velocity[2], delta_t, alpha);
+}
- deactivate();
- return true;
+double gravity_sensor::complementary(double angle, double angle_in, double vel, double delta_t, double alpha)
+{
+ double s, c;
+ angle = angle + vel * delta_t;
+ s = alpha * sin(angle) + (1 - alpha) * sin(angle_in);
+ c = alpha * cos(angle) + (1 - alpha) * cos(angle_in);
+ return ARCTAN(s, c);
}
-bool gravity_sensor::add_interval(int client_id, unsigned int interval)
+int gravity_sensor::get_data(sensor_data_t **data, int *length)
{
- AUTOLOCK(m_mutex);
- if (!m_hardware_fusion) {
- m_accel_sensor->add_interval(client_id, interval, false);
- m_gyro_sensor->add_interval(client_id, interval, false);
- m_magnetic_sensor->add_interval(client_id, interval, false);
- }
+ /* if It is batch sensor, remains can be 2+ */
+ int remains = 1;
- m_fusion_sensor->add_interval(client_id, interval, false);
+ sensor_data_t *sensor_data;
+ sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
- return sensor_base::add_interval(client_id, interval, false);
+ 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 --remains;
}
-bool gravity_sensor::delete_interval(int client_id)
+bool gravity_sensor::set_batch_latency(unsigned long latency)
{
- AUTOLOCK(m_mutex);
- if (!m_hardware_fusion) {
- m_accel_sensor->delete_interval(client_id, false);
- m_gyro_sensor->delete_interval(client_id, false);
- m_magnetic_sensor->delete_interval(client_id, false);
- }
+ return false;
+}
- m_fusion_sensor->delete_interval(client_id, false);
+bool gravity_sensor::set_wakeup(int wakeup)
+{
+ return false;
+}
+
+bool gravity_sensor::on_start(void)
+{
+ if (m_fusion)
+ m_fusion->start();
+
+ if (m_accel_sensor)
+ m_accel_sensor->start();
- return sensor_base::delete_interval(client_id, false);
+ if (m_gyro_sensor)
+ m_gyro_sensor->start();
+
+ m_time = 0;
+ m_fusion_phase = 0;
+ m_angle[0] = INV_ANGLE;
+
+ return activate();
}
-void gravity_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
+bool gravity_sensor::on_stop(void)
{
- sensor_event_t gravity_event;
- float pitch, roll, azimuth;
- unsigned long long diff_time;
- float azimuth_offset;
-
- if (event.event_type == FUSION_EVENT) {
- diff_time = event.data.timestamp - m_time;
-
- if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
- return;
-
- quaternion<float> quat(event.data.values[0], event.data.values[1],
- event.data.values[2], event.data.values[3]);
-
- euler_angles<float> euler = quat2euler(quat);
-
- if(m_orientation_data_unit == "DEGREES") {
- euler = rad2deg(euler);
- azimuth_offset = AZIMUTH_OFFSET_DEGREES;
- }
- else {
- azimuth_offset = AZIMUTH_OFFSET_RADIANS;
- }
-
- euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation;
- euler.m_ang.m_vec[1] *= m_roll_rotation_compensation;
- euler.m_ang.m_vec[2] *= m_azimuth_rotation_compensation;
-
- pitch = euler.m_ang.m_vec[0];
- roll = euler.m_ang.m_vec[1];
- if (euler.m_ang.m_vec[2] >= 0)
- azimuth = euler.m_ang.m_vec[2];
- else
- azimuth = euler.m_ang.m_vec[2] + azimuth_offset;
-
- if(m_orientation_data_unit == "DEGREES") {
- azimuth *= DEG2RAD;
- pitch *= DEG2RAD;
- roll *= DEG2RAD;
- }
-
- m_time = get_timestamp();
- gravity_event.sensor_id = get_id();
- gravity_event.event_type = GRAVITY_RAW_DATA_EVENT;
-
- if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) ||
- (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) {
- gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth);
- gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth);
- gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll);
- } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) ||
- (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) {
- gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth);
- gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth);
- gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch);
- } else {
- gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll);
- gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch);
- gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch);
- }
- gravity_event.data.value_count = 3;
- gravity_event.data.timestamp = m_time;
- gravity_event.data.accuracy = SENSOR_ACCURACY_GOOD;
-
- push(gravity_event);
- }
+ if (m_fusion)
+ m_fusion->stop();
+
+ if (m_accel_sensor)
+ m_accel_sensor->stop();
- return;
+ if (m_gyro_sensor)
+ m_gyro_sensor->stop();
+
+ m_time = 0;
+
+ return deactivate();
}
-int gravity_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
+bool gravity_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
{
- sensor_data_t fusion_data;
- float azimuth_offset;
- float pitch, roll, azimuth;
+ if (m_fusion)
+ m_fusion->set_interval(FUSION_EVENT_AGM, client_id, interval);
- if (event_type != GRAVITY_RAW_DATA_EVENT)
- return -1;
+ if (m_accel_sensor)
+ m_accel_sensor->add_interval(client_id, interval, true);
- m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data);
+ if (m_gyro_sensor)
+ m_gyro_sensor->add_interval(client_id, interval, true);
- quaternion<float> quat(fusion_data.values[0], fusion_data.values[1],
- fusion_data.values[2], fusion_data.values[3]);
+ return sensor_base::add_interval(client_id, interval, is_processor);
+}
- euler_angles<float> euler = quat2euler(quat);
+bool gravity_sensor::delete_interval(int client_id, bool is_processor)
+{
+ if (m_fusion)
+ m_fusion->unset_interval(FUSION_EVENT_AGM, client_id);
- if(m_orientation_data_unit == "DEGREES") {
- euler = rad2deg(euler);
- azimuth_offset = AZIMUTH_OFFSET_DEGREES;
- }
- else {
- azimuth_offset = AZIMUTH_OFFSET_RADIANS;
- }
+ if (m_accel_sensor)
+ m_accel_sensor->delete_interval(client_id, true);
- pitch = euler.m_ang.m_vec[0];
- roll = euler.m_ang.m_vec[1];
+ if (m_gyro_sensor)
+ m_gyro_sensor->delete_interval(client_id, true);
- if (euler.m_ang.m_vec[2] >= 0)
- azimuth = euler.m_ang.m_vec[2];
- else
- azimuth = euler.m_ang.m_vec[2] + azimuth_offset;
+ return sensor_base::delete_interval(client_id, is_processor);
+}
- if(m_orientation_data_unit == "DEGREES") {
- azimuth *= DEG2RAD;
- pitch *= DEG2RAD;
- roll *= DEG2RAD;
- }
+bool gravity_sensor::set_interval(unsigned long interval)
+{
+ m_interval = interval;
+ return true;
+}
- data.accuracy = SENSOR_ACCURACY_GOOD;
- data.timestamp = get_timestamp();
- if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) ||
- (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) {
- data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth);
- data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth);
- data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll);
- } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) ||
- (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) {
- data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth);
- data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth);
- data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch);
- } else {
- data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll);
- data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch);
- data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch);
- }
- data.value_count = 3;
+bool gravity_sensor::rotation_to_gravity(const float *rotation, float *gravity)
+{
+ float R[9];
- return 0;
+ if (quat_to_matrix(rotation, R) < 0)
+ return false;
+
+ gravity[0] = R[6] * GRAVITY;
+ gravity[1] = R[7] * GRAVITY;
+ gravity[2] = R[8] * GRAVITY;
+
+ return true;
}
-bool gravity_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties)
+bool gravity_sensor::check_sampling_time(unsigned long long timestamp)
{
- properties.min_range = -GRAVITY;
- properties.max_range = GRAVITY;
- properties.resolution = 0.000001;
- properties.vendor = m_vendor;
- properties.name = SENSOR_NAME;
- properties.fifo_count = 0;
- properties.max_batch_count = 0;
- properties.min_interval = 1;
+ const float MIN_DELIVERY_DIFF_FACTOR = 0.75f;
+ const int MS_TO_US = 1000;
+ long long diff_time;
+
+ diff_time = timestamp - m_time;
+
+ if (m_time && (diff_time < m_interval * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR))
+ return false;
return true;
}
#ifndef _GRAVITY_SENSOR_H_
#define _GRAVITY_SENSOR_H_
-#include <sensor_internal.h>
#include <virtual_sensor.h>
-#include <orientation_filter.h>
+#include <sensor_types.h>
+#include <sensor_fusion.h>
class gravity_sensor : public virtual_sensor {
public:
gravity_sensor();
virtual ~gravity_sensor();
- bool init();
- virtual void get_types(std::vector<sensor_type_t> &types);
+ /* initialize sensor */
+ bool init(void);
- void synthesize(const sensor_event_t& event, std::vector<sensor_event_t> &outs);
+ /* sensor info */
+ virtual sensor_type_t get_type(void);
+ virtual unsigned int get_event_type(void);
+ virtual const char* get_name(void);
- bool add_interval(int client_id, unsigned int interval);
- bool delete_interval(int client_id);
+ virtual bool get_sensor_info(sensor_info &info);
- int get_sensor_data(const unsigned int event_type, sensor_data_t &data);
- virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties);
+ /* 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_fusion *m_fusion;
sensor_base *m_accel_sensor;
sensor_base *m_gyro_sensor;
- sensor_base *m_magnetic_sensor;
- sensor_base *m_fusion_sensor;
-
- sensor_data<float> m_accel;
- sensor_data<float> m_gyro;
- sensor_data<float> m_magnetic;
-
- cmutex m_value_mutex;
+ int m_fusion_phase;
+ float m_x;
+ float m_y;
+ float m_z;
int m_accuracy;
unsigned long long m_time;
- unsigned int m_interval;
-
- std::string m_vendor;
- std::string m_raw_data_unit;
- std::string m_orientation_data_unit;
- int m_default_sampling_time;
- int m_gravity_sign_compensation[3];
- int m_azimuth_rotation_compensation;
- int m_pitch_rotation_compensation;
- int m_roll_rotation_compensation;
-
- bool on_start(void);
- bool on_stop(void);
+ unsigned long m_interval;
+
+ double m_angle[3];
+ double m_angle_n[3];
+ double m_accel_mag;
+ double m_velocity[3];
+ unsigned long long m_time_new;
+
+ virtual bool set_interval(unsigned long interval);
+ virtual bool set_batch_latency(unsigned long latency);
+ virtual bool set_wakeup(int wakeup);
+
+ virtual bool on_start(void);
+ virtual bool on_stop(void);
+
+ bool rotation_to_gravity(const float *rotation, float *gravity);
+ bool check_sampling_time(unsigned long long timestamp);
+
+ void synthesize_rv(const sensor_event_t& event);
+ void synthesize_lowpass(const sensor_event_t& event);
+ void synthesize_fusion(const sensor_event_t& event);
+
+ void fusion_set_accel(const sensor_event_t& event);
+ void fusion_set_gyro(const sensor_event_t& event);
+ void fusion_update_angle();
+ void fusion_get_gravity();
+ double complementary(double angle, double angle_in, double vel, double delta_t, double alpha);
+ void complementary(unsigned long long time_diff);
};
#endif /* _GRAVITY_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 <sensor_common.h>
+#include <sensor_log.h>
+#include "hrm_sensor.h"
+
+hrm_sensor::hrm_sensor()
+{
+ set_permission(SENSOR_PERMISSION_BIO);
+
+ _I("hrm_sensor is created : 0x%x", this);
+}
+
+hrm_sensor::~hrm_sensor()
+{
+
+}
--- /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 _HRM_SENSOR_H_
+#define _HRM_SENSOR_H_
+
+#include <physical_sensor.h>
+
+class hrm_sensor : public physical_sensor {
+public:
+ hrm_sensor();
+ virtual ~hrm_sensor();
+};
+
+#endif /* _HRM_SENSOR_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 <linear_accel_sensor.h>
#include <sensor_loader.h>
-#include <virtual_sensor_config.h>
-
-using std::string;
-using std::vector;
+#include <fusion_util.h>
#define SENSOR_NAME "LINEAR_ACCEL_SENSOR"
-#define SENSOR_TYPE_LINEAR_ACCEL "LINEAR_ACCEL"
-#define SENSOR_TYPE_GRAVITY "GRAVITY"
-#define SENSOR_TYPE_ORIENTATION "ORIENTATION"
-
-#define ELEMENT_NAME "NAME"
-#define ELEMENT_VENDOR "VENDOR"
-#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT"
-#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME"
-#define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS"
-#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION"
-#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE"
-#define ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION "LINEAR_ACCEL_SIGN_COMPENSATION"
-#define ELEMENT_ORIENTATION_DATA_UNIT "RAW_DATA_UNIT"
-#define ELEMENT_GRAVITY_SIGN_COMPENSATION "GRAVITY_SIGN_COMPENSATION"
-#define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION"
-#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION"
-#define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION"
-
-#define INITIAL_VALUE -1
-#define GRAVITY 9.80665
-#define DEVIATION 0.1
-
-#define PI 3.141593
-#define AZIMUTH_OFFSET_DEGREES 360
-#define AZIMUTH_OFFSET_RADIANS (2 * PI)
-
-#define MS_TO_US 1000
-#define MIN_DELIVERY_DIFF_FACTOR 0.75f
-#define ACCELEROMETER_ENABLED 0x01
-#define GRAVITY_ENABLED 0x02
-#define LINEAR_ACCEL_ENABLED 3
+#define GRAVITY 9.80665
linear_accel_sensor::linear_accel_sensor()
: m_accel_sensor(NULL)
-, m_gyro_sensor(NULL)
-, m_magnetic_sensor(NULL)
-, m_fusion_sensor(NULL)
+, m_gravity_sensor(NULL)
+, m_x(0)
+, m_y(0)
+, m_z(0)
+, m_gx(0)
+, m_gy(0)
+, m_gz(0)
+, m_accuracy(0)
, m_time(0)
{
- virtual_sensor_config &config = virtual_sensor_config::get_instance();
-
- m_name = string(SENSOR_NAME);
- m_enable_linear_accel = 0;
- register_supported_event(LINEAR_ACCEL_RAW_DATA_EVENT);
-
- sensor_hal *fusion_sensor_hal = sensor_loader::get_instance().get_sensor_hal(SENSOR_HAL_TYPE_FUSION);
- if (!fusion_sensor_hal)
- m_hardware_fusion = false;
- else
- m_hardware_fusion = true;
-
-
- if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_VENDOR, m_vendor)) {
- _E("[VENDOR] is empty\n");
- throw ENXIO;
- }
-
- _I("m_vendor = %s", m_vendor.c_str());
-
- if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) {
- _E("[RAW_DATA_UNIT] is empty\n");
- throw ENXIO;
- }
-
- _I("m_raw_data_unit = %s", m_raw_data_unit.c_str());
-
- if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ORIENTATION_DATA_UNIT, m_orientation_data_unit)) {
- _E("[ORIENTATION_DATA_UNIT] is empty\n");
- throw ENXIO;
- }
-
- _I("m_orientation_data_unit = %s", m_orientation_data_unit.c_str());
-
- if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) {
- _E("[DEFAULT_SAMPLING_TIME] is empty\n");
- throw ENXIO;
- }
-
- _I("m_default_sampling_time = %d", m_default_sampling_time);
-
- if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) {
- _E("[ACCEL_STATIC_BIAS] is empty\n");
- throw ENXIO;
- }
-
- if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) {
- _E("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n");
- throw ENXIO;
- }
-
- _I("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]);
-
- if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) {
- _E("[GRAVITY_SIGN_COMPENSATION] is empty\n");
- throw ENXIO;
- }
-
- _I("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]);
-
- if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION, m_linear_accel_sign_compensation, 3)) {
- _E("[LINEAR_ACCEL_SIGN_COMPENSATION] is empty\n");
- throw ENXIO;
- }
-
- _I("m_linear_accel_sign_compensation = (%d, %d, %d)", m_linear_accel_sign_compensation[0], m_linear_accel_sign_compensation[1], m_linear_accel_sign_compensation[2]);
-
- if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) {
- _E("[AZIMUTH_ROTATION_COMPENSATION] is empty\n");
- throw ENXIO;
- }
-
- _I("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation);
-
- if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) {
- _E("[PITCH_ROTATION_COMPENSATION] is empty\n");
- throw ENXIO;
- }
-
- _I("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation);
-
- if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) {
- _E("[ROLL_ROTATION_COMPENSATION] is empty\n");
- throw ENXIO;
- }
-
- _I("m_roll_rotation_compensation = %d", m_roll_rotation_compensation);
-
- m_interval = m_default_sampling_time * MS_TO_US;
-
}
linear_accel_sensor::~linear_accel_sensor()
bool linear_accel_sensor::init()
{
m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
- m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
- m_magnetic_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
- m_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR);
+ if (!m_accel_sensor) {
+ _E("cannot load accelerometer sensor_hal[%s]", get_name());
+ return false;
+ }
+
+ m_gravity_sensor = sensor_loader::get_instance().get_sensor(GRAVITY_SENSOR);
- if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) {
- _E("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x",
- m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor);
+ if (!m_gravity_sensor) {
+ _E("cannot load gravity sensor_hal[%s]", get_name());
return false;
}
- _I("%s is created!", sensor_base::get_name());
+ _I("%s is created!\n", get_name());
+
return true;
}
-void linear_accel_sensor::get_types(vector<sensor_type_t> &types)
+sensor_type_t linear_accel_sensor::get_type(void)
{
- types.push_back(LINEAR_ACCEL_SENSOR);
+ return LINEAR_ACCEL_SENSOR;
}
-bool linear_accel_sensor::on_start(void)
+unsigned int linear_accel_sensor::get_event_type(void)
{
- AUTOLOCK(m_mutex);
-
- m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT);
- m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
- m_accel_sensor->start();
-
- if (!m_hardware_fusion) {
- m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT);
- m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
- m_gyro_sensor->start();
- m_magnetic_sensor->add_client(GEOMAGNETIC_RAW_DATA_EVENT);
- m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
- m_magnetic_sensor->start();
- }
-
- m_fusion_sensor->register_supported_event(FUSION_EVENT);
- m_fusion_sensor->register_supported_event(FUSION_ORIENTATION_ENABLED);
- m_fusion_sensor->add_client(FUSION_EVENT);
- m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false);
- m_fusion_sensor->start();
-
- activate();
- return true;
+ return LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME;
}
-bool linear_accel_sensor::on_stop(void)
+const char* linear_accel_sensor::get_name(void)
{
- AUTOLOCK(m_mutex);
- m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT);
- m_accel_sensor->delete_interval((intptr_t)this, false);
- m_accel_sensor->stop();
-
- if (!m_hardware_fusion) {
- m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT);
- m_gyro_sensor->delete_interval((intptr_t)this, false);
- m_gyro_sensor->stop();
- m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT);
- m_magnetic_sensor->delete_interval((intptr_t)this, false);
- m_magnetic_sensor->stop();
- }
+ return SENSOR_NAME;
+}
- m_fusion_sensor->delete_client(FUSION_EVENT);
- m_fusion_sensor->delete_interval((intptr_t)this, false);
- m_fusion_sensor->unregister_supported_event(FUSION_EVENT);
- m_fusion_sensor->unregister_supported_event(FUSION_ORIENTATION_ENABLED);
- m_fusion_sensor->stop();
+bool linear_accel_sensor::get_sensor_info(sensor_info &info)
+{
+ info.set_type(get_type());
+ info.set_id(get_id());
+ info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); // FIXME
+ info.set_name("Linear Accelerometer Sensor");
+ info.set_vendor("Samsung Electronics");
+ info.set_min_range(-19.6);
+ info.set_max_range(19.6);
+ info.set_resolution(0.01);
+ 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);
- deactivate();
return true;
}
-bool linear_accel_sensor::add_interval(int client_id, unsigned int interval)
+void linear_accel_sensor::synthesize(const sensor_event_t& event)
{
- AUTOLOCK(m_mutex);
- m_accel_sensor->add_interval(client_id, interval, false);
-
- if (!m_hardware_fusion) {
- m_gyro_sensor->add_interval(client_id, interval, false);
- m_magnetic_sensor->add_interval(client_id, interval, false);
- }
+ if (event.event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ m_gx = event.data->values[0];
+ m_gy = event.data->values[1];
+ m_gz = event.data->values[2];
+ return;
+ }
+
+ if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ m_time = event.data->timestamp;
+ m_x = event.data->values[0] - m_gx;
+ m_y = event.data->values[1] - m_gy;
+ m_z = event.data->values[2] - m_gz;
+
+ sensor_event_t *linear_accel_event;
+ sensor_data_t *linear_accel_data;
+ int data_length;
+ int remains;
+
+ linear_accel_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
+ if (!linear_accel_event) {
+ _E("Failed to allocate memory");
+ return;
+ }
- m_fusion_sensor->add_interval(client_id, interval, false);
+ remains = get_data(&linear_accel_data, &data_length);
- return sensor_base::add_interval(client_id, interval, false);
-}
+ if (remains < 0)
+ return;
-bool linear_accel_sensor::delete_interval(int client_id)
-{
- AUTOLOCK(m_mutex);
- m_accel_sensor->delete_interval(client_id, false);
+ linear_accel_event->sensor_id = get_id();
+ linear_accel_event->event_type = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME;
+ linear_accel_event->data_length = data_length;
+ linear_accel_event->data = linear_accel_data;
- if (!m_hardware_fusion) {
- m_gyro_sensor->delete_interval(client_id, false);
- m_magnetic_sensor->delete_interval(client_id, false);
+ push(linear_accel_event);
}
-
- m_fusion_sensor->delete_interval(client_id, false);
-
- return sensor_base::delete_interval(client_id, false);
}
-sensor_data_t linear_accel_sensor::calculate_gravity(sensor_data_t data)
+int linear_accel_sensor::get_data(sensor_data_t **data, int *length)
{
- sensor_data_t gravity_data;
- float pitch, roll, azimuth;
- float azimuth_offset;
+ /* if It is batch sensor, remains can be 2+ */
+ int remains = 1;
- quaternion<float> quat(data.values[0], data.values[1],
- data.values[2], data.values[3]);
+ sensor_data_t *sensor_data;
+ sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
- euler_angles<float> euler = quat2euler(quat);
-
- if(m_orientation_data_unit == "DEGREES") {
- euler = rad2deg(euler);
- azimuth_offset = AZIMUTH_OFFSET_DEGREES;
- }
- else {
- azimuth_offset = AZIMUTH_OFFSET_RADIANS;
- }
-
- euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation;
- euler.m_ang.m_vec[1] *= m_roll_rotation_compensation;
- euler.m_ang.m_vec[2] *= m_azimuth_rotation_compensation;
-
- pitch = euler.m_ang.m_vec[0];
- roll = euler.m_ang.m_vec[1];
- if (euler.m_ang.m_vec[2] >= 0)
- azimuth = euler.m_ang.m_vec[2];
- else
- azimuth = euler.m_ang.m_vec[2] + azimuth_offset;
-
- if(m_orientation_data_unit == "DEGREES") {
- azimuth *= DEG2RAD;
- pitch *= DEG2RAD;
- roll *= DEG2RAD;
- }
+ sensor_data->accuracy = SENSOR_ACCURACY_GOOD;
+ 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);
- if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) ||
- (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) {
- gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth);
- gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth);
- gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll);
- } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) ||
- (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) {
- gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth);
- gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth);
- gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch);
- } else {
- gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll);
- gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch);
- gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch);
- }
- gravity_data.value_count = 3;
- gravity_data.timestamp = m_time;
- gravity_data.accuracy = SENSOR_ACCURACY_GOOD;
-
- return gravity_data;
+ return --remains;
}
-void linear_accel_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
+bool linear_accel_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
{
- sensor_event_t lin_accel_event;
- sensor_data_t gravity_data;
+ if (m_accel_sensor)
+ m_accel_sensor->add_interval(client_id, interval, true);
- unsigned long long diff_time;
+ if (m_gravity_sensor)
+ m_gravity_sensor->add_interval(client_id, interval, true);
- if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) {
- diff_time = event.data.timestamp - m_time;
+ return sensor_base::add_interval(client_id, interval, is_processor);
+}
- if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
- return;
+bool linear_accel_sensor::delete_interval(int client_id, bool is_processor)
+{
+ if (m_accel_sensor)
+ m_accel_sensor->delete_interval(client_id, true);
- m_accel.m_data.m_vec[0] = m_accel_rotation_direction_compensation[0] * (event.data.values[0] - m_accel_static_bias[0]) / ACCEL_SCALE;
- m_accel.m_data.m_vec[1] = m_accel_rotation_direction_compensation[1] * (event.data.values[1] - m_accel_static_bias[1]) / ACCEL_SCALE;
- m_accel.m_data.m_vec[2] = m_accel_rotation_direction_compensation[2] * (event.data.values[2] - m_accel_static_bias[2]) / ACCEL_SCALE;
+ if (m_gravity_sensor)
+ m_gravity_sensor->delete_interval(client_id, true);
- m_accel.m_time_stamp = event.data.timestamp;
+ return sensor_base::delete_interval(client_id, is_processor);
+}
- m_enable_linear_accel |= ACCELEROMETER_ENABLED;
- }
- else if (event.event_type == FUSION_EVENT) {
- diff_time = event.data.timestamp - m_time;
+bool linear_accel_sensor::set_interval(unsigned long interval)
+{
+ m_interval = interval;
+ return true;
+}
- if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
- return;
+bool linear_accel_sensor::set_batch_latency(unsigned long latency)
+{
+ return false;
+}
- gravity_data = calculate_gravity(event.data);
+bool linear_accel_sensor::set_wakeup(int wakeup)
+{
+ return false;
+}
- m_enable_linear_accel |= GRAVITY_ENABLED;
- }
+bool linear_accel_sensor::on_start(void)
+{
+ if (m_accel_sensor)
+ m_accel_sensor->start();
- if (m_enable_linear_accel == LINEAR_ACCEL_ENABLED) {
- m_enable_linear_accel = 0;
-
- m_time = get_timestamp();
- lin_accel_event.sensor_id = get_id();
- lin_accel_event.event_type = LINEAR_ACCEL_RAW_DATA_EVENT;
- lin_accel_event.data.value_count = 3;
- lin_accel_event.data.timestamp = m_time;
- lin_accel_event.data.accuracy = SENSOR_ACCURACY_GOOD;
- lin_accel_event.data.values[0] = m_linear_accel_sign_compensation[0] * (m_accel.m_data.m_vec[0] - gravity_data.values[0]);
- lin_accel_event.data.values[1] = m_linear_accel_sign_compensation[1] * (m_accel.m_data.m_vec[1] - gravity_data.values[1]);
- lin_accel_event.data.values[2] = m_linear_accel_sign_compensation[2] * (m_accel.m_data.m_vec[2] - gravity_data.values[2]);
- push(lin_accel_event);
- }
+ if (m_gravity_sensor)
+ m_gravity_sensor->start();
- return;
+ m_time = 0;
+ return activate();
}
-int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
+bool linear_accel_sensor::on_stop(void)
{
- sensor_data_t gravity_data, accel_data, fusion_data;
- m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data);
- m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
-
- gravity_data = calculate_gravity(fusion_data);
-
- accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / ACCEL_SCALE;
- accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / ACCEL_SCALE;
- accel_data.values[2] = m_accel_rotation_direction_compensation[2] * (accel_data.values[2] - m_accel_static_bias[2]) / ACCEL_SCALE;
-
- if (event_type != LINEAR_ACCEL_RAW_DATA_EVENT)
- return -1;
-
- data.accuracy = SENSOR_ACCURACY_GOOD;
- data.timestamp = get_timestamp();
- data.values[0] = m_linear_accel_sign_compensation[0] * (accel_data.values[0] - gravity_data.values[0]);
- data.values[1] = m_linear_accel_sign_compensation[1] * (accel_data.values[1] - gravity_data.values[1]);
- data.values[2] = m_linear_accel_sign_compensation[2] * (accel_data.values[2] - gravity_data.values[2]);
- data.value_count = 3;
- return 0;
-}
+ if (m_accel_sensor)
+ m_accel_sensor->stop();
-bool linear_accel_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties)
-{
- m_accel_sensor->get_properties(ACCELEROMETER_SENSOR, properties);
- properties.name = "Linear Acceleration Sensor";
- properties.vendor = m_vendor;
- properties.resolution = 0.000001;
+ if (m_gravity_sensor)
+ m_gravity_sensor->stop();
- return true;
+ m_time = 0;
+ return deactivate();
}
-
#ifndef _LINEAR_ACCEL_SENSOR_H_
#define _LINEAR_ACCEL_SENSOR_H_
-#include <sensor_internal.h>
#include <virtual_sensor.h>
-#include <orientation_filter.h>
+#include <sensor_types.h>
class linear_accel_sensor : public virtual_sensor {
public:
linear_accel_sensor();
virtual ~linear_accel_sensor();
- bool init();
- virtual void get_types(std::vector<sensor_type_t> &types);
+ /* initialize sensor */
+ bool init(void);
- void synthesize(const sensor_event_t& event, std::vector<sensor_event_t> &outs);
+ /* sensor info */
+ virtual sensor_type_t get_type(void);
+ virtual unsigned int get_event_type(void);
+ virtual const char* get_name(void);
- bool add_interval(int client_id, unsigned int interval);
- bool delete_interval(int client_id);
+ virtual bool get_sensor_info(sensor_info &info);
- int get_sensor_data(const unsigned int event_type, sensor_data_t &data);
- virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties);
+ /* 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_gyro_sensor;
- sensor_base *m_magnetic_sensor;
- sensor_base *m_fusion_sensor;
-
- sensor_data<float> m_accel;
- sensor_data<float> m_gyro;
- sensor_data<float> m_magnetic;
-
- cmutex m_value_mutex;
+ sensor_base *m_gravity_sensor;
+ float m_x;
+ float m_y;
+ float m_z;
+ float m_gx;
+ float m_gy;
+ float m_gz;
+ int m_accuracy;
unsigned long long m_time;
- unsigned int m_interval;
-
- unsigned int m_enable_linear_accel;
+ unsigned long m_interval;
- std::string m_vendor;
- std::string m_raw_data_unit;
- std::string m_orientation_data_unit;
- int m_default_sampling_time;
- float m_accel_static_bias[3];
- int m_accel_rotation_direction_compensation[3];
- int m_linear_accel_sign_compensation[3];
- int m_gravity_sign_compensation[3];
- int m_azimuth_rotation_compensation;
- int m_pitch_rotation_compensation;
- int m_roll_rotation_compensation;
+ virtual bool set_interval(unsigned long interval);
+ virtual bool set_batch_latency(unsigned long latency);
+ virtual bool set_wakeup(int wakeup);
- bool on_start(void);
- bool on_stop(void);
- sensor_data_t calculate_gravity(sensor_data_t data);
+ virtual bool on_start(void);
+ virtual bool on_stop(void);
};
#endif
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2014 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 <sensor_fusion.h>
+
+sensor_fusion::sensor_fusion()
+{
+
+}
+
+sensor_fusion::~sensor_fusion()
+{
+
+}
+
+bool sensor_fusion::is_fusion(void)
+{
+ return true;
+}
+
+bool sensor_fusion::is_virtual(void)
+{
+ return false;
+}
+
+bool sensor_fusion::is_data_ready(void)
+{
+ return true;
+}
+
+void sensor_fusion::clear_data(void)
+{
+ return;
+}
+
+bool sensor_fusion::get_rotation_matrix(arr33_t &rot, int &accuracy)
+{
+ return false;
+}
+
+bool sensor_fusion::get_attitude(float &x, float &y, float &z, float &w)
+{
+ return false;
+}
+
+bool sensor_fusion::get_gyro_bias(float &x, float &y, float &z)
+{
+ return false;
+}
+
+bool sensor_fusion::get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy)
+{
+ return false;
+}
+
+bool sensor_fusion::get_linear_acceleration(float &x, float &y, float &z, int &accuracy)
+{
+ return false;
+}
+
+bool sensor_fusion::get_gravity(float &x, float &y, float &z, int &accuracy)
+{
+ return false;
+}
+
+bool sensor_fusion::get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy)
+{
+ return false;
+}
+
+bool sensor_fusion::get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w, int &accuracy)
+{
+ return false;
+}
+
+bool sensor_fusion::get_orientation(float &azimuth, float &pitch, float &roll, int &accuracy)
+{
+ return false;
+}
+
+bool sensor_fusion::set_interval(unsigned int event_type, int client_id, unsigned int interval)
+{
+ return sensor_base::add_interval(client_id, interval, true);
+}
+
+bool sensor_fusion::unset_interval(unsigned int event_type, int client_id)
+{
+ return sensor_base::delete_interval(client_id, true);
+}
+
--- /dev/null
+/*
+ * sensord
+ *
+ * Copyright (c) 2014 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 _SENSOR_FUSION_H_
+#define _SENSOR_FUSION_H_
+
+#include <array>
+#include <sensor_base.h>
+
+enum fusion_event_type {
+ FUSION_EVENT_AG = (FUSION_SENSOR << 16) | 0x0001,
+ FUSION_EVENT_AM = (FUSION_SENSOR << 16) | 0x0002,
+ FUSION_EVENT_AGM = (FUSION_SENSOR << 16) | 0x0004,
+};
+
+typedef std::array<std::array<float,3> ,3> arr33_t;
+
+class sensor_fusion : public sensor_base {
+public:
+ sensor_fusion();
+ virtual ~sensor_fusion();
+
+ virtual void fuse(const sensor_event_t& event) = 0;
+ bool is_fusion(void);
+ bool is_virtual(void);
+ virtual bool is_data_ready(void);
+ virtual void clear_data(void);
+ virtual bool set_interval(unsigned int event_type, int client_id, unsigned int interval);
+ virtual bool unset_interval(unsigned int event_type, int client_id);
+ virtual unsigned long long get_data_timestamp(void) = 0;
+
+ virtual bool get_rotation_matrix(arr33_t &rot, int &accuracy);
+ virtual bool get_attitude(float &x, float &y, float &z, float &w);
+ virtual bool get_gyro_bias(float &x, float &y, float &z);
+ virtual bool get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy);
+ virtual bool get_linear_acceleration(float &x, float &y, float &z, int &accuracy);
+ virtual bool get_gravity(float &x, float &y, float &z, int &accuracy);
+ virtual bool get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy);
+ virtual bool get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w, int &accuracy);
+ virtual bool get_orientation(float &azimuth, float &pitch, float &roll, int &accuracy);
+};
+
+#endif
#include <unordered_set>
#include <algorithm>
-#include <accel_sensor.h>
+#include <hrm_sensor.h>
+
#ifdef ENABLE_AUTO_ROTATION
#include <auto_rotation_sensor.h>
#endif
+#ifdef ENABLE_GRAVITY
+#include <gravity_sensor.h>
+#endif
+#ifdef ENABLE_LINEAR_ACCEL
+#include <linear_accel_sensor.h>
+#endif
using std::vector;
using std::string;
void sensor_loader::create_sensors(void)
{
- create_physical_sensors<accel_sensor>(ACCELEROMETER_SENSOR);
+ /* HRM sensors need SENSOR_PERMISSION_BIO */
+ create_physical_sensors<hrm_sensor>(HRM_RAW_SENSOR);
+ create_physical_sensors<hrm_sensor>(HRM_SENSOR);
+ create_physical_sensors<hrm_sensor>(HRM_LED_GREEN_SENSOR);
+ create_physical_sensors<hrm_sensor>(HRM_LED_IR_SENSOR);
+ create_physical_sensors<hrm_sensor>(HRM_LED_RED_SENSOR);
+
create_physical_sensors<physical_sensor>(UNKNOWN_SENSOR);
+#ifdef ENABLE_AUTO_ROTATION
create_virtual_sensors<auto_rotation_sensor>("Auto Rotation");
+#endif
+#ifdef ENABLE_GRAVITY
+ create_virtual_sensors<gravity_sensor>("Gravity");
+#endif
+#ifdef ENABLE_LINEAR_ACCEL
+ create_virtual_sensors<linear_accel_sensor>("Linear Accel");
+#endif
}
template<typename _sensor>
projects / platform / core / system / sensord.git / commitdiff