4 * Copyright (c) 2016 Samsung Electronics Co., Ltd.
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
10 * http://www.apache.org/licenses/LICENSE-2.0
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
26 #include <sys/types.h>
29 #include <sensor_log.h>
30 #include <sensor_types.h>
32 #include <sensor_common.h>
33 #include <virtual_sensor.h>
34 #include <rotation_vector_sensor.h>
35 #include <sensor_loader.h>
36 #include <fusion_util.h>
38 #define SENSOR_NAME "SENSOR_ROTATION_VECTOR"
40 #define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z))
42 #define STATE_ACCEL 0x1
43 #define STATE_MAGNETIC 0x2
45 rotation_vector_sensor::rotation_vector_sensor()
46 : m_accel_sensor(NULL)
57 rotation_vector_sensor::~rotation_vector_sensor()
59 _I("%s is destroyed!", SENSOR_NAME);
62 bool rotation_vector_sensor::init(void)
64 m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
65 m_mag_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
67 if (!m_accel_sensor || !m_mag_sensor) {
68 _E("cannot load sensors[%s]", SENSOR_NAME);
72 _I("%s is created!", SENSOR_NAME);
76 sensor_type_t rotation_vector_sensor::get_type(void)
78 return ROTATION_VECTOR_SENSOR;
81 unsigned int rotation_vector_sensor::get_event_type(void)
83 return CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR);
86 const char* rotation_vector_sensor::get_name(void)
91 bool rotation_vector_sensor::get_sensor_info(sensor_info &info)
93 info.set_type(get_type());
94 info.set_id(get_id());
95 info.set_privilege(SENSOR_PRIVILEGE_PUBLIC);
96 info.set_name(get_name());
97 info.set_vendor("Samsung Electronics");
98 info.set_min_range(0);
99 info.set_max_range(1);
100 info.set_resolution(1);
101 info.set_min_interval(1);
102 info.set_fifo_count(0);
103 info.set_max_batch_count(0);
104 info.set_supported_event(get_event_type());
105 info.set_wakeup_supported(false);
110 void rotation_vector_sensor::synthesize(const sensor_event_t& event)
112 sensor_event_t *rotation_vector_event;
118 if (event.event_type != GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME &&
119 event.event_type != ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME)
122 if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
123 m_mag[0] = event.data->values[0];
124 m_mag[1] = event.data->values[1];
125 m_mag[2] = event.data->values[2];
126 m_accuracy = event.data->accuracy;
128 m_state |= STATE_MAGNETIC;
131 if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
132 m_acc[0] = event.data->values[0];
133 m_acc[1] = event.data->values[1];
134 m_acc[2] = event.data->values[2];
136 m_state |= STATE_ACCEL;
139 if (m_state != (STATE_ACCEL | STATE_MAGNETIC))
144 unsigned long long timestamp = event.data->timestamp;
146 error = calculate_rotation_matrix(m_acc, m_mag, R, I);
149 error = matrix_to_quat(R, quat);
152 rotation_vector_event = (sensor_event_t *)malloc(sizeof(sensor_event_t));
153 if (!rotation_vector_event) {
154 _E("Failed to allocate memory");
157 rotation_vector_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
158 if (!rotation_vector_event->data) {
159 _E("Failed to allocate memory");
160 free(rotation_vector_event);
164 rotation_vector_event->sensor_id = get_id();
165 rotation_vector_event->event_type = CONVERT_TYPE_EVENT(ROTATION_VECTOR_SENSOR);
166 rotation_vector_event->data_length = sizeof(sensor_data_t);
167 rotation_vector_event->data->accuracy = m_accuracy;
168 rotation_vector_event->data->timestamp = timestamp;
169 rotation_vector_event->data->value_count = 4;
170 rotation_vector_event->data->values[0] = quat[0];
171 rotation_vector_event->data->values[1] = quat[1];
172 rotation_vector_event->data->values[2] = quat[2];
173 rotation_vector_event->data->values[3] = quat[3];
174 push(rotation_vector_event);
181 m_accuracy = event.data->accuracy;
183 _D("[rotation_vector] : [%10f] [%10f] [%10f] [%10f]", m_x, m_y, m_z, m_w);
186 int rotation_vector_sensor::get_data(sensor_data_t **data, int *length)
188 sensor_data_t *sensor_data;
189 sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
191 sensor_data->accuracy = m_accuracy;
192 sensor_data->timestamp = m_time;
193 sensor_data->value_count = 3;
194 sensor_data->values[0] = m_x;
195 sensor_data->values[1] = m_y;
196 sensor_data->values[2] = m_z;
199 *length = sizeof(sensor_data_t);
204 bool rotation_vector_sensor::set_interval(unsigned long interval)
206 m_interval = interval;
210 bool rotation_vector_sensor::set_batch_latency(unsigned long latency)
215 bool rotation_vector_sensor::on_start(void)
218 m_accel_sensor->start();
221 m_mag_sensor->start();
227 bool rotation_vector_sensor::on_stop(void)
230 m_accel_sensor->stop();
233 m_mag_sensor->stop();
241 bool rotation_vector_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
244 m_accel_sensor->add_interval(client_id, interval, true);
247 m_mag_sensor->add_interval(client_id, interval, true);
249 return sensor_base::add_interval(client_id, interval, is_processor);
252 bool rotation_vector_sensor::delete_interval(int client_id, bool is_processor)
255 m_accel_sensor->delete_interval(client_id, true);
258 m_mag_sensor->delete_interval(client_id, true);
260 return sensor_base::delete_interval(client_id, is_processor);