4 * Copyright (c) 2017 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.
20 #include "gravity_comp_sensor.h"
22 #include <sensor_log.h>
23 #include <sensor_types.h>
24 #include <fusion_util.h>
27 #define NAME_SENSOR "http://tizen.org/sensor/gravity/complementary"
28 #define NAME_VENDOR "Samsung"
30 #define SRC_ID_ACC 0x1
31 #define SRC_STR_ACC "http://tizen.org/sensor/accelerometer"
33 #define SRC_ID_GYRO 0x2
34 #define SRC_STR_GYRO "http://tizen.org/sensor/gyroscope"
36 #define GRAVITY 9.80665
38 #define PHASE_ACCEL_READY 0x01
39 #define PHASE_GYRO_READY 0x02
40 #define PHASE_FUSION_READY 0x03
41 #define US_PER_SEC 1000000
42 #define MS_PER_SEC 1000
43 #define INV_ANGLE -1000
48 #define DEG2RAD(x) ((x) * M_PI / 180.0)
49 #define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z))
50 #define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x), (y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0)
52 static sensor_info2_t sensor_info = {
62 wakeup_supported: false,
66 static required_sensor_s required_sensors[] = {
67 {SRC_ID_ACC, SRC_STR_ACC},
68 {SRC_ID_GYRO, SRC_STR_GYRO},
71 gravity_comp_sensor::gravity_comp_sensor()
83 gravity_comp_sensor::~gravity_comp_sensor()
87 int gravity_comp_sensor::get_sensor_info(const sensor_info2_t **info)
93 int gravity_comp_sensor::get_required_sensors(const required_sensor_s **sensors)
95 *sensors = required_sensors;
99 int gravity_comp_sensor::update(uint32_t id, sensor_data_t *data, int len)
101 if (id == SRC_ID_ACC) {
102 fusion_set_accel(data);
103 m_fusion_phase |= PHASE_ACCEL_READY;
104 } else if (id == SRC_ID_GYRO) {
105 fusion_set_gyro(data);
106 m_fusion_phase |= PHASE_GYRO_READY;
109 if (m_fusion_phase != PHASE_FUSION_READY)
114 fusion_update_angle();
115 fusion_get_gravity();
120 void gravity_comp_sensor::fusion_set_accel(sensor_data_t *data)
122 double x = data->values[0];
123 double y = data->values[1];
124 double z = data->values[2];
126 m_accel_mag = NORM(x, y, z);
128 m_angle_n[0] = ARCTAN(z, y);
129 m_angle_n[1] = ARCTAN(x, z);
130 m_angle_n[2] = ARCTAN(y, x);
132 m_accuracy = data->accuracy;
133 m_time_new = data->timestamp;
135 _D("AccIn: (%f, %f, %f)", x/m_accel_mag, y/m_accel_mag, z/m_accel_mag);
138 void gravity_comp_sensor::fusion_set_gyro(sensor_data_t *data)
140 m_velocity[0] = -DEG2RAD(data->values[0]);
141 m_velocity[1] = -DEG2RAD(data->values[1]);
142 m_velocity[2] = -DEG2RAD(data->values[2]);
144 m_time_new = data->timestamp;
147 void gravity_comp_sensor::fusion_update_angle(void)
149 _D("AngleIn: (%f, %f, %f)", m_angle_n[0], m_angle_n[1], m_angle_n[2]);
150 _D("AngAccl: (%f, %f, %f)", m_velocity[0], m_velocity[1], m_velocity[2]);
151 _D("Angle : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]);
153 if (m_angle[0] == INV_ANGLE) {
155 m_angle[0] = m_angle_n[0];
156 m_angle[1] = m_angle_n[1];
157 m_angle[2] = m_angle_n[2];
159 complementary(m_time_new - m_time);
162 _D("Angle' : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]);
165 void gravity_comp_sensor::fusion_get_gravity(void)
167 double x = 0, y = 0, z = 0;
171 /* Rotating along y-axis then z-axis */
172 vec[0][2] = cos(m_angle[1]);
173 vec[0][0] = sin(m_angle[1]);
174 vec[0][1] = vec[0][0] * tan(m_angle[2]);
176 /* Rotating along z-axis then x-axis */
177 vec[1][0] = cos(m_angle[2]);
178 vec[1][1] = sin(m_angle[2]);
179 vec[1][2] = vec[1][1] * tan(m_angle[0]);
181 /* Rotating along x-axis then y-axis */
182 vec[2][1] = cos(m_angle[0]);
183 vec[2][2] = sin(m_angle[0]);
184 vec[2][0] = vec[2][2] * tan(m_angle[1]);
187 for (int i = 0; i < 3; ++i) {
188 norm = NORM(vec[i][0], vec[i][1], vec[i][2]);
197 norm = NORM(x, y, z);
199 m_x = x / norm * GRAVITY;
200 m_y = y / norm * GRAVITY;
201 m_z = z / norm * GRAVITY;
205 void gravity_comp_sensor::complementary(unsigned long long time_diff)
207 double err = fabs(m_accel_mag - GRAVITY) / GRAVITY;
208 double tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID);
209 double delta_t = (double)time_diff/ US_PER_SEC;
210 double alpha = tau / (tau + delta_t);
212 _D("mag, err, tau, dt, alpha = %f, %f, %f, %f, %f", m_accel_mag, err, tau, delta_t, alpha);
214 m_angle[0] = complementary(m_angle[0], m_angle_n[0], m_velocity[0], delta_t, alpha);
215 m_angle[1] = complementary(m_angle[1], m_angle_n[1], m_velocity[1], delta_t, alpha);
216 m_angle[2] = complementary(m_angle[2], m_angle_n[2], m_velocity[2], delta_t, alpha);
219 double gravity_comp_sensor::complementary(double angle, double angle_in, double vel, double delta_t, double alpha)
222 angle = angle + vel * delta_t;
223 s = alpha * sin(angle) + (1 - alpha) * sin(angle_in);
224 c = alpha * cos(angle) + (1 - alpha) * cos(angle_in);
228 int gravity_comp_sensor::get_data(sensor_data_t **data, int *len)
230 sensor_data_t *sensor_data;
231 sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t));
233 sensor_data->accuracy = m_accuracy;
234 sensor_data->timestamp = m_time;
235 sensor_data->value_count = 3;
236 sensor_data->values[0] = m_x;
237 sensor_data->values[1] = m_y;
238 sensor_data->values[2] = m_z;
241 *len = sizeof(sensor_data_t);