src/accelprocessor.cpp

   1 /* Medfield sensor plugins
   2  * Copyright (C) 2013 Intel Corporation
   3  *
   4  * This library is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU Library General Public
   6  * License as published by the Free Software Foundation; version 2.1.
   7  *
   8  * This library is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * Lesser General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU Lesser General Public
  14  * License along with this library; if not, write to the Free Software
  15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301USA
  16  */
  17
  18 #include "log.h"
  19 #include "accelprocessor.h"
  20 #include <sensor_accel.h>
  21 #include <vconf.h>
  22 #include <math.h>
  23
  24 static const float XY_THRESHOLD = 2;
  25
  26 static const float RADIAN_MULTIPLIER = 180/M_PI;
  27
  28 static const int ROTATION_THD = 45;
  29
  30 static const int ROTATION_0 = 0;
  31 static const int ROTATION_90 = 90;
  32 static const int ROTATION_180 = 180;
  33 static const int ROTATION_360 = 360;
  34
  35 static const char *ROTATION_KEY = DEFAULT_SENSOR_KEY_PREFIX"10001";
  36
  37 AccelProcessor::AccelProcessor():
  38      mX(0),
  39      mY(0),
  40      mZ(0),
  41      mBeta(0),
  42      mGamma(0)
  43 {
  44      mInputEventCount = 4;
  45      find_input_device_by_name("accel");
  46      BaseProcessor::find_device_from_udev("accel");
  47      mSysfsPath.append("/lis3dh");
  48      mName = "accel";
  49
  50      mSupportedEvents.push_back(ACCELEROMETER_EVENT_ROTATION_CHECK);
  51      mSupportedEvents.push_back(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
  52      mSupportedEvents.push_back(ACCELEROMETER_EVENT_ORIENTATION_DATA_REPORT_ON_TIME);
  53 }
  54
  55 bool AccelProcessor::fill_values(unsigned int type, int &count,
  56                                  data_unit_idx_t &unit, data_accuracy &accuracy)
  57 {
  58      DbgPrint("");
  59      if (type == ACCELEROMETER_BASE_DATA_SET) {
  60           mValues[0] = -mX;
  61           mValues[1] = mY;
  62           mValues[2] = mZ;
  63           DbgPrint("Returning ACCELEROMETER_BASE_DATA_SET -mX, mY, mZ");
  64           count = 3;
  65           unit = IDX_UNIT_METRE_PER_SECOND_SQUARED;
  66           accuracy = ACCURACY_GOOD;
  67
  68           return true;
  69      }
  70
  71      if (type == ACCELEROMETER_ORIENTATION_DATA_SET) {
  72           mValues[0] = 0;
  73           mValues[1] = mBeta;
  74           mValues[2] = mGamma;
  75           count = 3;
  76           unit = IDX_UNIT_DEGREE;
  77           accuracy = ACCURACY_NORMAL;
  78           return true;
  79      }
  80
  81      return false;
  82 }
  83
  84 int AccelProcessor::get_property(unsigned int property_level, base_property_struct &result)
  85 {
  86      strcpy(result.sensor_name, "lis3dh");
  87      strcpy(result.sensor_vendor, "st");
  88
  89      if (property_level == ACCELEROMETER_BASE_DATA_SET) {
  90           result.sensor_unit_idx = IDX_UNIT_METRE_PER_SECOND_SQUARED;
  91           result.sensor_min_range = -20;
  92           result.sensor_max_range = 20;
  93           result.sensor_resolution = 0.0002;
  94      } else if (property_level == ACCELEROMETER_ORIENTATION_DATA_SET) {
  95           result.sensor_unit_idx = IDX_UNIT_DEGREE;
  96           result.sensor_min_range = ROTATION_0;
  97           result.sensor_max_range = ROTATION_360;
  98           result.sensor_resolution = 0.0002;
  99      } else {
 100           return -1;
 101      }
 102
 103      return 0;
 104 }
 105
 106 void AccelProcessor::calculateOrientation()
 107 {
 108      double absx = mX < 0 ? -mX : mX;
 109      double absy = mY < 0 ? -mY : mY;
 110
 111      double atan_value = 0;
 112      if (mZ == 0 && mX == 0)
 113           atan_value = 0;
 114      else
 115           atan_value = atan2(mZ,mX);
 116
 117      //Todo: Check if calculations could be avoided here
 118      int angle = ((int) (atan_value * (RADIAN_MULTIPLIER)));
 119
 120
 121      if (angle > 0 && angle <= 180) {
 122           mGamma = (angle-ROTATION_90);
 123      }
 124
 125      else if (angle <= 0 && angle >= -ROTATION_180) {
 126           mGamma = -(ROTATION_90+(angle));
 127      }
 128
 129      DbgPrint("Angle is %d and gamma %d\n",angle,mGamma);
 130
 131      double normZ = mZ / 9.8;
 132      if ( normZ > 1.0 ) {
 133           normZ = 1.0;
 134      } else if ( normZ < -1.0 ) {
 135           normZ = -1.0;
 136      }
 137
 138      DbgPrint("Normalized zeta is %f\n",normZ);
 139
 140      mBeta = (int)( acos(normZ) *(RADIAN_MULTIPLIER));
 141      if (mY > 0)
 142           mBeta = -mBeta;
 143
 144      if (absx < XY_THRESHOLD && absy < XY_THRESHOLD ) {
 145           DbgPrint("absx %f absy %f\n",absx,absy);
 146           return;
 147      }
 148
 149      angle = (int) (atan2(mY,mX)*RADIAN_MULTIPLIER);
 150      DbgPrint("Angle between y and x is is %d",angle);
 151      int section = (angle+ROTATION_180)/45;
 152      DbgPrint("\n\nSection is %d\n\n",section);
 153
 154      /**************************
 155       *  ->>>>> Read clockwise, number presents power button
 156       *        5
 157       *      6   4
 158       *    7       3
 159       *     0    2
 160       *        1
 161       *
 162       */
 163
 164      if(section == 6 || section == 5) {
 165           DbgPrint("Power button up");
 166           mRotation.rotation = ROTATION_EVENT_0;
 167           mRotation.rm[0] = ROTATION_PORTRAIT_TOP;
 168           mRotation.rm[1] = ROTATION_LANDSCAPE_LEFT;
 169      }
 170
 171      else if(section == 4 || section == 3) {
 172           DbgPrint("Power button right");
 173           mRotation.rotation = ROTATION_EVENT_270;
 174           mRotation.rm[0] = ROTATION_LANDSCAPE_RIGHT;
 175           mRotation.rm[1] = ROTATION_PORTRAIT_TOP;
 176      }
 177
 178      else if (section ==  2 || section == 1) {
 179           DbgPrint("Power button down");
 180           mRotation.rotation = ROTATION_EVENT_180;
 181           mRotation.rm[0] = ROTATION_PORTRAIT_BTM;
 182           mRotation.rm[1] = ROTATION_LANDSCAPE_LEFT;
 183      }
 184
 185      else {
 186           DbgPrint("Power button left");
 187           mRotation.rotation = ROTATION_EVENT_90;
 188           mRotation.rm[0] = ROTATION_LANDSCAPE_LEFT;
 189           mRotation.rm[1] = ROTATION_PORTRAIT_BTM;
 190      }
 191 }
 192
 193 void AccelProcessor::process_input_events(const std::vector <input_event *> &events)
 194 {
 195      DbgPrint();
 196
 197      mValues[0] = -1;
 198      mValues[1] = -1;
 199      mValues[2] = -1;
 200      BaseProcessor::process_input_events(events);
 201
 202      //Sensors input z and y value in negative from value
 203      //expected from sensor framework
 204      //and all values as integer (multiplied by 100)
 205      mX = mValues[0] /= 100.0;
 206      mY = mValues[1] /= -100.0;
 207      mZ = mValues[2] /= -100.0;
 208
 209      accelerometer_rotate_state oldRotation = mRotation.rotation;
 210      calculateOrientation();
 211      if (oldRotation != mRotation.rotation) {
 212           vconf_set_int(ROTATION_KEY, mRotation.rotation);
 213      }
 214 }
 215
 216