From a1e10a246eb1bda21da90cdd9d3c675ff0f71d78 Mon Sep 17 00:00:00 2001 From: Amit Dharmapurikar Date: Tue, 12 Aug 2014 17:49:16 +0530 Subject: [PATCH] Modified geo_sensor_hal plugin for IIO driver compatibility - The geo_sensor_hal plugin is modified as per present geo sensor IIO driver interface. * the plugin reads data from raw data files instead of char-dev file in /dev/ directory, * time stamp for data samples is not available * enable and disable actions for geo sensor are not available. - The geo sensor is enabled in the sensord.spec file Change-Id: I8a02be99c37bae3e37f36fa4291810410fcc3976 Signed-off-by: Amit Dharmapurikar --- packaging/sensord.spec | 2 +- src/geo/geo_sensor_hal.cpp | 395 +++++++++------------------------------------ src/geo/geo_sensor_hal.h | 48 ++++-- 3 files changed, 120 insertions(+), 325 deletions(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index d6ac9c2..4b094de 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -12,7 +12,7 @@ Source2: sensord.socket %define gyro_state ON %define proxi_state OFF %define light_state OFF -%define geo_state OFF +%define geo_state ON %define gravity_state OFF %define linear_accel_state OFF %define motion_state OFF diff --git a/src/geo/geo_sensor_hal.cpp b/src/geo/geo_sensor_hal.cpp index b1cadcf..0d65ab7 100755 --- a/src/geo/geo_sensor_hal.cpp +++ b/src/geo/geo_sensor_hal.cpp @@ -27,346 +27,155 @@ #include using std::ifstream; +using std::string; using config::CConfig; -#define NODE_NAME "name" -#define NODE_INPUT "input" -#define NODE_ENABLE "enable" -#define NODE_POLL_DELAY "poll_delay" -#define NODE_MAG_POLL_DELAY "mag_poll_delay" -#define SENSOR_NODE "/sys/class/sensors/" -#define SENSORHUB_NODE "/sys/class/sensors/ssp_sensor/" -#define INPUT_DEVICE_NODE "/sys/class/input/" -#define DEV_INPUT_NODE "/dev/input/event/" - -#define LBS_TO_UTESLA 0.06f - #define SENSOR_TYPE_MAGNETIC "MAGNETIC" #define ELEMENT_NAME "NAME" #define ELEMENT_VENDOR "VENDOR" #define ATTR_VALUE "value" -#define INPUT_NAME "geomagnetic_sensor" +#define SENSOR_MIN_RANGE -1200 +#define SENSOR_MAX_RANGE 1200 +#define INITIAL_TIME 0 +#define INITIAL_VALUE -1 +#define GAUSS_TO_UTESLA(val) ((val) * 100) geo_sensor_hal::geo_sensor_hal() : m_x(INITIAL_VALUE) , m_y(INITIAL_VALUE) , m_z(INITIAL_VALUE) -, m_node_handle(INITIAL_VALUE) , m_polling_interval(POLL_1HZ_MS) , m_fired_time(INITIAL_TIME) , m_sensorhub_supported(false) { - if (!check_hw_node()) { + if (!check_hw_node()) + { ERR("check_hw_node() fail"); throw ENXIO; } CConfig &config = CConfig::get_instance(); - if (!config.get(SENSOR_TYPE_MAGNETIC, m_model_id, ELEMENT_VENDOR, m_vendor)) { + if (!config.get(SENSOR_TYPE_MAGNETIC, m_model_id, ELEMENT_VENDOR, m_vendor)) + { ERR("[VENDOR] is empty"); throw ENXIO; } INFO("m_vendor = %s", m_vendor.c_str()); - if (!config.get(SENSOR_TYPE_MAGNETIC, m_model_id, ELEMENT_NAME, m_chip_name)) { + if (!config.get(SENSOR_TYPE_MAGNETIC, m_model_id, ELEMENT_NAME, m_chip_name)) + { ERR("[NAME] is empty"); throw ENXIO; } INFO("m_chip_name = %s", m_chip_name.c_str()); - if ((m_node_handle = open(m_resource.c_str(), O_RDWR)) < 0) { - ERR("Failed to open handle(%d)", m_node_handle); + if (!init_resources()) throw ENXIO; - } - - int clockId = CLOCK_MONOTONIC; - - if (ioctl(m_node_handle, EVIOCSCLOCKID, &clockId) != 0) { - ERR("Fail to set monotonic timestamp for %s", m_resource.c_str()); - throw ENXIO; - } INFO("geo_sensor_hal is created!"); } geo_sensor_hal::~geo_sensor_hal() { - close(m_node_handle); - m_node_handle = INITIAL_VALUE; - INFO("geo_sensor_hal is destroyed!"); } -string geo_sensor_hal::get_model_id(void) -{ - return m_model_id; -} - -sensor_type_t geo_sensor_hal::get_type(void) +bool geo_sensor_hal::init_resources(void) { - return GEOMAGNETIC_SENSOR; -} + ifstream temp_handle; -bool geo_sensor_hal::enable_resource(string &resource_node, bool enable) -{ - int prev_status, status; - FILE *fp = NULL; - fp = fopen(resource_node.c_str(), "r"); - - if (!fp) { - ERR("Fail to open a resource file: %s", resource_node.c_str()); + if (!read_node_value(m_x_scale_node, m_x_scale)) return false; - } - if (fscanf(fp, "%d", &prev_status) < 0) { - ERR("Failed to get data from %s", resource_node.c_str()); - fclose(fp); + if (!read_node_value(m_y_scale_node, m_y_scale)) return false; - } - fclose(fp); - - if (enable) { - if (m_sensorhub_supported) - status = prev_status | (1 << SENSORHUB_GEOMAGNETIC_ENABLE_BIT); - else - status = 1; - } else { - if (m_sensorhub_supported) - status = prev_status ^ (1 << SENSORHUB_GEOMAGNETIC_ENABLE_BIT); - else - status = 0; - } - - fp = fopen(resource_node.c_str(), "w"); - - if (!fp) { - ERR("Failed to open a resource file: %s", resource_node.c_str()); + if (!read_node_value(m_z_scale_node, m_z_scale)) return false; - } - - if (fprintf(fp, "%d", status) < 0) { - ERR("Failed to enable a resource file: %s", resource_node.c_str()); - fclose(fp); - return false; - } - - if (fp) - fclose(fp); + INFO("Scale Values: %f, %f, %f", m_x_scale, m_y_scale, m_z_scale); return true; } -bool geo_sensor_hal::enable(void) +string geo_sensor_hal::get_model_id(void) { - AUTOLOCK(m_mutex); + return m_model_id; +} - enable_resource(m_enable_resource, true); - set_interval(m_polling_interval); +sensor_type_t geo_sensor_hal::get_type(void) +{ + return GEOMAGNETIC_SENSOR; +} - m_fired_time = 0; - INFO("Geo sensor real starting"); +bool geo_sensor_hal::enable(void) +{ + INFO("Resource already enabled. Enable not supported."); return true; } bool geo_sensor_hal::disable(void) { - AUTOLOCK(m_mutex); - - enable_resource(m_enable_resource, false); - INFO("Geo sensor real stopping"); + INFO("Disable not supported."); return true; } bool geo_sensor_hal::set_interval(unsigned long val) { - unsigned long long polling_interval_ns; - FILE *fp = NULL; - - AUTOLOCK(m_mutex); - - polling_interval_ns = ((unsigned long long)(val) * MS_TO_SEC * MS_TO_SEC); - fp = fopen(m_polling_resource.c_str(), "w"); - - if (!fp) { - ERR("Failed to open a resource file: %s", m_polling_resource.c_str()); - return false; - } - - if (fprintf(fp, "%llu", polling_interval_ns) < 0) { - ERR("Failed to set data %llu", polling_interval_ns); - fclose(fp); - return false; - } - - if (fp) - fclose(fp); - - INFO("Interval is changed from %dms to %dms]", m_polling_interval, val); - m_polling_interval = val; + INFO("Polling interval cannot be changed."); return true; } -bool geo_sensor_hal::update_value(bool wait) +bool geo_sensor_hal::update_value(void) { - const int TIMEOUT = 1; - int geo_raw[4] = {0,}; - bool x, y, z, hdst; - int read_input_cnt = 0; - const int INPUT_MAX_BEFORE_SYN = 10; - unsigned long long fired_time = 0; - bool syn = false; - x = y = z = hdst = false; - - struct timeval tv; - fd_set readfds, exceptfds; - - FD_ZERO(&readfds); - FD_ZERO(&exceptfds); - FD_SET(m_node_handle, &readfds); - FD_SET(m_node_handle, &exceptfds); - - if (wait) { - tv.tv_sec = TIMEOUT; - tv.tv_usec = 0; - } else { - tv.tv_sec = 0; - tv.tv_usec = 0; - } - - int ret; - ret = select(m_node_handle + 1, &readfds, NULL, &exceptfds, &tv); + int raw_values[3] = {0,}; + ifstream temp_handle; - if (ret == -1) { - ERR("select error:%s m_node_handle:%d", strerror(errno), m_node_handle); + if (!read_node_value(m_x_node, raw_values[0])) return false; - } else if (!ret) { - DBG("select timeout: %d seconds elapsed", tv.tv_sec); - return false; - } - if (FD_ISSET(m_node_handle, &exceptfds)) { - ERR("select exception occurred!"); + if (!read_node_value(m_y_node, raw_values[1])) return false; - } - - if (FD_ISSET(m_node_handle, &readfds)) { - struct input_event geo_input; - DBG("geo event detection!"); - - while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) { - int len = read(m_node_handle, &geo_input, sizeof(geo_input)); - - if (len != sizeof(geo_input)) { - ERR("geo_file read fail, read_len = %d", len); - return false; - } - ++read_input_cnt; - - if (geo_input.type == EV_REL) { - switch (geo_input.code) { - case REL_RX: - geo_raw[0] = (int)geo_input.value; - x = true; - break; - case REL_RY: - geo_raw[1] = (int)geo_input.value; - y = true; - break; - case REL_RZ: - geo_raw[2] = (int)geo_input.value; - z = true; - break; - case REL_HWHEEL: - geo_raw[3] = (int)geo_input.value; - hdst = true; - break; - default: - ERR("geo_input event[type = %d, code = %d] is unknown.", geo_input.type, geo_input.code); - return false; - break; - } - } else if (geo_input.type == EV_SYN) { - syn = true; - fired_time = get_timestamp(&geo_input.time); - } else { - ERR("geo_input event[type = %d, code = %d] is unknown.", geo_input.type, geo_input.code); - return false; - } - } - } else { - ERR("select nothing to read!!!"); - return false; - } - - if (syn == false) { - ERR("EV_SYN didn't come until %d inputs had come", read_input_cnt); + if (!read_node_value(m_z_node, raw_values[2])) return false; - } - AUTOLOCK(m_value_mutex); + m_x = GAUSS_TO_UTESLA(raw_values[0] * m_x_scale); + m_y = GAUSS_TO_UTESLA(raw_values[1] * m_y_scale); + m_z = GAUSS_TO_UTESLA(raw_values[2] * m_z_scale); - if (x) - m_x = geo_raw[0]; + m_fired_time = INITIAL_TIME; + INFO("x = %d, y = %d, z = %d, time = %lluus", raw_values[0], raw_values[0], raw_values[0], m_fired_time); - if (y) - m_y = geo_raw[1]; - - if (z) - m_z = geo_raw[2]; - - if (hdst) - m_hdst = geo_raw[3] - 1; /* accuracy bias: -1 */ - - m_fired_time = fired_time; - - DBG("m_x = %d, m_y = %d, m_z = %d, m_hdst = %d, time = %lluus", m_x, m_y, m_z, m_hdst, m_fired_time); return true; } bool geo_sensor_hal::is_data_ready(bool wait) { bool ret; - ret = update_value(wait); + ret = update_value(); return ret; } int geo_sensor_hal::get_sensor_data(sensor_data_t &data) { - const int chance = 3; - int retry = 0; - - while ((m_fired_time == 0) && (retry++ < chance)) { - INFO("Try usleep for getting a valid BASE DATA value"); - usleep(m_polling_interval * MS_TO_SEC); - } - - if (m_fired_time == 0) { - ERR("get_sensor_data failed"); - return -1; - } - - data.data_accuracy = (m_hdst == 1) ? 0 : m_hdst; /* hdst 0 and 1 are needed to calibrate */ data.data_unit_idx = SENSOR_UNIT_MICRO_TESLA; data.timestamp = m_fired_time; data.values_num = 3; - data.values[0] = (float)m_x * LBS_TO_UTESLA; - data.values[1] = (float)m_y * LBS_TO_UTESLA; - data.values[2] = (float)m_z * LBS_TO_UTESLA; - + data.values[0] = (float)m_x; + data.values[1] = (float)m_y; + data.values[2] = (float)m_z; return 0; } bool geo_sensor_hal::get_properties(sensor_properties_t &properties) { properties.sensor_unit_idx = SENSOR_UNIT_MICRO_TESLA; - properties.sensor_min_range = -1200; - properties.sensor_max_range = 1200; + properties.sensor_min_range = SENSOR_MIN_RANGE; + properties.sensor_max_range = SENSOR_MAX_RANGE; snprintf(properties.sensor_name, sizeof(properties.sensor_name), "%s", m_chip_name.c_str()); snprintf(properties.sensor_vendor, sizeof(properties.sensor_vendor), "%s", m_vendor.c_str()); properties.sensor_resolution = 1; @@ -375,24 +184,13 @@ bool geo_sensor_hal::get_properties(sensor_properties_t &properties) bool geo_sensor_hal::is_sensorhub_supported(void) { - FILE *fp = NULL; - string mag_polling_resource = string(SENSORHUB_NODE) + string(MAG_POLL_DELAY); - fp = fopen(mag_polling_resource.c_str(), "r"); - - if (!fp) { - ERR("Fail to open a resource file"); - return false; - } - - INFO("Supported by Sensor-Hub"); - fclose(fp); - return true; + return false; } bool geo_sensor_hal::check_hw_node(void) { - string name_node; string hw_name; + string file_name; DIR *main_dir = NULL; struct dirent *dir_entry = NULL; bool find_node = false; @@ -400,87 +198,51 @@ bool geo_sensor_hal::check_hw_node(void) INFO("======================start check_hw_node============================="); m_sensorhub_supported = is_sensorhub_supported(); - main_dir = opendir(SENSOR_NODE); + main_dir = opendir(IIO_DIR); - if (!main_dir) { - ERR("Directory open failed to collect data"); + if (!main_dir) + { + ERR("Could not open IIO directory\n"); return false; } - while ((!find_node) && (dir_entry = readdir(main_dir))) { - if ((strncasecmp(dir_entry->d_name , ".", 1 ) != 0) && (strncasecmp(dir_entry->d_name , "..", 2 ) != 0) && (dir_entry->d_ino != 0)) { - name_node = string(SENSOR_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_NAME); + while (!find_node) + { + dir_entry = readdir(main_dir); + if(dir_entry == NULL) + break; - ifstream infile(name_node.c_str()); + if ((strncasecmp(dir_entry->d_name , ".", 1 ) != 0) && (strncasecmp(dir_entry->d_name , "..", 2 ) != 0) && (dir_entry->d_ino != 0)) + { + file_name = string(IIO_DIR) + string(dir_entry->d_name) + string(NAME_NODE); + ifstream infile(file_name.c_str()); if (!infile) continue; infile >> hw_name; - if (CConfig::get_instance().is_supported(SENSOR_TYPE_MAGNETIC, hw_name) == true) { + if (CConfig::get_instance().is_supported(SENSOR_TYPE_MAGNETIC, hw_name) == true) + { m_name = m_model_id = hw_name; INFO("m_model_id = %s", m_model_id.c_str()); - find_node = true; - break; - } - } - } - - closedir(main_dir); - - if (find_node) { - main_dir = opendir(INPUT_DEVICE_NODE); - - if (!main_dir) { - ERR("Directory open failed to collect data"); - return false; - } - find_node = false; + string temp = string(IIO_DIR) + string(dir_entry->d_name); - while ((!find_node) && (dir_entry = readdir(main_dir))) { - if (strncasecmp(dir_entry->d_name, NODE_INPUT, 5) == 0) { - name_node = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_NAME); - ifstream infile(name_node.c_str()); + m_x_node = temp + string(X_RAW_VAL_NODE); + m_y_node = temp + string(Y_RAW_VAL_NODE); + m_z_node = temp + string(Z_RAW_VAL_NODE); + m_x_scale_node = temp + string(X_SCALE_NODE); + m_y_scale_node = temp + string(Y_SCALE_NODE); + m_z_scale_node = temp + string(Z_SCALE_NODE); - if (!infile) - continue; - - infile >> hw_name; - - if (hw_name == string(INPUT_NAME)) { - INFO("name_node = %s", name_node.c_str()); - DBG("Find H/W for mag_sensor"); - - find_node = true; - string dir_name; - dir_name = string(dir_entry->d_name); - unsigned found = dir_name.find_first_not_of(NODE_INPUT); - m_resource = string(DEV_INPUT_NODE) + dir_name.substr(found); - - if (m_sensorhub_supported) { - m_enable_resource = string(SENSORHUB_NODE) + string(NODE_ENABLE); - m_polling_resource = string(SENSORHUB_NODE) + string(NODE_MAG_POLL_DELAY); - } else { - m_enable_resource = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_ENABLE); - m_polling_resource = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_POLL_DELAY); - } - - break; - } + find_node = true; + break; } } - - closedir(main_dir); - } - - if (find_node) { - INFO("m_resource = %s", m_resource.c_str()); - INFO("m_enable_resource = %s", m_enable_resource.c_str()); - INFO("m_polling_resource = %s", m_polling_resource.c_str()); } + closedir(main_dir); return find_node; } @@ -488,9 +250,12 @@ extern "C" void *create(void) { geo_sensor_hal *inst; - try { + try + { inst = new geo_sensor_hal(); - } catch (int err) { + } + catch (int err) + { ERR("Failed to create geo_sensor_hal class, errno : %d, errstr : %s", err, strerror(err)); return NULL; } diff --git a/src/geo/geo_sensor_hal.h b/src/geo/geo_sensor_hal.h index 066d672..471e7a7 100755 --- a/src/geo/geo_sensor_hal.h +++ b/src/geo/geo_sensor_hal.h @@ -22,8 +22,19 @@ #include #include +#include + +#define IIO_DIR "/sys/bus/iio/devices/" +#define X_RAW_VAL_NODE "/in_magn_x_raw" +#define Y_RAW_VAL_NODE "/in_magn_y_raw" +#define Z_RAW_VAL_NODE "/in_magn_z_raw" +#define X_SCALE_NODE "/in_magn_x_scale" +#define Y_SCALE_NODE "/in_magn_y_scale" +#define Z_SCALE_NODE "/in_magn_z_scale" +#define NAME_NODE "/name" using std::string; +using std::ifstream; class geo_sensor_hal : public sensor_hal { @@ -41,14 +52,13 @@ public: bool check_hw_node(void); private: - long a_x; - long a_y; - long a_z; double m_x; double m_y; double m_z; - int m_hdst; - int m_node_handle; + double m_x_scale; + double m_y_scale; + double m_z_scale; + unsigned long m_polling_interval; unsigned long long m_fired_time; bool m_sensorhub_supported; @@ -58,14 +68,34 @@ private: string m_vendor; string m_chip_name; - string m_resource; - string m_enable_resource; - string m_polling_resource; + string m_x_node; + string m_y_node; + string m_z_node; + string m_x_scale_node; + string m_y_scale_node; + string m_z_scale_node; cmutex m_value_mutex; bool enable_resource(string &resource_node, bool enable); - bool update_value(bool wait); + bool update_value(void); bool is_sensorhub_supported(void); + bool init_resources(void); + + template + bool read_node_value(string node_path, value_t &value) + { + ifstream handle; + handle.open(node_path.c_str()); + if (!handle) + { + ERR("Failed to open handle(%s)", node_path.c_str()); + return false; + } + handle >> value; + handle.close(); + + return true; + } }; #endif /*_GEO_SENSOR_HAL_H_*/ -- 2.7.4