src/orientation/orientation_sensor.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2014 Samsung Electronics Co., Ltd.
   5  *
   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
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  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.
  17  *
  18  */
  19
  20 #include <stdio.h>
  21 #include <stdlib.h>
  22 #include <unistd.h>
  23 #include <errno.h>
  24 #include <math.h>
  25 #include <time.h>
  26 #include <sys/types.h>
  27 #include <dlfcn.h>
  28 #include <common.h>
  29 #include <sf_common.h>
  30 #include <orientation_sensor.h>
  31 #include <sensor_plugin_loader.h>
  32 #include <orientation_filter.h>
  33
  34 #define SENSOR_NAME "ORIENTATION_SENSOR"
  35
  36 #define ACCELEROMETER_ENABLED 0x01
  37 #define GYROSCOPE_ENABLED 0x02
  38 #define GEOMAGNETIC_ENABLED 0x04
  39 #define ORIENTATION_ENABLED 7
  40
  41 #define INITIAL_VALUE -1
  42 #define INITIAL_TIME 0
  43
  44 // Below Defines const variables to be input from sensor config files once code is stabilized
  45 #define SAMPLING_TIME 100
  46 #define MS_TO_US 1000
  47
  48 float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)};
  49 float bias_gyro[] = {-5.3539, 0.24325, 2.3391};
  50 float bias_magnetic[] = {0, -37.6, +37.6};
  51 int sign_accel[] = {+1, +1, +1};
  52 int sign_gyro[] = {+1, +1, +1};
  53 int sign_magnetic[] = {+1, -1, +1};
  54 float scale_accel = 1;
  55 float scale_gyro = 580 * 2;
  56 float scale_magnetic = 1;
  57
  58 int pitch_phase_compensation = 1;
  59 int roll_phase_compensation = 1;
  60 int yaw_phase_compensation = -1;
  61 int magnetic_alignment_factor = 1;
  62
  63 void pre_process_data(sensor_data<float> &data_out, sensor_data_t &data_in, float *bias, int *sign, float scale)
  64 {
  65         data_out.m_data.m_vec[0] = sign[0] * (data_in.values[0] - bias[0]) / scale;
  66         data_out.m_data.m_vec[1] = sign[1] * (data_in.values[1] - bias[1]) / scale;
  67         data_out.m_data.m_vec[2] = sign[2] * (data_in.values[2] - bias[2]) / scale;
  68
  69         data_out.m_time_stamp = data_in.timestamp;
  70 }
  71
  72 orientation_sensor::orientation_sensor()
  73 : m_accel_sensor(NULL)
  74 , m_gyro_sensor(NULL)
  75 , m_magnetic_sensor(NULL)
  76 , m_roll(INITIAL_VALUE)
  77 , m_pitch(INITIAL_VALUE)
  78 , m_yaw(INITIAL_VALUE)
  79 {
  80         m_name = string(SENSOR_NAME);
  81         register_supported_event(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME);
  82         m_interval = SAMPLING_TIME * MS_TO_US;
  83         m_timestamp = get_timestamp();
  84         m_enable_orientation = 0;
  85 }
  86
  87 orientation_sensor::~orientation_sensor()
  88 {
  89         INFO("orientation_sensor is destroyed!\n");
  90 }
  91
  92 bool orientation_sensor::init(void)
  93 {
  94         m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
  95         m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
  96         m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
  97
  98         if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) {
  99                 ERR("Failed to load sensors,  accel: 0x%x, gyro: 0x%x, mag: 0x%x",
 100                         m_accel_sensor, m_gyro_sensor, m_magnetic_sensor);
 101                 return false;
 102         }
 103
 104         INFO("%s is created!", sensor_base::get_name());
 105         return true;
 106 }
 107
 108
 109 sensor_type_t orientation_sensor::get_type(void)
 110 {
 111         return ORIENTATION_SENSOR;
 112 }
 113
 114 bool orientation_sensor::on_start(void)
 115 {
 116         AUTOLOCK(m_mutex);
 117
 118         m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
 119         m_accel_sensor->start();
 120         m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
 121         m_gyro_sensor->start();
 122         m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
 123         m_magnetic_sensor->start();
 124
 125         activate();
 126         return true;
 127 }
 128
 129 bool orientation_sensor::on_stop(void)
 130 {
 131         m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
 132         m_accel_sensor->stop();
 133         m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME);
 134         m_gyro_sensor->stop();
 135         m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME);
 136         m_magnetic_sensor->stop();
 137
 138         deactivate();
 139         return true;
 140 }
 141
 142 bool orientation_sensor::add_interval(int client_id, unsigned int interval)
 143 {
 144         AUTOLOCK(m_mutex);
 145
 146         m_accel_sensor->add_interval(client_id, interval, true);
 147         m_gyro_sensor->add_interval(client_id, interval, true);
 148         m_magnetic_sensor->add_interval(client_id, interval, true);
 149
 150         return sensor_base::add_interval(client_id, interval, true);
 151 }
 152
 153 bool orientation_sensor::delete_interval(int client_id)
 154 {
 155         AUTOLOCK(m_mutex);
 156
 157         m_accel_sensor->delete_interval(client_id, true);
 158         m_gyro_sensor->delete_interval(client_id, true);
 159         m_magnetic_sensor->delete_interval(client_id, true);
 160
 161         return sensor_base::delete_interval(client_id, true);
 162 }
 163
 164 void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
 165 {
 166         const float MIN_DELIVERY_DIFF_FACTOR = 0.75f;
 167         unsigned long long diff_time;
 168
 169         sensor_data<float> accel;
 170         sensor_data<float> gyro;
 171         sensor_data<float> magnetic;
 172
 173         sensor_data_t accel_data;
 174         sensor_data_t gyro_data;
 175         sensor_data_t mag_data;
 176
 177         sensor_event_t orientation_event;
 178         euler_angles<float> euler_orientation;
 179
 180         if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
 181                 diff_time = event.data.timestamp - m_timestamp;
 182
 183                 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 184                         return;
 185
 186                 pre_process_data(accel, accel_data, bias_accel, sign_accel, scale_accel);
 187
 188                 m_enable_orientation |= ACCELEROMETER_ENABLED;
 189         }
 190         else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) {
 191                 diff_time = event.data.timestamp - m_timestamp;
 192
 193                 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 194                         return;
 195
 196                 pre_process_data(gyro, gyro_data, bias_gyro, sign_gyro, scale_gyro);
 197
 198                 m_enable_orientation |= GYROSCOPE_ENABLED;
 199         }
 200         else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
 201                 diff_time = event.data.timestamp - m_timestamp;
 202
 203                 if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
 204                         return;
 205
 206                 pre_process_data(magnetic, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
 207
 208                 m_enable_orientation |= GEOMAGNETIC_ENABLED;
 209         }
 210
 211         if (m_enable_orientation == ORIENTATION_ENABLED) {
 212                 m_enable_orientation = 0;
 213                 m_timestamp = get_timestamp();
 214
 215                 m_orientation.m_pitch_phase_compensation = pitch_phase_compensation;
 216                 m_orientation.m_roll_phase_compensation = roll_phase_compensation;
 217                 m_orientation.m_yaw_phase_compensation = yaw_phase_compensation;
 218                 m_orientation.m_magnetic_alignment_factor = magnetic_alignment_factor;
 219
 220                 euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic);
 221
 222                 orientation_event.data.data_accuracy = SENSOR_ACCURACY_GOOD;
 223                 orientation_event.data.data_unit_idx = SENSOR_UNIT_DEGREE;
 224                 orientation_event.event_type = ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME;
 225                 orientation_event.data.timestamp = m_timestamp;
 226                 orientation_event.data.values_num = 3;
 227                 orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[0];
 228                 orientation_event.data.values[1] = euler_orientation.m_ang.m_vec[1];
 229                 orientation_event.data.values[2] = euler_orientation.m_ang.m_vec[2];
 230
 231                 outs.push_back(orientation_event);
 232         }
 233
 234         return;
 235 }
 236
 237 int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
 238 {
 239         sensor_data<float> accel;
 240         sensor_data<float> gyro;
 241         sensor_data<float> magnetic;
 242
 243         sensor_data_t accel_data;
 244         sensor_data_t gyro_data;
 245         sensor_data_t magnetic_data;
 246
 247         euler_angles<float> euler_orientation;
 248
 249         if (event_type != ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME)
 250                 return -1;
 251
 252         m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data);
 253         m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data);
 254         m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data);
 255
 256         pre_process_data(accel, accel_data, bias_accel, sign_accel, scale_accel);
 257         pre_process_data(gyro, gyro_data, bias_gyro, sign_gyro, scale_gyro);
 258         pre_process_data(magnetic, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
 259
 260         m_orientation.m_pitch_phase_compensation = pitch_phase_compensation;
 261         m_orientation.m_roll_phase_compensation = roll_phase_compensation;
 262         m_orientation.m_yaw_phase_compensation = yaw_phase_compensation;
 263         m_orientation.m_magnetic_alignment_factor = magnetic_alignment_factor;
 264
 265         euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic);
 266
 267         data.data_accuracy = SENSOR_ACCURACY_GOOD;
 268         data.data_unit_idx = SENSOR_UNIT_DEGREE;
 269         data.timestamp = get_timestamp();
 270         data.values[0] = euler_orientation.m_ang.m_vec[0];
 271         data.values[1] = euler_orientation.m_ang.m_vec[1];
 272         data.values[2] = euler_orientation.m_ang.m_vec[2];
 273         data.values_num = 3;
 274
 275         return 0;
 276 }
 277
 278 bool orientation_sensor::get_properties(sensor_properties_t &properties)
 279 {
 280         properties.sensor_unit_idx = SENSOR_UNIT_DEGREE;
 281         properties.sensor_min_range = -180;
 282         properties.sensor_max_range = 180;
 283         properties.sensor_resolution = 1;
 284
 285         strncpy(properties.sensor_vendor, "Samsung", MAX_KEY_LENGTH);
 286         strncpy(properties.sensor_name, SENSOR_NAME, MAX_KEY_LENGTH);
 287
 288         return true;
 289 }
 290
 291 extern "C" void *create(void)
 292 {
 293         orientation_sensor *inst;
 294
 295         try {
 296                 inst = new orientation_sensor();
 297         } catch (int err) {
 298                 ERR("orientation_sensor class create fail , errno : %d , errstr : %s", err, strerror(err));
 299                 return NULL;
 300         }
 301
 302         return (void *)inst;
 303 }
 304
 305 extern "C" void destroy(void *inst)
 306 {
 307         delete (orientation_sensor *)inst;
 308 }