From cac600307aea5d48f6e073eece3dc9b3e8ce318e Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Tue, 15 Mar 2016 09:58:54 +0900 Subject: [PATCH 01/16] hal: add/sync sensor device types Change-Id: I5b0b6eb2a5cdc9e81be1b2e47a24864c4400fc9a Signed-off-by: kibak.yoon --- src/hal/sensor_hal.h | 54 ++++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 44 insertions(+), 10 deletions(-) diff --git a/src/hal/sensor_hal.h b/src/hal/sensor_hal.h index 8d89f75..c52f25d 100644 --- a/src/hal/sensor_hal.h +++ b/src/hal/sensor_hal.h @@ -21,8 +21,8 @@ #include #include -#define SENSOR_HAL_VERSION(maj,min) \ - ((((maj) & 0xffff) << 24) | ((min) & 0xffff)) +#define SENSOR_HAL_VERSION(maj, min) \ + ((((maj) & 0xFFFF) << 24) | ((min) & 0xFFFF)) #ifdef __cplusplus extern "C" @@ -74,6 +74,7 @@ typedef enum { SENSOR_DEVICE_FUSION = 0x900, SENSOR_DEVICE_AUTO_ROTATION, + SENSOR_DEVICE_AUTO_BRIGHTNESS, SENSOR_DEVICE_CONTEXT = 0x1000, SENSOR_DEVICE_MOTION, @@ -86,16 +87,33 @@ typedef enum { SENSOR_DEVICE_HRM_RAW, SENSOR_DEVICE_TILT, SENSOR_DEVICE_ROTATION_VECTOR_RAW, - - SENSOR_DEVICE_ACTIVITY_STATIONARY = 0x1100, - SENSOR_DEVICE_ACTIVITY_WALK, - SENSOR_DEVICE_ACTIVITY_RUN, - SENSOR_DEVICE_ACTIVITY_IN_VEHICLE, - SENSOR_DEVICE_ACTIVITY_ON_BICYCLE, + SENSOR_DEVICE_EXERCISE, + SENSOR_DEVICE_GSR, + SENSOR_DEVICE_SIMSENSE, + SENSOR_DEVICE_PPG, SENSOR_DEVICE_GESTURE_MOVEMENT = 0x1200, SENSOR_DEVICE_GESTURE_WRIST_UP, SENSOR_DEVICE_GESTURE_WRIST_DOWN, + SENSOR_DEVICE_GESTURE_MOVEMENT_STATE, + + SENSOR_DEVICE_WEAR_STATUS = 0x1A00, + SENSOR_DEVICE_WEAR_ON_MONITOR, + SENSOR_DEVICE_GPS_BATCH, + SENSOR_DEVICE_ACTIVITY_TRACKER, + SENSOR_DEVICE_SLEEP_DETECTOR, + SENSOR_DEVICE_NO_MOVE_DETECTOR = 0x1A80, + SENSOR_DEVICE_HRM_CTRL, + SENSOR_DEVICE_EXERCISE_COACH, + SENSOR_DEVICE_EXERCISE_HR, + SENSOR_DEVICE_RESTING_HR, + SENSOR_DEVICE_STEP_LEVEL_MONITOR, + SENSOR_DEVICE_ACTIVITY_LEVEL_MONITOR, + SENSOR_DEVICE_CYCLE_MONITOR, + SENSOR_DEVICE_STRESS_MONITOR, + SENSOR_DEVICE_AUTOSESSION_EXERCISE, + SENSOR_DEVICE_STAIR_TRACKER, + } sensor_device_type; /* @@ -141,8 +159,24 @@ typedef struct sensor_data_t { typedef struct sensorhub_data_t { int accuracy; unsigned long long timestamp; - int value_count; - char values[SENSORHUB_DATA_VALUE_SIZE]; + + /* + * Use "value_count" instead of "hub_data_size" + * which is going to be removed soon + */ + union { + int value_count; + int hub_data_size; /* deprecated */ + }; + + /* + * Use "values" instead of "hub_data" + * which is going to be removed soon + */ + union { + char values[SENSORHUB_DATA_VALUE_SIZE]; + char hub_data[SENSORHUB_DATA_VALUE_SIZE]; /* deprecated */ + }; } sensorhub_data_t; /* -- 2.7.4 From 0e91f9a6b372f3cfb7a2881d4303f2e7a93d38b8 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Tue, 15 Mar 2016 11:33:20 +0900 Subject: [PATCH 02/16] sensord: sync/clean up sensord source code Change-Id: I3fa2c377700e828443ec747e226f89c6249d541f Signed-off-by: kibak.yoon --- src/client/CMakeLists.txt | 2 +- src/client/client.cpp | 6 +++--- src/client/client_common.h | 1 + src/client/sensor_event_listener.cpp | 12 ++---------- src/client/sensor_event_listener.h | 1 - src/client/sensor_handle_info.cpp | 2 +- src/server/command_worker.cpp | 4 ++-- src/server/physical_sensor.h | 14 ++++++++------ src/server/sensor_event_dispatcher.cpp | 4 ++-- src/server/sensor_event_poller.cpp | 5 ++++- src/server/sensor_loader.cpp | 4 ++-- src/server/sensor_usage.cpp | 2 +- src/shared/command_common.h | 4 ++-- 13 files changed, 29 insertions(+), 32 deletions(-) diff --git a/src/client/CMakeLists.txt b/src/client/CMakeLists.txt index 0ad7db0..2f32b9c 100644 --- a/src/client/CMakeLists.txt +++ b/src/client/CMakeLists.txt @@ -1,7 +1,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) PROJECT(sensor CXX) -SET(DEPENDENTS "vconf glib-2.0 dlog") +SET(DEPENDENTS "vconf glib-2.0 gio-2.0 dlog") SET(VERSION ${FULLVER}) SET(PREFIX ${CMAKE_INSTALL_PREFIX}) SET(EXEC_PREFIX "${CMAKE_INSTALL_PREFIX}/bin") diff --git a/src/client/client.cpp b/src/client/client.cpp index 4366792..0a3024e 100644 --- a/src/client/client.cpp +++ b/src/client/client.cpp @@ -18,6 +18,7 @@ */ #include +#include #include #include #include @@ -543,7 +544,7 @@ API int sensord_connect(sensor_t sensor) atexit(good_bye); handle = sensor_client_info::get_instance().create_handle(sensor_id); - if (handle == MAX_HANDLE_REACHED) { + if (handle == MAX_HANDLE) { _E("Maximum number of handles reached, sensor: %s in client %s", get_sensor_name(sensor_id), get_client_name()); return OP_ERROR; } @@ -1108,11 +1109,10 @@ API bool sensord_get_data(int handle, unsigned int data_id, sensor_data_t* senso return false; } - if(!cmd_channel->cmd_get_data(data_id, sensor_data)) { + if (!cmd_channel->cmd_get_data(data_id, sensor_data)) { _E("cmd_get_data(%d, %d, 0x%x) failed for %s", client_id, data_id, sensor_data, get_client_name()); return false; } return true; - } diff --git a/src/client/client_common.h b/src/client/client_common.h index 8b5ec8e..ff6eb2c 100644 --- a/src/client/client_common.h +++ b/src/client/client_common.h @@ -27,6 +27,7 @@ #include #define BASE_GATHERING_INTERVAL 100 + #define CLIENT_NAME_SIZE NAME_MAX+10 struct log_attr { diff --git a/src/client/sensor_event_listener.cpp b/src/client/sensor_event_listener.cpp index 778392d..c71b9cd 100644 --- a/src/client/sensor_event_listener.cpp +++ b/src/client/sensor_event_listener.cpp @@ -47,14 +47,6 @@ sensor_event_listener::~sensor_event_listener() stop_event_listener(); } -sensor_event_listener::sensor_event_listener(const sensor_event_listener& listener) -: m_poller(listener.m_poller) -, m_thread_state(listener.m_thread_state) -, m_hup_observer(listener.m_hup_observer) -, m_client_info(listener.m_client_info) -{ -} - sensor_event_listener& sensor_event_listener::get_instance(void) { static sensor_event_listener inst; @@ -409,7 +401,7 @@ void sensor_event_listener::listen_events(void) bool sensor_event_listener::create_event_channel(void) { int client_id; - event_channel_ready_t event_channel_ready; + channel_ready_t event_channel_ready; if (!m_event_socket.create(SOCK_SEQPACKET)) return false; @@ -437,7 +429,7 @@ bool sensor_event_listener::create_event_channel(void) return false; } - if ((event_channel_ready.magic != EVENT_CHANNEL_MAGIC) || (event_channel_ready.client_id != client_id)) { + if ((event_channel_ready.magic != CHANNEL_MAGIC_NUM) || (event_channel_ready.client_id != client_id)) { _E("Event_channel_ready packet is wrong, magic = 0x%x, client id = %d", event_channel_ready.magic, event_channel_ready.client_id); return false; diff --git a/src/client/sensor_event_listener.h b/src/client/sensor_event_listener.h index 4af7092..fa7b77e 100644 --- a/src/client/sensor_event_listener.h +++ b/src/client/sensor_event_listener.h @@ -102,7 +102,6 @@ private: sensor_event_listener(); ~sensor_event_listener(); - sensor_event_listener(const sensor_event_listener&); sensor_event_listener& operator=(const sensor_event_listener&); bool create_event_channel(void); diff --git a/src/client/sensor_handle_info.cpp b/src/client/sensor_handle_info.cpp index 9ea4d0d..796107f 100644 --- a/src/client/sensor_handle_info.cpp +++ b/src/client/sensor_handle_info.cpp @@ -149,7 +149,7 @@ void sensor_handle_info::get_batch(unsigned int &interval, unsigned int &latency { if (m_reg_event_infos.empty()) { _D("No events are registered for client %s", get_client_name()); - interval = POLL_MAX_HZ_MS; + interval = POLL_1HZ_MS; latency = 0; return; } diff --git a/src/server/command_worker.cpp b/src/server/command_worker.cpp index b99ba0f..c8804d7 100644 --- a/src/server/command_worker.cpp +++ b/src/server/command_worker.cpp @@ -772,7 +772,7 @@ bool command_worker::cmd_set_attribute_int(void *payload) cmd = (cmd_set_attribute_int_t*)payload; if (!is_permission_allowed()) { - _E("Permission denied to set command for client [%d], for sensor [0x%llx] with attribute [%d]", + _E("Permission denied to set attribute for client [%d], for sensor [0x%llx] with attribute [%d]", m_client_id, m_sensor_id, cmd->attribute); ret_value = OP_ERROR; goto out; @@ -797,7 +797,7 @@ bool command_worker::cmd_set_attribute_str(void *payload) cmd = (cmd_set_attribute_str_t*)payload; if (!is_permission_allowed()) { - _E("Permission denied to send sensorhub_data for client [%d], for sensor [0x%llx]", + _E("Permission denied to set attribute for client [%d], for sensor [0x%llx]", m_client_id, m_sensor_id); ret_value = OP_ERROR; goto out; diff --git a/src/server/physical_sensor.h b/src/server/physical_sensor.h index 7bad87d..a2e8763 100644 --- a/src/server/physical_sensor.h +++ b/src/server/physical_sensor.h @@ -43,20 +43,22 @@ public: virtual bool read_fd(std::vector &ids); virtual int get_data(sensor_data_t **data, int *length); virtual bool flush(void); -private: - static cmutex m_mutex; +protected: const sensor_info_t *m_info; sensor_device *m_sensor_device; uint32_t hal_id; - virtual bool set_interval(unsigned long interval); - virtual bool set_batch_latency(unsigned long latency); - virtual int set_attribute(int32_t attribute, int32_t value); - virtual int set_attribute(int32_t attribute, char *value, int value_len); virtual bool on_start(void); virtual bool on_stop(void); + virtual int set_attribute(int32_t attribute, int32_t value); + virtual int set_attribute(int32_t attribute, char *value, int value_len); + virtual bool set_interval(unsigned long interval); + virtual bool set_batch_latency(unsigned long latency); virtual bool get_sensor_info(sensor_info &info); + +private: + static cmutex m_mutex; }; #endif /* _PHYSICAL_SENSOR_H_ */ diff --git a/src/server/sensor_event_dispatcher.cpp b/src/server/sensor_event_dispatcher.cpp index b03a5e4..977a657 100644 --- a/src/server/sensor_event_dispatcher.cpp +++ b/src/server/sensor_event_dispatcher.cpp @@ -53,7 +53,7 @@ bool sensor_event_dispatcher::run(void) void sensor_event_dispatcher::accept_event_channel(csocket client_socket) { int client_id; - event_channel_ready_t event_channel_ready; + channel_ready_t event_channel_ready; client_info_manager& client_info_manager = get_client_info_manager(); client_socket.set_connection_mode(); @@ -71,7 +71,7 @@ void sensor_event_dispatcher::accept_event_channel(csocket client_socket) return; } - event_channel_ready.magic = EVENT_CHANNEL_MAGIC; + event_channel_ready.magic = CHANNEL_MAGIC_NUM; event_channel_ready.client_id = client_id; _I("Event channel is accepted for %s on socket[%d]", diff --git a/src/server/sensor_event_poller.cpp b/src/server/sensor_event_poller.cpp index 0063871..af7d8cd 100644 --- a/src/server/sensor_event_poller.cpp +++ b/src/server/sensor_event_poller.cpp @@ -131,8 +131,11 @@ bool sensor_event_poller::process_event(int fd, const std::vector &ids while (remains > 0) { event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); - remains = sensor->get_data(&data, &data_length); + if (remains < 0) { + _E("Failed to get sensor data"); + break; + } event->sensor_id = sensor->get_id(); event->event_type = sensor->get_event_type(); diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index 5626214..a050df7 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -141,9 +141,9 @@ void sensor_loader::create_physical_sensors(sensor_type_t type) physical_sensor *sensor; sensor_device *device; - sensor_device_map_t::iterator it = m_devices.begin(); + sensor_device_map_t::iterator it; - for (sensor_device_map_t::iterator it = m_devices.begin(); it != m_devices.end(); ++it) { + for (it = m_devices.begin(); it != m_devices.end(); ++it) { info = it->first; device = it->second; if (m_devices[info] == NULL) diff --git a/src/server/sensor_usage.cpp b/src/server/sensor_usage.cpp index 4b6c881..83ead8d 100644 --- a/src/server/sensor_usage.cpp +++ b/src/server/sensor_usage.cpp @@ -23,7 +23,7 @@ #include sensor_usage::sensor_usage() -: m_interval(POLL_MAX_HZ_MS) +: m_interval(POLL_1HZ_MS) , m_latency(0) , m_option(SENSOR_OPTION_DEFAULT) , m_start(false) diff --git a/src/shared/command_common.h b/src/shared/command_common.h index 0e2a5ac..48dd750 100644 --- a/src/shared/command_common.h +++ b/src/shared/command_common.h @@ -126,12 +126,12 @@ typedef struct { char value[0]; } cmd_set_attribute_str_t; -#define EVENT_CHANNEL_MAGIC 0xCAFECAFE +#define CHANNEL_MAGIC_NUM 0xCAFECAFE typedef struct { unsigned int magic; int client_id; -} event_channel_ready_t; +} channel_ready_t; typedef void *(*cmd_func_t)(void *data, void *cb_data); -- 2.7.4 From 46f32216111080aa6ac4c0e7875d0a30d72ffd88 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Tue, 15 Mar 2016 11:39:53 +0900 Subject: [PATCH 03/16] sensord: add sensord_flush internal API for batching Change-Id: I9ce1ec8259b7c592ce269451a74a9a018ee5700f Signed-off-by: kibak.yoon --- src/client/client.cpp | 36 ++++++++++++++++++++++++++++++++++++ src/client/command_channel.cpp | 33 +++++++++++++++++++++++++++++++++ src/client/command_channel.h | 2 ++ src/client/sensor_internal.h | 14 ++++++++++++++ src/server/command_worker.cpp | 29 +++++++++++++++++++++++++++++ src/server/command_worker.h | 1 + src/shared/command_common.h | 4 ++++ 7 files changed, 119 insertions(+) diff --git a/src/client/client.cpp b/src/client/client.cpp index 0a3024e..fcd4c0c 100644 --- a/src/client/client.cpp +++ b/src/client/client.cpp @@ -1116,3 +1116,39 @@ API bool sensord_get_data(int handle, unsigned int data_id, sensor_data_t* senso return true; } + +API bool sensord_flush(int handle) +{ + sensor_id_t sensor_id; + command_channel *cmd_channel; + int sensor_state; + int client_id; + + AUTOLOCK(lock); + + if (!sensor_client_info::get_instance().get_sensor_state(handle, sensor_state) || + !sensor_client_info::get_instance().get_sensor_id(handle, sensor_id)) { + _E("client %s failed to get handle information", get_client_name()); + return false; + } + + if (!sensor_client_info::get_instance().get_command_channel(sensor_id, &cmd_channel)) { + _E("client %s failed to get command channel for %s", get_client_name(), get_sensor_name(sensor_id)); + return false; + } + + client_id = sensor_client_info::get_instance().get_client_id(); + retvm_if ((client_id < 0), false, "Invalid client id : %d, handle: %d, %s, %s", client_id, handle, get_sensor_name(sensor_id), get_client_name()); + + if (sensor_state != SENSOR_STATE_STARTED) { + _E("Sensor %s is not started for client %s with handle: %d, sensor_state: %d", get_sensor_name(sensor_id), get_client_name(), handle, sensor_state); + return false; + } + + if (!cmd_channel->cmd_flush()) { + _E("cmd_flush() failed for %s", get_client_name()); + return false; + } + + return true; +} \ No newline at end of file diff --git a/src/client/command_channel.cpp b/src/client/command_channel.cpp index d1c91c3..ca6b025 100644 --- a/src/client/command_channel.cpp +++ b/src/client/command_channel.cpp @@ -684,3 +684,36 @@ bool command_channel::cmd_set_attribute_str(int attribute, const char* value, in delete packet; return true; } + +bool command_channel::cmd_flush(void) +{ + cpacket *packet; + cmd_done_t *cmd_done; + + packet = new(std::nothrow) cpacket(sizeof(cmd_flush_t)); + retvm_if(!packet, false, "Failed to allocate memory"); + + packet->set_cmd(CMD_FLUSH); + + _I("%s send cmd_flush(client_id=%d)", get_client_name(), m_client_id); + + if (!command_handler(packet, (void **)&cmd_done)) { + _E("%s failed to send flush with client_id [%d]", + get_client_name(), m_client_id); + delete packet; + return false; + } + + if (cmd_done->value < 0) { + _E("%s got error[%d] from server with client_id [%d]", + get_client_name(), cmd_done->value, m_client_id); + + delete [] (char *)cmd_done; + delete packet; + return false; + } + + delete [] (char *)cmd_done; + delete packet; + return true; +} diff --git a/src/client/command_channel.h b/src/client/command_channel.h index b99891b..15b579e 100644 --- a/src/client/command_channel.h +++ b/src/client/command_channel.h @@ -49,6 +49,8 @@ public: bool cmd_get_data(unsigned int type, sensor_data_t *values); bool cmd_set_attribute_int(int attribute, int value); bool cmd_set_attribute_str(int attribute, const char *value, int value_len); + bool cmd_flush(void); + private: csocket m_command_socket; int m_client_id; diff --git a/src/client/sensor_internal.h b/src/client/sensor_internal.h index 94a05b5..cd1db11 100644 --- a/src/client/sensor_internal.h +++ b/src/client/sensor_internal.h @@ -355,6 +355,20 @@ bool sensord_send_command(int handle, const char *command, int command_len); bool sensord_get_data(int handle, unsigned int data_id, sensor_data_t* sensor_data); /** + * @brief flush sensor data from a connected sensor + * + * @param[in] handle a handle represensting a connected context sensor. + * @return true on success, otherwise false. + */ +bool sensord_flush(int handle); + +typedef void (*sensor_external_command_cb_t)(int handle, const char* data, int data_cnt, void *user_data); + +int sensord_external_connect(const char *key, sensor_external_command_cb_t cb, void *user_data); +bool sensord_external_disconnect(int handle); +bool sensord_external_post(int handle, unsigned long long timestamp, const float* data, int data_cnt); + +/** * @} */ diff --git a/src/server/command_worker.cpp b/src/server/command_worker.cpp index c8804d7..1f87342 100644 --- a/src/server/command_worker.cpp +++ b/src/server/command_worker.cpp @@ -87,6 +87,7 @@ void command_worker::init_cmd_handlers(void) m_cmd_handlers[CMD_GET_DATA] = &command_worker::cmd_get_data; m_cmd_handlers[CMD_SET_ATTRIBUTE_INT] = &command_worker::cmd_set_attribute_int; m_cmd_handlers[CMD_SET_ATTRIBUTE_STR] = &command_worker::cmd_set_attribute_str; + m_cmd_handlers[CMD_FLUSH] = &command_worker::cmd_flush; } void command_worker::get_sensor_list(int permissions, cpacket &sensor_list) @@ -812,6 +813,34 @@ out: return true; } +bool command_worker::cmd_flush(void *payload) +{ + long ret_value = OP_ERROR; + + _D("CMD_FLUSH Handler invoked"); + + if (!is_permission_allowed()) { + _E("Permission denied to flush sensor data for client [%d], for sensor [0x%llx]", + m_client_id, m_sensor_id); + ret_value = OP_ERROR; + goto out; + } + + if (!m_module->flush()) { + _E("Failed to flush sensor_data [%d]", m_client_id); + ret_value = OP_ERROR; + goto out; + } + + ret_value = OP_SUCCESS; + +out: + if (!send_cmd_done(ret_value)) + _E("Failed to send cmd_done to a client"); + + return true; +} + void command_worker::get_info(string &info) { const char *client_info = NULL; diff --git a/src/server/command_worker.h b/src/server/command_worker.h index c3ce538..3e01883 100644 --- a/src/server/command_worker.h +++ b/src/server/command_worker.h @@ -73,6 +73,7 @@ private: bool cmd_get_data(void *payload); bool cmd_set_attribute_int(void *payload); bool cmd_set_attribute_str(void *payload); + bool cmd_flush(void *payload); void get_info(std::string &info); diff --git a/src/shared/command_common.h b/src/shared/command_common.h index 48dd750..4a73f1d 100644 --- a/src/shared/command_common.h +++ b/src/shared/command_common.h @@ -46,6 +46,7 @@ enum packet_type_t { CMD_GET_DATA, CMD_SET_ATTRIBUTE_INT, CMD_SET_ATTRIBUTE_STR, + CMD_FLUSH, CMD_CNT, }; @@ -126,6 +127,9 @@ typedef struct { char value[0]; } cmd_set_attribute_str_t; +typedef struct { +} cmd_flush_t; + #define CHANNEL_MAGIC_NUM 0xCAFECAFE typedef struct { -- 2.7.4 From bfba23f53c8b4786f8f99e7292519d4b20c4535e Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Tue, 15 Mar 2016 11:45:34 +0900 Subject: [PATCH 04/16] sensord: call on_event() after invoking sensor event from fd Change-Id: I616af576afe22cb512cfc3f8c5be76c458af7e8d Signed-off-by: kibak.yoon --- src/server/physical_sensor.cpp | 5 +++++ src/server/physical_sensor.h | 2 ++ src/server/sensor_event_poller.cpp | 6 ++++++ 3 files changed, 13 insertions(+) diff --git a/src/server/physical_sensor.cpp b/src/server/physical_sensor.cpp index 49858f4..7964065 100644 --- a/src/server/physical_sensor.cpp +++ b/src/server/physical_sensor.cpp @@ -77,6 +77,11 @@ int physical_sensor::get_poll_fd() return m_sensor_device->get_poll_fd(); } +bool physical_sensor::on_event(const sensor_data_t *data, int remains) +{ + return true; +} + bool physical_sensor::read_fd(std::vector &ids) { AUTOLOCK(m_mutex); diff --git a/src/server/physical_sensor.h b/src/server/physical_sensor.h index a2e8763..393e4d7 100644 --- a/src/server/physical_sensor.h +++ b/src/server/physical_sensor.h @@ -40,6 +40,8 @@ public: int get_poll_fd(); + virtual bool on_event(const sensor_data_t *data, int remains); + virtual bool read_fd(std::vector &ids); virtual int get_data(sensor_data_t **data, int *length); virtual bool flush(void); diff --git a/src/server/sensor_event_poller.cpp b/src/server/sensor_event_poller.cpp index af7d8cd..a7ec9a4 100644 --- a/src/server/sensor_event_poller.cpp +++ b/src/server/sensor_event_poller.cpp @@ -137,6 +137,12 @@ bool sensor_event_poller::process_event(int fd, const std::vector &ids break; } + if (!sensor->on_event(data, remains)) { + free(event); + free(data); + break; + } + event->sensor_id = sensor->get_id(); event->event_type = sensor->get_event_type(); event->data_length = data_length; -- 2.7.4 From df57020c8580d24ed361b67be6a613153a09bb44 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Tue, 15 Mar 2016 11:52:42 +0900 Subject: [PATCH 05/16] sensord: enable hrm/gravity/linear accel sensors Change-Id: I9595bbcb4a0f194b06b1d1f458fb410f179afcd6 Signed-off-by: kibak.yoon --- src/sensor/CMakeLists.txt | 6 +- src/sensor/fusion_util.cpp | 56 ++ src/sensor/fusion_util.h | 34 ++ src/sensor/gravity/gravity_sensor.cpp | 669 ++++++++++++++---------- src/sensor/gravity/gravity_sensor.h | 79 +-- src/sensor/hrm/hrm_sensor.cpp | 34 ++ src/sensor/hrm/hrm_sensor.h | 32 ++ src/sensor/linear_accel/linear_accel_sensor.cpp | 457 +++++----------- src/sensor/linear_accel/linear_accel_sensor.h | 64 +-- src/server/sensor_fusion.cpp | 106 ++++ src/server/sensor_fusion.h | 59 +++ src/server/sensor_loader.cpp | 25 +- 12 files changed, 961 insertions(+), 660 deletions(-) create mode 100644 src/sensor/fusion_util.cpp create mode 100644 src/sensor/fusion_util.h create mode 100644 src/sensor/hrm/hrm_sensor.cpp create mode 100644 src/sensor/hrm/hrm_sensor.h create mode 100644 src/server/sensor_fusion.cpp create mode 100644 src/server/sensor_fusion.h diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt index b37f7bf..0e83c6d 100644 --- a/src/sensor/CMakeLists.txt +++ b/src/sensor/CMakeLists.txt @@ -2,11 +2,11 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) PROJECT(sensors CXX) INCLUDE(GNUInstallDirs) -SET(HRM "OFF") +SET(HRM "ON") SET(HRM_VIRT "OFF") SET(AUTO_ROTATION "ON") -SET(GRAVITY "OFF") -SET(LINEAR_ACCEL "OFF") +SET(GRAVITY "ON") +SET(LINEAR_ACCEL "ON") SET(ORIENTATION "OFF") SET(FUSION "OFF") SET(MOTION "OFF") diff --git a/src/sensor/fusion_util.cpp b/src/sensor/fusion_util.cpp new file mode 100644 index 0000000..1f2692d --- /dev/null +++ b/src/sensor/fusion_util.cpp @@ -0,0 +1,56 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include + +int quat_to_matrix(const float *quat, float *R) +{ + if(quat == NULL || R == NULL) + return -1; + + float q0 = quat[3]; + float q1 = quat[0]; + float q2 = quat[1]; + float q3 = quat[2]; + + float sq_q1 = 2 * q1 * q1; + float sq_q2 = 2 * q2 * q2; + float sq_q3 = 2 * q3 * q3; + float q1_q2 = 2 * q1 * q2; + float q3_q0 = 2 * q3 * q0; + float q1_q3 = 2 * q1 * q3; + float q2_q0 = 2 * q2 * q0; + float q2_q3 = 2 * q2 * q3; + float q1_q0 = 2 * q1 * q0; + + R[0] = 1 - sq_q2 - sq_q3; + R[1] = q1_q2 - q3_q0; + R[2] = q1_q3 + q2_q0; + R[3] = q1_q2 + q3_q0; + R[4] = 1 - sq_q1 - sq_q3; + R[5] = q2_q3 - q1_q0; + R[6] = q1_q3 - q2_q0; + R[7] = q2_q3 + q1_q0; + R[8] = 1 - sq_q1 - sq_q2; + + return 0; +} + diff --git a/src/sensor/fusion_util.h b/src/sensor/fusion_util.h new file mode 100644 index 0000000..b4d2bc2 --- /dev/null +++ b/src/sensor/fusion_util.h @@ -0,0 +1,34 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef _FUSION_UTIL_H_ +#define _FUSION_UTIL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +int quat_to_matrix(const float *quat, float *R); + +#ifdef __cplusplus +} +#endif /* __cplusplus */ + +#endif /* _FUSION_UTIL_H_ */ diff --git a/src/sensor/gravity/gravity_sensor.cpp b/src/sensor/gravity/gravity_sensor.cpp index d81242a..49147a6 100644 --- a/src/sensor/gravity/gravity_sensor.cpp +++ b/src/sensor/gravity/gravity_sensor.cpp @@ -25,367 +25,498 @@ #include #include #include + #include +#include + +#include +#include #include #include -#include +#include -using std::string; -using std::vector; +#define SENSOR_NAME "GRAVITY_SENSOR" -#define INITIAL_VALUE -1 #define GRAVITY 9.80665 -#define DEVIATION 0.1 - -#define PI 3.141593 -#define AZIMUTH_OFFSET_DEGREES 360 -#define AZIMUTH_OFFSET_RADIANS (2 * PI) +#define PHASE_ACCEL_READY 0x01 +#define PHASE_GYRO_READY 0x02 +#define PHASE_FUSION_READY 0x03 +#define US_PER_SEC 1000000 +#define MS_PER_SEC 1000 +#define INV_ANGLE -1000 +#define TAU_LOW 0.4 +#define TAU_MID 0.75 +#define TAU_HIGH 0.99 -#define SENSOR_NAME "GRAVITY_SENSOR" -#define SENSOR_TYPE_GRAVITY "GRAVITY" -#define SENSOR_TYPE_ORIENTATION "ORIENTATION" - -#define MS_TO_US 1000 -#define MIN_DELIVERY_DIFF_FACTOR 0.75f - -#define ELEMENT_NAME "NAME" -#define ELEMENT_VENDOR "VENDOR" -#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" -#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" -#define ELEMENT_GRAVITY_SIGN_COMPENSATION "GRAVITY_SIGN_COMPENSATION" -#define ELEMENT_ORIENTATION_DATA_UNIT "RAW_DATA_UNIT" -#define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION" -#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" -#define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION" +#define DEG2RAD(x) ((x) * M_PI / 180.0) +#define NORM(x, y, z) sqrt((x)*(x) + (y)*(y) + (z)*(z)) +#define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x),(y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0) gravity_sensor::gravity_sensor() -: m_accel_sensor(NULL) +: m_fusion(NULL) +, m_accel_sensor(NULL) , m_gyro_sensor(NULL) -, m_magnetic_sensor(NULL) -, m_fusion_sensor(NULL) +, m_fusion_phase(0) +, m_x(-1) +, m_y(-1) +, m_z(-1) +, m_accuracy(-1) , m_time(0) { - virtual_sensor_config &config = virtual_sensor_config::get_instance(); +} - sensor_hal *fusion_sensor_hal = sensor_loader::get_instance().get_sensor_hal(SENSOR_HAL_TYPE_FUSION); - if (!fusion_sensor_hal) - m_hardware_fusion = false; - else - m_hardware_fusion = true; +gravity_sensor::~gravity_sensor() +{ + _I("gravity_sensor is destroyed!\n"); +} - m_name = std::string(SENSOR_NAME); - register_supported_event(GRAVITY_RAW_DATA_EVENT); +bool gravity_sensor::init() +{ + /* Acc (+ Gyro) fusion */ + m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); - if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_VENDOR, m_vendor)) { - _E("[VENDOR] is empty\n"); - throw ENXIO; + if (!m_accel_sensor) { + _E("cannot load accelerometer sensor_hal[%s]", get_name()); + return false; } - _I("m_vendor = %s", m_vendor.c_str()); + m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); + + _I("%s (%s) is created!\n", get_name(), m_gyro_sensor ? "Acc+Gyro Fusion" : "LowPass Acc"); + return true; +} - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ORIENTATION_DATA_UNIT, m_orientation_data_unit)) { - _E("[ORIENTATION_DATA_UNIT] is empty\n"); - throw ENXIO; - } +sensor_type_t gravity_sensor::get_type(void) +{ + return GRAVITY_SENSOR; +} - _I("m_orientation_data_unit = %s", m_orientation_data_unit.c_str()); +unsigned int gravity_sensor::get_event_type(void) +{ + return GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; +} - if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) { - _E("[RAW_DATA_UNIT] is empty\n"); - throw ENXIO; - } +const char* gravity_sensor::get_name(void) +{ + return SENSOR_NAME; +} - _I("m_raw_data_unit = %s", m_raw_data_unit.c_str()); +bool gravity_sensor::get_sensor_info(sensor_info &info) +{ + info.set_type(get_type()); + info.set_id(get_id()); + info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); // FIXME + info.set_name("Gravity Sensor"); + info.set_vendor("Samsung Electronics"); + info.set_min_range(-19.6); + info.set_max_range(19.6); + info.set_resolution(0.01); + info.set_min_interval(1); + info.set_fifo_count(0); + info.set_max_batch_count(0); + info.set_supported_event(get_event_type()); + info.set_wakeup_supported(false); - if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { - _E("[DEFAULT_SAMPLING_TIME] is empty\n"); - throw ENXIO; - } + return true; +} - _I("m_default_sampling_time = %d", m_default_sampling_time); +void gravity_sensor::synthesize(const sensor_event_t& event) +{ + /* If the rotation vector sensor is available */ + if (m_fusion) { + synthesize_rv(event); + return; + } - if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) { - _E("[GRAVITY_SIGN_COMPENSATION] is empty\n"); - throw ENXIO; + /* If both Acc & Gyro are available */ + if (m_gyro_sensor) { + synthesize_fusion(event); + return; } - _I("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]); + /* If only Acc is available */ + synthesize_lowpass(event); +} + +void gravity_sensor::synthesize_rv(const sensor_event_t& event) +{ + if (!m_fusion->is_data_ready()) + return; + + sensor_event_t *gravity_event; + float gravity[3]; + float x, y, z, w, heading_accuracy; + int accuracy; - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) { - _E("[AZIMUTH_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; + if (!m_fusion->get_rotation_vector(x, y, z, w, heading_accuracy, accuracy)) { + _E("Failed to get rotation vector"); + return; } - _I("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation); + unsigned long long timestamp = m_fusion->get_data_timestamp(); + + if (!check_sampling_time(timestamp)) + return; + + float rotation[4] = {x, y, z, w}; - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { - _E("[PITCH_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; + rotation_to_gravity(rotation, gravity); + + gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!gravity_event) { + _E("Failed to allocate memory"); + return; + } + gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + if (!gravity_event->data) { + _E("Failed to allocate memory"); + return; } - _I("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); + gravity_event->sensor_id = get_id(); + gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; + gravity_event->data_length = sizeof(sensor_data_t); + gravity_event->data->accuracy = accuracy; + gravity_event->data->timestamp = m_fusion->get_data_timestamp(); + gravity_event->data->value_count = 3; + gravity_event->data->values[0] = gravity[0]; + gravity_event->data->values[1] = gravity[1]; + gravity_event->data->values[2] = gravity[2]; + push(gravity_event); + + m_time = event.data->timestamp; + m_x = gravity[0]; + m_y = gravity[1]; + m_z = gravity[2]; + m_accuracy = accuracy; +} - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { - _E("[ROLL_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; +void gravity_sensor::synthesize_lowpass(const sensor_event_t& event) +{ + sensor_event_t *gravity_event; + float x, y, z, norm, alpha, tau, err; + + norm = NORM(event.data->values[0], event.data->values[1], event.data->values[2]); + x = event.data->values[0] / norm * GRAVITY; + y = event.data->values[1] / norm * GRAVITY; + z = event.data->values[2] / norm * GRAVITY; + + if (m_time > 0) { + err = fabs(norm - GRAVITY) / GRAVITY; + tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID); + alpha = tau / (tau + (float)(event.data->timestamp - m_time) / US_PER_SEC); + x = alpha * m_x + (1 - alpha) * x; + y = alpha * m_y + (1 - alpha) * y; + z = alpha * m_z + (1 - alpha) * z; + norm = NORM(x, y, z); + x = x / norm * GRAVITY; + y = y / norm * GRAVITY; + z = z / norm * GRAVITY; } - _I("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); + gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!gravity_event) { + _E("Failed to allocate memory"); + return; + } + gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + if (!gravity_event->data) { + _E("Failed to allocate memory"); + return; + } - m_interval = m_default_sampling_time * MS_TO_US; + gravity_event->sensor_id = get_id(); + gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; + gravity_event->data_length = sizeof(sensor_data_t); + gravity_event->data->accuracy = event.data->accuracy; + gravity_event->data->timestamp = event.data->timestamp; + gravity_event->data->value_count = 3; + gravity_event->data->values[0] = x; + gravity_event->data->values[1] = y; + gravity_event->data->values[2] = z; + push(gravity_event); + + m_time = event.data->timestamp; + m_x = x; + m_y = y; + m_z = z; + m_accuracy = event.data->accuracy; } -gravity_sensor::~gravity_sensor() +void gravity_sensor::synthesize_fusion(const sensor_event_t& event) { - _I("gravity_sensor is destroyed!\n"); + sensor_event_t *gravity_event; + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + fusion_set_accel(event); + m_fusion_phase |= PHASE_ACCEL_READY; + } else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) { + fusion_set_gyro(event); + m_fusion_phase |= PHASE_GYRO_READY; + } + + if (m_fusion_phase != PHASE_FUSION_READY) + return; + + m_fusion_phase = 0; + + fusion_update_angle(); + fusion_get_gravity(); + + gravity_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!gravity_event) { + _E("Failed to allocate memory"); + return; + } + gravity_event->data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + if (!gravity_event->data) { + _E("Failed to allocate memory"); + return; + } + + gravity_event->sensor_id = get_id(); + gravity_event->event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME; + gravity_event->data_length = sizeof(sensor_data_t); + gravity_event->data->accuracy = m_accuracy; + gravity_event->data->timestamp = m_time; + gravity_event->data->value_count = 3; + gravity_event->data->values[0] = m_x; + gravity_event->data->values[1] = m_y; + gravity_event->data->values[2] = m_z; + push(gravity_event); } -bool gravity_sensor::init() +void gravity_sensor::fusion_set_accel(const sensor_event_t& event) { - m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); - m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); - m_magnetic_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); + double x = event.data->values[0]; + double y = event.data->values[1]; + double z = event.data->values[2]; - m_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR); + m_accel_mag = NORM(x, y, z); - if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) { - _E("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x", - m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor); - return false; - } + m_angle_n[0] = ARCTAN(z, y); + m_angle_n[1] = ARCTAN(x, z); + m_angle_n[2] = ARCTAN(y, x); - _I("%s is created!", sensor_base::get_name()); - return true; + m_time_new = event.data->timestamp; + + _D("AccIn: (%f, %f, %f)", x/m_accel_mag, y/m_accel_mag, z/m_accel_mag); } -void gravity_sensor::get_types(vector &types) +void gravity_sensor::fusion_set_gyro(const sensor_event_t& event) { - types.push_back(GRAVITY_SENSOR); + m_velocity[0] = -DEG2RAD(event.data->values[0]); + m_velocity[1] = -DEG2RAD(event.data->values[1]); + m_velocity[2] = -DEG2RAD(event.data->values[2]); + + m_time_new = event.data->timestamp; } -bool gravity_sensor::on_start(void) +void gravity_sensor::fusion_update_angle() { - AUTOLOCK(m_mutex); + _D("AngleIn: (%f, %f, %f)", m_angle_n[0], m_angle_n[1], m_angle_n[2]); + _D("AngAccl: (%f, %f, %f)", m_velocity[0], m_velocity[1], m_velocity[2]); + _D("Angle : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]); + + if (m_angle[0] == INV_ANGLE) { + /* 1st iteration */ + m_angle[0] = m_angle_n[0]; + m_angle[1] = m_angle_n[1]; + m_angle[2] = m_angle_n[2]; + } else { + complementary(m_time_new - m_time); + } - if (!m_hardware_fusion) { - m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_gyro_sensor->start(); - m_magnetic_sensor->add_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_magnetic_sensor->start(); + _D("Angle' : (%f, %f, %f)", m_angle[0], m_angle[1], m_angle[2]); +} + +void gravity_sensor::fusion_get_gravity() +{ + double x = 0, y = 0, z = 0; + double norm; + double vec[3][3]; + + /* Rotating along y-axis then z-axis */ + vec[0][2] = cos(m_angle[1]); + vec[0][0] = sin(m_angle[1]); + vec[0][1] = vec[0][0] * tan(m_angle[2]); + + /* Rotating along z-axis then x-axis */ + vec[1][0] = cos(m_angle[2]); + vec[1][1] = sin(m_angle[2]); + vec[1][2] = vec[1][1] * tan(m_angle[0]); + + /* Rotating along x-axis then y-axis */ + vec[2][1] = cos(m_angle[0]); + vec[2][2] = sin(m_angle[0]); + vec[2][0] = vec[2][2] * tan(m_angle[1]); + + /* Normalize */ + for (int i = 0; i < 3; ++i) { + norm = NORM(vec[i][0], vec[i][1], vec[i][2]); + vec[i][0] /= norm; + vec[i][1] /= norm; + vec[i][2] /= norm; + x += vec[i][0]; + y += vec[i][1]; + z += vec[i][2]; } - m_fusion_sensor->register_supported_event(FUSION_EVENT); - m_fusion_sensor->register_supported_event(FUSION_ORIENTATION_ENABLED); - m_fusion_sensor->add_client(FUSION_EVENT); - m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_fusion_sensor->start(); + norm = NORM(x, y, z); - activate(); - return true; + m_x = x / norm * GRAVITY; + m_y = y / norm * GRAVITY; + m_z = z / norm * GRAVITY; + m_time = m_time_new; } -bool gravity_sensor::on_stop(void) +void gravity_sensor::complementary(unsigned long long time_diff) { - AUTOLOCK(m_mutex); + double err = fabs(m_accel_mag - GRAVITY) / GRAVITY; + double tau = (err < 0.1 ? TAU_LOW : err > 0.9 ? TAU_HIGH : TAU_MID); + double delta_t = (double)time_diff/ US_PER_SEC; + double alpha = tau / (tau + delta_t); - if (!m_hardware_fusion) { - m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->delete_interval((intptr_t)this, false); - m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->delete_interval((intptr_t)this, false); - m_gyro_sensor->stop(); - m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->delete_interval((intptr_t)this, false); - m_magnetic_sensor->stop(); - } + _D("mag, err, tau, dt, alpha = %f, %f, %f, %f, %f", m_accel_mag, err, tau, delta_t, alpha); - m_fusion_sensor->delete_client(FUSION_EVENT); - m_fusion_sensor->delete_interval((intptr_t)this, false); - m_fusion_sensor->unregister_supported_event(FUSION_EVENT); - m_fusion_sensor->unregister_supported_event(FUSION_ORIENTATION_ENABLED); - m_fusion_sensor->stop(); + m_angle[0] = complementary(m_angle[0], m_angle_n[0], m_velocity[0], delta_t, alpha); + m_angle[1] = complementary(m_angle[1], m_angle_n[1], m_velocity[1], delta_t, alpha); + m_angle[2] = complementary(m_angle[2], m_angle_n[2], m_velocity[2], delta_t, alpha); +} - deactivate(); - return true; +double gravity_sensor::complementary(double angle, double angle_in, double vel, double delta_t, double alpha) +{ + double s, c; + angle = angle + vel * delta_t; + s = alpha * sin(angle) + (1 - alpha) * sin(angle_in); + c = alpha * cos(angle) + (1 - alpha) * cos(angle_in); + return ARCTAN(s, c); } -bool gravity_sensor::add_interval(int client_id, unsigned int interval) +int gravity_sensor::get_data(sensor_data_t **data, int *length) { - AUTOLOCK(m_mutex); - if (!m_hardware_fusion) { - m_accel_sensor->add_interval(client_id, interval, false); - m_gyro_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); - } + /* if It is batch sensor, remains can be 2+ */ + int remains = 1; - m_fusion_sensor->add_interval(client_id, interval, false); + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); - return sensor_base::add_interval(client_id, interval, false); + sensor_data->accuracy = m_accuracy; + sensor_data->timestamp = m_time; + sensor_data->value_count = 3; + sensor_data->values[0] = m_x; + sensor_data->values[1] = m_y; + sensor_data->values[2] = m_z; + + *data = sensor_data; + *length = sizeof(sensor_data_t); + + return --remains; } -bool gravity_sensor::delete_interval(int client_id) +bool gravity_sensor::set_batch_latency(unsigned long latency) { - AUTOLOCK(m_mutex); - if (!m_hardware_fusion) { - m_accel_sensor->delete_interval(client_id, false); - m_gyro_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); - } + return false; +} - m_fusion_sensor->delete_interval(client_id, false); +bool gravity_sensor::set_wakeup(int wakeup) +{ + return false; +} + +bool gravity_sensor::on_start(void) +{ + if (m_fusion) + m_fusion->start(); + + if (m_accel_sensor) + m_accel_sensor->start(); - return sensor_base::delete_interval(client_id, false); + if (m_gyro_sensor) + m_gyro_sensor->start(); + + m_time = 0; + m_fusion_phase = 0; + m_angle[0] = INV_ANGLE; + + return activate(); } -void gravity_sensor::synthesize(const sensor_event_t &event, vector &outs) +bool gravity_sensor::on_stop(void) { - sensor_event_t gravity_event; - float pitch, roll, azimuth; - unsigned long long diff_time; - float azimuth_offset; - - if (event.event_type == FUSION_EVENT) { - diff_time = event.data.timestamp - m_time; - - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; - - quaternion quat(event.data.values[0], event.data.values[1], - event.data.values[2], event.data.values[3]); - - euler_angles euler = quat2euler(quat); - - if(m_orientation_data_unit == "DEGREES") { - euler = rad2deg(euler); - azimuth_offset = AZIMUTH_OFFSET_DEGREES; - } - else { - azimuth_offset = AZIMUTH_OFFSET_RADIANS; - } - - euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation; - euler.m_ang.m_vec[1] *= m_roll_rotation_compensation; - euler.m_ang.m_vec[2] *= m_azimuth_rotation_compensation; - - pitch = euler.m_ang.m_vec[0]; - roll = euler.m_ang.m_vec[1]; - if (euler.m_ang.m_vec[2] >= 0) - azimuth = euler.m_ang.m_vec[2]; - else - azimuth = euler.m_ang.m_vec[2] + azimuth_offset; - - if(m_orientation_data_unit == "DEGREES") { - azimuth *= DEG2RAD; - pitch *= DEG2RAD; - roll *= DEG2RAD; - } - - m_time = get_timestamp(); - gravity_event.sensor_id = get_id(); - gravity_event.event_type = GRAVITY_RAW_DATA_EVENT; - - if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || - (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) { - gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth); - gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth); - gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll); - } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) || - (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) { - gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth); - gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth); - gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch); - } else { - gravity_event.data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll); - gravity_event.data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch); - gravity_event.data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch); - } - gravity_event.data.value_count = 3; - gravity_event.data.timestamp = m_time; - gravity_event.data.accuracy = SENSOR_ACCURACY_GOOD; - - push(gravity_event); - } + if (m_fusion) + m_fusion->stop(); + + if (m_accel_sensor) + m_accel_sensor->stop(); - return; + if (m_gyro_sensor) + m_gyro_sensor->stop(); + + m_time = 0; + + return deactivate(); } -int gravity_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +bool gravity_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) { - sensor_data_t fusion_data; - float azimuth_offset; - float pitch, roll, azimuth; + if (m_fusion) + m_fusion->set_interval(FUSION_EVENT_AGM, client_id, interval); - if (event_type != GRAVITY_RAW_DATA_EVENT) - return -1; + if (m_accel_sensor) + m_accel_sensor->add_interval(client_id, interval, true); - m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); + if (m_gyro_sensor) + m_gyro_sensor->add_interval(client_id, interval, true); - quaternion quat(fusion_data.values[0], fusion_data.values[1], - fusion_data.values[2], fusion_data.values[3]); + return sensor_base::add_interval(client_id, interval, is_processor); +} - euler_angles euler = quat2euler(quat); +bool gravity_sensor::delete_interval(int client_id, bool is_processor) +{ + if (m_fusion) + m_fusion->unset_interval(FUSION_EVENT_AGM, client_id); - if(m_orientation_data_unit == "DEGREES") { - euler = rad2deg(euler); - azimuth_offset = AZIMUTH_OFFSET_DEGREES; - } - else { - azimuth_offset = AZIMUTH_OFFSET_RADIANS; - } + if (m_accel_sensor) + m_accel_sensor->delete_interval(client_id, true); - pitch = euler.m_ang.m_vec[0]; - roll = euler.m_ang.m_vec[1]; + if (m_gyro_sensor) + m_gyro_sensor->delete_interval(client_id, true); - if (euler.m_ang.m_vec[2] >= 0) - azimuth = euler.m_ang.m_vec[2]; - else - azimuth = euler.m_ang.m_vec[2] + azimuth_offset; + return sensor_base::delete_interval(client_id, is_processor); +} - if(m_orientation_data_unit == "DEGREES") { - azimuth *= DEG2RAD; - pitch *= DEG2RAD; - roll *= DEG2RAD; - } +bool gravity_sensor::set_interval(unsigned long interval) +{ + m_interval = interval; + return true; +} - data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = get_timestamp(); - if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || - (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) { - data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth); - data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth); - data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll); - } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) || - (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) { - data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth); - data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth); - data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch); - } else { - data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll); - data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch); - data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch); - } - data.value_count = 3; +bool gravity_sensor::rotation_to_gravity(const float *rotation, float *gravity) +{ + float R[9]; - return 0; + if (quat_to_matrix(rotation, R) < 0) + return false; + + gravity[0] = R[6] * GRAVITY; + gravity[1] = R[7] * GRAVITY; + gravity[2] = R[8] * GRAVITY; + + return true; } -bool gravity_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties) +bool gravity_sensor::check_sampling_time(unsigned long long timestamp) { - properties.min_range = -GRAVITY; - properties.max_range = GRAVITY; - properties.resolution = 0.000001; - properties.vendor = m_vendor; - properties.name = SENSOR_NAME; - properties.fifo_count = 0; - properties.max_batch_count = 0; - properties.min_interval = 1; + const float MIN_DELIVERY_DIFF_FACTOR = 0.75f; + const int MS_TO_US = 1000; + long long diff_time; + + diff_time = timestamp - m_time; + + if (m_time && (diff_time < m_interval * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR)) + return false; return true; } diff --git a/src/sensor/gravity/gravity_sensor.h b/src/sensor/gravity/gravity_sensor.h index b5114ee..c9ed7d7 100644 --- a/src/sensor/gravity/gravity_sensor.h +++ b/src/sensor/gravity/gravity_sensor.h @@ -20,52 +20,71 @@ #ifndef _GRAVITY_SENSOR_H_ #define _GRAVITY_SENSOR_H_ -#include #include -#include +#include +#include class gravity_sensor : public virtual_sensor { public: gravity_sensor(); virtual ~gravity_sensor(); - bool init(); - virtual void get_types(std::vector &types); + /* initialize sensor */ + bool init(void); - void synthesize(const sensor_event_t& event, std::vector &outs); + /* sensor info */ + virtual sensor_type_t get_type(void); + virtual unsigned int get_event_type(void); + virtual const char* get_name(void); - bool add_interval(int client_id, unsigned int interval); - bool delete_interval(int client_id); + virtual bool get_sensor_info(sensor_info &info); - int get_sensor_data(const unsigned int event_type, sensor_data_t &data); - virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties); + /* synthesize event */ + virtual void synthesize(const sensor_event_t& event); + + bool add_interval(int client_id, unsigned int interval, bool is_processor); + bool delete_interval(int client_id, bool is_processor); + /* get data */ + virtual int get_data(sensor_data_t **data, int *length); private: + sensor_fusion *m_fusion; sensor_base *m_accel_sensor; sensor_base *m_gyro_sensor; - sensor_base *m_magnetic_sensor; - sensor_base *m_fusion_sensor; - - sensor_data m_accel; - sensor_data m_gyro; - sensor_data m_magnetic; - - cmutex m_value_mutex; + int m_fusion_phase; + float m_x; + float m_y; + float m_z; int m_accuracy; unsigned long long m_time; - unsigned int m_interval; - - std::string m_vendor; - std::string m_raw_data_unit; - std::string m_orientation_data_unit; - int m_default_sampling_time; - int m_gravity_sign_compensation[3]; - int m_azimuth_rotation_compensation; - int m_pitch_rotation_compensation; - int m_roll_rotation_compensation; - - bool on_start(void); - bool on_stop(void); + unsigned long m_interval; + + double m_angle[3]; + double m_angle_n[3]; + double m_accel_mag; + double m_velocity[3]; + unsigned long long m_time_new; + + virtual bool set_interval(unsigned long interval); + virtual bool set_batch_latency(unsigned long latency); + virtual bool set_wakeup(int wakeup); + + virtual bool on_start(void); + virtual bool on_stop(void); + + bool rotation_to_gravity(const float *rotation, float *gravity); + bool check_sampling_time(unsigned long long timestamp); + + void synthesize_rv(const sensor_event_t& event); + void synthesize_lowpass(const sensor_event_t& event); + void synthesize_fusion(const sensor_event_t& event); + + void fusion_set_accel(const sensor_event_t& event); + void fusion_set_gyro(const sensor_event_t& event); + void fusion_update_angle(); + void fusion_get_gravity(); + double complementary(double angle, double angle_in, double vel, double delta_t, double alpha); + void complementary(unsigned long long time_diff); }; #endif /* _GRAVITY_SENSOR_H_ */ diff --git a/src/sensor/hrm/hrm_sensor.cpp b/src/sensor/hrm/hrm_sensor.cpp new file mode 100644 index 0000000..908f1b5 --- /dev/null +++ b/src/sensor/hrm/hrm_sensor.cpp @@ -0,0 +1,34 @@ +/* + * sensord + * + * Copyright (c) 2016 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include "hrm_sensor.h" + +hrm_sensor::hrm_sensor() +{ + set_permission(SENSOR_PERMISSION_BIO); + + _I("hrm_sensor is created : 0x%x", this); +} + +hrm_sensor::~hrm_sensor() +{ + +} diff --git a/src/sensor/hrm/hrm_sensor.h b/src/sensor/hrm/hrm_sensor.h new file mode 100644 index 0000000..1c64bc5 --- /dev/null +++ b/src/sensor/hrm/hrm_sensor.h @@ -0,0 +1,32 @@ +/* + * sensord + * + * Copyright (c) 2016 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef _HRM_SENSOR_H_ +#define _HRM_SENSOR_H_ + +#include + +class hrm_sensor : public physical_sensor { +public: + hrm_sensor(); + virtual ~hrm_sensor(); +}; + +#endif /* _HRM_SENSOR_H_ */ + diff --git a/src/sensor/linear_accel/linear_accel_sensor.cpp b/src/sensor/linear_accel/linear_accel_sensor.cpp index 67095f8..33d1548 100644 --- a/src/sensor/linear_accel/linear_accel_sensor.cpp +++ b/src/sensor/linear_accel/linear_accel_sensor.cpp @@ -25,145 +25,32 @@ #include #include #include + #include +#include + +#include +#include #include #include -#include - -using std::string; -using std::vector; +#include #define SENSOR_NAME "LINEAR_ACCEL_SENSOR" -#define SENSOR_TYPE_LINEAR_ACCEL "LINEAR_ACCEL" -#define SENSOR_TYPE_GRAVITY "GRAVITY" -#define SENSOR_TYPE_ORIENTATION "ORIENTATION" - -#define ELEMENT_NAME "NAME" -#define ELEMENT_VENDOR "VENDOR" -#define ELEMENT_RAW_DATA_UNIT "RAW_DATA_UNIT" -#define ELEMENT_DEFAULT_SAMPLING_TIME "DEFAULT_SAMPLING_TIME" -#define ELEMENT_ACCEL_STATIC_BIAS "ACCEL_STATIC_BIAS" -#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION" -#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE" -#define ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION "LINEAR_ACCEL_SIGN_COMPENSATION" -#define ELEMENT_ORIENTATION_DATA_UNIT "RAW_DATA_UNIT" -#define ELEMENT_GRAVITY_SIGN_COMPENSATION "GRAVITY_SIGN_COMPENSATION" -#define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION" -#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION" -#define ELEMENT_AZIMUTH_ROTATION_COMPENSATION "AZIMUTH_ROTATION_COMPENSATION" - -#define INITIAL_VALUE -1 -#define GRAVITY 9.80665 -#define DEVIATION 0.1 - -#define PI 3.141593 -#define AZIMUTH_OFFSET_DEGREES 360 -#define AZIMUTH_OFFSET_RADIANS (2 * PI) - -#define MS_TO_US 1000 -#define MIN_DELIVERY_DIFF_FACTOR 0.75f -#define ACCELEROMETER_ENABLED 0x01 -#define GRAVITY_ENABLED 0x02 -#define LINEAR_ACCEL_ENABLED 3 +#define GRAVITY 9.80665 linear_accel_sensor::linear_accel_sensor() : m_accel_sensor(NULL) -, m_gyro_sensor(NULL) -, m_magnetic_sensor(NULL) -, m_fusion_sensor(NULL) +, m_gravity_sensor(NULL) +, m_x(0) +, m_y(0) +, m_z(0) +, m_gx(0) +, m_gy(0) +, m_gz(0) +, m_accuracy(0) , m_time(0) { - virtual_sensor_config &config = virtual_sensor_config::get_instance(); - - m_name = string(SENSOR_NAME); - m_enable_linear_accel = 0; - register_supported_event(LINEAR_ACCEL_RAW_DATA_EVENT); - - sensor_hal *fusion_sensor_hal = sensor_loader::get_instance().get_sensor_hal(SENSOR_HAL_TYPE_FUSION); - if (!fusion_sensor_hal) - m_hardware_fusion = false; - else - m_hardware_fusion = true; - - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_VENDOR, m_vendor)) { - _E("[VENDOR] is empty\n"); - throw ENXIO; - } - - _I("m_vendor = %s", m_vendor.c_str()); - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_RAW_DATA_UNIT, m_raw_data_unit)) { - _E("[RAW_DATA_UNIT] is empty\n"); - throw ENXIO; - } - - _I("m_raw_data_unit = %s", m_raw_data_unit.c_str()); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ORIENTATION_DATA_UNIT, m_orientation_data_unit)) { - _E("[ORIENTATION_DATA_UNIT] is empty\n"); - throw ENXIO; - } - - _I("m_orientation_data_unit = %s", m_orientation_data_unit.c_str()); - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_DEFAULT_SAMPLING_TIME, &m_default_sampling_time)) { - _E("[DEFAULT_SAMPLING_TIME] is empty\n"); - throw ENXIO; - } - - _I("m_default_sampling_time = %d", m_default_sampling_time); - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_STATIC_BIAS, m_accel_static_bias, 3)) { - _E("[ACCEL_STATIC_BIAS] is empty\n"); - throw ENXIO; - } - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION, m_accel_rotation_direction_compensation, 3)) { - _E("[ACCEL_ROTATION_DIRECTION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]); - - if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) { - _E("[GRAVITY_SIGN_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_gravity_sign_compensation = (%d, %d, %d)", m_gravity_sign_compensation[0], m_gravity_sign_compensation[1], m_gravity_sign_compensation[2]); - - if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_LINEAR_ACCEL_SIGN_COMPENSATION, m_linear_accel_sign_compensation, 3)) { - _E("[LINEAR_ACCEL_SIGN_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_linear_accel_sign_compensation = (%d, %d, %d)", m_linear_accel_sign_compensation[0], m_linear_accel_sign_compensation[1], m_linear_accel_sign_compensation[2]); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_AZIMUTH_ROTATION_COMPENSATION, &m_azimuth_rotation_compensation)) { - _E("[AZIMUTH_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_azimuth_rotation_compensation = %d", m_azimuth_rotation_compensation); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_PITCH_ROTATION_COMPENSATION, &m_pitch_rotation_compensation)) { - _E("[PITCH_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_pitch_rotation_compensation = %d", m_pitch_rotation_compensation); - - if (!config.get(SENSOR_TYPE_ORIENTATION, ELEMENT_ROLL_ROTATION_COMPENSATION, &m_roll_rotation_compensation)) { - _E("[ROLL_ROTATION_COMPENSATION] is empty\n"); - throw ENXIO; - } - - _I("m_roll_rotation_compensation = %d", m_roll_rotation_compensation); - - m_interval = m_default_sampling_time * MS_TO_US; - } linear_accel_sensor::~linear_accel_sensor() @@ -174,249 +61,177 @@ linear_accel_sensor::~linear_accel_sensor() bool linear_accel_sensor::init() { m_accel_sensor = sensor_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); - m_gyro_sensor = sensor_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); - m_magnetic_sensor = sensor_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); - m_fusion_sensor = sensor_loader::get_instance().get_sensor(FUSION_SENSOR); + if (!m_accel_sensor) { + _E("cannot load accelerometer sensor_hal[%s]", get_name()); + return false; + } + + m_gravity_sensor = sensor_loader::get_instance().get_sensor(GRAVITY_SENSOR); - if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor || !m_fusion_sensor) { - _E("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x, fusion: 0x%x", - m_accel_sensor, m_gyro_sensor, m_magnetic_sensor, m_fusion_sensor); + if (!m_gravity_sensor) { + _E("cannot load gravity sensor_hal[%s]", get_name()); return false; } - _I("%s is created!", sensor_base::get_name()); + _I("%s is created!\n", get_name()); + return true; } -void linear_accel_sensor::get_types(vector &types) +sensor_type_t linear_accel_sensor::get_type(void) { - types.push_back(LINEAR_ACCEL_SENSOR); + return LINEAR_ACCEL_SENSOR; } -bool linear_accel_sensor::on_start(void) +unsigned int linear_accel_sensor::get_event_type(void) { - AUTOLOCK(m_mutex); - - m_accel_sensor->add_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_accel_sensor->start(); - - if (!m_hardware_fusion) { - m_gyro_sensor->add_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_gyro_sensor->start(); - m_magnetic_sensor->add_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_magnetic_sensor->start(); - } - - m_fusion_sensor->register_supported_event(FUSION_EVENT); - m_fusion_sensor->register_supported_event(FUSION_ORIENTATION_ENABLED); - m_fusion_sensor->add_client(FUSION_EVENT); - m_fusion_sensor->add_interval((intptr_t)this, (m_interval/MS_TO_US), false); - m_fusion_sensor->start(); - - activate(); - return true; + return LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME; } -bool linear_accel_sensor::on_stop(void) +const char* linear_accel_sensor::get_name(void) { - AUTOLOCK(m_mutex); - m_accel_sensor->delete_client(ACCELEROMETER_RAW_DATA_EVENT); - m_accel_sensor->delete_interval((intptr_t)this, false); - m_accel_sensor->stop(); - - if (!m_hardware_fusion) { - m_gyro_sensor->delete_client(GYROSCOPE_RAW_DATA_EVENT); - m_gyro_sensor->delete_interval((intptr_t)this, false); - m_gyro_sensor->stop(); - m_magnetic_sensor->delete_client(GEOMAGNETIC_RAW_DATA_EVENT); - m_magnetic_sensor->delete_interval((intptr_t)this, false); - m_magnetic_sensor->stop(); - } + return SENSOR_NAME; +} - m_fusion_sensor->delete_client(FUSION_EVENT); - m_fusion_sensor->delete_interval((intptr_t)this, false); - m_fusion_sensor->unregister_supported_event(FUSION_EVENT); - m_fusion_sensor->unregister_supported_event(FUSION_ORIENTATION_ENABLED); - m_fusion_sensor->stop(); +bool linear_accel_sensor::get_sensor_info(sensor_info &info) +{ + info.set_type(get_type()); + info.set_id(get_id()); + info.set_privilege(SENSOR_PRIVILEGE_PUBLIC); // FIXME + info.set_name("Linear Accelerometer Sensor"); + info.set_vendor("Samsung Electronics"); + info.set_min_range(-19.6); + info.set_max_range(19.6); + info.set_resolution(0.01); + info.set_min_interval(1); + info.set_fifo_count(0); + info.set_max_batch_count(0); + info.set_supported_event(get_event_type()); + info.set_wakeup_supported(false); - deactivate(); return true; } -bool linear_accel_sensor::add_interval(int client_id, unsigned int interval) +void linear_accel_sensor::synthesize(const sensor_event_t& event) { - AUTOLOCK(m_mutex); - m_accel_sensor->add_interval(client_id, interval, false); - - if (!m_hardware_fusion) { - m_gyro_sensor->add_interval(client_id, interval, false); - m_magnetic_sensor->add_interval(client_id, interval, false); - } + if (event.event_type == GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME) { + m_gx = event.data->values[0]; + m_gy = event.data->values[1]; + m_gz = event.data->values[2]; + return; + } + + if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) { + m_time = event.data->timestamp; + m_x = event.data->values[0] - m_gx; + m_y = event.data->values[1] - m_gy; + m_z = event.data->values[2] - m_gz; + + sensor_event_t *linear_accel_event; + sensor_data_t *linear_accel_data; + int data_length; + int remains; + + linear_accel_event = (sensor_event_t *)malloc(sizeof(sensor_event_t)); + if (!linear_accel_event) { + _E("Failed to allocate memory"); + return; + } - m_fusion_sensor->add_interval(client_id, interval, false); + remains = get_data(&linear_accel_data, &data_length); - return sensor_base::add_interval(client_id, interval, false); -} + if (remains < 0) + return; -bool linear_accel_sensor::delete_interval(int client_id) -{ - AUTOLOCK(m_mutex); - m_accel_sensor->delete_interval(client_id, false); + linear_accel_event->sensor_id = get_id(); + linear_accel_event->event_type = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME; + linear_accel_event->data_length = data_length; + linear_accel_event->data = linear_accel_data; - if (!m_hardware_fusion) { - m_gyro_sensor->delete_interval(client_id, false); - m_magnetic_sensor->delete_interval(client_id, false); + push(linear_accel_event); } - - m_fusion_sensor->delete_interval(client_id, false); - - return sensor_base::delete_interval(client_id, false); } -sensor_data_t linear_accel_sensor::calculate_gravity(sensor_data_t data) +int linear_accel_sensor::get_data(sensor_data_t **data, int *length) { - sensor_data_t gravity_data; - float pitch, roll, azimuth; - float azimuth_offset; + /* if It is batch sensor, remains can be 2+ */ + int remains = 1; - quaternion quat(data.values[0], data.values[1], - data.values[2], data.values[3]); + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); - euler_angles euler = quat2euler(quat); - - if(m_orientation_data_unit == "DEGREES") { - euler = rad2deg(euler); - azimuth_offset = AZIMUTH_OFFSET_DEGREES; - } - else { - azimuth_offset = AZIMUTH_OFFSET_RADIANS; - } - - euler.m_ang.m_vec[0] *= m_pitch_rotation_compensation; - euler.m_ang.m_vec[1] *= m_roll_rotation_compensation; - euler.m_ang.m_vec[2] *= m_azimuth_rotation_compensation; - - pitch = euler.m_ang.m_vec[0]; - roll = euler.m_ang.m_vec[1]; - if (euler.m_ang.m_vec[2] >= 0) - azimuth = euler.m_ang.m_vec[2]; - else - azimuth = euler.m_ang.m_vec[2] + azimuth_offset; - - if(m_orientation_data_unit == "DEGREES") { - azimuth *= DEG2RAD; - pitch *= DEG2RAD; - roll *= DEG2RAD; - } + sensor_data->accuracy = SENSOR_ACCURACY_GOOD; + sensor_data->timestamp = m_time; + sensor_data->value_count = 3; + sensor_data->values[0] = m_x; + sensor_data->values[1] = m_y; + sensor_data->values[2] = m_z; + *data = sensor_data; + *length = sizeof(sensor_data_t); - if ((roll >= (M_PI/2)-DEVIATION && roll <= (M_PI/2)+DEVIATION) || - (roll >= -(M_PI/2)-DEVIATION && roll <= -(M_PI/2)+DEVIATION)) { - gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll) * cos(azimuth); - gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(azimuth); - gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll); - } else if ((pitch >= (M_PI/2)-DEVIATION && pitch <= (M_PI/2)+DEVIATION) || - (pitch >= -(M_PI/2)-DEVIATION && pitch <= -(M_PI/2)+DEVIATION)) { - gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(azimuth); - gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch) * cos(azimuth); - gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(pitch); - } else { - gravity_data.values[0] = m_gravity_sign_compensation[0] * GRAVITY * sin(roll); - gravity_data.values[1] = m_gravity_sign_compensation[1] * GRAVITY * sin(pitch); - gravity_data.values[2] = m_gravity_sign_compensation[2] * GRAVITY * cos(roll) * cos(pitch); - } - gravity_data.value_count = 3; - gravity_data.timestamp = m_time; - gravity_data.accuracy = SENSOR_ACCURACY_GOOD; - - return gravity_data; + return --remains; } -void linear_accel_sensor::synthesize(const sensor_event_t &event, vector &outs) +bool linear_accel_sensor::add_interval(int client_id, unsigned int interval, bool is_processor) { - sensor_event_t lin_accel_event; - sensor_data_t gravity_data; + if (m_accel_sensor) + m_accel_sensor->add_interval(client_id, interval, true); - unsigned long long diff_time; + if (m_gravity_sensor) + m_gravity_sensor->add_interval(client_id, interval, true); - if (event.event_type == ACCELEROMETER_RAW_DATA_EVENT) { - diff_time = event.data.timestamp - m_time; + return sensor_base::add_interval(client_id, interval, is_processor); +} - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; +bool linear_accel_sensor::delete_interval(int client_id, bool is_processor) +{ + if (m_accel_sensor) + m_accel_sensor->delete_interval(client_id, true); - m_accel.m_data.m_vec[0] = m_accel_rotation_direction_compensation[0] * (event.data.values[0] - m_accel_static_bias[0]) / ACCEL_SCALE; - m_accel.m_data.m_vec[1] = m_accel_rotation_direction_compensation[1] * (event.data.values[1] - m_accel_static_bias[1]) / ACCEL_SCALE; - m_accel.m_data.m_vec[2] = m_accel_rotation_direction_compensation[2] * (event.data.values[2] - m_accel_static_bias[2]) / ACCEL_SCALE; + if (m_gravity_sensor) + m_gravity_sensor->delete_interval(client_id, true); - m_accel.m_time_stamp = event.data.timestamp; + return sensor_base::delete_interval(client_id, is_processor); +} - m_enable_linear_accel |= ACCELEROMETER_ENABLED; - } - else if (event.event_type == FUSION_EVENT) { - diff_time = event.data.timestamp - m_time; +bool linear_accel_sensor::set_interval(unsigned long interval) +{ + m_interval = interval; + return true; +} - if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR)) - return; +bool linear_accel_sensor::set_batch_latency(unsigned long latency) +{ + return false; +} - gravity_data = calculate_gravity(event.data); +bool linear_accel_sensor::set_wakeup(int wakeup) +{ + return false; +} - m_enable_linear_accel |= GRAVITY_ENABLED; - } +bool linear_accel_sensor::on_start(void) +{ + if (m_accel_sensor) + m_accel_sensor->start(); - if (m_enable_linear_accel == LINEAR_ACCEL_ENABLED) { - m_enable_linear_accel = 0; - - m_time = get_timestamp(); - lin_accel_event.sensor_id = get_id(); - lin_accel_event.event_type = LINEAR_ACCEL_RAW_DATA_EVENT; - lin_accel_event.data.value_count = 3; - lin_accel_event.data.timestamp = m_time; - lin_accel_event.data.accuracy = SENSOR_ACCURACY_GOOD; - lin_accel_event.data.values[0] = m_linear_accel_sign_compensation[0] * (m_accel.m_data.m_vec[0] - gravity_data.values[0]); - lin_accel_event.data.values[1] = m_linear_accel_sign_compensation[1] * (m_accel.m_data.m_vec[1] - gravity_data.values[1]); - lin_accel_event.data.values[2] = m_linear_accel_sign_compensation[2] * (m_accel.m_data.m_vec[2] - gravity_data.values[2]); - push(lin_accel_event); - } + if (m_gravity_sensor) + m_gravity_sensor->start(); - return; + m_time = 0; + return activate(); } -int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data) +bool linear_accel_sensor::on_stop(void) { - sensor_data_t gravity_data, accel_data, fusion_data; - m_fusion_sensor->get_sensor_data(FUSION_ORIENTATION_ENABLED, fusion_data); - m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data); - - gravity_data = calculate_gravity(fusion_data); - - accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / ACCEL_SCALE; - accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / ACCEL_SCALE; - accel_data.values[2] = m_accel_rotation_direction_compensation[2] * (accel_data.values[2] - m_accel_static_bias[2]) / ACCEL_SCALE; - - if (event_type != LINEAR_ACCEL_RAW_DATA_EVENT) - return -1; - - data.accuracy = SENSOR_ACCURACY_GOOD; - data.timestamp = get_timestamp(); - data.values[0] = m_linear_accel_sign_compensation[0] * (accel_data.values[0] - gravity_data.values[0]); - data.values[1] = m_linear_accel_sign_compensation[1] * (accel_data.values[1] - gravity_data.values[1]); - data.values[2] = m_linear_accel_sign_compensation[2] * (accel_data.values[2] - gravity_data.values[2]); - data.value_count = 3; - return 0; -} + if (m_accel_sensor) + m_accel_sensor->stop(); -bool linear_accel_sensor::get_properties(sensor_type_t sensor_type, sensor_properties_s &properties) -{ - m_accel_sensor->get_properties(ACCELEROMETER_SENSOR, properties); - properties.name = "Linear Acceleration Sensor"; - properties.vendor = m_vendor; - properties.resolution = 0.000001; + if (m_gravity_sensor) + m_gravity_sensor->stop(); - return true; + m_time = 0; + return deactivate(); } - diff --git a/src/sensor/linear_accel/linear_accel_sensor.h b/src/sensor/linear_accel/linear_accel_sensor.h index a9219cd..1c2be40 100644 --- a/src/sensor/linear_accel/linear_accel_sensor.h +++ b/src/sensor/linear_accel/linear_accel_sensor.h @@ -20,57 +20,51 @@ #ifndef _LINEAR_ACCEL_SENSOR_H_ #define _LINEAR_ACCEL_SENSOR_H_ -#include #include -#include +#include class linear_accel_sensor : public virtual_sensor { public: linear_accel_sensor(); virtual ~linear_accel_sensor(); - bool init(); - virtual void get_types(std::vector &types); + /* initialize sensor */ + bool init(void); - void synthesize(const sensor_event_t& event, std::vector &outs); + /* sensor info */ + virtual sensor_type_t get_type(void); + virtual unsigned int get_event_type(void); + virtual const char* get_name(void); - bool add_interval(int client_id, unsigned int interval); - bool delete_interval(int client_id); + virtual bool get_sensor_info(sensor_info &info); - int get_sensor_data(const unsigned int event_type, sensor_data_t &data); - virtual bool get_properties(sensor_type_t sensor_type, sensor_properties_s &properties); + /* synthesize event */ + virtual void synthesize(const sensor_event_t& event); + + bool add_interval(int client_id, unsigned int interval, bool is_processor); + bool delete_interval(int client_id, bool is_processor); + /* get data */ + virtual int get_data(sensor_data_t **data, int *length); private: sensor_base *m_accel_sensor; - sensor_base *m_gyro_sensor; - sensor_base *m_magnetic_sensor; - sensor_base *m_fusion_sensor; - - sensor_data m_accel; - sensor_data m_gyro; - sensor_data m_magnetic; - - cmutex m_value_mutex; + sensor_base *m_gravity_sensor; + float m_x; + float m_y; + float m_z; + float m_gx; + float m_gy; + float m_gz; + int m_accuracy; unsigned long long m_time; - unsigned int m_interval; - - unsigned int m_enable_linear_accel; + unsigned long m_interval; - std::string m_vendor; - std::string m_raw_data_unit; - std::string m_orientation_data_unit; - int m_default_sampling_time; - float m_accel_static_bias[3]; - int m_accel_rotation_direction_compensation[3]; - int m_linear_accel_sign_compensation[3]; - int m_gravity_sign_compensation[3]; - int m_azimuth_rotation_compensation; - int m_pitch_rotation_compensation; - int m_roll_rotation_compensation; + virtual bool set_interval(unsigned long interval); + virtual bool set_batch_latency(unsigned long latency); + virtual bool set_wakeup(int wakeup); - bool on_start(void); - bool on_stop(void); - sensor_data_t calculate_gravity(sensor_data_t data); + virtual bool on_start(void); + virtual bool on_stop(void); }; #endif diff --git a/src/server/sensor_fusion.cpp b/src/server/sensor_fusion.cpp new file mode 100644 index 0000000..b6379f1 --- /dev/null +++ b/src/server/sensor_fusion.cpp @@ -0,0 +1,106 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +sensor_fusion::sensor_fusion() +{ + +} + +sensor_fusion::~sensor_fusion() +{ + +} + +bool sensor_fusion::is_fusion(void) +{ + return true; +} + +bool sensor_fusion::is_virtual(void) +{ + return false; +} + +bool sensor_fusion::is_data_ready(void) +{ + return true; +} + +void sensor_fusion::clear_data(void) +{ + return; +} + +bool sensor_fusion::get_rotation_matrix(arr33_t &rot, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_attitude(float &x, float &y, float &z, float &w) +{ + return false; +} + +bool sensor_fusion::get_gyro_bias(float &x, float &y, float &z) +{ + return false; +} + +bool sensor_fusion::get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_linear_acceleration(float &x, float &y, float &z, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_gravity(float &x, float &y, float &z, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w, int &accuracy) +{ + return false; +} + +bool sensor_fusion::get_orientation(float &azimuth, float &pitch, float &roll, int &accuracy) +{ + return false; +} + +bool sensor_fusion::set_interval(unsigned int event_type, int client_id, unsigned int interval) +{ + return sensor_base::add_interval(client_id, interval, true); +} + +bool sensor_fusion::unset_interval(unsigned int event_type, int client_id) +{ + return sensor_base::delete_interval(client_id, true); +} + diff --git a/src/server/sensor_fusion.h b/src/server/sensor_fusion.h new file mode 100644 index 0000000..ec9701d --- /dev/null +++ b/src/server/sensor_fusion.h @@ -0,0 +1,59 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef _SENSOR_FUSION_H_ +#define _SENSOR_FUSION_H_ + +#include +#include + +enum fusion_event_type { + FUSION_EVENT_AG = (FUSION_SENSOR << 16) | 0x0001, + FUSION_EVENT_AM = (FUSION_SENSOR << 16) | 0x0002, + FUSION_EVENT_AGM = (FUSION_SENSOR << 16) | 0x0004, +}; + +typedef std::array ,3> arr33_t; + +class sensor_fusion : public sensor_base { +public: + sensor_fusion(); + virtual ~sensor_fusion(); + + virtual void fuse(const sensor_event_t& event) = 0; + bool is_fusion(void); + bool is_virtual(void); + virtual bool is_data_ready(void); + virtual void clear_data(void); + virtual bool set_interval(unsigned int event_type, int client_id, unsigned int interval); + virtual bool unset_interval(unsigned int event_type, int client_id); + virtual unsigned long long get_data_timestamp(void) = 0; + + virtual bool get_rotation_matrix(arr33_t &rot, int &accuracy); + virtual bool get_attitude(float &x, float &y, float &z, float &w); + virtual bool get_gyro_bias(float &x, float &y, float &z); + virtual bool get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy); + virtual bool get_linear_acceleration(float &x, float &y, float &z, int &accuracy); + virtual bool get_gravity(float &x, float &y, float &z, int &accuracy); + virtual bool get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy, int &accuracy); + virtual bool get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w, int &accuracy); + virtual bool get_orientation(float &azimuth, float &pitch, float &roll, int &accuracy); +}; + +#endif diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index a050df7..dfc8f26 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -29,10 +29,17 @@ #include #include -#include +#include + #ifdef ENABLE_AUTO_ROTATION #include #endif +#ifdef ENABLE_GRAVITY +#include +#endif +#ifdef ENABLE_LINEAR_ACCEL +#include +#endif using std::vector; using std::string; @@ -127,10 +134,24 @@ bool sensor_loader::load_sensor_devices(const string &path, void* &handle) void sensor_loader::create_sensors(void) { - create_physical_sensors(ACCELEROMETER_SENSOR); + /* HRM sensors need SENSOR_PERMISSION_BIO */ + create_physical_sensors(HRM_RAW_SENSOR); + create_physical_sensors(HRM_SENSOR); + create_physical_sensors(HRM_LED_GREEN_SENSOR); + create_physical_sensors(HRM_LED_IR_SENSOR); + create_physical_sensors(HRM_LED_RED_SENSOR); + create_physical_sensors(UNKNOWN_SENSOR); +#ifdef ENABLE_AUTO_ROTATION create_virtual_sensors("Auto Rotation"); +#endif +#ifdef ENABLE_GRAVITY + create_virtual_sensors("Gravity"); +#endif +#ifdef ENABLE_LINEAR_ACCEL + create_virtual_sensors("Linear Accel"); +#endif } template -- 2.7.4 From 0264b170e6524b8fc24754349ba9d0ffbd20ee3f Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Tue, 15 Mar 2016 13:38:08 +0900 Subject: [PATCH 06/16] sensord: version 2.0.3 Change-Id: I611a00ecddc428613d492f4dcde289ea670fa54a Signed-off-by: kibak.yoon --- packaging/sensord.spec | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index fdc6316..38c80a5 100644 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -1,6 +1,6 @@ Name: sensord Summary: Sensor daemon -Version: 2.0.2 +Version: 2.0.3 Release: 0 Group: System/Sensor Framework License: Apache-2.0 -- 2.7.4 From be08985956e1166d95b9e774990fc817f7950b17 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Tue, 15 Mar 2016 18:06:35 +0900 Subject: [PATCH 07/16] sensord: fix build break about using {libdir} macro instead of {unitdir} for systemd path Change-Id: I8848ccc0f373f1390b0d33de54dee58c0a741b95 Signed-off-by: kibak.yoon --- packaging/sensord.spec | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 38c80a5..801cec4 100644 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -89,11 +89,11 @@ make %{?jobs:-j%jobs} rm -rf %{buildroot} %make_install -mkdir -p %{buildroot}%{_libdir}/systemd/system/ +mkdir -p %{buildroot}%{_unitdir} -install -m 0644 %SOURCE1 %{buildroot}%{_libdir}/systemd/system/ -install -m 0644 %SOURCE2 %{buildroot}%{_libdir}/systemd/system/ -install -m 0644 %SOURCE3 %{buildroot}%{_libdir}/systemd/system/ +install -m 0644 %SOURCE1 %{buildroot}%{_unitdir} +install -m 0644 %SOURCE2 %{buildroot}%{_unitdir} +install -m 0644 %SOURCE3 %{buildroot}%{_unitdir} %install_service multi-user.target.wants sensord.service %install_service sockets.target.wants sensord_event.socket -- 2.7.4 From 1a41cb6cc62f87e19660892b663ebb5fa96c45dd Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Thu, 17 Mar 2016 11:32:42 +0900 Subject: [PATCH 08/16] sensord: add AUTO_ROTATION_SENSOR to log Change-Id: Id1d51345cf1f563e2916a355854b7aa4eb968e03 Signed-off-by: kibak.yoon --- src/client/client_common.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/client/client_common.cpp b/src/client/client_common.cpp index 519880b..ae7479c 100644 --- a/src/client/client_common.cpp +++ b/src/client/client_common.cpp @@ -49,6 +49,7 @@ static sensor_type_map g_log_maps = { {GYROSCOPE_RV_SENSOR, {"GYROSCOPE_RV", "GYROSCOPE_RV_RAW_DATA_EVENT"}}, {GEOMAGNETIC_RV_SENSOR, {"GEOMAGNETIC_RV", "GEOMAGNETIC_RV_RAW_DATA_EVENT"}}, {CONTEXT_SENSOR, {"CONTEXT", "CONTEXT_RAW_DATA_EVENT"}}, + {AUTO_ROTATION_SENSOR, {"AUTO_ROTATION", "AUTO_ROTATION_CHANGE_STATE_EVENT"}}, }; const char* get_sensor_name(sensor_id_t sensor_id) -- 2.7.4 From f4cff4540f48f5e973cea4576bad3490b7392933 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Thu, 17 Mar 2016 17:19:42 +0900 Subject: [PATCH 09/16] sensord: [svace] change readdir to readdir_r Change-Id: I3736cc026ac64efd05a9ca0b41f1a60daae3a306 Signed-off-by: kibak.yoon --- src/server/sensor_loader.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index dfc8f26..c65915e 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -270,7 +270,10 @@ void sensor_loader::show_sensor_info(void) bool sensor_loader::get_paths_from_dir(const string &dir_path, vector &hal_paths) { DIR *dir = NULL; - struct dirent *dir_entry = NULL; + struct dirent dir_entry; + struct dirent *result; + string name; + int error; dir = opendir(dir_path.c_str()); @@ -279,10 +282,16 @@ bool sensor_loader::get_paths_from_dir(const string &dir_path, vector &h return false; } - string name; + while (true) { + error = readdir_r(dir, &dir_entry, &result); + + if (error != 0) + continue; + + if (result == NULL) + break; - while ((dir_entry = readdir(dir))) { - name = string(dir_entry->d_name); + name = string(dir_entry.d_name); if (name == "." || name == "..") continue; -- 2.7.4 From 13dfb326d22e16dea9e37c3cd838d54de8ff6741 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Thu, 17 Mar 2016 20:10:27 +0900 Subject: [PATCH 10/16] sensord: [Svace] change strerror to strerror_r - make macro for strerror_r - strerror_r returns error message that it is from buf or static string according to a man page * details "This may be either a pointer to a string that the function stores in buf, or a pointer to some (immutable) static string (ini which case buf is unused)." Change-Id: Ifbca885a211e53d74e29268a25b597ff4d2897af Signed-off-by: kibak.yoon --- src/server/sensor_loader.cpp | 2 +- src/shared/csocket.cpp | 71 +++++++++++++++++++++++++++----------------- src/shared/poller.cpp | 4 +-- src/shared/sensor_log.h | 10 +++++++ 4 files changed, 57 insertions(+), 30 deletions(-) diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index c65915e..9066d95 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -239,7 +239,7 @@ sensor_base* sensor_loader::create_sensor(void) _E("Failed to create sensor, exception: %s", e.what()); return NULL; } catch (int err) { - _E("Failed to create sensor err: %d, cause: %s", err, strerror(err)); + _ERRNO(errno); return NULL; } diff --git a/src/shared/csocket.cpp b/src/shared/csocket.cpp index c6e7998..16cb296 100644 --- a/src/shared/csocket.cpp +++ b/src/shared/csocket.cpp @@ -69,8 +69,8 @@ bool csocket::create(int sock_type) m_sock_fd = socket(AF_UNIX, sock_type, 0); if (!is_valid()) { - _E("Failed to create socket for %s, errno : %d , errstr : %s ", - get_client_name(), errno, strerror(errno)); + _E("Failed to create socket for %s", get_client_name()); + _ERRNO(errno); return false; } @@ -100,14 +100,16 @@ bool csocket::bind (const char *sock_path) length = strlen(m_addr.sun_path) + sizeof(m_addr.sun_family); if (::bind(m_sock_fd, (struct sockaddr *)&m_addr, length) < 0) { - _E("Binding failed for socket(%d), errno : %d , errstr : %s", m_sock_fd, errno, strerror(errno)); + _E("Binding failed for socket(%d)", m_sock_fd); + _ERRNO(errno); close(); return false; } socket_mode = ( S_IRWXU | S_IRWXG | S_IRWXO ); if (chmod(sock_path, socket_mode) < 0) { - _E("chmod failed for socket(%d), errno : %d , errstr : %s", m_sock_fd, errno, strerror(errno)); + _E("chmod failed for socket(%d)", m_sock_fd); + _ERRNO(errno); close(); return false; } @@ -123,7 +125,8 @@ bool csocket::listen(const int max_connections) } if (::listen(m_sock_fd, max_connections) < 0) { - _E("Listening failed for socket(%d), errno : %d , errstr : %s", m_sock_fd, errno, strerror(errno)); + _E("Listening failed for socket(%d)", m_sock_fd); + _ERRNO(errno); close(); return false; } @@ -145,7 +148,8 @@ bool csocket::accept(csocket& client_socket) const } while (err == EINTR); if (!client_socket.is_valid()) { - _E("Accept failed for socket(%d), errno : %d , errstr : %s", m_sock_fd, errno, strerror(errno)); + _E("Accept failed for socket(%d)", m_sock_fd); + _ERRNO(errno); return false; } @@ -162,8 +166,9 @@ ssize_t csocket::send_for_seqpacket(const void *buffer, size_t size) const } while (err == EINTR); if (err) { - _E("send(%d, 0x%x, %d, 0x%x) = %d cause = %s(%d)", - m_sock_fd, buffer, size, m_send_flags, len, strerror(errno), errno); + _E("send(%d, 0x%x, %d, 0x%x) = %d", + m_sock_fd, buffer, size, m_send_flags, len); + _ERRNO(errno); } return err == 0 ? len : -err; @@ -188,14 +193,16 @@ ssize_t csocket::recv_for_seqpacket(void* buffer, size_t size) const } while (err == EINTR); if ((err == EAGAIN) || (err == EWOULDBLOCK)) { - _D("recv(%d, 0x%x, %d, 0x%x) = %d cause = %s(%d)", - m_sock_fd, buffer, size, m_recv_flags, len, strerror(errno), errno); + _D("recv(%d, 0x%x, %d, 0x%x) = %d", + m_sock_fd, buffer, size, m_recv_flags, len); + _ERRNO(errno); return 0; } if (err) { - _E("recv(%d, 0x%x, %d, 0x%x) = %d cause = %s(%d)", - m_sock_fd, buffer, size, m_recv_flags, len, strerror(errno), errno); + _E("recv(%d, 0x%x, %d, 0x%x) = %d", + m_sock_fd, buffer, size, m_recv_flags, len); + _ERRNO(errno); } return err == 0 ? len : -err; @@ -215,9 +222,10 @@ ssize_t csocket::send_for_stream(const void *buffer, size_t size) const total_sent_size += len; err = 0; } else { - _E("send(%d, 0x%p + %d, %d - %d) = %d, error: %s(%d) for %s", + _E("send(%d, 0x%p + %d, %d - %d) = %d for %s", m_sock_fd, buffer, total_sent_size, size, total_sent_size, - len, strerror(errno), errno, get_client_name()); + len, get_client_name()); + _ERRNO(errno); if (errno != EINTR) { err = errno; @@ -246,9 +254,10 @@ ssize_t csocket::recv_for_stream(void* buffer, size_t size) const err = 1; break; } else { - _E("recv(%d, 0x%p + %d, %d - %d) = %d, error: %s(%d) for %s", + _E("recv(%d, 0x%p + %d, %d - %d) = %d for %s", m_sock_fd, buffer, total_recv_size, size, total_recv_size, - len, strerror(errno), errno, get_client_name()); + len, get_client_name()); + _ERRNO(errno); if (errno != EINTR) { err = errno; @@ -301,7 +310,8 @@ bool csocket::connect(const char *sock_path) addr_len = strlen(m_addr.sun_path) + sizeof(m_addr.sun_family); if (::connect(m_sock_fd,(sockaddr *) &m_addr, addr_len) < 0) { - _E("connect error: %s sock_fd: %d\n for %s", strerror(errno), m_sock_fd, get_client_name()); + _E("connect error: sock_fd: %d\n for %s", m_sock_fd, get_client_name()); + _ERRNO(errno); return false; } @@ -315,7 +325,8 @@ bool csocket::connect(const char *sock_path) ret = select(m_sock_fd + 1, NULL, &write_fds, NULL, &tv); if (ret == -1) { - _E("select error: %s sock_fd: %d\n for %s", strerror(errno), m_sock_fd, get_client_name()); + _E("select error: sock_fd: %d\n for %s", m_sock_fd, get_client_name()); + _ERRNO(errno); close(); return false; } else if (!ret) { @@ -334,8 +345,8 @@ bool csocket::connect(const char *sock_path) socklen_t len = sizeof(so_error); if (getsockopt(m_sock_fd, SOL_SOCKET, SO_ERROR, &so_error, &len) == -1) { - _E("getsockopt failed for %s, m_sock_fd : %d, errno : %d , errstr : %s", - get_client_name(), m_sock_fd, errno, strerror(errno)); + _E("getsockopt failed for %s, m_sock_fd : %d", get_client_name(), m_sock_fd); + _ERRNO(errno); close(); return false; } @@ -360,7 +371,8 @@ bool csocket::set_blocking_mode(bool blocking) flags = fcntl(m_sock_fd, F_GETFL); if (flags == -1) { - _E("fcntl(F_GETFL) failed for %s, m_sock_fd: %d, errno : %d , errstr : %s", get_client_name(), m_sock_fd, errno, strerror(errno)); + _E("fcntl(F_GETFL) failed for %s, m_sock_fd: %d", get_client_name(), m_sock_fd); + _ERRNO(errno); return false; } @@ -369,7 +381,8 @@ bool csocket::set_blocking_mode(bool blocking) flags = fcntl(m_sock_fd, F_SETFL, flags); if (flags == -1) { - _E("fcntl(F_SETFL) failed for %s, m_sock_fd: %d, errno : %d , errstr : %s", get_client_name(), m_sock_fd, errno, strerror(errno)); + _E("fcntl(F_SETFL) failed for %s, m_sock_fd: %d", get_client_name(), m_sock_fd); + _ERRNO(errno); return false; } @@ -385,7 +398,9 @@ bool csocket::set_sock_type(void) opt_len = sizeof(sock_type); if (getsockopt(m_sock_fd, SOL_SOCKET, SO_TYPE, &sock_type, &opt_len) < 0) { - _E("getsockopt(SOL_SOCKET, SO_TYPE) failed for %s, m_sock_fd: %d, errno : %d , errstr : %s", get_client_name(), m_sock_fd, errno, strerror(errno)); + _E("getsockopt(SOL_SOCKET, SO_TYPE) failed for %s, m_sock_fd: %d", + get_client_name(), m_sock_fd); + _ERRNO(errno); return false; } @@ -404,8 +419,8 @@ bool csocket::set_connection_mode(void) tv.tv_usec = 0; if(setsockopt(m_sock_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { - _E("Set SO_RCVTIMEO failed for %s, m_sock_fd : %d, errno : %d , errstr : %s", - get_client_name(), m_sock_fd, errno, strerror(errno)); + _E("Set SO_RCVTIMEO failed for %s, m_sock_fd : %d", get_client_name(), m_sock_fd); + _ERRNO(errno); close(); return false; } @@ -434,7 +449,8 @@ bool csocket::is_blocking_mode(void) flags = fcntl(m_sock_fd, F_GETFL); if (flags == -1) { - _E("fcntl(F_GETFL) failed for %s, m_sock_fd: %d, errno : %d , errstr : %s", get_client_name(), m_sock_fd, errno, strerror(errno)); + _E("fcntl(F_GETFL) failed for %s, m_sock_fd: %d", get_client_name(), m_sock_fd); + _ERRNO(errno); return false; } @@ -456,7 +472,8 @@ bool csocket::close(void) { if (m_sock_fd >= 0) { if (::close(m_sock_fd) < 0) { - _E("Socket(%d) close failed, errno : %d , errstr : %s", m_sock_fd, errno, strerror(errno)); + _E("Socket(%d) close failed", m_sock_fd); + _ERRNO(errno); return false; } m_sock_fd = -1; diff --git a/src/shared/poller.cpp b/src/shared/poller.cpp index 6676632..ae01c8b 100644 --- a/src/shared/poller.cpp +++ b/src/shared/poller.cpp @@ -55,7 +55,7 @@ bool poller::add_fd(int fd) event.events = EPOLLIN | EPOLLERR | EPOLLHUP; if (epoll_ctl(m_epfd, EPOLL_CTL_ADD, fd, &event)) { - _E("errno : %d , errstr : %s", errno, strerror(errno)); + _ERRNO(errno); return false; } @@ -73,7 +73,7 @@ bool poller::fill_event_queue(void) if (errno == EINTR) return true; - _E("Epoll failed errrno : %d , errstr : %s", errno, strerror(errno)); + _ERRNO(errno); return false; } diff --git a/src/shared/sensor_log.h b/src/shared/sensor_log.h index be30df0..d4d7a06 100644 --- a/src/shared/sensor_log.h +++ b/src/shared/sensor_log.h @@ -49,6 +49,16 @@ #define _I INFO #define _D DBG +#define _ERRNO(errno) do { \ + char buf[1024]; \ + char *error = strerror_r(errno, buf, 1024); \ + if (!error) { \ + _E("Failed to strerror_r()"); \ + break; \ + } \ + _E("%s : [errno: %d]", error, errno); \ + } while (0) + #if defined(_DEBUG) # define warn_if(expr, fmt, arg...) do { \ if(expr) { \ -- 2.7.4 From c884b354521646e7e2113109a14f7b0930469292 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Fri, 18 Mar 2016 13:12:38 +0900 Subject: [PATCH 11/16] sensord: change return value from bool to int Change-Id: I8b6cc10e0c8ed9f126d58f3097545e64fd655f16 Signed-off-by: kibak.yoon --- src/client/client.cpp | 32 ++++++++++++++++---------------- src/client/sensor_internal.h | 14 ++++++++++---- 2 files changed, 26 insertions(+), 20 deletions(-) diff --git a/src/client/client.cpp b/src/client/client.cpp index fcd4c0c..56e7db9 100644 --- a/src/client/client.cpp +++ b/src/client/client.cpp @@ -1000,7 +1000,7 @@ API bool sensord_set_option(int handle, int option) } -API bool sensord_set_attribute_int(int handle, int attribute, int value) +API int sensord_set_attribute_int(int handle, int attribute, int value) { sensor_id_t sensor_id; command_channel *cmd_channel; @@ -1010,29 +1010,29 @@ API bool sensord_set_attribute_int(int handle, int attribute, int value) if (!sensor_client_info::get_instance().get_sensor_id(handle, sensor_id)) { _E("client %s failed to get handle information", get_client_name()); - return false; + return -EINVAL; } if (!sensor_client_info::get_instance().get_command_channel(sensor_id, &cmd_channel)) { _E("client %s failed to get command channel for %s", get_client_name(), get_sensor_name(sensor_id)); - return false; + return -EPERM; } client_id = sensor_client_info::get_instance().get_client_id(); - retvm_if ((client_id < 0), false, + retvm_if ((client_id < 0), -EPERM, "Invalid client id : %d, handle: %d, %s, %s", client_id, handle, get_sensor_name(sensor_id), get_client_name()); if (!cmd_channel->cmd_set_attribute_int(attribute, value)) { _E("Sending cmd_set_attribute_int(%d, %d) failed for %s", client_id, value, get_client_name); - return false; + return -EPERM; } - return true; + return OP_SUCCESS; } -API bool sensord_set_attribute_str(int handle, int attribute, const char *value, int value_len) +API int sensord_set_attribute_str(int handle, int attribute, const char *value, int value_len) { sensor_id_t sensor_id; command_channel *cmd_channel; @@ -1042,41 +1042,41 @@ API bool sensord_set_attribute_str(int handle, int attribute, const char *value, if (!sensor_client_info::get_instance().get_sensor_id(handle, sensor_id)) { _E("Client %s failed to get handle information", get_client_name()); - return false; + return -EINVAL; } if (!sensor_client_info::get_instance().get_command_channel(sensor_id, &cmd_channel)) { _E("Client %s failed to get command channel for %s", get_client_name(), get_sensor_name(sensor_id)); - return false; + return -EPERM; } - retvm_if((value_len < 0) || (value == NULL), false, + retvm_if((value_len < 0) || (value == NULL), -EINVAL, "Invalid value_len: %d, value: 0x%x, handle: %d, %s, %s", value_len, value, handle, get_sensor_name(sensor_id), get_client_name()); client_id = sensor_client_info::get_instance().get_client_id(); - retvm_if ((client_id < 0), false, + retvm_if ((client_id < 0), -EPERM, "Invalid client id : %d, handle: %d, %s, %s", client_id, handle, get_sensor_name(sensor_id), get_client_name()); if (!cmd_channel->cmd_set_attribute_str(attribute, value, value_len)) { _E("Sending cmd_set_attribute_str(%d, %d, 0x%x) failed for %s", client_id, value_len, value, get_client_name); - return false; + return -EPERM; } - return true; + return OP_SUCCESS; } API bool sensord_send_sensorhub_data(int handle, const char *data, int data_len) { - return sensord_set_attribute_str(handle, 0, data, data_len); + return (sensord_set_attribute_str(handle, 0, data, data_len) == OP_SUCCESS); } API bool sensord_send_command(int handle, const char *command, int command_len) { - return sensord_set_attribute_str(handle, 0, command, command_len); + return (sensord_set_attribute_str(handle, 0, command, command_len) == OP_SUCCESS); } API bool sensord_get_data(int handle, unsigned int data_id, sensor_data_t* sensor_data) @@ -1151,4 +1151,4 @@ API bool sensord_flush(int handle) } return true; -} \ No newline at end of file +} diff --git a/src/client/sensor_internal.h b/src/client/sensor_internal.h index cd1db11..2c6d56f 100644 --- a/src/client/sensor_internal.h +++ b/src/client/sensor_internal.h @@ -318,9 +318,12 @@ bool sensord_set_option(int handle, int option); * @param[in] handle a handle represensting a connected sensor. * @param[in] attribute an attribute to change * @param[in] value an attribute value - * @return true on success, otherwise false. + * @return 0 on success, otherwise a negative error value + * @retval 0 Successful + * @retval -EINVAL Invalid parameter + * @retval -EPERM Operation not permitted */ -bool sensord_set_attribute_int(int handle, int attribute, int value); +int sensord_set_attribute_int(int handle, int attribute, int value); /** * @brief Set the attribute to a connected sensor @@ -329,9 +332,12 @@ bool sensord_set_attribute_int(int handle, int attribute, int value); * @param[in] attribute an attribute to change * @param[in] value an attribute value * @param[in] value_len the length of value - * @return true on success, otherwise false. + * @return 0 on success, otherwise a negative error value + * @retval 0 Successful + * @retval -EINVAL Invalid parameter + * @retval -EPERM Operation not permitted */ -bool sensord_set_attribute_str(int handle, int attribute, const char *value, int value_len); +int sensord_set_attribute_str(int handle, int attribute, const char *value, int value_len); /** * @brief Send data to sensorhub -- 2.7.4 From 4abc6bf5e745d908dfbfa74c46d28ef32bbb4651 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Fri, 18 Mar 2016 20:09:54 +0900 Subject: [PATCH 12/16] sensord: add the checker whether m_sock_fd is valid or not Change-Id: I9b8fe4cd7825607197e618e1eede25036b1223cd Signed-off-by: kibak.yoon --- src/shared/csocket.cpp | 37 ++++++++++++++++++++++++++++++++----- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/src/shared/csocket.cpp b/src/shared/csocket.cpp index 16cb296..451354a 100644 --- a/src/shared/csocket.cpp +++ b/src/shared/csocket.cpp @@ -141,10 +141,8 @@ bool csocket::accept(csocket& client_socket) const do { client_socket.m_sock_fd = ::accept(m_sock_fd, (sockaddr *)&m_addr, (socklen_t *)&addr_length); - if (!client_socket.is_valid()) { + if (!client_socket.is_valid()) err = errno; - ::close(client_socket.m_sock_fd); - } } while (err == EINTR); if (!client_socket.is_valid()) { @@ -176,7 +174,7 @@ ssize_t csocket::send_for_seqpacket(const void *buffer, size_t size) const ssize_t csocket::recv_for_seqpacket(void* buffer, size_t size) const { - ssize_t err, len; + ssize_t err, len; do { len = ::recv(m_sock_fd, buffer, size, m_recv_flags); @@ -272,6 +270,11 @@ ssize_t csocket::recv_for_stream(void* buffer, size_t size) const ssize_t csocket::send(const void *buffer, size_t size) const { + if (!is_valid()) { + _E("Socket(%d) is invalid", m_sock_fd); + return -EINVAL; + } + if (m_sock_type == SOCK_STREAM) return send_for_stream(buffer, size); @@ -280,6 +283,11 @@ ssize_t csocket::send(const void *buffer, size_t size) const ssize_t csocket::recv(void* buffer, size_t size) const { + if (!is_valid()) { + _E("Socket(%d) is invalid", m_sock_fd); + return -EINVAL; + } + if (m_sock_type == SOCK_STREAM) return recv_for_stream(buffer, size); @@ -368,6 +376,11 @@ bool csocket::set_blocking_mode(bool blocking) { int flags; + if (!is_valid()) { + _E("Socket(%d) is invalid", m_sock_fd); + return false; + } + flags = fcntl(m_sock_fd, F_GETFL); if (flags == -1) { @@ -397,6 +410,11 @@ bool csocket::set_sock_type(void) opt_len = sizeof(sock_type); + if (!is_valid()) { + _E("Socket(%d) is invalid", m_sock_fd); + return false; + } + if (getsockopt(m_sock_fd, SOL_SOCKET, SO_TYPE, &sock_type, &opt_len) < 0) { _E("getsockopt(SOL_SOCKET, SO_TYPE) failed for %s, m_sock_fd: %d", get_client_name(), m_sock_fd); @@ -418,6 +436,11 @@ bool csocket::set_connection_mode(void) tv.tv_sec = TIMEOUT; tv.tv_usec = 0; + if (!is_valid()) { + _E("Socket(%d) is invalid", m_sock_fd); + return false; + } + if(setsockopt(m_sock_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { _E("Set SO_RCVTIMEO failed for %s, m_sock_fd : %d", get_client_name(), m_sock_fd); _ERRNO(errno); @@ -435,7 +458,6 @@ bool csocket::set_transfer_mode(void) { set_blocking_mode(false); - m_send_flags = MSG_DONTWAIT | MSG_NOSIGNAL; m_recv_flags = MSG_DONTWAIT | MSG_NOSIGNAL; @@ -446,6 +468,11 @@ bool csocket::is_blocking_mode(void) { int flags; + if (!is_valid()) { + _E("Socket(%d) is invalid", m_sock_fd); + return false; + } + flags = fcntl(m_sock_fd, F_GETFL); if (flags == -1) { -- 2.7.4 From ff531922d4a0068c8563fdf5fd9a4c4c10dbef25 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Thu, 24 Mar 2016 17:25:12 +0900 Subject: [PATCH 13/16] sensord: remove the error log that is not error actually Change-Id: Ie4d83fbf60eb4d348164b2e7498855bc3eb1bbd1 Signed-off-by: kibak.yoon --- src/shared/csocket.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/shared/csocket.cpp b/src/shared/csocket.cpp index 451354a..9c33c5e 100644 --- a/src/shared/csocket.cpp +++ b/src/shared/csocket.cpp @@ -190,12 +190,8 @@ ssize_t csocket::recv_for_seqpacket(void* buffer, size_t size) const } } while (err == EINTR); - if ((err == EAGAIN) || (err == EWOULDBLOCK)) { - _D("recv(%d, 0x%x, %d, 0x%x) = %d", - m_sock_fd, buffer, size, m_recv_flags, len); - _ERRNO(errno); + if ((err == EAGAIN) || (err == EWOULDBLOCK)) return 0; - } if (err) { _E("recv(%d, 0x%x, %d, 0x%x) = %d", -- 2.7.4 From bc45faf4fdb5a5cfdb4256c96866d1e663aab03b Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Fri, 25 Mar 2016 01:48:20 +0900 Subject: [PATCH 14/16] sensord: send permission-needed sensor information to client * client needs to know whether the error is operation_failed or permission_denied. therefore server has to send all of sensor list to client. * if it is previleged sensor, server sends the type information only. Change-Id: I2563d457b0aa039ebbc3f83121aa47b18dfa06bc Signed-off-by: kibak.yoon --- src/server/command_worker.cpp | 97 ++++++++++++++++++++----------------------- src/server/command_worker.h | 2 +- 2 files changed, 46 insertions(+), 53 deletions(-) diff --git a/src/server/command_worker.cpp b/src/server/command_worker.cpp index 1f87342..42262ea 100644 --- a/src/server/command_worker.cpp +++ b/src/server/command_worker.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -50,7 +51,6 @@ command_worker::command_worker(const csocket& socket) if (!init) { init_cmd_handlers(); - make_sensor_raw_data_map(); init = true; } @@ -90,30 +90,57 @@ void command_worker::init_cmd_handlers(void) m_cmd_handlers[CMD_FLUSH] = &command_worker::cmd_flush; } -void command_worker::get_sensor_list(int permissions, cpacket &sensor_list) +int command_worker::create_sensor_raw_list(int client_perms, std::vector &raw_list) { - const int PERMISSION_COUNT = sizeof(permissions) * 8; - vector sensor_raw_vec; size_t total_raw_data_size = 0; + vector sensors; + vector types; + sensor_info info; + int permission; - for (int i = 0; i < PERMISSION_COUNT; ++i) { - int perm = (permissions & (1 << i)); + types = sensor_loader::get_instance().get_sensor_types(); - if (perm) { - auto range = m_sensor_raw_data_map.equal_range(perm); + for (auto it_type = types.begin(); it_type != types.end(); ++it_type) { + sensor_type_t type = *it_type; + sensors = sensor_loader::get_instance().get_sensors(type); - sensor_raw_data_map::iterator it_raw_data; + for (auto it_sensor = sensors.begin(); it_sensor != sensors.end(); ++it_sensor) { + (*it_sensor)->get_sensor_info(info); + permission = (*it_sensor)->get_permission(); + permission = (client_perms & permission); - for (it_raw_data = range.first; it_raw_data != range.second; ++it_raw_data) { - total_raw_data_size += it_raw_data->second.size(); - sensor_raw_vec.push_back(&(it_raw_data->second)); + if (!permission) { + info.clear(); + info.set_id((sensor_id_t)-EACCES); + info.set_type(type); } + + raw_data_t *raw_data = new(std::nothrow) raw_data_t(); + retvm_if(!raw_data, -1, "Failed to allocated memory"); + info.get_raw_data(*raw_data); + + total_raw_data_size += raw_data->size(); + raw_list.push_back(raw_data); + + info.clear(); } } + return total_raw_data_size; +} + +void command_worker::get_sensor_list(int client_perms, cpacket &sensor_list) +{ + size_t total_raw_data_size = 0; + vector raw_list; int sensor_cnt; + int idx = 0; + + total_raw_data_size = create_sensor_raw_list(client_perms, raw_list); + if (total_raw_data_size < 0) + return; - sensor_cnt = sensor_raw_vec.size(); + sensor_cnt = raw_list.size(); sensor_list.set_payload_size(sizeof(cmd_get_sensor_list_done_t) + (sizeof(size_t) * sensor_cnt) + total_raw_data_size); sensor_list.set_cmd(CMD_GET_SENSOR_LIST); @@ -124,52 +151,18 @@ void command_worker::get_sensor_list(int permissions, cpacket &sensor_list) cmd_get_sensor_list_done->sensor_cnt = sensor_cnt; size_t* size_field = (size_t *) cmd_get_sensor_list_done->data; - for (int i = 0; i < sensor_cnt; ++i) - size_field[i] = sensor_raw_vec[i]->size(); + size_field[i] = raw_list[i]->size(); char* raw_data_field = cmd_get_sensor_list_done->data + (sizeof(size_t) * sensor_cnt); - int idx = 0; for (int i = 0; i < sensor_cnt; ++i) { - copy(sensor_raw_vec[i]->begin(), sensor_raw_vec[i]->end(), raw_data_field + idx); - idx += sensor_raw_vec[i]->size(); + copy(raw_list[i]->begin(), raw_list[i]->end(), raw_data_field + idx); + idx += raw_list[i]->size(); } -} - -void command_worker::make_sensor_raw_data_map(void) -{ - vector sensors; - vector types; - std::vector::iterator it_type; - std::vector::iterator it_sensor; - sensor_info info; - int permission; - - types = sensor_loader::get_instance().get_sensor_types(); - - it_type = types.begin(); - while (it_type != types.end()) { - sensor_type_t type; - type = *it_type; - - sensors = sensor_loader::get_instance().get_sensors(type); - it_sensor = sensors.begin(); - - while (it_sensor != sensors.end()) { - (*it_sensor)->get_sensor_info(info); - permission = (*it_sensor)->get_permission(); - - sensor_raw_data_map::iterator it_sensor_raw_data; - it_sensor_raw_data = m_sensor_raw_data_map.insert(std::make_pair(permission, raw_data_t())); - - info.get_raw_data(it_sensor_raw_data->second); - info.clear(); - ++it_sensor; - } - ++it_type; - } + for (auto it = raw_list.begin(); it != raw_list.end(); ++it) + delete *it; } bool command_worker::working(void *ctx) diff --git a/src/server/command_worker.h b/src/server/command_worker.h index 3e01883..cd5c336 100644 --- a/src/server/command_worker.h +++ b/src/server/command_worker.h @@ -45,7 +45,7 @@ private: static sensor_raw_data_map m_sensor_raw_data_map; static void init_cmd_handlers(void); - static void make_sensor_raw_data_map(void); + static int create_sensor_raw_list(int client_perms, std::vector &raw_list); static void get_sensor_list(int permissions, cpacket &sensor_list); static bool working(void *ctx); -- 2.7.4 From 8ffb2378d77c4bbe495cdd24f48838ed076d97e6 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Fri, 25 Mar 2016 01:57:05 +0900 Subject: [PATCH 15/16] sensord: add get_sensor_*_ex internal APIs for getting error properly * if clients uses sensord_get_sensor/sensord_get_sensor_list APIs to get sensor handle(s), they cannot know the error exactly. because APIs return only bool or handle itself, not including error info. Change-Id: I83e9c28217621a5a6ab3c5e35dd45c84a5056cea Signed-off-by: kibak.yoon --- src/client/client.cpp | 48 +++++++++++++++++++++++++++++++++++--------- src/client/sensor_internal.h | 26 ++++++++++++++++++++++++ 2 files changed, 65 insertions(+), 9 deletions(-) diff --git a/src/client/client.cpp b/src/client/client.cpp index 56e7db9..8bbf2fb 100644 --- a/src/client/client.cpp +++ b/src/client/client.cpp @@ -334,34 +334,64 @@ static bool get_sensor_list(void) return true; } -API bool sensord_get_sensor_list(sensor_type_t type, sensor_t **list, int *sensor_count) +API int sensord_get_sensor_list_ex(sensor_type_t type, sensor_t **list, int *sensor_count) { - retvm_if (!get_sensor_list(), false, "Fail to get sensor list from server"); + retvm_if (!get_sensor_list(), -EPERM, "Fail to get sensor list from server"); vector sensor_infos = sensor_info_manager::get_instance().get_infos(type); - if (!sensor_infos.empty()) { - *list = (sensor_t *) malloc(sizeof(sensor_info *) * sensor_infos.size()); - retvm_if(!*list, false, "Failed to allocate memory"); + if (sensor_infos.empty()) { + *sensor_count = 0; + return -ENODATA; + } + + if (type != ALL_SENSOR) { + if (sensor_infos[0]->get_id() == (sensor_id_t)(-EACCES)) + return -EACCES; } + *list = (sensor_t *) malloc(sizeof(sensor_info *) * sensor_infos.size()); + retvm_if(!*list, false, "Failed to allocate memory"); + for (unsigned int i = 0; i < sensor_infos.size(); ++i) *(*list + i) = sensor_info_to_sensor(sensor_infos[i]); *sensor_count = sensor_infos.size(); - return true; + return OP_SUCCESS; } -API sensor_t sensord_get_sensor(sensor_type_t type) +API int sensord_get_sensor_ex(sensor_type_t type, sensor_t *sensor) { - retvm_if (!get_sensor_list(), NULL, "Fail to get sensor list from server"); + retvm_if (!get_sensor_list(), -EPERM, "Fail to get sensor list from server"); const sensor_info *info; info = sensor_info_manager::get_instance().get_info(type); + if (info == NULL) { + *sensor = NULL; + return -ENODATA; + } + + if ((const_cast(info))->get_id() == (sensor_id_t)(-EACCES)) + return -EACCES; + + *sensor = sensor_info_to_sensor(info); + + return OP_SUCCESS; +} + +API bool sensord_get_sensor_list(sensor_type_t type, sensor_t **list, int *sensor_count) +{ + return (sensord_get_sensor_list_ex(type, list, sensor_count) == OP_SUCCESS); +} + +API sensor_t sensord_get_sensor(sensor_type_t type) +{ + sensor_t sensor; + sensord_get_sensor_ex(type, &sensor); - return sensor_info_to_sensor(info); + return sensor; } API bool sensord_get_type(sensor_t sensor, sensor_type_t *type) diff --git a/src/client/sensor_internal.h b/src/client/sensor_internal.h index 2c6d56f..86a1b00 100644 --- a/src/client/sensor_internal.h +++ b/src/client/sensor_internal.h @@ -62,6 +62,32 @@ bool sensord_get_sensor_list(sensor_type_t type, sensor_t **list, int *sensor_co sensor_t sensord_get_sensor(sensor_type_t type); /** + * @brief Get the list of available sensors of a certain type, use ALL_SENSOR to get all the sensors. + * + * @param[in] type the type of sensors requested. + * @param[out] list the list of sensors matching the asked type, the caller should explicitly free this list. + * @param[out] sensor count the count of sensors contained in the list. + * @return 0 on success, otherwise a negative error value + * @retval 0 Successful + * @retval -EPERM Operation not permitted + * @retval -EACCES Permission denied + * @retval -ENODATA NO sensor available + */ +int sensord_get_sensor_list_ex(sensor_type_t type, sensor_t **list, int *sensor_count); + +/** + * @brief Get the default sensor for a given type. + * + * @param[in] type the type of a sensor requested. + * @param[out] a sensor matching the asked type. + * @return 0 on success, otherwise a negative error value + * @retval 0 Successful + * @retval -EPERM Operation not permitted + * @retval -EACCES Permission denied + */ +int sensord_get_sensor_ex(sensor_type_t type, sensor_t *sensor); + +/** * @brief Get the type of this sensor. * * @param[in] sensor a sensor to get type. -- 2.7.4 From 09cfeddc85ad5714fb6a4c33dee5347bac8d28e3 Mon Sep 17 00:00:00 2001 From: "kibak.yoon" Date: Fri, 25 Mar 2016 17:36:26 +0900 Subject: [PATCH 16/16] sensord: remove the unnecessary checking code Change-Id: Icc7c48acc0421bbd77b4f6f34bb3097088a3fc9d Signed-off-by: kibak.yoon --- src/server/command_worker.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/server/command_worker.cpp b/src/server/command_worker.cpp index 42262ea..d8a48e3 100644 --- a/src/server/command_worker.cpp +++ b/src/server/command_worker.cpp @@ -137,8 +137,6 @@ void command_worker::get_sensor_list(int client_perms, cpacket &sensor_list) int idx = 0; total_raw_data_size = create_sensor_raw_list(client_perms, raw_list); - if (total_raw_data_size < 0) - return; sensor_cnt = raw_list.size(); -- 2.7.4