From e79399daaeb89464053807b8a80390305b3dedec Mon Sep 17 00:00:00 2001 From: Amit Dharmapurikar Date: Wed, 2 Jul 2014 12:34:57 +0530 Subject: [PATCH 01/16] Modified gyro_sensor_hal plugin for IIO driver compatibility - The gyro_sensor_hal plugin is modified as per gyroscope IIO driver interface - The gyro sensor is enabled in the sensord.spec file Signed-off-by: Amit Dharmapurikar Change-Id: I1d62cece29b0305cea76d984dfbb03b84a36c40c --- packaging/sensord.spec | 2 +- src/gyro/gyro_sensor_hal.cpp | 570 ++++++++++++++++++++++--------------------- src/gyro/gyro_sensor_hal.h | 59 ++++- 3 files changed, 354 insertions(+), 277 deletions(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 3c657d9..d6ac9c2 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -9,7 +9,7 @@ Source1: sensord.service Source2: sensord.socket %define accel_state ON -%define gyro_state OFF +%define gyro_state ON %define proxi_state OFF %define light_state OFF %define geo_state OFF diff --git a/src/gyro/gyro_sensor_hal.cpp b/src/gyro/gyro_sensor_hal.cpp index 0b94ae7..1dda6b9 100755 --- a/src/gyro/gyro_sensor_hal.cpp +++ b/src/gyro/gyro_sensor_hal.cpp @@ -25,20 +25,12 @@ #include #include #include +#include +#include using std::ifstream; using config::CConfig; -#define NODE_NAME "name" -#define NODE_INPUT "input" -#define NODE_ENABLE "enable" -#define NODE_POLL_DELAY "poll_delay" -#define NODE_ACCEL_POLL_DELAY "gyro_poll_delay" -#define SENSOR_NODE "/sys/class/sensors/" -#define SENSORHUB_NODE "/sys/class/sensors/ssp_sensor/" -#define INPUT_DEVICE_NODE "/sys/class/input/" -#define DEV_INPUT_NODE "/dev/input/event/" - #define INITIAL_VALUE -1 #define INITIAL_TIME 0 #define DPS_TO_MDPS 1000 @@ -46,6 +38,9 @@ using config::CConfig; #define MAX_RANGE(RES) (((2 << (RES))/2)-1) #define RAW_DATA_TO_DPS_UNIT(X) ((float)(X)/((float)DPS_TO_MDPS)) +#define SEC_MSEC 1000 +#define MSEC_TO_FREQ(VAL) (int)((SEC_MSEC) / (VAL)) + #define SENSOR_TYPE_GYRO "GYRO" #define ELEMENT_NAME "NAME" #define ELEMENT_VENDOR "VENDOR" @@ -53,32 +48,37 @@ using config::CConfig; #define ELEMENT_RESOLUTION "RESOLUTION" #define ATTR_VALUE "value" -#define INPUT_NAME "gyro_sensor" +#define ENABLE_VAL 1 +#define DISABLE_VAL 0 +#define DEV_DIR "/dev/" +#define TRIG_PATH "trigger/current_trigger" gyro_sensor_hal::gyro_sensor_hal() : m_x(INITIAL_VALUE) , m_y(INITIAL_VALUE) , m_z(INITIAL_VALUE) -, m_node_handle(INITIAL_VALUE) , m_polling_interval(POLL_1HZ_MS) , m_fired_time(INITIAL_TIME) , m_sensorhub_supported(false) { - if (!check_hw_node()) { + if (!check_hw_node()) + { ERR("check_hw_node() fail"); throw ENXIO; } CConfig &config = CConfig::get_instance(); - if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_VENDOR, m_vendor)) { + if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_VENDOR, m_vendor)) + { ERR("[VENDOR] is empty"); throw ENXIO; } INFO("m_vendor = %s", m_vendor.c_str()); - if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_NAME, m_chip_name)) { + if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_NAME, m_chip_name)) + { ERR("[NAME] is empty"); throw ENXIO; } @@ -87,7 +87,8 @@ gyro_sensor_hal::gyro_sensor_hal() long resolution; - if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_RESOLUTION, resolution)) { + if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_RESOLUTION, resolution)) + { ERR("[RESOLUTION] is empty"); throw ENXIO; } @@ -97,7 +98,8 @@ gyro_sensor_hal::gyro_sensor_hal() double raw_data_unit; - if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_RAW_DATA_UNIT, raw_data_unit)) { + if (!config.get(SENSOR_TYPE_GYRO, m_model_id, ELEMENT_RAW_DATA_UNIT, raw_data_unit)) + { ERR("[RAW_DATA_UNIT] is empty"); throw ENXIO; } @@ -106,25 +108,16 @@ gyro_sensor_hal::gyro_sensor_hal() INFO("m_raw_data_unit = %f", m_raw_data_unit); INFO("RAW_DATA_TO_DPS_UNIT(m_raw_data_unit) = [%f]", RAW_DATA_TO_DPS_UNIT(m_raw_data_unit)); - if ((m_node_handle = open(m_resource.c_str(), O_RDWR)) < 0) { - ERR("Failed to open handle(%d)", m_node_handle); - throw ENXIO; - } - - int clockId = CLOCK_MONOTONIC; - - if (ioctl(m_node_handle, EVIOCSCLOCKID, &clockId) != 0) { - ERR("Fail to set monotonic timestamp for %s", m_resource.c_str()); - throw ENXIO; - } - INFO("gyro_sensor_hal is created!"); } gyro_sensor_hal::~gyro_sensor_hal() { - close(m_node_handle); - m_node_handle = INITIAL_VALUE; + enable_resource(false); + if (m_data != NULL) + delete []m_data; + if (m_fp_buffer > 0) + close(m_fp_buffer); INFO("gyro_sensor_hal is destroyed!"); } @@ -139,52 +132,26 @@ sensor_type_t gyro_sensor_hal::get_type(void) return GYROSCOPE_SENSOR; } -bool gyro_sensor_hal::enable_resource(string &resource_node, bool enable) +bool gyro_sensor_hal::enable_resource(bool enable) { - int prev_status, status; - FILE *fp = NULL; - fp = fopen(resource_node.c_str(), "r"); - - if (!fp) { - ERR("Fail to open a resource file: %s", resource_node.c_str()); - return false; - } - - if (fscanf(fp, "%d", &prev_status) < 0) { - ERR("Failed to get data from %s", resource_node.c_str()); - fclose(fp); - return false; - } - - fclose(fp); - - if (enable) { - if (m_sensorhub_supported) - status = prev_status | (1 << SENSORHUB_GYROSCOPE_ENABLE_BIT); - else - status = 1; - } else { - if (m_sensorhub_supported) - status = prev_status ^ (1 << SENSORHUB_GYROSCOPE_ENABLE_BIT); - else - status = 0; - } - - fp = fopen(resource_node.c_str(), "w"); - - if (!fp) { - ERR("Failed to open a resource file: %s", resource_node.c_str()); - return false; - } - - if (fprintf(fp, "%d", status) < 0) { - ERR("Failed to enable a resource file: %s", resource_node.c_str()); - fclose(fp); - return false; - } - - if (fp) - fclose(fp); + string temp; + int enable_val; + + if(enable) + enable_val = ENABLE_VAL; + else + enable_val = DISABLE_VAL; + + temp = m_gyro_dir + string(SCAN_EL_DIR) + string(CHANNEL_NAME_X) + string(ENABLE_SUFFIX); + update_sysfs_num(temp.c_str(), enable_val); + temp = m_gyro_dir + string(SCAN_EL_DIR) + string(CHANNEL_NAME_Y) + string(ENABLE_SUFFIX); + update_sysfs_num(temp.c_str(), enable_val); + temp = m_gyro_dir + string(SCAN_EL_DIR) + string(CHANNEL_NAME_Z) + string(ENABLE_SUFFIX); + update_sysfs_num(temp.c_str(), enable_val); + temp = m_gyro_dir + string(SCAN_EL_DIR) + string(CHANNEL_NAME_TIME) + string(ENABLE_SUFFIX); + update_sysfs_num(temp.c_str(), enable_val); + setup_trigger(INPUT_TRIG_NAME, enable); + setup_buffer(enable_val); return true; } @@ -193,10 +160,10 @@ bool gyro_sensor_hal::enable(void) { AUTOLOCK(m_mutex); - enable_resource(m_enable_resource, true); + enable_resource(true); set_interval(m_polling_interval); - m_fired_time = 0; + m_fired_time = INITIAL_TIME; INFO("Gyro sensor real starting"); return true; } @@ -205,147 +172,63 @@ bool gyro_sensor_hal::disable(void) { AUTOLOCK(m_mutex); - enable_resource(m_enable_resource, false); + enable_resource(false); INFO("Gyro sensor real stopping"); return true; } -bool gyro_sensor_hal::set_interval(unsigned long val) +bool gyro_sensor_hal::set_interval(unsigned long ms_interval) { - unsigned long long polling_interval_ns; - FILE *fp = NULL; - - AUTOLOCK(m_mutex); - - polling_interval_ns = ((unsigned long long)(val) * MS_TO_SEC * MS_TO_SEC); - fp = fopen(m_polling_resource.c_str(), "w"); - - if (!fp) { - ERR("Failed to open a resource file: %s", m_polling_resource.c_str()); - return false; - } - - if (fprintf(fp, "%llu", polling_interval_ns) < 0) { - ERR("Failed to set data %llu", polling_interval_ns); - fclose(fp); - return false; - } - - if (fp) - fclose(fp); - - INFO("Interval is changed from %dms to %dms]", m_polling_interval, val); - m_polling_interval = val; - return true; -} - -bool gyro_sensor_hal::update_value(bool wait) -{ - const int TIMEOUT = 1; - int gyro_raw[3] = {0,}; - bool x, y, z; - int read_input_cnt = 0; - const int INPUT_MAX_BEFORE_SYN = 10; - unsigned long long fired_time = 0; - bool syn = false; - x = y = z = false; - - struct timeval tv; - fd_set readfds, exceptfds; - - FD_ZERO(&readfds); - FD_ZERO(&exceptfds); - FD_SET(m_node_handle, &readfds); - FD_SET(m_node_handle, &exceptfds); - - if (wait) { - tv.tv_sec = TIMEOUT; - tv.tv_usec = 0; - } else { - tv.tv_sec = 0; - tv.tv_usec = 0; - } - - int ret; - ret = select(m_node_handle + 1, &readfds, NULL, &exceptfds, &tv); - - if (ret == -1) { - ERR("select error:%s m_node_handle:d", strerror(errno), m_node_handle); - return false; - } else if (!ret) { - DBG("select timeout: %d seconds elapsed", tv.tv_sec); - return false; - } - - if (FD_ISSET(m_node_handle, &exceptfds)) { - ERR("select exception occurred!"); - return false; - } - - if (FD_ISSET(m_node_handle, &readfds)) { - struct input_event gyro_input; - DBG("gyro event detection!"); - - while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) { - int len = read(m_node_handle, &gyro_input, sizeof(gyro_input)); - - if (len != sizeof(gyro_input)) { - ERR("gyro_file read fail, read_len = %d", len); - return false; + int freq, i, approx_freq; + freq = MSEC_TO_FREQ(ms_interval); + + for (i=0; i < m_sample_freq_count; i++) + { + if (freq == m_sample_freq[i]) + { + if (update_sysfs_num(m_freq_resource.c_str(), freq, true) == 0) + { + INFO("Interval is changed from %lums to %lums]", m_polling_interval, ms_interval); + m_polling_interval = ms_interval; + return true; } - - ++read_input_cnt; - - if (gyro_input.type == EV_REL) { - switch (gyro_input.code) { - case REL_RX: - gyro_raw[0] = (int)gyro_input.value; - x = true; - break; - case REL_RY: - gyro_raw[1] = (int)gyro_input.value; - y = true; - break; - case REL_RZ: - gyro_raw[2] = (int)gyro_input.value; - z = true; - break; - default: - ERR("gyro_input event[type = %d, code = %d] is unknown.", gyro_input.type, gyro_input.code); - return false; - break; - } - } else if (gyro_input.type == EV_SYN) { - syn = true; - fired_time = sensor_hal::get_timestamp(&gyro_input.time); - } else { - ERR("gyro_input event[type = %d, code = %d] is unknown.", gyro_input.type, gyro_input.code); + else + { + ERR("Failed to set data %lu\n", ms_interval); return false; } } - } else { - ERR("select nothing to read!!!"); - return false; } - if (syn == false) { - ERR("EV_SYN didn't come until %d inputs had come", read_input_cnt); + INFO("The interval not supported: %lu\n", ms_interval); + ERR("Failed to set data %lu\n", ms_interval); + return false; +} + +bool gyro_sensor_hal::update_value(bool wait) +{ + int i; + struct pollfd pfd; + ssize_t read_size; + const int TIMEOUT = 1000; + + pfd.fd = m_fp_buffer; + pfd.events = POLLIN; + if (wait) + poll(&pfd, 1, TIMEOUT); + else + poll(&pfd, 1, 0); + + read_size = read(m_fp_buffer, m_data, GYRO_RINGBUF_LEN * m_scan_size); + + if (read_size <= 0) + { + ERR("No gyro data available to read\n"); return false; } - AUTOLOCK(m_value_mutex); - - if (x) - m_x = gyro_raw[0]; - - if (y) - m_y = gyro_raw[1]; - - if (z) - m_z = gyro_raw[2]; - - m_fired_time = fired_time; - DBG("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time); + for (i = 0; i < (read_size / m_scan_size); i++) + decode_data(); return true; } @@ -361,12 +244,14 @@ int gyro_sensor_hal::get_sensor_data(sensor_data_t &data) const int chance = 3; int retry = 0; - while ((m_fired_time == 0) && (retry++ < chance)) { + while ((m_fired_time == INITIAL_TIME) && (retry++ < chance)) + { INFO("Try usleep for getting a valid BASE DATA value"); usleep(m_polling_interval * MS_TO_SEC); } - if (m_fired_time == 0) { + if (m_fired_time == INITIAL_TIME) + { ERR("get_sensor_data failed"); return -1; } @@ -395,121 +280,262 @@ bool gyro_sensor_hal::get_properties(sensor_properties_t &properties) bool gyro_sensor_hal::is_sensorhub_supported(void) { - DIR *main_dir = NULL; - main_dir = opendir(SENSORHUB_NODE); - - if (!main_dir) { - INFO("Sensor Hub is not supported"); - return false; - } - - INFO("It supports sensor hub"); - closedir(main_dir); - return true; + return false; } bool gyro_sensor_hal::check_hw_node(void) { string name_node; string hw_name; + string file_name; + string temp; DIR *main_dir = NULL; struct dirent *dir_entry = NULL; bool find_node = false; + bool find_trigger = false; INFO("======================start check_hw_node============================="); m_sensorhub_supported = is_sensorhub_supported(); - main_dir = opendir(SENSOR_NODE); + main_dir = opendir(IIO_DIR); - if (!main_dir) { - ERR("Directory open failed to collect data"); + if (!main_dir) + { + ERR("Could not open IIO directory\n"); return false; } - while ((!find_node) && (dir_entry = readdir(main_dir))) { - if ((strncasecmp(dir_entry->d_name , ".", 1 ) != 0) && (strncasecmp(dir_entry->d_name , "..", 2 ) != 0) && (dir_entry->d_ino != 0)) { - name_node = string(SENSOR_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_NAME); + m_channels = (struct channel_parameters*) malloc(sizeof(struct channel_parameters) * NO_OF_CHANNELS); - ifstream infile(name_node.c_str()); + while (!(find_node && find_trigger)) + { + dir_entry = readdir(main_dir); + if(dir_entry == NULL) + break; + + if ((strncasecmp(dir_entry->d_name , ".", 1 ) != 0) && (strncasecmp(dir_entry->d_name , "..", 2 ) != 0) && (dir_entry->d_ino != 0)) + { + file_name = string(IIO_DIR) + string(dir_entry->d_name) + string(NAME_NODE); + ifstream infile(file_name.c_str()); if (!infile) continue; infile >> hw_name; - if (CConfig::get_instance().is_supported(SENSOR_TYPE_GYRO, hw_name) == true) { - m_name = m_model_id = hw_name; - INFO("m_model_id = %s", m_model_id.c_str()); - find_node = true; - break; + if (strncmp(dir_entry->d_name, IIO_DEV_BASE_NAME, IIO_DEV_STR_LEN) == 0) + { + if (CConfig::get_instance().is_supported(SENSOR_TYPE_GYRO, hw_name) == true) + { + m_gyro_dir = string(IIO_DIR) + string(dir_entry->d_name) + string("/"); + m_buffer_access = string(DEV_DIR) + string(dir_entry->d_name); + m_name = m_model_id = hw_name; + find_node = true; + INFO("m_gyro_dir:%s\n", m_gyro_dir.c_str()); + INFO("m_buffer_access:%s\n", m_buffer_access.c_str()); + INFO("m_name:%s\n", m_name.c_str()); + } + } + + if (strncmp(dir_entry->d_name, IIO_TRIG_BASE_NAME, IIO_TRIG_STR_LEN) == 0) + { + if (hw_name == string(INPUT_TRIG_NAME)) + { + m_gyro_trig_dir = string(IIO_DIR) + string(dir_entry->d_name) + string("/"); + find_trigger = true; + DBG("m_gyro_trig_dir:%s\n", m_gyro_trig_dir.c_str()); + } } + + if (find_node && find_trigger) + break; } } closedir(main_dir); - if (find_node) { - main_dir = opendir(INPUT_DEVICE_NODE); - - if (!main_dir) { - ERR("Directory open failed to collect data"); + if (find_node && find_trigger) + { + if (setup_channels() == true) + INFO("IIO channel setup successful"); + else + { + ERR("IIO channel setup failed"); return false; } + } + return (find_node && find_trigger); +} - find_node = false; +bool gyro_sensor_hal::add_gyro_channels_to_array(void) +{ + if (add_channel_to_array(m_gyro_dir.c_str(), CHANNEL_NAME_X, &m_channels[0]) < 0) + { + ERR("Failed to add %s to channel array", CHANNEL_NAME_X); + return false; + } + if (add_channel_to_array(m_gyro_dir.c_str(), CHANNEL_NAME_Y, &m_channels[1]) < 0) + { + ERR("Failed to add %s to channel array", CHANNEL_NAME_Y); + return false; + } + if (add_channel_to_array(m_gyro_dir.c_str(), CHANNEL_NAME_Z, &m_channels[2]) < 0) + { + ERR("Failed to add %s to channel array", CHANNEL_NAME_Z); + return false; + } + if (add_channel_to_array(m_gyro_dir.c_str(), CHANNEL_NAME_TIME, &m_channels[3]) < 0) + { + ERR("Failed to add channel time_stamp to channel array"); + return false; + } + return true; +} - while ((!find_node) && (dir_entry = readdir(main_dir))) { - if (strncasecmp(dir_entry->d_name, NODE_INPUT, 5) == 0) { - name_node = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_NAME); - ifstream infile(name_node.c_str()); +bool gyro_sensor_hal::setup_channels(void) +{ + int freq, i; + double sf; + string temp; - if (!infile) - continue; + enable_resource(true); - infile >> hw_name; + if (!add_gyro_channels_to_array()) + return false; - if (hw_name == string(INPUT_NAME)) { - INFO("name_node = %s", name_node.c_str()); - DBG("Find H/W for gyro_sensor"); + sort_channels_by_index(m_channels, NO_OF_CHANNELS); - find_node = true; - string dir_name; - dir_name = string(dir_entry->d_name); - unsigned found = dir_name.find_first_not_of(NODE_INPUT); - m_resource = string(DEV_INPUT_NODE) + dir_name.substr(found); - - if (m_sensorhub_supported) { - m_enable_resource = string(SENSORHUB_NODE) + string(NODE_ENABLE); - m_polling_resource = string(SENSORHUB_NODE) + string(NODE_GYRO_POLL_DELAY); - } else { - m_enable_resource = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_ENABLE); - m_polling_resource = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_POLL_DELAY); - } - - break; - } - } - } + m_scan_size = get_channel_array_size(m_channels, NO_OF_CHANNELS); + if (m_scan_size == 0) + { + ERR("Channel array size is zero"); + return false; + } - closedir(main_dir); + m_data = new (std::nothrow) char[m_scan_size * GYRO_RINGBUF_LEN]; + if (m_data == NULL) + { + ERR("Couldn't create data buffer\n"); + return false; } - if (find_node) { - INFO("m_resource = %s", m_resource.c_str()); - INFO("m_enable_resource = %s", m_enable_resource.c_str()); - INFO("m_polling_resource = %s", m_polling_resource.c_str()); + m_fp_buffer = open(m_buffer_access.c_str(), O_RDONLY | O_NONBLOCK); + if (m_fp_buffer == -1) + { + ERR("Failed to open ring buffer(%s)\n", m_buffer_access.c_str()); + return false; } - return find_node; + m_freq_resource = m_gyro_dir + string(GYRO_FREQ); + temp = m_gyro_dir + string(GYRO_FREQ_AVLBL); + + FILE *fp = NULL; + fp = fopen(temp.c_str(), "r"); + if (!fp) + { + ERR("Fail to open available frequencies file:%s\n", temp.c_str()); + return false; + } + + for (i = 0; i < MAX_FREQ_COUNT; i++) + m_sample_freq[i] = 0; + + i = 0; + while (fscanf(fp, "%d", &freq) > 0) + m_sample_freq[i++] = freq; + m_sample_freq_count = i; + + temp = m_gyro_dir + string(GYRO_SCALE_AVLBL); + fp = fopen(temp.c_str(), "r"); + if (!fp) + { + ERR("Fail to open available scale factors file:%s\n", temp.c_str()); + return false; + } + + for (i = 0; i < MAX_SCALING_COUNT; i++) + m_scale_factor[i] = 0; + + i = 0; + while (fscanf(fp, "%lf", &sf) > 0) + m_scale_factor[i++] = sf; + m_scale_factor_count = i; + + return true; +} + +void gyro_sensor_hal::decode_data(void) +{ + AUTOLOCK(m_value_mutex); + + m_x = convert_bytes_to_int(*(unsigned short int *)(m_data + m_channels[0].buf_index), &m_channels[0]); + m_y = convert_bytes_to_int(*(unsigned short int *)(m_data + m_channels[1].buf_index), &m_channels[1]); + m_z = convert_bytes_to_int(*(unsigned short int *)(m_data + m_channels[2].buf_index), &m_channels[2]); + + long long int val = *(long long int *)(m_data + m_channels[3].buf_index); + if ((val >> m_channels[3].valid_bits) & 1) + val = (val & m_channels[3].mask) | ~m_channels[3].mask; + + m_fired_time = (unsigned long long int)(val); + DBG("m_x = %d, m_y = %d, m_z = %d, time = %lluus", m_x, m_y, m_z, m_fired_time); +} + +bool gyro_sensor_hal::setup_trigger(char* trig_name, bool verify) +{ + string temp; + int ret; + + temp = m_gyro_dir + string(TRIG_PATH); + update_sysfs_string(temp.c_str(), trig_name, verify); + if (ret < 0) + { + ERR("failed to write to current_trigger\n"); + return false; + } + INFO("current_trigger setup successfully\n"); + return true; +} + +bool gyro_sensor_hal::setup_buffer(int enable) +{ + string temp; + int ret; + temp = m_gyro_dir + string(BUFFER_LEN); + INFO("Buffer Length Setup: %s", temp.c_str()); + ret = update_sysfs_num(temp.c_str(), GYRO_RINGBUF_LEN, true); + if (ret < 0) + { + ERR("failed to write to buffer/length\n"); + return false; + } + INFO("buffer/length setup successfully\n"); + + temp = m_gyro_dir + string(BUFFER_EN); + INFO("Buffer Enable: %s", temp.c_str()); + ret = update_sysfs_num(temp.c_str(), enable, true); + if (ret < 0) + { + ERR("failed to write to buffer/enable\n"); + return false; + } + if (enable) + INFO("buffer enabled\n"); + else + INFO("buffer disabled\n"); + + return true; } extern "C" void *create(void) { gyro_sensor_hal *inst; - try { + try + { inst = new gyro_sensor_hal(); - } catch (int err) { + } + catch (int err) + { ERR("Failed to create gyro_sensor_hal class, errno : %d, errstr : %s", err, strerror(err)); return NULL; } diff --git a/src/gyro/gyro_sensor_hal.h b/src/gyro/gyro_sensor_hal.h index 0a145c3..4865d6f 100755 --- a/src/gyro/gyro_sensor_hal.h +++ b/src/gyro/gyro_sensor_hal.h @@ -22,6 +22,39 @@ #include #include +#include + +#define INPUT_DEV_NAME "lsm330dlc-gyro" +#define INPUT_TRIG_NAME "lsm330dlc-gyro-trigger" + +#define IIO_DIR "/sys/bus/iio/devices/" +#define GYRO_FREQ "sampling_frequency" +#define GYRO_FREQ_AVLBL "sampling_frequency_available" +#define GYRO_SCALE_AVLBL "in_anglvel_scale_available" +#define GYRO_X_SCALE "in_anglvel_x_scale" +#define GYRO_Y_SCALE "in_anglvel_y_scale" +#define GYRO_Z_SCALE "in_anglvel_z_scale" + +#define NO_OF_CHANNELS 4 +#define MAX_FREQ_COUNT 16 +#define MAX_SCALING_COUNT 16 + +#define CHANNEL_NAME_X "in_anglvel_x" +#define CHANNEL_NAME_Y "in_anglvel_y" +#define CHANNEL_NAME_Z "in_anglvel_z" +#define CHANNEL_NAME_TIME "in_timestamp" +#define ENABLE_SUFFIX "_en" +#define NAME_NODE "/name" +#define BUFFER_EN "buffer/enable" +#define BUFFER_LEN "buffer/length" +#define SCAN_EL_DIR "scan_elements/" + +#define IIO_DEV_BASE_NAME "iio:device" +#define IIO_TRIG_BASE_NAME "trigger" +#define IIO_DEV_STR_LEN 10 +#define IIO_TRIG_STR_LEN 7 + +#define GYRO_RINGBUF_LEN 32 using std::string; @@ -44,7 +77,6 @@ private: int m_x; int m_y; int m_z; - int m_node_handle; unsigned long m_polling_interval; unsigned long long m_fired_time; bool m_sensorhub_supported; @@ -57,14 +89,33 @@ private: int m_resolution; float m_raw_data_unit; - string m_resource; - string m_enable_resource; string m_polling_resource; + string m_gyro_dir; + string m_gyro_trig_dir; + string m_buffer_access; + string m_freq_resource; + + int m_scale_factor_count; + int m_sample_freq_count; + int m_sample_freq[MAX_FREQ_COUNT]; + double m_scale_factor[MAX_SCALING_COUNT]; + + int m_fp_buffer; + char *m_data; + int m_scan_size; + struct channel_parameters *m_channels; + cmutex m_value_mutex; - bool enable_resource(string &resource_node, bool enable); + bool enable_resource(bool enable); bool update_value(bool wait); bool is_sensorhub_supported(void); + + bool add_gyro_channels_to_array(void); + bool setup_channels(void); + bool setup_buffer(int enable); + bool setup_trigger(char* trig_name, bool verify); + void decode_data(void); }; #endif /*_GYRO_SENSOR_HAL_H_*/ -- 2.7.4 From 3145e9c018890026a81942102c2564e0ed259195 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 4 Jul 2014 11:09:29 +0530 Subject: [PATCH 02/16] Updating vector and quaternion class Vector Class - Adding method to update vector like a queue - Adding method to compute variance of vector - Adding method to check if vector is initialized Quaternion Class - Updated the quaternion normalization method signed-off-by: Ramasamy Change-Id: I08d7fcb6c62b1fc02c53954009d13127b73e65c3 --- src/sensor_fusion/standalone/util/quaternion.cpp | 34 ++++++++--------- src/sensor_fusion/standalone/util/quaternion.h | 2 +- .../util/test/quaternion_test/quaternion_main.cpp | 4 +- .../util/test/vector_test/vector_main.cpp | 16 ++++++++ src/sensor_fusion/standalone/util/vector.cpp | 43 ++++++++++++++++++++++ src/sensor_fusion/standalone/util/vector.h | 3 ++ 6 files changed, 81 insertions(+), 21 deletions(-) diff --git a/src/sensor_fusion/standalone/util/quaternion.cpp b/src/sensor_fusion/standalone/util/quaternion.cpp index 37e8ac6..2649f73 100755 --- a/src/sensor_fusion/standalone/util/quaternion.cpp +++ b/src/sensor_fusion/standalone/util/quaternion.cpp @@ -62,6 +62,22 @@ quaternion quaternion::operator =(const quaternion& q) return *this; } +template +void quaternion::quat_normalize() +{ + TYPE w, x, y, z; + TYPE val; + + w = m_quat.m_vec[0] * m_quat.m_vec[0]; + x = m_quat.m_vec[1] * m_quat.m_vec[1]; + y = m_quat.m_vec[2] * m_quat.m_vec[2]; + z = m_quat.m_vec[3] * m_quat.m_vec[3]; + + val = sqrt(w + x + y + z); + + m_quat = m_quat / val; +} + template quaternion operator *(const quaternion q, const T val) { @@ -102,24 +118,6 @@ quaternion operator +(const quaternion q1, const quaternion q2) } template -quaternion quat_normalize(const quaternion q) -{ - T w, x, y, z; - T val; - - w = q.m_quat.m_vec[0] * q.m_quat.m_vec[0]; - x = q.m_quat.m_vec[1] * q.m_quat.m_vec[1]; - y = q.m_quat.m_vec[2] * q.m_quat.m_vec[2]; - z = q.m_quat.m_vec[3] * q.m_quat.m_vec[3]; - - val = sqrt(w + x + y + z); - - quaternion q1(q.m_quat / val); - - return (q1); -} - -template quaternion quat_conj(const quaternion q) { T w, x, y, z; diff --git a/src/sensor_fusion/standalone/util/quaternion.h b/src/sensor_fusion/standalone/util/quaternion.h index 0313797..6be945c 100755 --- a/src/sensor_fusion/standalone/util/quaternion.h +++ b/src/sensor_fusion/standalone/util/quaternion.h @@ -34,6 +34,7 @@ public: ~quaternion(); quaternion operator =(const quaternion& q); + void quat_normalize(); template friend quaternion operator *(const quaternion q, const T val); @@ -42,7 +43,6 @@ public: template friend quaternion operator +(const quaternion q1, const quaternion q2); - template friend quaternion quat_normalize(const quaternion q); template friend quaternion quat_conj(const quaternion q); }; diff --git a/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp b/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp index ca43b8d..e2e371f 100644 --- a/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp +++ b/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp @@ -60,9 +60,9 @@ int main() cout << "output\t" << q7.m_quat << "\n\n"; cout << "Quaternion Normalization\n"; - quaternion q8 = quat_normalize(q1); cout << "input\t" << q1.m_quat << "\n"; - cout << "output\t" << q8.m_quat << "\n\n"; + q1.quat_normalize(); + cout << "output\t" << q1.m_quat << "\n\n"; cout << "Quaternion Conjugate\n"; quaternion q9 = quat_conj(q1); diff --git a/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp b/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp index c78cf21..a5da38d 100644 --- a/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp +++ b/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp @@ -29,6 +29,7 @@ int main() float arr0[5] = {2344.98,345.24,456.12,98.33,144.67}; float arr1[6] = {1.234,4.5,6.8987,3.33,5.66,77.695}; float arr43[6] = {2.3454,-0.0384,-8.90,3.455,6.785,21.345}; + float arr5[5] = {0.2,-0.4,0.6,-0.8,1.0}; vector v1(5, arr0); vector v2(5, arr8); @@ -42,6 +43,7 @@ int main() vector v6(3); vector v13(5); vector v95(6); + vector v35(5, arr5); float arr57[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}}; matrix m12(3, 3, (float *) arr57); @@ -161,5 +163,19 @@ int main() cout<< "\n\n\nCross Product:"; cout << "\n\n" << v21 << "\n\n" << v22; cout << "\nResult:\n\n" << v111; + + cout << "\n\n\nQueue insert function:"; + cout << "\nInput:\n\n" << v111; + insert_end(v111, (float) 0.9191919); + cout << "\nResult:\n\n" << v111; + + cout << "\n\n\nVariance:"; + cout << "\nInput:\n\n" << v35; + val = var(v35); + cout << "\nResult:\n\n" << val; + + cout << "\n\n\nIs Initialized:"; + cout << "\nInput:\n\n" << v35; + cout << "\nResult:\n\n" << is_initialized(v35); } diff --git a/src/sensor_fusion/standalone/util/vector.cpp b/src/sensor_fusion/standalone/util/vector.cpp index 0d04c06..d2df648 100644 --- a/src/sensor_fusion/standalone/util/vector.cpp +++ b/src/sensor_fusion/standalone/util/vector.cpp @@ -260,6 +260,16 @@ T mul(const vector v, const matrix m) return result; } + +template +void insert_end(vector& v, T val) +{ + for (int i = 0; i < (v.m_size - 1); i++) + v.m_vec[i] = v.m_vec[i+1]; + + v.m_vec[v.m_size-1] = val; +} + template vector cross(const vector v1, const vector v2) { @@ -272,5 +282,38 @@ vector cross(const vector v1, const vector v2) return v3; } +template +bool is_initialized(const vector v) +{ + vector v1(v.m_size); + bool retval; + + retval = (v == v1) ? false : true; + + return retval; +} + +template +T var(const vector v) +{ + T val = 0; + T mean, var, diff; + + for (int i = 0; i < v.m_size; i++) + val += v.m_vec[i]; + + mean = val / v.m_size; + + val = 0; + for (int i = 0; i < v.m_size; i++) + { + diff = (v.m_vec[i] - mean); + val += diff * diff; + } + + var = val / (v.m_size - 1); + + return var; +} #endif diff --git a/src/sensor_fusion/standalone/util/vector.h b/src/sensor_fusion/standalone/util/vector.h index cdbb328..b0daa3e 100644 --- a/src/sensor_fusion/standalone/util/vector.h +++ b/src/sensor_fusion/standalone/util/vector.h @@ -60,9 +60,12 @@ public: const vector v2); template friend T mul(const vector v, const matrix m); + template friend void insert_end(vector& v, T val); template friend matrix transpose(const vector v); template friend vector cross(const vector v1, const vector v2); + template friend T var(const vector v); + template friend bool is_initialized(const vector v); }; #include "vector.cpp" -- 2.7.4 From c354d4e74a1aa60786790b844e59c23ef070f3f4 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 9 Jul 2014 10:01:40 +0530 Subject: [PATCH 03/16] Updating matrix and vector class functionality - Adding multiplication method for matrix class - Adding transpose method for vector class signed-off-by: Ramasamy Change-Id: I24fc52221e3d10bebf9016e944d38e3ffd314b2a --- src/sensor_fusion/standalone/util/matrix.cpp | 21 ++++++++++++++++++++- src/sensor_fusion/standalone/util/matrix.h | 4 +++- .../util/test/matrix_test/matrix_main.cpp | 7 ++++++- .../util/test/vector_test/vector_main.cpp | 4 ++++ src/sensor_fusion/standalone/util/vector.cpp | 11 +++++++++++ src/sensor_fusion/standalone/util/vector.h | 1 + 6 files changed, 45 insertions(+), 3 deletions(-) diff --git a/src/sensor_fusion/standalone/util/matrix.cpp b/src/sensor_fusion/standalone/util/matrix.cpp index 2c37690..9ce7487 100755 --- a/src/sensor_fusion/standalone/util/matrix.cpp +++ b/src/sensor_fusion/standalone/util/matrix.cpp @@ -257,7 +257,7 @@ bool operator !=(const matrix m1, const matrix m2) } template -matrix transpose(const matrix m) +matrix tran(const matrix m) { matrix m1(m.m_cols, m.m_rows); @@ -268,4 +268,23 @@ matrix transpose(const matrix m) return m1; } + +template +matrix mul(const matrix m1, const matrix m2) +{ + assert(m2.m_cols == 1); + assert(m1.m_cols == m2.m_rows); + + matrix m3(m1.m_rows, 1); + + for (int i = 0; i < m1.m_rows; i++) + { + m3.m_mat[i][0] = 0; + for (int k = 0; k < m2.m_rows; k++) + m3.m_mat[i][0] += m1.m_mat[i][k] * m2.m_mat[k][0]; + } + + return m3; +} + #endif //_MATRIX_H diff --git a/src/sensor_fusion/standalone/util/matrix.h b/src/sensor_fusion/standalone/util/matrix.h index 1a49c2f..d2b2c29 100755 --- a/src/sensor_fusion/standalone/util/matrix.h +++ b/src/sensor_fusion/standalone/util/matrix.h @@ -60,7 +60,9 @@ public: template friend bool operator !=(const matrix m1, const matrix m2); - template friend matrix transpose(const matrix m); + template friend matrix tran(const matrix m); + template friend matrix mul(const matrix m1, + const matrix m2); }; #include "matrix.cpp" diff --git a/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp b/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp index e065dd3..ed00949 100644 --- a/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp +++ b/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp @@ -30,6 +30,7 @@ int main() float arr3[3][3] = {{20.2, 40.5, 10.0}, {3.6, 52.0, 5.5}, {1.0, 45.5, 66.6}}; float arr4[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}}; float arr8[3][3] = {{4.75, 0.65, 0.123}, {0.075, 5.302, 0.56}, {1.113, 0.475, 2.362}}; + float arr9[3][3] = {{1, 2, 3}, {1, 2, 3}, {1, 2, 3}}; matrix m1(2, 2, (float *) arr0); matrix m2(2, 2, (float *) arr1); @@ -43,6 +44,7 @@ int main() matrix m20(1, 3, (float *) arr11); matrix m21(3, 1, (float *) arr12); matrix m22(2, 3, (float *) arr15); + matrix m9(3, 3, (float *) arr9); cout<< "Constructor Test\n"; cout<< "\n" << m6; @@ -71,6 +73,9 @@ int main() cout<< "\nProduct:\n" << m3 << endl; cout<< "\n" << m20 << "\n" << m21; cout<< "\nProduct:\n" << m7 << endl; + cout<< "\n" << m9 << "\n" << m21; + m21 = mul(m9, m21); + cout<< "\nProduct:\n" << m21 << endl; cout<< "\n\n\nDivision\n"; m3 = m1 / (float)2.5; @@ -116,7 +121,7 @@ int main() cout<< "\n\n" << m12; cout<< "\nResult:\n\n" << m6; - m6 = transpose(m15); + m6 = tran(m15); cout<< "\n\n\nTranspose:"; cout << "\n\n" << m15; cout << "\nResult:\n\n" << m6; diff --git a/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp b/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp index a5da38d..725934f 100644 --- a/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp +++ b/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp @@ -132,6 +132,10 @@ int main() cout << "\nResult:\n\n"; matrix m101 = (transpose(v20)); cout << m101; + cout << "\n\n" << m101; + cout << "\nResult:\n\n"; + v20 = (transpose(m101)); + cout << v20; cout << "\n\nv1:\n\n" << v1; cout << "\n\nv2:\n\n" << v2; diff --git a/src/sensor_fusion/standalone/util/vector.cpp b/src/sensor_fusion/standalone/util/vector.cpp index d2df648..e063d6e 100644 --- a/src/sensor_fusion/standalone/util/vector.cpp +++ b/src/sensor_fusion/standalone/util/vector.cpp @@ -247,6 +247,17 @@ matrix transpose(const vector v) } template +vector transpose(const matrix m) +{ + vector v(m.m_rows); + + for (int i = 0; i < m.m_rows; i++) + v.m_vec[i] = m.m_mat[i][0]; + + return v; +} + +template T mul(const vector v, const matrix m) { assert(m.m_rows == v.m_size); diff --git a/src/sensor_fusion/standalone/util/vector.h b/src/sensor_fusion/standalone/util/vector.h index b0daa3e..21566ea 100644 --- a/src/sensor_fusion/standalone/util/vector.h +++ b/src/sensor_fusion/standalone/util/vector.h @@ -62,6 +62,7 @@ public: template friend T mul(const vector v, const matrix m); template friend void insert_end(vector& v, T val); template friend matrix transpose(const vector v); + template friend vector transpose(const matrix m); template friend vector cross(const vector v1, const vector v2); template friend T var(const vector v); -- 2.7.4 From 88c3a2d3a2bfc809d1589faa4897e70557f0a0fb Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 10 Jul 2014 13:19:35 +0530 Subject: [PATCH 04/16] Updated rotation matrix class method rot_mat2quat Updated method with better implementation for rotation matrix to quaternion conversion. The best method for this conversion is based on eigen vectors. This complex implementation will be added at a later stage after further analysis. signed-off-by: Ramasamy Change-Id: I9fe29d4f8e5ff3b5989a90ecba691d38adc9f3a8 --- .../standalone/util/rotation_matrix.cpp | 86 ++++++++++------------ 1 file changed, 37 insertions(+), 49 deletions(-) diff --git a/src/sensor_fusion/standalone/util/rotation_matrix.cpp b/src/sensor_fusion/standalone/util/rotation_matrix.cpp index aecce4f..3a1ef86 100644 --- a/src/sensor_fusion/standalone/util/rotation_matrix.cpp +++ b/src/sensor_fusion/standalone/util/rotation_matrix.cpp @@ -98,61 +98,49 @@ quaternion rot_mat2quat(rotation_matrix rm) { T q0, q1, q2, q3; - q0 = (rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] + - rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; - q1 = (rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] - - rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; - q2 = (-rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] - - rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; - q3 = (-rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] + - rm.m_rot_mat.m_mat[2][2] + (T) 1) / (T) QUAT_LEN; - - if(q0 < (T) 0) - q0 = (T) 0; - if(q1 < (T) 0) - q1 = (T) 0; - if(q2 < (T) 0) - q2 = (T) 0; - if(q3 < (T) 0) - q3 = (T) 0; - - q0 = sqrt(q0); - q1 = sqrt(q1); - q2 = sqrt(q2); - q3 = sqrt(q3); - - if (q0 >= q1 && q0 >= q2 && q0 >= q3) - { - q0 *= (T) 1; - q1 *= get_sign(rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2]); - q2 *= get_sign(rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0]); - q3 *= get_sign(rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1]); - } - else if (q1 >= q0 && q1 >= q2 && q1 >= q3) - { - q0 *= get_sign(rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2]); - q1 *= (T) 1; - q2 *= get_sign(rm.m_rot_mat.m_mat[1][0] + rm.m_rot_mat.m_mat[0][1]); - q3 *= get_sign(rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0]); - } - else if (q2 >= q0 && q2 >= q1 && q2 >= q3) + T diag_sum = rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] + rm.m_rot_mat.m_mat[2][2]; + + if ( diag_sum > 0 ) { - q0 *= get_sign(rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0]); - q1 *= get_sign(rm.m_rot_mat.m_mat[1][0] + rm.m_rot_mat.m_mat[0][1]); - q2 *= (T) 1; - q3 *= get_sign(rm.m_rot_mat.m_mat[2][1] + rm.m_rot_mat.m_mat[1][2]); + T val = (T) 0.5 / sqrt(diag_sum + (T) 1.0); + q0 = (T) 0.25 / val; + q1 = ( rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) * val; + q2 = ( rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) * val; + q3 = ( rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) * val; } - else if(q3 >= q0 && q3 >= q1 && q3 >= q2) + else { - q0 *= get_sign(rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1]); - q1 *= get_sign(rm.m_rot_mat.m_mat[2][0] + rm.m_rot_mat.m_mat[0][2]); - q2 *= get_sign(rm.m_rot_mat.m_mat[2][1] + rm.m_rot_mat.m_mat[1][2]); - q3 *= (T) 1; + if ( rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[1][1] && rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[2][2] ) + { + T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[2][2]); + q0 = (rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) / val; + q1 = (T) 0.25 * val; + q2 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val; + q3 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val; + } + else if (rm.m_rot_mat.m_mat[1][1] > rm.m_rot_mat.m_mat[2][2]) + { + T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[2][2]); + q0 = (rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) / val; + q1 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val; + q2 = (T) 0.25 * val; + q3 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val; + } + else + { + T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[2][2] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] ); + q0 = (rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) / val; + q1 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val; + q2 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val; + q3 = (T) 0.25 * val; + } } - quaternion q(-q0, q1, q2, q3); + quaternion q(q0, q1, q2, q3); + + q.quat_normalize(); - return quat_normalize(q); + return q; } #endif /* _ROTATION_MATRIX_H */ -- 2.7.4 From 39814e6c08435919c577de8c7848f5eac2061c65 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Fri, 11 Jul 2014 13:38:33 +0530 Subject: [PATCH 05/16] Adding orientation_filter class implementation - complete implementation of the orientation filter class - test application for the orientation filter class signed-off-by: Ramasamy Change-Id: Iee7c01fe8d4dcc2a1f7f71f6f0fb424a2000d989 --- .../standalone/util/orientation_filter.cpp | 293 +++++++++++++++------ .../standalone/util/orientation_filter.h | 49 ++-- .../orientation_filter_main.cpp | 27 +- 3 files changed, 252 insertions(+), 117 deletions(-) diff --git a/src/sensor_fusion/standalone/util/orientation_filter.cpp b/src/sensor_fusion/standalone/util/orientation_filter.cpp index 8fcde52..f3479ce 100644 --- a/src/sensor_fusion/standalone/util/orientation_filter.cpp +++ b/src/sensor_fusion/standalone/util/orientation_filter.cpp @@ -17,34 +17,84 @@ * */ + +#ifdef _ORIENTATION_FILTER_H + #include "orientation_filter.h" #define MOVING_AVERAGE_WINDOW_LENGTH 20 -#define RAD2DEG 57.2957795 -#define DEG2RAD 0.0174532925 -#define US2S (1.0 / 1000000.0) #define GRAVITY 9.80665 -#define PI 3.141593 +#define PI 3.141593 +#define NON_ZERO_VAL 0.1 +#define US2S (1.0 / 1000000.0) +#define SAMPLE_FREQ 100000 // Gyro Types // Systron-donner "Horizon" -#define ZIGMA_W (0.05 * DEG2RAD) //deg/s -#define TAU_W 1000 //secs +#define ZIGMA_W (0.05 * DEG2RAD)//deg/s +#define TAU_W 1000//secs // Crossbow DMU-6X //#define ZIGMA_W 0.05 * DEG2RAD //deg/s //#define TAU_W 300 //secs // FOGs (KVH Autogyro and Crossbow DMU-FOG) //#define ZIGMA_W 0 //deg/s +#define QWB_CONST ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W) +#define F_CONST (-1 / TAU_W) + +#define BIAS_AX 0.098586 +#define BIAS_AY 0.18385 +#define BIAS_AZ (10.084 - GRAVITY) + +#define BIAS_GX -5.3539 +#define BIAS_GY 0.24325 +#define BIAS_GZ 2.3391 + +#define DRIVING_SYSTEM_PHASE_COMPENSATION -1 + +#define SCALE_GYRO 575 + +#define ENABLE_LPF false + #define M3X3R 3 -#define M3x3C 3 +#define M3X3C 3 + +#define M6X6R 6 +#define M6X6C 6 #define V1x3S 3 +#define V1x4S 4 +#define V1x6S 6 template orientation_filter::orientation_filter() { - + TYPE arr[MOVING_AVERAGE_WINDOW_LENGTH]; + + std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL); + + vector vec(MOVING_AVERAGE_WINDOW_LENGTH, arr); + vector vec1x3(V1x3S); + vector vec1x6(V1x6S); + matrix mat6x6(M6X6R, M6X6C); + + m_var_gyr_x = vec; + m_var_gyr_y = vec; + m_var_gyr_z = vec; + m_var_roll = vec; + m_var_pitch = vec; + m_var_yaw = vec; + + m_tran_mat = mat6x6; + m_measure_mat = mat6x6; + m_pred_cov = mat6x6; + m_driv_cov = mat6x6; + m_aid_cov = mat6x6; + + m_bias_correction = vec1x3; + m_state_new = vec1x6; + m_state_old = vec1x6; + m_state_error = vec1x6; } template @@ -60,42 +110,65 @@ inline void orientation_filter::filter_sensor_data(const sensor_data const TYPE iir_b[] = {0.98, 0}; const TYPE iir_a[] = {1.0000000, 0.02}; - filt_accel[0] = filt_accel[1]; - filt_gyro[0] = filt_gyro[1]; - filt_magnetic[0] = filt_magnetic[1]; + TYPE a_bias[] = {BIAS_AX, BIAS_AY, BIAS_AZ}; + TYPE g_bias[] = {BIAS_GX, BIAS_GY, BIAS_GZ}; + + vector acc_data(V1x3S); + vector gyr_data(V1x3S); + vector acc_bias(V1x3S, a_bias); + vector gyr_bias(V1x3S, g_bias); + + acc_data = accel.m_data - acc_bias; + gyr_data = (gyro.m_data - gyr_bias) / (TYPE) SCALE_GYRO; + + m_filt_accel[0] = m_filt_accel[1]; + m_filt_gyro[0] = m_filt_gyro[1]; + m_filt_magnetic[0] = m_filt_magnetic[1]; - filt_accel[1].m_data = accel.m_data * iir_b[0] - filt_accel[0].m_data * iir_a[1]; - filt_gyro[1].m_data = gyro.m_data * iir_b[0] - filt_gyro[0].m_data * iir_a[1]; - filt_magnetic[1].m_data = magnetic.m_data * iir_b[0] - filt_magnetic[0].m_data * iir_a[1]; + if (ENABLE_LPF) + { + m_filt_accel[1].m_data = acc_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1]; + m_filt_gyro[1].m_data = gyr_data * iir_b[0] - m_filt_gyro[0].m_data * iir_a[1]; + m_filt_magnetic[1].m_data = magnetic.m_data * iir_b[0] - m_filt_magnetic[0].m_data * iir_a[1]; + } + else + { + m_filt_accel[1].m_data = acc_data; + m_filt_gyro[1].m_data = gyr_data; + m_filt_magnetic[1].m_data = magnetic.m_data; + } - filt_accel[1].m_time_stamp = accel.m_time_stamp; - filt_gyro[1].m_time_stamp = accel.m_time_stamp; - filt_magnetic[1].m_time_stamp = accel.m_time_stamp; + m_filt_accel[1].m_time_stamp = accel.m_time_stamp; + m_filt_gyro[1].m_time_stamp = accel.m_time_stamp; + m_filt_magnetic[1].m_time_stamp = accel.m_time_stamp; + + normalize(m_filt_accel[1]); + normalize(m_filt_magnetic[1]); + + m_filt_gyro[1].m_data = m_filt_gyro[1].m_data - m_bias_correction; } template inline void orientation_filter::orientation_triad_algorithm() { TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0}; - TYPE arr_mag_e[V1x3S] = {0.0, 1.0, 0.0}; + TYPE arr_mag_e[V1x3S] = {0.0, -1.0, 0.0}; - // gravity vector in earth frame vector acc_e(V1x3S, arr_acc_e); - // magnetic field vector in earth frame vector mag_e(V1x3S, arr_mag_e); - vector acc_b_x_mag_b = cross(filt_accel[1].m_data, filt_magnetic[1].m_data); + vector acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data); vector acc_e_x_mag_e = cross(acc_e, mag_e); - vector cross1 = cross(acc_b_x_mag_b, filt_accel[1].m_data); + vector cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data); vector cross2 = cross(acc_e_x_mag_e, acc_e); - matrix mat_b(M3X3R, M3x3C); - matrix mat_e(M3X3R, M3x3C); + matrix mat_b(M3X3R, M3X3C); + matrix mat_e(M3X3R, M3X3C); for(int i = 0; i < M3X3R; i++) { - mat_b.m_mat[i][0] = filt_accel[1].m_data.m_vec[i]; + mat_b.m_mat[i][0] = m_filt_accel[1].m_data.m_vec[i]; mat_b.m_mat[i][1] = acc_b_x_mag_b.m_vec[i]; mat_b.m_mat[i][2] = cross1.m_vec[i]; mat_e.m_mat[i][0] = acc_e.m_vec[i]; @@ -103,77 +176,143 @@ inline void orientation_filter::orientation_triad_algorithm() mat_e.m_mat[i][2] = cross2.m_vec[i]; } - matrix mat_b_t = transpose(mat_b); + matrix mat_b_t = tran(mat_b); rotation_matrix rot_mat(mat_e * mat_b_t); - quaternion quat_aid = rot_mat2quat(rot_mat); + + m_quat_aid = rot_mat2quat(rot_mat); } template -inline void orientation_filter::compute_aiding_var() +inline void orientation_filter::compute_covariance() { - + TYPE var_gyr_x, var_gyr_y, var_gyr_z; + TYPE var_roll, var_pitch, var_yaw; + + insert_end(m_var_gyr_x, m_filt_gyro[1].m_data.m_vec[0]); + insert_end(m_var_gyr_y, m_filt_gyro[1].m_data.m_vec[1]); + insert_end(m_var_gyr_z, m_filt_gyro[1].m_data.m_vec[2]); + insert_end(m_var_roll, m_orientation.m_ang.m_vec[0]); + insert_end(m_var_pitch, m_orientation.m_ang.m_vec[1]); + insert_end(m_var_yaw, m_orientation.m_ang.m_vec[2]); + + var_gyr_x = var(m_var_gyr_x); + var_gyr_y = var(m_var_gyr_y); + var_gyr_z = var(m_var_gyr_z); + var_roll = var(m_var_roll); + var_pitch = var(m_var_pitch); + var_yaw = var(m_var_yaw); + + m_driv_cov.m_mat[0][0] = var_gyr_x; + m_driv_cov.m_mat[1][1] = var_gyr_y; + m_driv_cov.m_mat[2][2] = var_gyr_z; + m_driv_cov.m_mat[3][3] = (TYPE) QWB_CONST; + m_driv_cov.m_mat[4][4] = (TYPE) QWB_CONST; + m_driv_cov.m_mat[5][5] = (TYPE) QWB_CONST; + + m_aid_cov.m_mat[0][0] = var_roll; + m_aid_cov.m_mat[1][1] = var_pitch; + m_aid_cov.m_mat[2][2] = var_yaw; } template -inline void orientation_filter::compute_driving_var() +inline void orientation_filter::time_update() { + quaternion quat_diff, quat_error; + euler_angles euler_error; + euler_angles orientation; + unsigned long long sample_interval_gyro = SAMPLE_FREQ; + TYPE dt = 0; -} + if (m_filt_gyro[1].m_time_stamp != 0 && m_filt_gyro[0].m_time_stamp != 0) + sample_interval_gyro = m_filt_gyro[1].m_time_stamp - m_filt_gyro[0].m_time_stamp; -template -inline void orientation_filter::compute_process_covar() -{ + dt = sample_interval_gyro * US2S; -} + m_tran_mat.m_mat[0][1] = m_filt_gyro[1].m_data.m_vec[2]; + m_tran_mat.m_mat[0][2] = -m_filt_gyro[1].m_data.m_vec[1]; + m_tran_mat.m_mat[1][0] = -m_filt_gyro[1].m_data.m_vec[2]; + m_tran_mat.m_mat[1][2] = m_filt_gyro[1].m_data.m_vec[0]; + m_tran_mat.m_mat[2][0] = m_filt_gyro[1].m_data.m_vec[1]; + m_tran_mat.m_mat[2][1] = -m_filt_gyro[1].m_data.m_vec[0]; + m_tran_mat.m_mat[3][3] = (TYPE) F_CONST; + m_tran_mat.m_mat[4][4] = (TYPE) F_CONST; + m_tran_mat.m_mat[5][5] = (TYPE) F_CONST; -template -inline void orientation_filter::compute_measurement_covar() -{ + m_measure_mat.m_mat[0][0] = 1; + m_measure_mat.m_mat[1][1] = 1; + m_measure_mat.m_mat[2][2] = 1; -} + if (is_initialized(m_state_old)) + m_state_new = transpose(mul(m_tran_mat, transpose(m_state_old))); -template -inline void orientation_filter::compute_prediction_covar() -{ + m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov; -} + if(!is_initialized(m_quat_driv.m_quat)) + m_quat_driv = m_quat_aid; -template -inline void orientation_filter::compute_quat_diff() -{ + quaternion quat_rot_inc(0, m_filt_gyro[1].m_data.m_vec[0], m_filt_gyro[1].m_data.m_vec[1], + m_filt_gyro[1].m_data.m_vec[2]); -} + quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5; -template -inline void orientation_filter::compute_quat_sum() -{ + m_quat_driv = m_quat_driv + (quat_diff * (TYPE) dt * (TYPE) PI); -} + m_quat_driv.quat_normalize(); -template -inline void orientation_filter::update_quat_sum() -{ + orientation = quat2euler(m_quat_driv); -} + m_orientation = orientation.m_ang * (TYPE) DRIVING_SYSTEM_PHASE_COMPENSATION; -template -inline void orientation_filter::time_update() -{ + quat_error = m_quat_aid * m_quat_driv; + + euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; + + quaternion quat_eu_er(1, euler_error.m_ang.m_vec[0], euler_error.m_ang.m_vec[1], + euler_error.m_ang.m_vec[2]); + m_quat_driv = (m_quat_driv * quat_eu_er) * (TYPE) PI; + m_quat_driv.quat_normalize(); + + if (is_initialized(m_state_new)) + { + m_state_error.m_vec[0] = euler_error.m_ang.m_vec[0]; + m_state_error.m_vec[1] = euler_error.m_ang.m_vec[1]; + m_state_error.m_vec[2] = euler_error.m_ang.m_vec[2]; + m_state_error.m_vec[3] = m_state_new.m_vec[3]; + m_state_error.m_vec[4] = m_state_new.m_vec[4]; + m_state_error.m_vec[5] = m_state_new.m_vec[5]; + } } template inline void orientation_filter::measurement_update() { + matrix gain(M6X6R, M6X6C); + TYPE iden = 0; -} + for (int j = 0; j < M6X6C; j++) + { + for (int i = 0; i < M6X6R; i++) + { + gain.m_mat[i][j] = m_pred_cov.m_mat[j][i] / (m_pred_cov.m_mat[j][j] + m_aid_cov.m_mat[j][j]); -template -inline euler_angles orientation_filter::get_corrected_orientation() -{ - euler_angles euler_ang; + m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j]; + + if (i == j) + iden = 1; + else + iden = 0; + + m_pred_cov.m_mat[i][j] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) * m_pred_cov.m_mat[i][j]; + } + } + + m_state_old = m_state_new; + + TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]}; + vector vec(V1x3S, arr_bias); - return euler_ang; + m_bias_correction = vec; } template @@ -184,31 +323,19 @@ euler_angles orientation_filter::get_orientation(const sensor_data class orientation_filter { public: - sensor_data filt_accel[2]; - sensor_data filt_gyro[2]; - sensor_data filt_magnetic[2]; - matrix transition_matrix; - matrix prediction_covariance; - vector state_new; - vector state_old; - vector state_error; - vector bias_correction; - quaternion quat_diff; - quaternion quat_sum; - quaternion quat_aid; - quaternion quat_driv; - quaternion quat_error; - euler_angles euler_error; - rotation_matrix rot_matrix; - euler_angles orientation; + sensor_data m_filt_accel[2]; + sensor_data m_filt_gyro[2]; + sensor_data m_filt_magnetic[2]; + vector m_var_gyr_x; + vector m_var_gyr_y; + vector m_var_gyr_z; + vector m_var_roll; + vector m_var_pitch; + vector m_var_yaw; + matrix m_driv_cov; + matrix m_aid_cov; + matrix m_tran_mat; + matrix m_measure_mat; + matrix m_pred_cov; + vector m_state_new; + vector m_state_old; + vector m_state_error; + vector m_bias_correction; + quaternion m_quat_aid; + quaternion m_quat_driv; + rotation_matrix m_rot_matrix; + euler_angles m_orientation; orientation_filter(); ~orientation_filter(); @@ -54,17 +59,9 @@ public: inline void filter_sensor_data(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); inline void orientation_triad_algorithm(); - inline void compute_aiding_var(); - inline void compute_driving_var(); - inline void compute_process_covar(); - inline void compute_measurement_covar(); - inline void compute_prediction_covar(); - inline void compute_quat_diff(); - inline void compute_quat_sum(); - inline void update_quat_sum(); + inline void compute_covariance(); inline void time_update(); inline void measurement_update(); - inline euler_angles get_corrected_orientation(); euler_angles get_orientation(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp b/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp index 862428d..2743f97 100644 --- a/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp +++ b/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp @@ -25,27 +25,30 @@ using namespace std; #define ORIENTATION_DATA_PATH "../../../../design/data/100ms/orientation/roll_pitch_yaw/" -#define ORIENTATION_DATA_SIZE 100 +#define ORIENTATION_DATA_SIZE 1095 int main() { int data_available = ORIENTATION_DATA_SIZE; - ifstream accel_file, gyro_file, magnetic_file; + ifstream accel_in, gyro_in, mag_in; + ofstream orien_file; string line_accel, line_gyro, line_magnetic; float sdata[3]; unsigned long long time_stamp; euler_angles orientation; orientation_filter orien_filter; - accel_file.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); - gyro_file.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); - magnetic_file.open(((string)ORIENTATION_DATA_PATH + (string)"magnetic.txt").c_str()); + accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); + gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); + mag_in.open(((string)ORIENTATION_DATA_PATH + (string)"magnetic.txt").c_str()); + + orien_file.open(((string)"orientation.txt").c_str()); char *token = NULL; while (data_available-- > 0) { - getline(accel_file, line_accel); + getline(accel_in, line_accel); sdata[0] = strtof(line_accel.c_str(), &token); sdata[1] = strtof(token, &token); sdata[2] = strtof(token, &token); @@ -54,7 +57,7 @@ int main() cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n"; - getline(gyro_file, line_gyro); + getline(gyro_in, line_gyro); sdata[0] = strtof(line_gyro.c_str(), &token); sdata[1] = strtof(token, &token); sdata[2] = strtof(token, &token); @@ -63,7 +66,7 @@ int main() cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n"; - getline(magnetic_file, line_magnetic); + getline(mag_in, line_magnetic); sdata[0] = strtof(line_magnetic.c_str(), &token); sdata[1] = strtof(token, &token); sdata[2] = strtof(token, &token); @@ -74,7 +77,15 @@ int main() orientation = orien_filter.get_orientation(accel_data, gyro_data, magnetic_data); + orien_file << orientation.m_ang; + + cout << "Orientation Data\t" << orientation.m_ang << "\n\n"; } + accel_in.close(); + gyro_in.close(); + mag_in.close(); + orien_file.close(); + return 0; } -- 2.7.4 From ec68ba08acfaadab627782f7548e488d82264a0f Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Tue, 15 Jul 2014 10:27:45 +0530 Subject: [PATCH 06/16] Adding compute_gravity class and implementation - Adding class definition and implementation of compute_gravity class - Adding testing code for compute_gravity class signed-off-by: Ramasamy Change-Id: I4150961ddef0f6a7a9c10a369e0355b9615a9e58 --- .../standalone/util/compute_gravity.cpp | 57 +++++++++++ .../standalone/util/compute_gravity.h | 39 +++++++ .../util/test/compute_gravity_test/.cproject | 112 +++++++++++++++++++++ .../util/test/compute_gravity_test/.project | 89 ++++++++++++++++ .../compute_gravity_test/compute_gravity_main.cpp | 92 +++++++++++++++++ 5 files changed, 389 insertions(+) create mode 100644 src/sensor_fusion/standalone/util/compute_gravity.cpp create mode 100644 src/sensor_fusion/standalone/util/compute_gravity.h create mode 100644 src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/compute_gravity_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp diff --git a/src/sensor_fusion/standalone/util/compute_gravity.cpp b/src/sensor_fusion/standalone/util/compute_gravity.cpp new file mode 100644 index 0000000..3cf6e19 --- /dev/null +++ b/src/sensor_fusion/standalone/util/compute_gravity.cpp @@ -0,0 +1,57 @@ +/* + * 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. + * + */ + + +#ifdef _COMPUTE_GRAVITY_H + +#include "compute_gravity.h" + +#define GRAVITY 9.80665 + +template +compute_gravity::compute_gravity() +{ + +} + +template +compute_gravity::~compute_gravity() +{ + +} + +template +sensor_data compute_gravity::orientation2gravity(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic) +{ + orientation_filter orien_filter; + euler_angles orientation; + sensor_data gravity; + + orientation = orien_filter.get_orientation(accel, gyro, magnetic); + + gravity.m_data.m_vec[0] = GRAVITY * sin(orien_filter.m_orientation.m_ang.m_vec[0]); + gravity.m_data.m_vec[1] = GRAVITY * sin(orien_filter.m_orientation.m_ang.m_vec[1]); + gravity.m_data.m_vec[2] = GRAVITY * cos(orien_filter.m_orientation.m_ang.m_vec[0]) * + cos(orien_filter.m_orientation.m_ang.m_vec[1]); + + return gravity; +} + +#endif diff --git a/src/sensor_fusion/standalone/util/compute_gravity.h b/src/sensor_fusion/standalone/util/compute_gravity.h new file mode 100644 index 0000000..745eb5c --- /dev/null +++ b/src/sensor_fusion/standalone/util/compute_gravity.h @@ -0,0 +1,39 @@ +/* + * 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 _COMPUTE_GRAVITY_H +#define _COMPUTE_GRAVITY_H + +#include "orientation_filter.h" + +template +class compute_gravity { +public: + orientation_filter estimate_orientation; + + compute_gravity(); + ~compute_gravity(); + + sensor_data orientation2gravity(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic); +}; + +#include "compute_gravity.cpp" + +#endif /* _COMPUTE_GRAVITY_H */ diff --git a/src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject b/src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject new file mode 100644 index 0000000..88e1d8a --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/compute_gravity_test/.project b/src/sensor_fusion/standalone/util/test/compute_gravity_test/.project new file mode 100644 index 0000000..79f09e9 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/compute_gravity_test/.project @@ -0,0 +1,89 @@ + + + compute_gravity_test + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${workspace_loc:/compute_gravity_test/Debug} + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + org.tizen.nativecpp.apichecker.core.builder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.tizen.nativecpp.apichecker.core.tizenCppNature + + diff --git a/src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp b/src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp new file mode 100644 index 0000000..bd56100 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp @@ -0,0 +1,92 @@ +/* + * 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 "../../compute_gravity.h" +#include +#include +#include +#include +using namespace std; + +#define GRAVITY_DATA_PATH "../../../../design/data/100ms/gravity/throw/" +#define GRAVITY_DATA_SIZE 135 + +int main() +{ + int data_available = GRAVITY_DATA_SIZE; + ifstream accel_in, gyro_in, mag_in; + ofstream gravity_file; + string line_accel, line_gyro, line_magnetic; + float sdata[3]; + unsigned long long time_stamp; + sensor_data gravity; + compute_gravity comp_grav; + + accel_in.open(((string)GRAVITY_DATA_PATH + (string)"accel.txt").c_str()); + gyro_in.open(((string)GRAVITY_DATA_PATH + (string)"gyro.txt").c_str()); + mag_in.open(((string)GRAVITY_DATA_PATH + (string)"magnetic.txt").c_str()); + + gravity_file.open(((string)"gravity.txt").c_str()); + + char *token = NULL; + + while (data_available-- > 0) + { + getline(accel_in, line_accel); + sdata[0] = strtof(line_accel.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data accel_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n"; + + getline(gyro_in, line_gyro); + sdata[0] = strtof(line_gyro.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data gyro_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n"; + + getline(mag_in, line_magnetic); + sdata[0] = strtof(line_magnetic.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data magnetic_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n"; + + gravity = comp_grav.orientation2gravity(accel_data, gyro_data, magnetic_data); + + gravity_file << gravity.m_data; + + cout << "Gravity Data\t" << gravity.m_data << "\n\n"; + } + + accel_in.close(); + gyro_in.close(); + mag_in.close(); + gravity_file.close(); + + return 0; +} + -- 2.7.4 From c5817a1a695f8e15a1aec102994b030c5d9438ab Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 24 Jul 2014 13:50:03 +0530 Subject: [PATCH 07/16] Updating orientation_filter class - Updating orientation_filter class to output rotation matrix signed-off-by: Ramasamy Change-Id: I85cf21232bbf466f8ab52e4a0a5d8a4786e0ca29 --- src/sensor_fusion/standalone/util/orientation_filter.cpp | 11 +++++++++++ src/sensor_fusion/standalone/util/orientation_filter.h | 2 ++ 2 files changed, 13 insertions(+) diff --git a/src/sensor_fusion/standalone/util/orientation_filter.cpp b/src/sensor_fusion/standalone/util/orientation_filter.cpp index f3479ce..0cdb7fd 100644 --- a/src/sensor_fusion/standalone/util/orientation_filter.cpp +++ b/src/sensor_fusion/standalone/util/orientation_filter.cpp @@ -263,6 +263,8 @@ inline void orientation_filter::time_update() m_orientation = orientation.m_ang * (TYPE) DRIVING_SYSTEM_PHASE_COMPENSATION; + m_rot_matrix = quat2rot_mat(m_quat_driv); + quat_error = m_quat_aid * m_quat_driv; euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI; @@ -338,4 +340,13 @@ euler_angles orientation_filter::get_orientation(const sensor_data +rotation_matrix orientation_filter::get_rotation_matrix(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic) +{ + get_orientation(accel, gyro, magnetic); + + return m_rot_matrix; +} + #endif //_ORIENTATION_FILTER_H diff --git a/src/sensor_fusion/standalone/util/orientation_filter.h b/src/sensor_fusion/standalone/util/orientation_filter.h index fb1747c..6f9ce37 100644 --- a/src/sensor_fusion/standalone/util/orientation_filter.h +++ b/src/sensor_fusion/standalone/util/orientation_filter.h @@ -65,6 +65,8 @@ public: euler_angles get_orientation(const sensor_data accel, const sensor_data gyro, const sensor_data magnetic); + rotation_matrix get_rotation_matrix(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic); }; #include "orientation_filter.cpp" -- 2.7.4 From bbb081a13a1527fa8603fb096d55ede28eaf9240 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 24 Jul 2014 13:57:04 +0530 Subject: [PATCH 08/16] Adding orientation_sensor virtual sensor class - Adding class definition and implementation for orientation virtual sensor - Adding test code for orientation_sensor class - interface to sensor framework for virtual class will be added during porting signed-off-by: Ramasamy Change-Id: I17becdbe159345c446e6944596900db4557d51ba --- .../standalone/orientation_sensor.cpp | 34 +++++++ src/sensor_fusion/standalone/orientation_sensor.h | 38 +++++++ .../util/test/orientation_sensor_test/.cproject | 112 +++++++++++++++++++++ .../util/test/orientation_sensor_test/.project | 89 ++++++++++++++++ .../orientation_sensor_main.cpp | 96 ++++++++++++++++++ 5 files changed, 369 insertions(+) create mode 100644 src/sensor_fusion/standalone/orientation_sensor.cpp create mode 100644 src/sensor_fusion/standalone/orientation_sensor.h create mode 100644 src/sensor_fusion/standalone/util/test/orientation_sensor_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/orientation_sensor_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/orientation_sensor_test/orientation_sensor_main.cpp diff --git a/src/sensor_fusion/standalone/orientation_sensor.cpp b/src/sensor_fusion/standalone/orientation_sensor.cpp new file mode 100644 index 0000000..b86919d --- /dev/null +++ b/src/sensor_fusion/standalone/orientation_sensor.cpp @@ -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. + * + */ + +#ifdef _ORIENTATION_SENSOR_H + +euler_angles orientation_sensor::get_orientation(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic) +{ + return orien_filter.get_orientation(accel, gyro, magnetic); +} + +rotation_matrix orientation_sensor::get_rotation_matrix(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic) +{ + return orien_filter.get_rotation_matrix(accel, gyro, magnetic); +} + +#endif diff --git a/src/sensor_fusion/standalone/orientation_sensor.h b/src/sensor_fusion/standalone/orientation_sensor.h new file mode 100644 index 0000000..89312c3 --- /dev/null +++ b/src/sensor_fusion/standalone/orientation_sensor.h @@ -0,0 +1,38 @@ +/* + * 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 _ORIENTATION_SENSOR_H +#define _ORIENTATION_SENSOR_H + +#include "./util/orientation_filter.h" + +class orientation_sensor +{ +public: + orientation_filter orien_filter; + + euler_angles get_orientation(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic); + rotation_matrix get_rotation_matrix(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic); +}; + +#include "orientation_sensor.cpp" + +#endif /* _ORIENTATION_SENSOR_H */ diff --git a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/.cproject b/src/sensor_fusion/standalone/util/test/orientation_sensor_test/.cproject new file mode 100644 index 0000000..65deaf8 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/orientation_sensor_test/.cproject @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/.project b/src/sensor_fusion/standalone/util/test/orientation_sensor_test/.project new file mode 100644 index 0000000..7ceec3c --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/orientation_sensor_test/.project @@ -0,0 +1,89 @@ + + + orientation_sensor_test + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${workspace_loc:/orientation_sensor_test/Debug} + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + org.tizen.nativecpp.apichecker.core.builder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.tizen.nativecpp.apichecker.core.tizenCppNature + + diff --git a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/orientation_sensor_main.cpp b/src/sensor_fusion/standalone/util/test/orientation_sensor_test/orientation_sensor_main.cpp new file mode 100644 index 0000000..c755cbe --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/orientation_sensor_test/orientation_sensor_main.cpp @@ -0,0 +1,96 @@ +/* + * 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 "../../../orientation_sensor.h" +#include +#include +#include +#include +using namespace std; + +#define ORIENTATION_DATA_PATH "../../../../design/data/100ms/orientation/roll_pitch_yaw/" +#define ORIENTATION_DATA_SIZE 1095 + +int main() +{ + int data_available = ORIENTATION_DATA_SIZE; + ifstream accel_in, gyro_in, mag_in; + ofstream orien_file; + string line_accel, line_gyro, line_magnetic; + float sdata[3]; + unsigned long long time_stamp; + euler_angles orientation; + rotation_matrix orientation_mat; + orientation_sensor orien_sensor1, orien_sensor2; + + accel_in.open(((string)ORIENTATION_DATA_PATH + (string)"accel.txt").c_str()); + gyro_in.open(((string)ORIENTATION_DATA_PATH + (string)"gyro.txt").c_str()); + mag_in.open(((string)ORIENTATION_DATA_PATH + (string)"magnetic.txt").c_str()); + + orien_file.open(((string)"orientation.txt").c_str()); + + char *token = NULL; + + while (data_available-- > 0) + { + getline(accel_in, line_accel); + sdata[0] = strtof(line_accel.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data accel_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n"; + + getline(gyro_in, line_gyro); + sdata[0] = strtof(line_gyro.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data gyro_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n"; + + getline(mag_in, line_magnetic); + sdata[0] = strtof(line_magnetic.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data magnetic_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n"; + + orientation = orien_sensor1.get_orientation(accel_data, gyro_data, magnetic_data); + + orien_file << orientation.m_ang; + + cout << "Orientation angles\t" << orientation.m_ang << "\n\n"; + + orientation_mat = orien_sensor1.get_rotation_matrix(accel_data, gyro_data, magnetic_data); + + cout << "Orientation matrix\t" << orientation_mat.m_rot_mat << "\n\n"; + } + + accel_in.close(); + gyro_in.close(); + mag_in.close(); + orien_file.close(); + + return 0; +} -- 2.7.4 From de7cfaee524f0a0dde1c79460f6bdcaf7cc40eb0 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 24 Jul 2014 14:01:44 +0530 Subject: [PATCH 09/16] Adding gravity_sensor virtual sensor class - Adding class definition and implementation for gravity virtual sensor - Adding test code for gravity_sensor class - interface to sensor framework for virtual class will be added during porting signed-off-by: Ramasamy Change-Id: I6756922fb4d66bfab491715bac8e93b4cfbef88b --- src/sensor_fusion/standalone/gravity_sensor.cpp | 28 +++++ src/sensor_fusion/standalone/gravity_sensor.h | 36 +++++++ .../util/test/gravity_sensor_test/.cproject | 117 +++++++++++++++++++++ .../util/test/gravity_sensor_test/.project | 89 ++++++++++++++++ .../gravity_sensor_test/gravity_sensor_main.cpp | 91 ++++++++++++++++ 5 files changed, 361 insertions(+) create mode 100644 src/sensor_fusion/standalone/gravity_sensor.cpp create mode 100644 src/sensor_fusion/standalone/gravity_sensor.h create mode 100644 src/sensor_fusion/standalone/util/test/gravity_sensor_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/gravity_sensor_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/gravity_sensor_test/gravity_sensor_main.cpp diff --git a/src/sensor_fusion/standalone/gravity_sensor.cpp b/src/sensor_fusion/standalone/gravity_sensor.cpp new file mode 100644 index 0000000..abc6ff9 --- /dev/null +++ b/src/sensor_fusion/standalone/gravity_sensor.cpp @@ -0,0 +1,28 @@ +/* + * 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. + * + */ + +#ifdef _GRAVITY_SENSOR_H + +sensor_data gravity_sensor::get_gravity(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic) +{ + return comp_grav.orientation2gravity(accel, gyro, magnetic); +} + +#endif diff --git a/src/sensor_fusion/standalone/gravity_sensor.h b/src/sensor_fusion/standalone/gravity_sensor.h new file mode 100644 index 0000000..40baf9e --- /dev/null +++ b/src/sensor_fusion/standalone/gravity_sensor.h @@ -0,0 +1,36 @@ +/* + * 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 _GRAVITY_SENSOR_H +#define _GRAVITY_SENSOR_H + +#include "./util/compute_gravity.h" + +class gravity_sensor +{ +public: + compute_gravity comp_grav; + + sensor_data get_gravity(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic); +}; + +#include "gravity_sensor.cpp" + +#endif /* _GRAVITY_SENSOR_H */ diff --git a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/.cproject b/src/sensor_fusion/standalone/util/test/gravity_sensor_test/.cproject new file mode 100644 index 0000000..ee95ab0 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/gravity_sensor_test/.cproject @@ -0,0 +1,117 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/.project b/src/sensor_fusion/standalone/util/test/gravity_sensor_test/.project new file mode 100644 index 0000000..db26c33 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/gravity_sensor_test/.project @@ -0,0 +1,89 @@ + + + gravity_sensor_test + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${workspace_loc:/gravity_sensor_test/Debug} + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + org.tizen.nativecpp.apichecker.core.builder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.tizen.nativecpp.apichecker.core.tizenCppNature + + diff --git a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/gravity_sensor_main.cpp b/src/sensor_fusion/standalone/util/test/gravity_sensor_test/gravity_sensor_main.cpp new file mode 100644 index 0000000..0dfeb11 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/gravity_sensor_test/gravity_sensor_main.cpp @@ -0,0 +1,91 @@ +/* + * 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 "../../../gravity_sensor.h" +#include +#include +#include +#include +using namespace std; + +#define GRAVITY_DATA_PATH "../../../../design/data/100ms/gravity/throw/" +#define GRAVITY_DATA_SIZE 135 + +int main() +{ + int data_available = GRAVITY_DATA_SIZE; + ifstream accel_in, gyro_in, mag_in; + ofstream gravity_file; + string line_accel, line_gyro, line_magnetic; + float sdata[3]; + unsigned long long time_stamp; + sensor_data gravity; + gravity_sensor grav_sensor; + + accel_in.open(((string)GRAVITY_DATA_PATH + (string)"accel.txt").c_str()); + gyro_in.open(((string)GRAVITY_DATA_PATH + (string)"gyro.txt").c_str()); + mag_in.open(((string)GRAVITY_DATA_PATH + (string)"magnetic.txt").c_str()); + + gravity_file.open(((string)"gravity.txt").c_str()); + + char *token = NULL; + + while (data_available-- > 0) + { + getline(accel_in, line_accel); + sdata[0] = strtof(line_accel.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data accel_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n"; + + getline(gyro_in, line_gyro); + sdata[0] = strtof(line_gyro.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data gyro_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n"; + + getline(mag_in, line_magnetic); + sdata[0] = strtof(line_magnetic.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data magnetic_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n"; + + gravity = grav_sensor.get_gravity(accel_data, gyro_data, magnetic_data); + + gravity_file << gravity.m_data; + + cout << "Gravity Data\t" << gravity.m_data << "\n\n"; + } + + accel_in.close(); + gyro_in.close(); + mag_in.close(); + gravity_file.close(); + + return 0; +} -- 2.7.4 From a9b3d57d858b74fa345c11a87a9ca22db18cbce9 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 24 Jul 2014 14:06:58 +0530 Subject: [PATCH 10/16] Adding linear_acceleration_sensor class - Adding class definition and implementation for linear acceleration virtual sensor - Adding test code for linear_acceleration_sensor class - interface to sensor framework for virtual class will be added during porting signed-off-by: Ramasamy Change-Id: Id0c979fb771e60cd5675e6759de7a83f4f18807f --- .../standalone/linear_acceleration_sensor.cpp | 39 +++++++ .../standalone/linear_acceleration_sensor.h | 36 +++++++ .../test/linear_acceleration_sensor_test/.cproject | 112 +++++++++++++++++++++ .../test/linear_acceleration_sensor_test/.project | 89 ++++++++++++++++ .../linear_acceleration_sensor_main.cpp | 91 +++++++++++++++++ 5 files changed, 367 insertions(+) create mode 100644 src/sensor_fusion/standalone/linear_acceleration_sensor.cpp create mode 100644 src/sensor_fusion/standalone/linear_acceleration_sensor.h create mode 100644 src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp diff --git a/src/sensor_fusion/standalone/linear_acceleration_sensor.cpp b/src/sensor_fusion/standalone/linear_acceleration_sensor.cpp new file mode 100644 index 0000000..ea219cc --- /dev/null +++ b/src/sensor_fusion/standalone/linear_acceleration_sensor.cpp @@ -0,0 +1,39 @@ +/* + * 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. + * + */ + +#ifdef _LINEAR_ACCELERATION_SENSOR_H + +sensor_data linear_acceleration_sensor::get_linear_acceleration(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic) +{ + sensor_data gravity_data; + float la_x, la_y, la_z; + + gravity_data = comp_grav.orientation2gravity(accel, gyro, magnetic); + + la_x = accel.m_data.m_vec[0] - gravity_data.m_data.m_vec[1]; + la_y = accel.m_data.m_vec[1] - gravity_data.m_data.m_vec[0]; + la_z = accel.m_data.m_vec[2] - gravity_data.m_data.m_vec[2]; + + sensor_data lin_accel_data(la_x, la_y, la_z); + + return lin_accel_data; +} + +#endif diff --git a/src/sensor_fusion/standalone/linear_acceleration_sensor.h b/src/sensor_fusion/standalone/linear_acceleration_sensor.h new file mode 100644 index 0000000..81e8d91 --- /dev/null +++ b/src/sensor_fusion/standalone/linear_acceleration_sensor.h @@ -0,0 +1,36 @@ +/* + * 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 _LINEAR_ACCELERATION_SENSOR_H +#define _LINEAR_ACCELERATION_SENSOR_H + +#include "./util/compute_gravity.h" + +class linear_acceleration_sensor +{ +public: + compute_gravity comp_grav; + + sensor_data get_linear_acceleration(const sensor_data accel, + const sensor_data gyro, const sensor_data magnetic); +}; + +#include "linear_acceleration_sensor.cpp" + +#endif /* _LINEAR_ACCELERATION_SENSOR_H */ diff --git a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.cproject b/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.cproject new file mode 100644 index 0000000..2cab1cd --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.cproject @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.project b/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.project new file mode 100644 index 0000000..b5b1ce9 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.project @@ -0,0 +1,89 @@ + + + linear_acceleration_sensor_test + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${workspace_loc:/linear_acceleration_sensor_test/Debug} + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + org.tizen.nativecpp.apichecker.core.builder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.tizen.nativecpp.apichecker.core.tizenCppNature + + diff --git a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp b/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp new file mode 100644 index 0000000..b5686ba --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp @@ -0,0 +1,91 @@ +/* + * 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 "../../../linear_acceleration_sensor.h" +#include +#include +#include +#include +using namespace std; + +#define LA_DATA_PATH "../../../../design/data/100ms/linear_acceleration/move_x_y_z/" +#define LA_DATA_SIZE 170 + +int main() +{ + int data_available = LA_DATA_SIZE; + ifstream accel_in, gyro_in, mag_in; + ofstream la_file; + string line_accel, line_gyro, line_magnetic; + float sdata[3]; + unsigned long long time_stamp; + sensor_data lin_accel; + linear_acceleration_sensor la_sensor; + + accel_in.open(((string)LA_DATA_PATH + (string)"accel.txt").c_str()); + gyro_in.open(((string)LA_DATA_PATH + (string)"gyro.txt").c_str()); + mag_in.open(((string)LA_DATA_PATH + (string)"magnetic.txt").c_str()); + + la_file.open(((string)"linear_acceleration.txt").c_str()); + + char *token = NULL; + + while (data_available-- > 0) + { + getline(accel_in, line_accel); + sdata[0] = strtof(line_accel.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data accel_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n"; + + getline(gyro_in, line_gyro); + sdata[0] = strtof(line_gyro.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data gyro_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n"; + + getline(mag_in, line_magnetic); + sdata[0] = strtof(line_magnetic.c_str(), &token); + sdata[1] = strtof(token, &token); + sdata[2] = strtof(token, &token); + time_stamp = strtoull (token, NULL, 10); + sensor_data magnetic_data(sdata[0], sdata[1], sdata[2], time_stamp); + + cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n"; + + lin_accel = la_sensor.get_linear_acceleration(accel_data, gyro_data, magnetic_data); + + la_file << lin_accel.m_data; + + cout << "Linear Acceleration Data\t" << lin_accel.m_data << "\n\n"; + } + + accel_in.close(); + gyro_in.close(); + mag_in.close(); + la_file.close(); + + return 0; +} -- 2.7.4 From 5f59dd4fad6e65ef2e7d58d7c19d4dfe19c25155 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Wed, 6 Aug 2014 09:02:49 +0530 Subject: [PATCH 11/16] Restructuring of sensor fusion files and directory - restructured sensor fusion files and directory to enable standalone code testing and for code to support streaming sensor data. - removed lib-sensor-fusion files as sensor fusion alone will not constitute a virtual sensor. - virtual sensors would be orientation, gravity and linear acceleration which will use software sensor fusion solution. signed-off-by: Ramasamy Change-Id: Id0c979fb771e60ef5675e6759de7a83f4f18807f --- packaging/sensord.spec | 4 +- src/sensor_fusion/CMakeLists.txt | 9 +- .../{standalone/util => }/compute_gravity.cpp | 0 .../{standalone/util => }/compute_gravity.h | 0 .../{standalone/util => }/euler_angles.cpp | 0 .../{standalone/util => }/euler_angles.h | 0 src/sensor_fusion/lib_sensor_fusion.cpp | 183 --------------------- src/sensor_fusion/lib_sensor_fusion.h | 55 ------- src/sensor_fusion/{standalone/util => }/matrix.cpp | 0 src/sensor_fusion/{standalone/util => }/matrix.h | 0 .../{standalone/util => }/orientation_filter.cpp | 0 .../{standalone/util => }/orientation_filter.h | 0 .../{standalone/util => }/quaternion.cpp | 0 .../{standalone/util => }/quaternion.h | 0 .../{standalone/util => }/rotation_matrix.cpp | 0 .../{standalone/util => }/rotation_matrix.h | 0 .../{standalone/util => }/sensor_data.cpp | 0 .../{standalone/util => }/sensor_data.h | 0 src/sensor_fusion/standalone/gravity_sensor.h | 2 +- .../standalone/linear_acceleration_sensor.h | 2 +- src/sensor_fusion/standalone/orientation_sensor.h | 2 +- .../{util => }/test/compute_gravity_test/.cproject | 0 .../{util => }/test/compute_gravity_test/.project | 0 .../compute_gravity_test/compute_gravity_main.cpp | 4 +- .../{util => }/test/euler_angles_test/.cproject | 0 .../{util => }/test/euler_angles_test/.project | 0 .../test/euler_angles_test/euler_angles_main.cpp | 2 +- .../{util => }/test/gravity_sensor_test/.cproject | 0 .../{util => }/test/gravity_sensor_test/.project | 0 .../gravity_sensor_test/gravity_sensor_main.cpp | 4 +- .../test/linear_acceleration_sensor_test/.cproject | 0 .../test/linear_acceleration_sensor_test/.project | 0 .../linear_acceleration_sensor_main.cpp | 4 +- .../{util => }/test/matrix_test/.cproject | 0 .../{util => }/test/matrix_test/.project | 0 .../{util => }/test/matrix_test/matrix_main.cpp | 2 +- .../test/orientation_filter_test/.cproject | 0 .../test/orientation_filter_test/.project | 0 .../orientation_filter_main.cpp | 4 +- .../test/orientation_sensor_test/.cproject | 0 .../test/orientation_sensor_test/.project | 0 .../orientation_sensor_main.cpp | 4 +- .../{util => }/test/quaternion_test/.cproject | 0 .../{util => }/test/quaternion_test/.project | 0 .../test/quaternion_test/quaternion_main.cpp | 2 +- .../{util => }/test/rotation_matrix_test/.cproject | 0 .../{util => }/test/rotation_matrix_test/.project | 0 .../rotation_matrix_test/rotation_matrix_main.cpp | 2 +- .../{util => }/test/sensor_data_test/.cproject | 0 .../{util => }/test/sensor_data_test/.project | 0 .../test/sensor_data_test/sensor_data_main.cpp | 10 +- .../{util => }/test/vector_test/.cproject | 0 .../{util => }/test/vector_test/.project | 0 .../{util => }/test/vector_test/vector_main.cpp | 2 +- src/sensor_fusion/{standalone/util => }/vector.cpp | 0 src/sensor_fusion/{standalone/util => }/vector.h | 0 56 files changed, 34 insertions(+), 263 deletions(-) rename src/sensor_fusion/{standalone/util => }/compute_gravity.cpp (100%) rename src/sensor_fusion/{standalone/util => }/compute_gravity.h (100%) rename src/sensor_fusion/{standalone/util => }/euler_angles.cpp (100%) rename src/sensor_fusion/{standalone/util => }/euler_angles.h (100%) delete mode 100755 src/sensor_fusion/lib_sensor_fusion.cpp delete mode 100755 src/sensor_fusion/lib_sensor_fusion.h rename src/sensor_fusion/{standalone/util => }/matrix.cpp (100%) rename src/sensor_fusion/{standalone/util => }/matrix.h (100%) rename src/sensor_fusion/{standalone/util => }/orientation_filter.cpp (100%) rename src/sensor_fusion/{standalone/util => }/orientation_filter.h (100%) rename src/sensor_fusion/{standalone/util => }/quaternion.cpp (100%) rename src/sensor_fusion/{standalone/util => }/quaternion.h (100%) rename src/sensor_fusion/{standalone/util => }/rotation_matrix.cpp (100%) rename src/sensor_fusion/{standalone/util => }/rotation_matrix.h (100%) rename src/sensor_fusion/{standalone/util => }/sensor_data.cpp (100%) rename src/sensor_fusion/{standalone/util => }/sensor_data.h (100%) rename src/sensor_fusion/standalone/{util => }/test/compute_gravity_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/compute_gravity_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/compute_gravity_test/compute_gravity_main.cpp (96%) rename src/sensor_fusion/standalone/{util => }/test/euler_angles_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/euler_angles_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/euler_angles_test/euler_angles_main.cpp (98%) rename src/sensor_fusion/standalone/{util => }/test/gravity_sensor_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/gravity_sensor_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/gravity_sensor_test/gravity_sensor_main.cpp (96%) rename src/sensor_fusion/standalone/{util => }/test/linear_acceleration_sensor_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/linear_acceleration_sensor_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp (95%) rename src/sensor_fusion/standalone/{util => }/test/matrix_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/matrix_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/matrix_test/matrix_main.cpp (99%) rename src/sensor_fusion/standalone/{util => }/test/orientation_filter_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/orientation_filter_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/orientation_filter_test/orientation_filter_main.cpp (95%) rename src/sensor_fusion/standalone/{util => }/test/orientation_sensor_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/orientation_sensor_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/orientation_sensor_test/orientation_sensor_main.cpp (95%) rename src/sensor_fusion/standalone/{util => }/test/quaternion_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/quaternion_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/quaternion_test/quaternion_main.cpp (98%) rename src/sensor_fusion/standalone/{util => }/test/rotation_matrix_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/rotation_matrix_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/rotation_matrix_test/rotation_matrix_main.cpp (98%) rename src/sensor_fusion/standalone/{util => }/test/sensor_data_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/sensor_data_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/sensor_data_test/sensor_data_main.cpp (89%) rename src/sensor_fusion/standalone/{util => }/test/vector_test/.cproject (100%) rename src/sensor_fusion/standalone/{util => }/test/vector_test/.project (100%) rename src/sensor_fusion/standalone/{util => }/test/vector_test/vector_main.cpp (99%) rename src/sensor_fusion/{standalone/util => }/vector.cpp (100%) rename src/sensor_fusion/{standalone/util => }/vector.h (100%) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index d6ac9c2..f9c9215 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -16,6 +16,7 @@ Source2: sensord.socket %define gravity_state OFF %define linear_accel_state OFF %define motion_state OFF +%define sensor_fusion_state ON BuildRequires: cmake BuildRequires: vconf-keys-devel @@ -63,7 +64,8 @@ Sensord library (devel) cmake . -DCMAKE_INSTALL_PREFIX=%{_prefix} -DACCEL=%{accel_state} \ -DGYRO=%{gyro_state} -DPROXI=%{proxi_state} -DLIGHT=%{light_state} \ -DGEO=%{geo_state} -DGRAVITY=%{gravity_state} \ - -DLINEAR_ACCEL=%{linear_accel_state} -DMOTION=%{motion_state} + -DLINEAR_ACCEL=%{linear_accel_state} -DMOTION=%{motion_state} \ + -DSENSOR_FUSION=%{sensor_fusion_state} make %{?jobs:-j%jobs} diff --git a/src/sensor_fusion/CMakeLists.txt b/src/sensor_fusion/CMakeLists.txt index d03dbde..fa4e151 100755 --- a/src/sensor_fusion/CMakeLists.txt +++ b/src/sensor_fusion/CMakeLists.txt @@ -35,7 +35,14 @@ add_definitions(-Iinclude) SET(SENSOR_FUSION_NAME sensor_fusion) add_library(${SENSOR_FUSION_NAME} SHARED - lib_sensor_fusion.cpp + compute_gravity.cpp + euler_angles.cpp + matrix.cpp + orientation_filter.cpp + quaternion.cpp + rotation_matrix.cpp + sensor_data.cpp + vector.cpp ) target_link_libraries(${SENSOR_FUSION_NAME} ${rpkgs_LDFLAGS} ${GLES_LDFLAGS} "-lm") diff --git a/src/sensor_fusion/standalone/util/compute_gravity.cpp b/src/sensor_fusion/compute_gravity.cpp similarity index 100% rename from src/sensor_fusion/standalone/util/compute_gravity.cpp rename to src/sensor_fusion/compute_gravity.cpp diff --git a/src/sensor_fusion/standalone/util/compute_gravity.h b/src/sensor_fusion/compute_gravity.h similarity index 100% rename from src/sensor_fusion/standalone/util/compute_gravity.h rename to src/sensor_fusion/compute_gravity.h diff --git a/src/sensor_fusion/standalone/util/euler_angles.cpp b/src/sensor_fusion/euler_angles.cpp similarity index 100% rename from src/sensor_fusion/standalone/util/euler_angles.cpp rename to src/sensor_fusion/euler_angles.cpp diff --git a/src/sensor_fusion/standalone/util/euler_angles.h b/src/sensor_fusion/euler_angles.h similarity index 100% rename from src/sensor_fusion/standalone/util/euler_angles.h rename to src/sensor_fusion/euler_angles.h diff --git a/src/sensor_fusion/lib_sensor_fusion.cpp b/src/sensor_fusion/lib_sensor_fusion.cpp deleted file mode 100755 index 0217c97..0000000 --- a/src/sensor_fusion/lib_sensor_fusion.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/* - * 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define SENSOR_NAME "Sensor Fusion" - -lib_sensor_fusion::lib_sensor_fusion() -{ - m_name = string(SENSOR_NAME); -} - -lib_sensor_fusion::~lib_sensor_fusion() -{ -} - -bool lib_sensor_fusion::init(void) -{ - m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR); - m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR); - m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR); - - if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) { - ERR("Fail to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x", - m_accel_sensor, m_gyro_sensor, m_magnetic_sensor); - return false; - } - - INFO("%s is created!", sensor_base::get_name()); - return true; -} - -bool lib_sensor_fusion::on_start(void) -{ - AUTOLOCK(m_mutex); - - m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); - m_accel_sensor->start(); - m_gyro_sensor->add_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); - m_gyro_sensor->start(); - m_magnetic_sensor->add_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); - m_magnetic_sensor->start(); - return true; -} - -bool lib_sensor_fusion::on_stop(void) -{ - m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME); - m_accel_sensor->stop(); - m_gyro_sensor->delete_client(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME); - m_gyro_sensor->stop(); - m_magnetic_sensor->delete_client(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME); - m_magnetic_sensor->stop(); - return true; -} - -bool lib_sensor_fusion::add_interval(int client_id, unsigned int interval) -{ - AUTOLOCK(m_mutex); - - m_accel_sensor->add_interval(client_id, interval, true); - m_gyro_sensor->add_interval(client_id, interval, true); - m_magnetic_sensor->add_interval(client_id, interval, true); - return true; -} - -bool lib_sensor_fusion::delete_interval(int client_id) -{ - AUTOLOCK(m_mutex); - - m_accel_sensor->delete_interval(client_id, true); - m_gyro_sensor->delete_interval(client_id, true); - m_magnetic_sensor->delete_interval(client_id, true); - return true; -} - -bool lib_sensor_fusion::get_properties(sensor_properties_t &properties) -{ - properties.sensor_unit_idx = SENSOR_UNDEFINED_UNIT; - properties.sensor_min_range = 0; - properties.sensor_max_range = 1; - properties.sensor_resolution = 1; - strncpy(properties.sensor_vendor, "Samsung", MAX_KEY_LENGTH); - strncpy(properties.sensor_name, SENSOR_NAME, MAX_KEY_LENGTH); - return true; -} - -void lib_sensor_fusion::fuse(const sensor_event_t &event) -{ - return; -} - -bool lib_sensor_fusion::get_rotation_matrix(arr33_t &rot) -{ - return true; -} - -bool lib_sensor_fusion::get_attitude(float &x, float &y, float &z, float &w) -{ - return true; -} - -bool lib_sensor_fusion::get_gyro_bias(float &x, float &y, float &z) -{ - return true; -} - -bool lib_sensor_fusion::get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy) -{ - return true; -} - -bool lib_sensor_fusion::get_linear_acceleration(float &x, float &y, float &z) -{ - return true; -} - -bool lib_sensor_fusion::get_gravity(float &x, float &y, float &z) -{ - return true; -} - -bool lib_sensor_fusion::get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy) -{ - return true; -} - -bool lib_sensor_fusion::get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w) -{ - return true; -} - -bool lib_sensor_fusion::get_orientation(float &azimuth, float &pitch, float &roll) -{ - return true; -} - -extern "C" void *create(void) -{ - lib_sensor_fusion *inst; - - try { - inst = new lib_sensor_fusion(); - } catch (int err) { - ERR("lib_sensor_fusion class create fail , errno : %d , errstr : %s", err, strerror(err)); - return NULL; - } - - return (void *)inst; -} - -extern "C" void destroy(void *inst) -{ - delete (lib_sensor_fusion *)inst; -} diff --git a/src/sensor_fusion/lib_sensor_fusion.h b/src/sensor_fusion/lib_sensor_fusion.h deleted file mode 100755 index 85ddd3e..0000000 --- a/src/sensor_fusion/lib_sensor_fusion.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * 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 _LIB_SENSOR_FUSION_H_ -#define _LIB_SENSOR_FUSION_H_ - -#include - -class lib_sensor_fusion : public sensor_fusion -{ -public: - lib_sensor_fusion(); - ~lib_sensor_fusion(); - - bool init(void); - bool on_start(void); - bool on_stop(void); - - bool add_interval(int client_id, unsigned int interval); - bool delete_interval(int client_id); - bool get_properties(sensor_properties_t &properties); - - void fuse(const sensor_event_t &event); - bool get_rotation_matrix(arr33_t &rot); - bool get_attitude(float &x, float &y, float &z, float &w); - bool get_gyro_bias(float &x, float &y, float &z); - bool get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy); - bool get_linear_acceleration(float &x, float &y, float &z); - bool get_gravity(float &x, float &y, float &z); - bool get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy); - bool get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w); - bool get_orientation(float &azimuth, float &pitch, float &roll); -private: - sensor_base *m_accel_sensor; - sensor_base *m_gyro_sensor; - sensor_base *m_magnetic_sensor; -}; - -#endif /*_LIB_SENSOR_FUSION_H_*/ diff --git a/src/sensor_fusion/standalone/util/matrix.cpp b/src/sensor_fusion/matrix.cpp similarity index 100% rename from src/sensor_fusion/standalone/util/matrix.cpp rename to src/sensor_fusion/matrix.cpp diff --git a/src/sensor_fusion/standalone/util/matrix.h b/src/sensor_fusion/matrix.h similarity index 100% rename from src/sensor_fusion/standalone/util/matrix.h rename to src/sensor_fusion/matrix.h diff --git a/src/sensor_fusion/standalone/util/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp similarity index 100% rename from src/sensor_fusion/standalone/util/orientation_filter.cpp rename to src/sensor_fusion/orientation_filter.cpp diff --git a/src/sensor_fusion/standalone/util/orientation_filter.h b/src/sensor_fusion/orientation_filter.h similarity index 100% rename from src/sensor_fusion/standalone/util/orientation_filter.h rename to src/sensor_fusion/orientation_filter.h diff --git a/src/sensor_fusion/standalone/util/quaternion.cpp b/src/sensor_fusion/quaternion.cpp similarity index 100% rename from src/sensor_fusion/standalone/util/quaternion.cpp rename to src/sensor_fusion/quaternion.cpp diff --git a/src/sensor_fusion/standalone/util/quaternion.h b/src/sensor_fusion/quaternion.h similarity index 100% rename from src/sensor_fusion/standalone/util/quaternion.h rename to src/sensor_fusion/quaternion.h diff --git a/src/sensor_fusion/standalone/util/rotation_matrix.cpp b/src/sensor_fusion/rotation_matrix.cpp similarity index 100% rename from src/sensor_fusion/standalone/util/rotation_matrix.cpp rename to src/sensor_fusion/rotation_matrix.cpp diff --git a/src/sensor_fusion/standalone/util/rotation_matrix.h b/src/sensor_fusion/rotation_matrix.h similarity index 100% rename from src/sensor_fusion/standalone/util/rotation_matrix.h rename to src/sensor_fusion/rotation_matrix.h diff --git a/src/sensor_fusion/standalone/util/sensor_data.cpp b/src/sensor_fusion/sensor_data.cpp similarity index 100% rename from src/sensor_fusion/standalone/util/sensor_data.cpp rename to src/sensor_fusion/sensor_data.cpp diff --git a/src/sensor_fusion/standalone/util/sensor_data.h b/src/sensor_fusion/sensor_data.h similarity index 100% rename from src/sensor_fusion/standalone/util/sensor_data.h rename to src/sensor_fusion/sensor_data.h diff --git a/src/sensor_fusion/standalone/gravity_sensor.h b/src/sensor_fusion/standalone/gravity_sensor.h index 40baf9e..94113f1 100644 --- a/src/sensor_fusion/standalone/gravity_sensor.h +++ b/src/sensor_fusion/standalone/gravity_sensor.h @@ -20,7 +20,7 @@ #ifndef _GRAVITY_SENSOR_H #define _GRAVITY_SENSOR_H -#include "./util/compute_gravity.h" +#include "../compute_gravity.h" class gravity_sensor { diff --git a/src/sensor_fusion/standalone/linear_acceleration_sensor.h b/src/sensor_fusion/standalone/linear_acceleration_sensor.h index 81e8d91..caed8d2 100644 --- a/src/sensor_fusion/standalone/linear_acceleration_sensor.h +++ b/src/sensor_fusion/standalone/linear_acceleration_sensor.h @@ -20,7 +20,7 @@ #ifndef _LINEAR_ACCELERATION_SENSOR_H #define _LINEAR_ACCELERATION_SENSOR_H -#include "./util/compute_gravity.h" +#include "../compute_gravity.h" class linear_acceleration_sensor { diff --git a/src/sensor_fusion/standalone/orientation_sensor.h b/src/sensor_fusion/standalone/orientation_sensor.h index 89312c3..487d67a 100644 --- a/src/sensor_fusion/standalone/orientation_sensor.h +++ b/src/sensor_fusion/standalone/orientation_sensor.h @@ -20,7 +20,7 @@ #ifndef _ORIENTATION_SENSOR_H #define _ORIENTATION_SENSOR_H -#include "./util/orientation_filter.h" +#include "../orientation_filter.h" class orientation_sensor { diff --git a/src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject b/src/sensor_fusion/standalone/test/compute_gravity_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/compute_gravity_test/.cproject rename to src/sensor_fusion/standalone/test/compute_gravity_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/compute_gravity_test/.project b/src/sensor_fusion/standalone/test/compute_gravity_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/compute_gravity_test/.project rename to src/sensor_fusion/standalone/test/compute_gravity_test/.project diff --git a/src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp b/src/sensor_fusion/standalone/test/compute_gravity_test/compute_gravity_main.cpp similarity index 96% rename from src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp rename to src/sensor_fusion/standalone/test/compute_gravity_test/compute_gravity_main.cpp index bd56100..6d6ec4f 100644 --- a/src/sensor_fusion/standalone/util/test/compute_gravity_test/compute_gravity_main.cpp +++ b/src/sensor_fusion/standalone/test/compute_gravity_test/compute_gravity_main.cpp @@ -17,14 +17,14 @@ * */ -#include "../../compute_gravity.h" +#include "../../../compute_gravity.h" #include #include #include #include using namespace std; -#define GRAVITY_DATA_PATH "../../../../design/data/100ms/gravity/throw/" +#define GRAVITY_DATA_PATH "../../../design/data/100ms/gravity/throw/" #define GRAVITY_DATA_SIZE 135 int main() diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject b/src/sensor_fusion/standalone/test/euler_angles_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject rename to src/sensor_fusion/standalone/test/euler_angles_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/.project b/src/sensor_fusion/standalone/test/euler_angles_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/euler_angles_test/.project rename to src/sensor_fusion/standalone/test/euler_angles_test/.project diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp b/src/sensor_fusion/standalone/test/euler_angles_test/euler_angles_main.cpp similarity index 98% rename from src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp rename to src/sensor_fusion/standalone/test/euler_angles_test/euler_angles_main.cpp index 592e782..5e29ce0 100644 --- a/src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp +++ b/src/sensor_fusion/standalone/test/euler_angles_test/euler_angles_main.cpp @@ -17,7 +17,7 @@ * */ -#include "../../euler_angles.h" +#include "../../../euler_angles.h" int main() { diff --git a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/.cproject b/src/sensor_fusion/standalone/test/gravity_sensor_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/gravity_sensor_test/.cproject rename to src/sensor_fusion/standalone/test/gravity_sensor_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/.project b/src/sensor_fusion/standalone/test/gravity_sensor_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/gravity_sensor_test/.project rename to src/sensor_fusion/standalone/test/gravity_sensor_test/.project diff --git a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/gravity_sensor_main.cpp b/src/sensor_fusion/standalone/test/gravity_sensor_test/gravity_sensor_main.cpp similarity index 96% rename from src/sensor_fusion/standalone/util/test/gravity_sensor_test/gravity_sensor_main.cpp rename to src/sensor_fusion/standalone/test/gravity_sensor_test/gravity_sensor_main.cpp index 0dfeb11..a9f801c 100644 --- a/src/sensor_fusion/standalone/util/test/gravity_sensor_test/gravity_sensor_main.cpp +++ b/src/sensor_fusion/standalone/test/gravity_sensor_test/gravity_sensor_main.cpp @@ -17,14 +17,14 @@ * */ -#include "../../../gravity_sensor.h" +#include "../../gravity_sensor.h" #include #include #include #include using namespace std; -#define GRAVITY_DATA_PATH "../../../../design/data/100ms/gravity/throw/" +#define GRAVITY_DATA_PATH "../../../design/data/100ms/gravity/throw/" #define GRAVITY_DATA_SIZE 135 int main() diff --git a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.cproject b/src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.cproject rename to src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.project b/src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/.project rename to src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/.project diff --git a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp b/src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp similarity index 95% rename from src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp rename to src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp index b5686ba..302affa 100644 --- a/src/sensor_fusion/standalone/util/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp +++ b/src/sensor_fusion/standalone/test/linear_acceleration_sensor_test/linear_acceleration_sensor_main.cpp @@ -17,14 +17,14 @@ * */ -#include "../../../linear_acceleration_sensor.h" +#include "../../linear_acceleration_sensor.h" #include #include #include #include using namespace std; -#define LA_DATA_PATH "../../../../design/data/100ms/linear_acceleration/move_x_y_z/" +#define LA_DATA_PATH "../../../design/data/100ms/linear_acceleration/move_x_y_z/" #define LA_DATA_SIZE 170 int main() diff --git a/src/sensor_fusion/standalone/util/test/matrix_test/.cproject b/src/sensor_fusion/standalone/test/matrix_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/matrix_test/.cproject rename to src/sensor_fusion/standalone/test/matrix_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/matrix_test/.project b/src/sensor_fusion/standalone/test/matrix_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/matrix_test/.project rename to src/sensor_fusion/standalone/test/matrix_test/.project diff --git a/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp b/src/sensor_fusion/standalone/test/matrix_test/matrix_main.cpp similarity index 99% rename from src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp rename to src/sensor_fusion/standalone/test/matrix_test/matrix_main.cpp index ed00949..d1a2153 100644 --- a/src/sensor_fusion/standalone/util/test/matrix_test/matrix_main.cpp +++ b/src/sensor_fusion/standalone/test/matrix_test/matrix_main.cpp @@ -17,7 +17,7 @@ * */ -#include "../../matrix.h" +#include "../../../matrix.h" int main() { diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject b/src/sensor_fusion/standalone/test/orientation_filter_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/orientation_filter_test/.cproject rename to src/sensor_fusion/standalone/test/orientation_filter_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/.project b/src/sensor_fusion/standalone/test/orientation_filter_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/orientation_filter_test/.project rename to src/sensor_fusion/standalone/test/orientation_filter_test/.project diff --git a/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp b/src/sensor_fusion/standalone/test/orientation_filter_test/orientation_filter_main.cpp similarity index 95% rename from src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp rename to src/sensor_fusion/standalone/test/orientation_filter_test/orientation_filter_main.cpp index 2743f97..5273904 100644 --- a/src/sensor_fusion/standalone/util/test/orientation_filter_test/orientation_filter_main.cpp +++ b/src/sensor_fusion/standalone/test/orientation_filter_test/orientation_filter_main.cpp @@ -17,14 +17,14 @@ * */ -#include "../../orientation_filter.h" +#include "../../../orientation_filter.h" #include #include #include #include using namespace std; -#define ORIENTATION_DATA_PATH "../../../../design/data/100ms/orientation/roll_pitch_yaw/" +#define ORIENTATION_DATA_PATH "../../../design/data/100ms/orientation/roll_pitch_yaw/" #define ORIENTATION_DATA_SIZE 1095 int main() diff --git a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/.cproject b/src/sensor_fusion/standalone/test/orientation_sensor_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/orientation_sensor_test/.cproject rename to src/sensor_fusion/standalone/test/orientation_sensor_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/.project b/src/sensor_fusion/standalone/test/orientation_sensor_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/orientation_sensor_test/.project rename to src/sensor_fusion/standalone/test/orientation_sensor_test/.project diff --git a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/orientation_sensor_main.cpp b/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp similarity index 95% rename from src/sensor_fusion/standalone/util/test/orientation_sensor_test/orientation_sensor_main.cpp rename to src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp index c755cbe..b3ef1f5 100644 --- a/src/sensor_fusion/standalone/util/test/orientation_sensor_test/orientation_sensor_main.cpp +++ b/src/sensor_fusion/standalone/test/orientation_sensor_test/orientation_sensor_main.cpp @@ -17,14 +17,14 @@ * */ -#include "../../../orientation_sensor.h" +#include "../../orientation_sensor.h" #include #include #include #include using namespace std; -#define ORIENTATION_DATA_PATH "../../../../design/data/100ms/orientation/roll_pitch_yaw/" +#define ORIENTATION_DATA_PATH "../../../design/data/100ms/orientation/roll_pitch_yaw/" #define ORIENTATION_DATA_SIZE 1095 int main() diff --git a/src/sensor_fusion/standalone/util/test/quaternion_test/.cproject b/src/sensor_fusion/standalone/test/quaternion_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/quaternion_test/.cproject rename to src/sensor_fusion/standalone/test/quaternion_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/quaternion_test/.project b/src/sensor_fusion/standalone/test/quaternion_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/quaternion_test/.project rename to src/sensor_fusion/standalone/test/quaternion_test/.project diff --git a/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp b/src/sensor_fusion/standalone/test/quaternion_test/quaternion_main.cpp similarity index 98% rename from src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp rename to src/sensor_fusion/standalone/test/quaternion_test/quaternion_main.cpp index e2e371f..2708d5e 100644 --- a/src/sensor_fusion/standalone/util/test/quaternion_test/quaternion_main.cpp +++ b/src/sensor_fusion/standalone/test/quaternion_test/quaternion_main.cpp @@ -17,7 +17,7 @@ * */ -#include "../../quaternion.h" +#include "../../../quaternion.h" int main() { diff --git a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/.cproject b/src/sensor_fusion/standalone/test/rotation_matrix_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/rotation_matrix_test/.cproject rename to src/sensor_fusion/standalone/test/rotation_matrix_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/.project b/src/sensor_fusion/standalone/test/rotation_matrix_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/rotation_matrix_test/.project rename to src/sensor_fusion/standalone/test/rotation_matrix_test/.project diff --git a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/rotation_matrix_main.cpp b/src/sensor_fusion/standalone/test/rotation_matrix_test/rotation_matrix_main.cpp similarity index 98% rename from src/sensor_fusion/standalone/util/test/rotation_matrix_test/rotation_matrix_main.cpp rename to src/sensor_fusion/standalone/test/rotation_matrix_test/rotation_matrix_main.cpp index fa1b639..21ecadf 100644 --- a/src/sensor_fusion/standalone/util/test/rotation_matrix_test/rotation_matrix_main.cpp +++ b/src/sensor_fusion/standalone/test/rotation_matrix_test/rotation_matrix_main.cpp @@ -17,7 +17,7 @@ * */ -#include "../../rotation_matrix.h" +#include "../../../rotation_matrix.h" int main() { diff --git a/src/sensor_fusion/standalone/util/test/sensor_data_test/.cproject b/src/sensor_fusion/standalone/test/sensor_data_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/sensor_data_test/.cproject rename to src/sensor_fusion/standalone/test/sensor_data_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/sensor_data_test/.project b/src/sensor_fusion/standalone/test/sensor_data_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/sensor_data_test/.project rename to src/sensor_fusion/standalone/test/sensor_data_test/.project diff --git a/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp b/src/sensor_fusion/standalone/test/sensor_data_test/sensor_data_main.cpp similarity index 89% rename from src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp rename to src/sensor_fusion/standalone/test/sensor_data_test/sensor_data_main.cpp index 511e949..1ebb100 100644 --- a/src/sensor_fusion/standalone/util/test/sensor_data_test/sensor_data_main.cpp +++ b/src/sensor_fusion/standalone/test/sensor_data_test/sensor_data_main.cpp @@ -17,7 +17,7 @@ * */ -#include "../../sensor_data.h" +#include "../../../sensor_data.h" int main() { @@ -45,12 +45,12 @@ int main() cout<< "\nSum:\n" << sd9.m_data << endl; cout<< "\n\n\nNormalization:\n"; - sensor_data sd6 = normalize(sd3); cout<< "\n" << sd3.m_data; - cout<< "\nResult:\n" << sd6.m_data << endl; - sensor_data sd7 = normalize(sd2); + normalize(sd3); + cout<< "\nResult:\n" << sd3.m_data << endl; cout<< "\n" << sd2.m_data; - cout<< "\nResult:\n" << sd7.m_data << endl; + normalize(sd2); + cout<< "\nResult:\n" << sd2.m_data << endl; float xx = 2.5; cout<<"\n\n\nScale data:\n"; diff --git a/src/sensor_fusion/standalone/util/test/vector_test/.cproject b/src/sensor_fusion/standalone/test/vector_test/.cproject similarity index 100% rename from src/sensor_fusion/standalone/util/test/vector_test/.cproject rename to src/sensor_fusion/standalone/test/vector_test/.cproject diff --git a/src/sensor_fusion/standalone/util/test/vector_test/.project b/src/sensor_fusion/standalone/test/vector_test/.project similarity index 100% rename from src/sensor_fusion/standalone/util/test/vector_test/.project rename to src/sensor_fusion/standalone/test/vector_test/.project diff --git a/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp b/src/sensor_fusion/standalone/test/vector_test/vector_main.cpp similarity index 99% rename from src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp rename to src/sensor_fusion/standalone/test/vector_test/vector_main.cpp index 725934f..ee5b996 100644 --- a/src/sensor_fusion/standalone/util/test/vector_test/vector_main.cpp +++ b/src/sensor_fusion/standalone/test/vector_test/vector_main.cpp @@ -17,7 +17,7 @@ * */ -#include "../../vector.h" +#include "../../../vector.h" int main() { diff --git a/src/sensor_fusion/standalone/util/vector.cpp b/src/sensor_fusion/vector.cpp similarity index 100% rename from src/sensor_fusion/standalone/util/vector.cpp rename to src/sensor_fusion/vector.cpp diff --git a/src/sensor_fusion/standalone/util/vector.h b/src/sensor_fusion/vector.h similarity index 100% rename from src/sensor_fusion/standalone/util/vector.h rename to src/sensor_fusion/vector.h -- 2.7.4 From a1e10a246eb1bda21da90cdd9d3c675ff0f71d78 Mon Sep 17 00:00:00 2001 From: Amit Dharmapurikar Date: Tue, 12 Aug 2014 17:49:16 +0530 Subject: [PATCH 12/16] Modified geo_sensor_hal plugin for IIO driver compatibility - The geo_sensor_hal plugin is modified as per present geo sensor IIO driver interface. * the plugin reads data from raw data files instead of char-dev file in /dev/ directory, * time stamp for data samples is not available * enable and disable actions for geo sensor are not available. - The geo sensor is enabled in the sensord.spec file Change-Id: I8a02be99c37bae3e37f36fa4291810410fcc3976 Signed-off-by: Amit Dharmapurikar --- packaging/sensord.spec | 2 +- src/geo/geo_sensor_hal.cpp | 395 +++++++++------------------------------------ src/geo/geo_sensor_hal.h | 48 ++++-- 3 files changed, 120 insertions(+), 325 deletions(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index d6ac9c2..4b094de 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -12,7 +12,7 @@ Source2: sensord.socket %define gyro_state ON %define proxi_state OFF %define light_state OFF -%define geo_state OFF +%define geo_state ON %define gravity_state OFF %define linear_accel_state OFF %define motion_state OFF diff --git a/src/geo/geo_sensor_hal.cpp b/src/geo/geo_sensor_hal.cpp index b1cadcf..0d65ab7 100755 --- a/src/geo/geo_sensor_hal.cpp +++ b/src/geo/geo_sensor_hal.cpp @@ -27,346 +27,155 @@ #include using std::ifstream; +using std::string; using config::CConfig; -#define NODE_NAME "name" -#define NODE_INPUT "input" -#define NODE_ENABLE "enable" -#define NODE_POLL_DELAY "poll_delay" -#define NODE_MAG_POLL_DELAY "mag_poll_delay" -#define SENSOR_NODE "/sys/class/sensors/" -#define SENSORHUB_NODE "/sys/class/sensors/ssp_sensor/" -#define INPUT_DEVICE_NODE "/sys/class/input/" -#define DEV_INPUT_NODE "/dev/input/event/" - -#define LBS_TO_UTESLA 0.06f - #define SENSOR_TYPE_MAGNETIC "MAGNETIC" #define ELEMENT_NAME "NAME" #define ELEMENT_VENDOR "VENDOR" #define ATTR_VALUE "value" -#define INPUT_NAME "geomagnetic_sensor" +#define SENSOR_MIN_RANGE -1200 +#define SENSOR_MAX_RANGE 1200 +#define INITIAL_TIME 0 +#define INITIAL_VALUE -1 +#define GAUSS_TO_UTESLA(val) ((val) * 100) geo_sensor_hal::geo_sensor_hal() : m_x(INITIAL_VALUE) , m_y(INITIAL_VALUE) , m_z(INITIAL_VALUE) -, m_node_handle(INITIAL_VALUE) , m_polling_interval(POLL_1HZ_MS) , m_fired_time(INITIAL_TIME) , m_sensorhub_supported(false) { - if (!check_hw_node()) { + if (!check_hw_node()) + { ERR("check_hw_node() fail"); throw ENXIO; } CConfig &config = CConfig::get_instance(); - if (!config.get(SENSOR_TYPE_MAGNETIC, m_model_id, ELEMENT_VENDOR, m_vendor)) { + if (!config.get(SENSOR_TYPE_MAGNETIC, m_model_id, ELEMENT_VENDOR, m_vendor)) + { ERR("[VENDOR] is empty"); throw ENXIO; } INFO("m_vendor = %s", m_vendor.c_str()); - if (!config.get(SENSOR_TYPE_MAGNETIC, m_model_id, ELEMENT_NAME, m_chip_name)) { + if (!config.get(SENSOR_TYPE_MAGNETIC, m_model_id, ELEMENT_NAME, m_chip_name)) + { ERR("[NAME] is empty"); throw ENXIO; } INFO("m_chip_name = %s", m_chip_name.c_str()); - if ((m_node_handle = open(m_resource.c_str(), O_RDWR)) < 0) { - ERR("Failed to open handle(%d)", m_node_handle); + if (!init_resources()) throw ENXIO; - } - - int clockId = CLOCK_MONOTONIC; - - if (ioctl(m_node_handle, EVIOCSCLOCKID, &clockId) != 0) { - ERR("Fail to set monotonic timestamp for %s", m_resource.c_str()); - throw ENXIO; - } INFO("geo_sensor_hal is created!"); } geo_sensor_hal::~geo_sensor_hal() { - close(m_node_handle); - m_node_handle = INITIAL_VALUE; - INFO("geo_sensor_hal is destroyed!"); } -string geo_sensor_hal::get_model_id(void) -{ - return m_model_id; -} - -sensor_type_t geo_sensor_hal::get_type(void) +bool geo_sensor_hal::init_resources(void) { - return GEOMAGNETIC_SENSOR; -} + ifstream temp_handle; -bool geo_sensor_hal::enable_resource(string &resource_node, bool enable) -{ - int prev_status, status; - FILE *fp = NULL; - fp = fopen(resource_node.c_str(), "r"); - - if (!fp) { - ERR("Fail to open a resource file: %s", resource_node.c_str()); + if (!read_node_value(m_x_scale_node, m_x_scale)) return false; - } - if (fscanf(fp, "%d", &prev_status) < 0) { - ERR("Failed to get data from %s", resource_node.c_str()); - fclose(fp); + if (!read_node_value(m_y_scale_node, m_y_scale)) return false; - } - fclose(fp); - - if (enable) { - if (m_sensorhub_supported) - status = prev_status | (1 << SENSORHUB_GEOMAGNETIC_ENABLE_BIT); - else - status = 1; - } else { - if (m_sensorhub_supported) - status = prev_status ^ (1 << SENSORHUB_GEOMAGNETIC_ENABLE_BIT); - else - status = 0; - } - - fp = fopen(resource_node.c_str(), "w"); - - if (!fp) { - ERR("Failed to open a resource file: %s", resource_node.c_str()); + if (!read_node_value(m_z_scale_node, m_z_scale)) return false; - } - - if (fprintf(fp, "%d", status) < 0) { - ERR("Failed to enable a resource file: %s", resource_node.c_str()); - fclose(fp); - return false; - } - - if (fp) - fclose(fp); + INFO("Scale Values: %f, %f, %f", m_x_scale, m_y_scale, m_z_scale); return true; } -bool geo_sensor_hal::enable(void) +string geo_sensor_hal::get_model_id(void) { - AUTOLOCK(m_mutex); + return m_model_id; +} - enable_resource(m_enable_resource, true); - set_interval(m_polling_interval); +sensor_type_t geo_sensor_hal::get_type(void) +{ + return GEOMAGNETIC_SENSOR; +} - m_fired_time = 0; - INFO("Geo sensor real starting"); +bool geo_sensor_hal::enable(void) +{ + INFO("Resource already enabled. Enable not supported."); return true; } bool geo_sensor_hal::disable(void) { - AUTOLOCK(m_mutex); - - enable_resource(m_enable_resource, false); - INFO("Geo sensor real stopping"); + INFO("Disable not supported."); return true; } bool geo_sensor_hal::set_interval(unsigned long val) { - unsigned long long polling_interval_ns; - FILE *fp = NULL; - - AUTOLOCK(m_mutex); - - polling_interval_ns = ((unsigned long long)(val) * MS_TO_SEC * MS_TO_SEC); - fp = fopen(m_polling_resource.c_str(), "w"); - - if (!fp) { - ERR("Failed to open a resource file: %s", m_polling_resource.c_str()); - return false; - } - - if (fprintf(fp, "%llu", polling_interval_ns) < 0) { - ERR("Failed to set data %llu", polling_interval_ns); - fclose(fp); - return false; - } - - if (fp) - fclose(fp); - - INFO("Interval is changed from %dms to %dms]", m_polling_interval, val); - m_polling_interval = val; + INFO("Polling interval cannot be changed."); return true; } -bool geo_sensor_hal::update_value(bool wait) +bool geo_sensor_hal::update_value(void) { - const int TIMEOUT = 1; - int geo_raw[4] = {0,}; - bool x, y, z, hdst; - int read_input_cnt = 0; - const int INPUT_MAX_BEFORE_SYN = 10; - unsigned long long fired_time = 0; - bool syn = false; - x = y = z = hdst = false; - - struct timeval tv; - fd_set readfds, exceptfds; - - FD_ZERO(&readfds); - FD_ZERO(&exceptfds); - FD_SET(m_node_handle, &readfds); - FD_SET(m_node_handle, &exceptfds); - - if (wait) { - tv.tv_sec = TIMEOUT; - tv.tv_usec = 0; - } else { - tv.tv_sec = 0; - tv.tv_usec = 0; - } - - int ret; - ret = select(m_node_handle + 1, &readfds, NULL, &exceptfds, &tv); + int raw_values[3] = {0,}; + ifstream temp_handle; - if (ret == -1) { - ERR("select error:%s m_node_handle:%d", strerror(errno), m_node_handle); + if (!read_node_value(m_x_node, raw_values[0])) return false; - } else if (!ret) { - DBG("select timeout: %d seconds elapsed", tv.tv_sec); - return false; - } - if (FD_ISSET(m_node_handle, &exceptfds)) { - ERR("select exception occurred!"); + if (!read_node_value(m_y_node, raw_values[1])) return false; - } - - if (FD_ISSET(m_node_handle, &readfds)) { - struct input_event geo_input; - DBG("geo event detection!"); - - while ((syn == false) && (read_input_cnt < INPUT_MAX_BEFORE_SYN)) { - int len = read(m_node_handle, &geo_input, sizeof(geo_input)); - - if (len != sizeof(geo_input)) { - ERR("geo_file read fail, read_len = %d", len); - return false; - } - ++read_input_cnt; - - if (geo_input.type == EV_REL) { - switch (geo_input.code) { - case REL_RX: - geo_raw[0] = (int)geo_input.value; - x = true; - break; - case REL_RY: - geo_raw[1] = (int)geo_input.value; - y = true; - break; - case REL_RZ: - geo_raw[2] = (int)geo_input.value; - z = true; - break; - case REL_HWHEEL: - geo_raw[3] = (int)geo_input.value; - hdst = true; - break; - default: - ERR("geo_input event[type = %d, code = %d] is unknown.", geo_input.type, geo_input.code); - return false; - break; - } - } else if (geo_input.type == EV_SYN) { - syn = true; - fired_time = get_timestamp(&geo_input.time); - } else { - ERR("geo_input event[type = %d, code = %d] is unknown.", geo_input.type, geo_input.code); - return false; - } - } - } else { - ERR("select nothing to read!!!"); - return false; - } - - if (syn == false) { - ERR("EV_SYN didn't come until %d inputs had come", read_input_cnt); + if (!read_node_value(m_z_node, raw_values[2])) return false; - } - AUTOLOCK(m_value_mutex); + m_x = GAUSS_TO_UTESLA(raw_values[0] * m_x_scale); + m_y = GAUSS_TO_UTESLA(raw_values[1] * m_y_scale); + m_z = GAUSS_TO_UTESLA(raw_values[2] * m_z_scale); - if (x) - m_x = geo_raw[0]; + m_fired_time = INITIAL_TIME; + INFO("x = %d, y = %d, z = %d, time = %lluus", raw_values[0], raw_values[0], raw_values[0], m_fired_time); - if (y) - m_y = geo_raw[1]; - - if (z) - m_z = geo_raw[2]; - - if (hdst) - m_hdst = geo_raw[3] - 1; /* accuracy bias: -1 */ - - m_fired_time = fired_time; - - DBG("m_x = %d, m_y = %d, m_z = %d, m_hdst = %d, time = %lluus", m_x, m_y, m_z, m_hdst, m_fired_time); return true; } bool geo_sensor_hal::is_data_ready(bool wait) { bool ret; - ret = update_value(wait); + ret = update_value(); return ret; } int geo_sensor_hal::get_sensor_data(sensor_data_t &data) { - const int chance = 3; - int retry = 0; - - while ((m_fired_time == 0) && (retry++ < chance)) { - INFO("Try usleep for getting a valid BASE DATA value"); - usleep(m_polling_interval * MS_TO_SEC); - } - - if (m_fired_time == 0) { - ERR("get_sensor_data failed"); - return -1; - } - - data.data_accuracy = (m_hdst == 1) ? 0 : m_hdst; /* hdst 0 and 1 are needed to calibrate */ data.data_unit_idx = SENSOR_UNIT_MICRO_TESLA; data.timestamp = m_fired_time; data.values_num = 3; - data.values[0] = (float)m_x * LBS_TO_UTESLA; - data.values[1] = (float)m_y * LBS_TO_UTESLA; - data.values[2] = (float)m_z * LBS_TO_UTESLA; - + data.values[0] = (float)m_x; + data.values[1] = (float)m_y; + data.values[2] = (float)m_z; return 0; } bool geo_sensor_hal::get_properties(sensor_properties_t &properties) { properties.sensor_unit_idx = SENSOR_UNIT_MICRO_TESLA; - properties.sensor_min_range = -1200; - properties.sensor_max_range = 1200; + properties.sensor_min_range = SENSOR_MIN_RANGE; + properties.sensor_max_range = SENSOR_MAX_RANGE; snprintf(properties.sensor_name, sizeof(properties.sensor_name), "%s", m_chip_name.c_str()); snprintf(properties.sensor_vendor, sizeof(properties.sensor_vendor), "%s", m_vendor.c_str()); properties.sensor_resolution = 1; @@ -375,24 +184,13 @@ bool geo_sensor_hal::get_properties(sensor_properties_t &properties) bool geo_sensor_hal::is_sensorhub_supported(void) { - FILE *fp = NULL; - string mag_polling_resource = string(SENSORHUB_NODE) + string(MAG_POLL_DELAY); - fp = fopen(mag_polling_resource.c_str(), "r"); - - if (!fp) { - ERR("Fail to open a resource file"); - return false; - } - - INFO("Supported by Sensor-Hub"); - fclose(fp); - return true; + return false; } bool geo_sensor_hal::check_hw_node(void) { - string name_node; string hw_name; + string file_name; DIR *main_dir = NULL; struct dirent *dir_entry = NULL; bool find_node = false; @@ -400,87 +198,51 @@ bool geo_sensor_hal::check_hw_node(void) INFO("======================start check_hw_node============================="); m_sensorhub_supported = is_sensorhub_supported(); - main_dir = opendir(SENSOR_NODE); + main_dir = opendir(IIO_DIR); - if (!main_dir) { - ERR("Directory open failed to collect data"); + if (!main_dir) + { + ERR("Could not open IIO directory\n"); return false; } - while ((!find_node) && (dir_entry = readdir(main_dir))) { - if ((strncasecmp(dir_entry->d_name , ".", 1 ) != 0) && (strncasecmp(dir_entry->d_name , "..", 2 ) != 0) && (dir_entry->d_ino != 0)) { - name_node = string(SENSOR_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_NAME); + while (!find_node) + { + dir_entry = readdir(main_dir); + if(dir_entry == NULL) + break; - ifstream infile(name_node.c_str()); + if ((strncasecmp(dir_entry->d_name , ".", 1 ) != 0) && (strncasecmp(dir_entry->d_name , "..", 2 ) != 0) && (dir_entry->d_ino != 0)) + { + file_name = string(IIO_DIR) + string(dir_entry->d_name) + string(NAME_NODE); + ifstream infile(file_name.c_str()); if (!infile) continue; infile >> hw_name; - if (CConfig::get_instance().is_supported(SENSOR_TYPE_MAGNETIC, hw_name) == true) { + if (CConfig::get_instance().is_supported(SENSOR_TYPE_MAGNETIC, hw_name) == true) + { m_name = m_model_id = hw_name; INFO("m_model_id = %s", m_model_id.c_str()); - find_node = true; - break; - } - } - } - - closedir(main_dir); - - if (find_node) { - main_dir = opendir(INPUT_DEVICE_NODE); - - if (!main_dir) { - ERR("Directory open failed to collect data"); - return false; - } - find_node = false; + string temp = string(IIO_DIR) + string(dir_entry->d_name); - while ((!find_node) && (dir_entry = readdir(main_dir))) { - if (strncasecmp(dir_entry->d_name, NODE_INPUT, 5) == 0) { - name_node = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_NAME); - ifstream infile(name_node.c_str()); + m_x_node = temp + string(X_RAW_VAL_NODE); + m_y_node = temp + string(Y_RAW_VAL_NODE); + m_z_node = temp + string(Z_RAW_VAL_NODE); + m_x_scale_node = temp + string(X_SCALE_NODE); + m_y_scale_node = temp + string(Y_SCALE_NODE); + m_z_scale_node = temp + string(Z_SCALE_NODE); - if (!infile) - continue; - - infile >> hw_name; - - if (hw_name == string(INPUT_NAME)) { - INFO("name_node = %s", name_node.c_str()); - DBG("Find H/W for mag_sensor"); - - find_node = true; - string dir_name; - dir_name = string(dir_entry->d_name); - unsigned found = dir_name.find_first_not_of(NODE_INPUT); - m_resource = string(DEV_INPUT_NODE) + dir_name.substr(found); - - if (m_sensorhub_supported) { - m_enable_resource = string(SENSORHUB_NODE) + string(NODE_ENABLE); - m_polling_resource = string(SENSORHUB_NODE) + string(NODE_MAG_POLL_DELAY); - } else { - m_enable_resource = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_ENABLE); - m_polling_resource = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_POLL_DELAY); - } - - break; - } + find_node = true; + break; } } - - closedir(main_dir); - } - - if (find_node) { - INFO("m_resource = %s", m_resource.c_str()); - INFO("m_enable_resource = %s", m_enable_resource.c_str()); - INFO("m_polling_resource = %s", m_polling_resource.c_str()); } + closedir(main_dir); return find_node; } @@ -488,9 +250,12 @@ extern "C" void *create(void) { geo_sensor_hal *inst; - try { + try + { inst = new geo_sensor_hal(); - } catch (int err) { + } + catch (int err) + { ERR("Failed to create geo_sensor_hal class, errno : %d, errstr : %s", err, strerror(err)); return NULL; } diff --git a/src/geo/geo_sensor_hal.h b/src/geo/geo_sensor_hal.h index 066d672..471e7a7 100755 --- a/src/geo/geo_sensor_hal.h +++ b/src/geo/geo_sensor_hal.h @@ -22,8 +22,19 @@ #include #include +#include + +#define IIO_DIR "/sys/bus/iio/devices/" +#define X_RAW_VAL_NODE "/in_magn_x_raw" +#define Y_RAW_VAL_NODE "/in_magn_y_raw" +#define Z_RAW_VAL_NODE "/in_magn_z_raw" +#define X_SCALE_NODE "/in_magn_x_scale" +#define Y_SCALE_NODE "/in_magn_y_scale" +#define Z_SCALE_NODE "/in_magn_z_scale" +#define NAME_NODE "/name" using std::string; +using std::ifstream; class geo_sensor_hal : public sensor_hal { @@ -41,14 +52,13 @@ public: bool check_hw_node(void); private: - long a_x; - long a_y; - long a_z; double m_x; double m_y; double m_z; - int m_hdst; - int m_node_handle; + double m_x_scale; + double m_y_scale; + double m_z_scale; + unsigned long m_polling_interval; unsigned long long m_fired_time; bool m_sensorhub_supported; @@ -58,14 +68,34 @@ private: string m_vendor; string m_chip_name; - string m_resource; - string m_enable_resource; - string m_polling_resource; + string m_x_node; + string m_y_node; + string m_z_node; + string m_x_scale_node; + string m_y_scale_node; + string m_z_scale_node; cmutex m_value_mutex; bool enable_resource(string &resource_node, bool enable); - bool update_value(bool wait); + bool update_value(void); bool is_sensorhub_supported(void); + bool init_resources(void); + + template + bool read_node_value(string node_path, value_t &value) + { + ifstream handle; + handle.open(node_path.c_str()); + if (!handle) + { + ERR("Failed to open handle(%s)", node_path.c_str()); + return false; + } + handle >> value; + handle.close(); + + return true; + } }; #endif /*_GEO_SENSOR_HAL_H_*/ -- 2.7.4 From 1d4c6728c25f0e1cac92da50cbe51e5bfbd8b2c6 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 14 Aug 2014 14:49:38 +0530 Subject: [PATCH 13/16] Changing all 'Vector' Class references to 'vect' Changing all class name references 'Vector' to 'vect' as std library already has definition for 'Vector' and causes compiler errors. signed-off-by: Ramasamy Change-Id: Idec973fb771e60ef5675e6759de7a83f4f18807f --- src/sensor_fusion/euler_angles.cpp | 4 +- src/sensor_fusion/euler_angles.h | 4 +- src/sensor_fusion/orientation_filter.cpp | 28 ++++----- src/sensor_fusion/orientation_filter.h | 20 +++---- src/sensor_fusion/quaternion.cpp | 4 +- src/sensor_fusion/quaternion.h | 4 +- src/sensor_fusion/sensor_data.cpp | 4 +- src/sensor_fusion/sensor_data.h | 4 +- .../test/euler_angles_test/euler_angles_main.cpp | 8 +-- .../test/quaternion_test/quaternion_main.cpp | 4 +- .../test/sensor_data_test/sensor_data_main.cpp | 2 +- .../standalone/test/vector_test/vector_main.cpp | 30 +++++----- src/sensor_fusion/vector.cpp | 68 +++++++++++----------- src/sensor_fusion/vector.h | 60 +++++++++---------- 14 files changed, 122 insertions(+), 122 deletions(-) diff --git a/src/sensor_fusion/euler_angles.cpp b/src/sensor_fusion/euler_angles.cpp index 4241788..c668318 100644 --- a/src/sensor_fusion/euler_angles.cpp +++ b/src/sensor_fusion/euler_angles.cpp @@ -34,12 +34,12 @@ euler_angles::euler_angles(const TYPE roll, const TYPE pitch, const TYPE y { TYPE euler_data[EULER_SIZE] = {roll, pitch, yaw}; - vector v(EULER_SIZE, euler_data); + vect v(EULER_SIZE, euler_data); m_ang = v; } template -euler_angles::euler_angles(const vector v) +euler_angles::euler_angles(const vect v) { m_ang = v; } diff --git a/src/sensor_fusion/euler_angles.h b/src/sensor_fusion/euler_angles.h index bafe8df..69fe0dd 100644 --- a/src/sensor_fusion/euler_angles.h +++ b/src/sensor_fusion/euler_angles.h @@ -26,11 +26,11 @@ template class euler_angles { public: - vector m_ang; + vect m_ang; euler_angles(); euler_angles(const TYPE roll, const TYPE pitch, const TYPE yaw); - euler_angles(const vector v); + euler_angles(const vect v); euler_angles(const euler_angles& e); ~euler_angles(); diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp index 0cdb7fd..6acc52f 100644 --- a/src/sensor_fusion/orientation_filter.cpp +++ b/src/sensor_fusion/orientation_filter.cpp @@ -73,9 +73,9 @@ orientation_filter::orientation_filter() std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL); - vector vec(MOVING_AVERAGE_WINDOW_LENGTH, arr); - vector vec1x3(V1x3S); - vector vec1x6(V1x6S); + vect vec(MOVING_AVERAGE_WINDOW_LENGTH, arr); + vect vec1x3(V1x3S); + vect vec1x6(V1x6S); matrix mat6x6(M6X6R, M6X6C); m_var_gyr_x = vec; @@ -113,10 +113,10 @@ inline void orientation_filter::filter_sensor_data(const sensor_data TYPE a_bias[] = {BIAS_AX, BIAS_AY, BIAS_AZ}; TYPE g_bias[] = {BIAS_GX, BIAS_GY, BIAS_GZ}; - vector acc_data(V1x3S); - vector gyr_data(V1x3S); - vector acc_bias(V1x3S, a_bias); - vector gyr_bias(V1x3S, g_bias); + vect acc_data(V1x3S); + vect gyr_data(V1x3S); + vect acc_bias(V1x3S, a_bias); + vect gyr_bias(V1x3S, g_bias); acc_data = accel.m_data - acc_bias; gyr_data = (gyro.m_data - gyr_bias) / (TYPE) SCALE_GYRO; @@ -154,14 +154,14 @@ inline void orientation_filter::orientation_triad_algorithm() TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0}; TYPE arr_mag_e[V1x3S] = {0.0, -1.0, 0.0}; - vector acc_e(V1x3S, arr_acc_e); - vector mag_e(V1x3S, arr_mag_e); + vect acc_e(V1x3S, arr_acc_e); + vect mag_e(V1x3S, arr_mag_e); - vector acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data); - vector acc_e_x_mag_e = cross(acc_e, mag_e); + vect acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data); + vect acc_e_x_mag_e = cross(acc_e, mag_e); - vector cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data); - vector cross2 = cross(acc_e_x_mag_e, acc_e); + vect cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data); + vect cross2 = cross(acc_e_x_mag_e, acc_e); matrix mat_b(M3X3R, M3X3C); matrix mat_e(M3X3R, M3X3C); @@ -312,7 +312,7 @@ inline void orientation_filter::measurement_update() m_state_old = m_state_new; TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]}; - vector vec(V1x3S, arr_bias); + vect vec(V1x3S, arr_bias); m_bias_correction = vec; } diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h index 6f9ce37..de07af9 100644 --- a/src/sensor_fusion/orientation_filter.h +++ b/src/sensor_fusion/orientation_filter.h @@ -33,21 +33,21 @@ public: sensor_data m_filt_accel[2]; sensor_data m_filt_gyro[2]; sensor_data m_filt_magnetic[2]; - vector m_var_gyr_x; - vector m_var_gyr_y; - vector m_var_gyr_z; - vector m_var_roll; - vector m_var_pitch; - vector m_var_yaw; + vect m_var_gyr_x; + vect m_var_gyr_y; + vect m_var_gyr_z; + vect m_var_roll; + vect m_var_pitch; + vect m_var_yaw; matrix m_driv_cov; matrix m_aid_cov; matrix m_tran_mat; matrix m_measure_mat; matrix m_pred_cov; - vector m_state_new; - vector m_state_old; - vector m_state_error; - vector m_bias_correction; + vect m_state_new; + vect m_state_old; + vect m_state_error; + vect m_bias_correction; quaternion m_quat_aid; quaternion m_quat_driv; rotation_matrix m_rot_matrix; diff --git a/src/sensor_fusion/quaternion.cpp b/src/sensor_fusion/quaternion.cpp index 2649f73..5a9148a 100755 --- a/src/sensor_fusion/quaternion.cpp +++ b/src/sensor_fusion/quaternion.cpp @@ -33,12 +33,12 @@ quaternion::quaternion(const TYPE w, const TYPE x, const TYPE y, const TYP { TYPE vec_data[QUAT_SIZE] = {w, x, y, z}; - vector v(QUAT_SIZE, vec_data); + vect v(QUAT_SIZE, vec_data); m_quat = v; } template -quaternion::quaternion(const vector v) +quaternion::quaternion(const vect v) { m_quat = v; } diff --git a/src/sensor_fusion/quaternion.h b/src/sensor_fusion/quaternion.h index 6be945c..90ce4e8 100755 --- a/src/sensor_fusion/quaternion.h +++ b/src/sensor_fusion/quaternion.h @@ -25,11 +25,11 @@ template class quaternion { public: - vector m_quat; + vect m_quat; quaternion(); quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z); - quaternion(const vector v); + quaternion(const vect v); quaternion(const quaternion& q); ~quaternion(); diff --git a/src/sensor_fusion/sensor_data.cpp b/src/sensor_fusion/sensor_data.cpp index 2671be9..72b9395 100644 --- a/src/sensor_fusion/sensor_data.cpp +++ b/src/sensor_fusion/sensor_data.cpp @@ -34,13 +34,13 @@ sensor_data::sensor_data(const TYPE x, const TYPE y, { TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z}; - vector v(SENSOR_DATA_SIZE, vec_data); + vect v(SENSOR_DATA_SIZE, vec_data); m_data = v; m_time_stamp = time_stamp; } template -sensor_data::sensor_data(const vector v, +sensor_data::sensor_data(const vect v, const unsigned long long time_stamp = 0) { m_data = v; diff --git a/src/sensor_fusion/sensor_data.h b/src/sensor_fusion/sensor_data.h index c24c564..4c84bd9 100644 --- a/src/sensor_fusion/sensor_data.h +++ b/src/sensor_fusion/sensor_data.h @@ -25,13 +25,13 @@ template class sensor_data { public: - vector m_data; + vect m_data; unsigned long long m_time_stamp; sensor_data(); sensor_data(const TYPE x, const TYPE y, const TYPE z, const unsigned long long time_stamp); - sensor_data(const vector v, + sensor_data(const vect v, const unsigned long long time_stamp); sensor_data(const sensor_data& s); ~sensor_data(); diff --git a/src/sensor_fusion/standalone/test/euler_angles_test/euler_angles_main.cpp b/src/sensor_fusion/standalone/test/euler_angles_test/euler_angles_main.cpp index 5e29ce0..06e449d 100644 --- a/src/sensor_fusion/standalone/test/euler_angles_test/euler_angles_main.cpp +++ b/src/sensor_fusion/standalone/test/euler_angles_test/euler_angles_main.cpp @@ -26,10 +26,10 @@ int main() float arr2[4] = {0.6, 0.6, -.18, -.44}; float arr3[4] = {-0.5, -0.36, .43, .03}; - vector v0(3, arr0); - vector v1(3, arr1); - vector v2(4, arr2); - vector v3(4, arr3); + vect v0(3, arr0); + vect v1(3, arr1); + vect v2(4, arr2); + vect v3(4, arr3); quaternion q1(v2); quaternion q2(v3); diff --git a/src/sensor_fusion/standalone/test/quaternion_test/quaternion_main.cpp b/src/sensor_fusion/standalone/test/quaternion_test/quaternion_main.cpp index 2708d5e..d2ec86b 100644 --- a/src/sensor_fusion/standalone/test/quaternion_test/quaternion_main.cpp +++ b/src/sensor_fusion/standalone/test/quaternion_test/quaternion_main.cpp @@ -24,8 +24,8 @@ int main() float arr0[4] = {2344.98, 345.24, 456.12, 98.33}; float arr1[4] = {0.056, 0.34, -0.0076, 0.001}; - vector v0(4, arr0); - vector v1(4, arr1); + vect v0(4, arr0); + vect v1(4, arr1); quaternion q0(v0); quaternion q1(v1); diff --git a/src/sensor_fusion/standalone/test/sensor_data_test/sensor_data_main.cpp b/src/sensor_fusion/standalone/test/sensor_data_test/sensor_data_main.cpp index 1ebb100..ef7e5e7 100644 --- a/src/sensor_fusion/standalone/test/sensor_data_test/sensor_data_main.cpp +++ b/src/sensor_fusion/standalone/test/sensor_data_test/sensor_data_main.cpp @@ -23,7 +23,7 @@ int main() { float arr1[3] = {1.04, -4.678, -2.34}; - vector v1(3, arr1); + vect v1(3, arr1); sensor_data sd1(2.0, 3.0, 4.0, 140737488355328); sensor_data sd2(1.04, -4.678, -2.34); diff --git a/src/sensor_fusion/standalone/test/vector_test/vector_main.cpp b/src/sensor_fusion/standalone/test/vector_test/vector_main.cpp index ee5b996..befdeee 100644 --- a/src/sensor_fusion/standalone/test/vector_test/vector_main.cpp +++ b/src/sensor_fusion/standalone/test/vector_test/vector_main.cpp @@ -31,19 +31,19 @@ int main() float arr43[6] = {2.3454,-0.0384,-8.90,3.455,6.785,21.345}; float arr5[5] = {0.2,-0.4,0.6,-0.8,1.0}; - vector v1(5, arr0); - vector v2(5, arr8); - vector v10(4, arr3); - vector v12(4, arr4); - vector v15(6, arr1); - vector v20(6, arr43); - vector v21(3, arr2); - vector v22(3, arr15); - vector v3(4); - vector v6(3); - vector v13(5); - vector v95(6); - vector v35(5, arr5); + vect v1(5, arr0); + vect v2(5, arr8); + vect v10(4, arr3); + vect v12(4, arr4); + vect v15(6, arr1); + vect v20(6, arr43); + vect v21(3, arr2); + vect v22(3, arr15); + vect v3(4); + vect v6(3); + vect v13(5); + vect v95(6); + vect v35(5, arr5); float arr57[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}}; matrix m12(3, 3, (float *) arr57); @@ -102,7 +102,7 @@ int main() cout<< "\nProduct:\n"<< m102 << endl; cout<< "\n\n\nVector x Multiplication matrix:\n"; - vector v102 = (v22 * m12); + vect v102 = (v22 * m12); cout<< "\n" << v22 << "\n" << m12; cout<< "\nProduct:\n" << v102 << endl; float val = mul(v22, m32); @@ -163,7 +163,7 @@ int main() cout<< "\nOutput:\n" << v3 << endl; - vector v111 = cross(v21, v22); + vect v111 = cross(v21, v22); cout<< "\n\n\nCross Product:"; cout << "\n\n" << v21 << "\n\n" << v22; cout << "\nResult:\n\n" << v111; diff --git a/src/sensor_fusion/vector.cpp b/src/sensor_fusion/vector.cpp index e063d6e..bf2ee65 100644 --- a/src/sensor_fusion/vector.cpp +++ b/src/sensor_fusion/vector.cpp @@ -20,13 +20,13 @@ #if defined (_VECTOR_H) && defined (_MATRIX_H) template -vector::vector(void) +vect::vect(void) { m_vec = NULL; } template -vector::vector(const int size) +vect::vect(const int size) { m_size = size; m_vec = NULL; @@ -34,7 +34,7 @@ vector::vector(const int size) } template -vector::vector(const int size, TYPE *vec_data) +vect::vect(const int size, TYPE *vec_data) { m_size = size; m_vec = NULL; @@ -45,7 +45,7 @@ vector::vector(const int size, TYPE *vec_data) } template -vector::vector(const vector& v) +vect::vect(const vect& v) { m_size = v.m_size; m_vec = NULL; @@ -56,14 +56,14 @@ vector::vector(const vector& v) } template -vector::~vector() +vect::~vect() { if (m_vec != NULL) delete[] m_vec; } template -vector vector::operator =(const vector& v) +vect vect::operator =(const vect& v) { if (this == &v) { @@ -95,7 +95,7 @@ vector vector::operator =(const vector& v) } template -ostream& operator <<(ostream& dout, vector& v) +ostream& operator <<(ostream& dout, vect& v) { for (int j = 0; j < v.m_size; j++) { @@ -108,11 +108,11 @@ ostream& operator <<(ostream& dout, vector& v) } template -vector operator +(const vector v1, const vector v2) +vect operator +(const vect v1, const vect v2) { assert(v1.m_size == v2.m_size); - vector v3(v1.m_size); + vect v3(v1.m_size); for (int j = 0; j < v1.m_size; j++) v3.m_vec[j] = v1.m_vec[j] + v2.m_vec[j]; @@ -121,9 +121,9 @@ vector operator +(const vector v1, const vector v2) } template -vector operator +(const vector v, const T val) +vect operator +(const vect v, const T val) { - vector v1(v.m_size); + vect v1(v.m_size); for (int j = 0; j < v.m_size; j++) v1.m_vec[j] = v.m_vec[j] + val; @@ -132,11 +132,11 @@ vector operator +(const vector v, const T val) } template -vector operator -(const vector v1, const vector v2) +vect operator -(const vect v1, const vect v2) { assert(v1.m_size == v2.m_size); - vector v3(v1.m_size); + vect v3(v1.m_size); for (int j = 0; j < v1.m_size; j++) v3.m_vec[j] = v1.m_vec[j] - v2.m_vec[j]; @@ -145,9 +145,9 @@ vector operator -(const vector v1, const vector v2) } template -vector operator -(const vector v, const T val) +vect operator -(const vect v, const T val) { - vector v1(v.m_size); + vect v1(v.m_size); for (int j = 0; j < v.m_size; j++) v1.m_vec[j] = v.m_vec[j] - val; @@ -156,7 +156,7 @@ vector operator -(const vector v, const T val) } template -matrix operator *(const matrix m, const vector v) +matrix operator *(const matrix m, const vect v) { assert(m.m_rows == v.m_size); assert(m.m_cols == 1); @@ -175,12 +175,12 @@ matrix operator *(const matrix m, const vector v) } template -vector operator *(const vector v, const matrix m) +vect operator *(const vect v, const matrix m) { assert(m.m_rows == v.m_size); assert(m.m_cols != 1); - vector v1(m.m_cols); + vect v1(m.m_cols); for (int j = 0; j < m.m_cols; j++) { @@ -193,9 +193,9 @@ vector operator *(const vector v, const matrix m) } template -vector operator *(const vector v, const T val) +vect operator *(const vect v, const T val) { - vector v1(v.m_size); + vect v1(v.m_size); for (int j = 0; j < v.m_size; j++) v1.m_vec[j] = v.m_vec[j] * val; @@ -204,9 +204,9 @@ vector operator *(const vector v, const T val) } template -vector operator /(const vector v, const T val) +vect operator /(const vect v, const T val) { - vector v1(v.m_size); + vect v1(v.m_size); for (int j = 0; j < v.m_size; j++) v1.m_vec[j] = v.m_vec[j] / val; @@ -215,7 +215,7 @@ vector operator /(const vector v, const T val) } template -bool operator ==(const vector v1, const vector v2) +bool operator ==(const vect v1, const vect v2) { if (v1.m_size == v2.m_size) { @@ -230,13 +230,13 @@ bool operator ==(const vector v1, const vector v2) } template -bool operator !=(const vector v1, const vector v2) +bool operator !=(const vect v1, const vect v2) { return (!(v1 == v2)); } template -matrix transpose(const vector v) +matrix transpose(const vect v) { matrix m(v.m_size, 1); @@ -247,9 +247,9 @@ matrix transpose(const vector v) } template -vector transpose(const matrix m) +vect transpose(const matrix m) { - vector v(m.m_rows); + vect v(m.m_rows); for (int i = 0; i < m.m_rows; i++) v.m_vec[i] = m.m_mat[i][0]; @@ -258,7 +258,7 @@ vector transpose(const matrix m) } template -T mul(const vector v, const matrix m) +T mul(const vect v, const matrix m) { assert(m.m_rows == v.m_size); assert(m.m_cols == 1); @@ -273,7 +273,7 @@ T mul(const vector v, const matrix m) template -void insert_end(vector& v, T val) +void insert_end(vect& v, T val) { for (int i = 0; i < (v.m_size - 1); i++) v.m_vec[i] = v.m_vec[i+1]; @@ -282,9 +282,9 @@ void insert_end(vector& v, T val) } template -vector cross(const vector v1, const vector v2) +vect cross(const vect v1, const vect v2) { - vector v3(v1.m_size); + vect v3(v1.m_size); v3.m_vec[0] = ((v1.m_vec[1] * v2.m_vec[2]) - (v1.m_vec[2] * v2.m_vec[1])); v3.m_vec[1] = ((v1.m_vec[2] * v2.m_vec[0]) - (v1.m_vec[0] * v2.m_vec[2])); @@ -294,9 +294,9 @@ vector cross(const vector v1, const vector v2) } template -bool is_initialized(const vector v) +bool is_initialized(const vect v) { - vector v1(v.m_size); + vect v1(v.m_size); bool retval; retval = (v == v1) ? false : true; @@ -305,7 +305,7 @@ bool is_initialized(const vector v) } template -T var(const vector v) +T var(const vect v) { T val = 0; T mean, var, diff; diff --git a/src/sensor_fusion/vector.h b/src/sensor_fusion/vector.h index 21566ea..4c3eaf1 100644 --- a/src/sensor_fusion/vector.h +++ b/src/sensor_fusion/vector.h @@ -23,50 +23,50 @@ #include "matrix.h" template -class vector { +class vect { public: int m_size; TYPE *m_vec; - vector(void); - vector(const int size); - vector(const int size, TYPE *vec_data); - vector(const vector& v); - ~vector(); + vect(void); + vect(const int size); + vect(const int size, TYPE *vec_data); + vect(const vect& v); + ~vect(); - vector operator =(const vector& v); + vect operator =(const vect& v); template friend ostream& operator << (ostream& dout, - vector& v); - template friend vector operator +(const vector v1, - const vector v2); - template friend vector operator +(const vector v, + vect& v); + template friend vect operator +(const vect v1, + const vect v2); + template friend vect operator +(const vect v, const T val); - template friend vector operator -(const vector v1, - const vector v2); - template friend vector operator -(const vector v, + template friend vect operator -(const vect v1, + const vect v2); + template friend vect operator -(const vect v, const T val); template friend matrix operator *(const matrix v1, - const vector v2); - template friend vector operator *(const vector v, + const vect v2); + template friend vect operator *(const vect v, const matrix m); - template friend vector operator *(const vector v, + template friend vect operator *(const vect v, const T val); - template friend vector operator /(const vector v1, + template friend vect operator /(const vect v1, const T val); - template friend bool operator ==(const vector v1, - const vector v2); - template friend bool operator !=(const vector v1, - const vector v2); + template friend bool operator ==(const vect v1, + const vect v2); + template friend bool operator !=(const vect v1, + const vect v2); - template friend T mul(const vector v, const matrix m); - template friend void insert_end(vector& v, T val); - template friend matrix transpose(const vector v); - template friend vector transpose(const matrix m); - template friend vector cross(const vector v1, - const vector v2); - template friend T var(const vector v); - template friend bool is_initialized(const vector v); + template friend T mul(const vect v, const matrix m); + template friend void insert_end(vect& v, T val); + template friend matrix transpose(const vect v); + template friend vect transpose(const matrix m); + template friend vect cross(const vect v1, + const vect v2); + template friend T var(const vect v); + template friend bool is_initialized(const vect v); }; #include "vector.cpp" -- 2.7.4 From fd47b5ea628303501bd8982938cfb943b59ba8a7 Mon Sep 17 00:00:00 2001 From: Amit Dharmapurikar Date: Mon, 4 Aug 2014 13:35:55 +0530 Subject: [PATCH 14/16] Modified proxi_sensor_hal plugin for IIO driver compatibility - The proxi_sensor_hal plugin is modified as per proximity IIO driver interface - The proxi sensor is enabled in the sensord.spec file Change-Id: I87256d7e96174003af7c2d04ac1fde7e33c26116 Signed-off-by: Amit Dharmapurikar --- packaging/sensord.spec | 2 +- src/proxi/proxi_sensor_hal.cpp | 277 ++++++++++++++++------------------------- src/proxi/proxi_sensor_hal.h | 29 +++-- 3 files changed, 128 insertions(+), 180 deletions(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index d6ac9c2..f5d834d 100755 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -10,7 +10,7 @@ Source2: sensord.socket %define accel_state ON %define gyro_state ON -%define proxi_state OFF +%define proxi_state ON %define light_state OFF %define geo_state OFF %define gravity_state OFF diff --git a/src/proxi/proxi_sensor_hal.cpp b/src/proxi/proxi_sensor_hal.cpp index eed0c9b..c1fb0d7 100755 --- a/src/proxi/proxi_sensor_hal.cpp +++ b/src/proxi/proxi_sensor_hal.cpp @@ -25,21 +25,13 @@ #include #include #include +#include using std::ifstream; using config::CConfig; -#define NODE_NAME "name" -#define NODE_INPUT "input" -#define NODE_ENABLE "enable" -#define SENSOR_NODE "/sys/class/sensors/" -#define SENSORHUB_NODE "/sys/class/sensors/ssp_sensor/" -#define INPUT_DEVICE_NODE "/sys/class/input/" -#define DEV_INPUT_NODE "/dev/input/event/" - #define INITIAL_VALUE -1 #define INITIAL_TIME 0 -#define PROXI_CODE 0x0019 #define SENSOR_TYPE_PROXI "PROXI" #define ELEMENT_NAME "NAME" @@ -48,42 +40,56 @@ using config::CConfig; #define INPUT_NAME "proximity_sensor" +#define NO_FLAG 0 +#define ENABLE_VAL true +#define DISABLE_VAL false + proxi_sensor_hal::proxi_sensor_hal() : m_state(PROXIMITY_STATE_FAR) , m_node_handle(INITIAL_VALUE) , m_fired_time(INITIAL_TIME) , m_sensorhub_supported(false) { - if (!check_hw_node()) { + int fd, ret; + + if (!check_hw_node()) + { ERR("check_hw_node() fail"); throw ENXIO; } CConfig &config = CConfig::get_instance(); - if (!config.get(SENSOR_TYPE_PROXI, m_model_id, ELEMENT_VENDOR, m_vendor)) { + if (!config.get(SENSOR_TYPE_PROXI, m_model_id, ELEMENT_VENDOR, m_vendor)) + { ERR("[VENDOR] is empty"); throw ENXIO; } INFO("m_vendor = %s", m_vendor.c_str()); - if (!config.get(SENSOR_TYPE_PROXI, m_model_id, ELEMENT_NAME, m_chip_name)) { + if (!config.get(SENSOR_TYPE_PROXI, m_model_id, ELEMENT_NAME, m_chip_name)) + { ERR("[NAME] is empty"); throw ENXIO; } INFO("m_chip_name = %s", m_chip_name.c_str()); - if ((m_node_handle = open(m_resource.c_str(), O_RDWR)) < 0) { - ERR("Failed to open handle(%d)", m_node_handle); + fd = open(m_event_resource.c_str(), NO_FLAG); + if (fd == -1) + { + ERR("Could not open event resource"); throw ENXIO; } - int clockId = CLOCK_MONOTONIC; + ret = ioctl(fd, IOCTL_IIO_EVENT_FD, &m_event_fd); + + close(fd); - if (ioctl(m_node_handle, EVIOCSCLOCKID, &clockId) != 0) { - ERR("Fail to set monotonic timestamp for %s", m_resource.c_str()); + if ((ret == -1) || (m_event_fd == -1)) + { + ERR("Failed to retrieve event fd"); throw ENXIO; } @@ -92,9 +98,7 @@ proxi_sensor_hal::proxi_sensor_hal() proxi_sensor_hal::~proxi_sensor_hal() { - close(m_node_handle); - m_node_handle = INITIAL_VALUE; - + close(m_event_fd); INFO("proxi_sensor_hal is destroyed!"); } @@ -108,53 +112,9 @@ sensor_type_t proxi_sensor_hal::get_type(void) return PROXIMITY_SENSOR; } -bool proxi_sensor_hal::enable_resource(string &resource_node, bool enable) +bool proxi_sensor_hal::enable_resource(bool enable) { - int prev_status, status; - FILE *fp = NULL; - fp = fopen(resource_node.c_str(), "r"); - - if (!fp) { - ERR("Fail to open a resource file: %s", resource_node.c_str()); - return false; - } - - if (fscanf(fp, "%d", &prev_status) < 0) { - ERR("Failed to get data from %s", resource_node.c_str()); - fclose(fp); - return false; - } - - fclose(fp); - - if (enable) { - if (m_sensorhub_supported) - status = prev_status | (1 << SENSORHUB_PROXIMITY_ENABLE_BIT); - else - status = 1; - } else { - if (m_sensorhub_supported) - status = prev_status ^ (1 << SENSORHUB_PROXIMITY_ENABLE_BIT); - else - status = 0; - } - - fp = fopen(resource_node.c_str(), "w"); - - if (!fp) { - ERR("Failed to open a resource file: %s", resource_node.c_str()); - return false; - } - - if (fprintf(fp, "%d", status) < 0) { - ERR("Failed to enable a resource file: %s", resource_node.c_str()); - fclose(fp); - return false; - } - - if (fp) - fclose(fp); - + update_sysfs_num(m_enable_resource.c_str(), enable); return true; } @@ -162,10 +122,10 @@ bool proxi_sensor_hal::enable(void) { AUTOLOCK(m_mutex); - enable_resource(m_enable_resource, true); + enable_resource(ENABLE_VAL); m_fired_time = 0; - INFO("Proxi sensor real starting"); + INFO("Proximity sensor real starting"); return true; } @@ -173,68 +133,81 @@ bool proxi_sensor_hal::disable(void) { AUTOLOCK(m_mutex); - enable_resource(m_enable_resource, false); - INFO("Proxi sensor real stopping"); + enable_resource(DISABLE_VAL); + + INFO("Proximity sensor real stopping"); return true; } bool proxi_sensor_hal::update_value(bool wait) { - struct input_event proxi_event; + iio_event_t proxi_event; fd_set readfds, exceptfds; FD_ZERO(&readfds); FD_ZERO(&exceptfds); - FD_SET(m_node_handle, &readfds); - FD_SET(m_node_handle, &exceptfds); + FD_SET(m_event_fd, &readfds); + FD_SET(m_event_fd, &exceptfds); int ret; - ret = select(m_node_handle + 1, &readfds, NULL, &exceptfds, NULL); + ret = select(m_event_fd + 1, &readfds, NULL, &exceptfds, NULL); - if (ret == -1) { - ERR("select error:%s m_node_handle:d", strerror(errno), m_node_handle); + if (ret == -1) + { + ERR("select error:%s m_event_fd:d", strerror(errno), m_event_fd); return false; - } else if (!ret) { + } + else if (!ret) + { DBG("select timeout"); return false; } - if (FD_ISSET(m_node_handle, &exceptfds)) { + if (FD_ISSET(m_event_fd, &exceptfds)) + { ERR("select exception occurred!"); return false; } - if (FD_ISSET(m_node_handle, &readfds)) { - INFO("proxi event detection!"); - int len = read(m_node_handle, &proxi_event, sizeof(proxi_event)); + if (FD_ISSET(m_event_fd, &readfds)) + { + INFO("proximity event detection!"); + int len = read(m_event_fd, &proxi_event, sizeof(proxi_event)); - if (len == -1) { - DBG("read(m_node_handle) is error:%s.", strerror(errno)); + if (len == -1) + { + DBG("Error in read(m_event_fd):%s.", strerror(errno)); return false; } - DBG("read event, len : %d , type : %x , code : %x , value : %x", len, proxi_event.type, proxi_event.code, proxi_event.value); - - if ((proxi_event.type == EV_ABS) && (proxi_event.code == PROXI_CODE)) { + ull_bytes_t ev_data; + ev_data.num = proxi_event.event_id; + if (ev_data.bytes[CH_TYPE] == PROXIMITY_TYPE) + { AUTOLOCK(m_value_mutex); - - if (proxi_event.value == PROXIMITY_NODE_STATE_FAR) { - INFO("PROXIMITY_STATE_FAR state occured"); + int temp; + temp = GET_DIR_VAL(ev_data.bytes[DIRECTION]); + if (temp == PROXIMITY_NODE_STATE_FAR) + { + INFO("PROXIMITY_STATE_FAR state occurred"); m_state = PROXIMITY_STATE_FAR; - } else if (proxi_event.value == PROXIMITY_NODE_STATE_NEAR) { - INFO("PROXIMITY_STATE_NEAR state occured"); + } + else if (temp == PROXIMITY_NODE_STATE_NEAR) + { + INFO("PROXIMITY_STATE_NEAR state occurred"); m_state = PROXIMITY_STATE_NEAR; - } else { - ERR("PROXIMITY_STATE Unknown: %d", proxi_event.value); + } + else + { + ERR("PROXIMITY_STATE Unknown: %d", proxi_event.event_id); return false; } - - m_fired_time = sensor_hal::get_timestamp(&proxi_event.time); - } else { - return false; } - } else { - ERR("select nothing to read!!!"); + m_fired_time = proxi_event.timestamp; + } + else + { + ERR("No proximity event data available to read"); return false; } @@ -255,7 +228,7 @@ int proxi_sensor_hal::get_sensor_data(sensor_data_t &data) data.data_unit_idx = SENSOR_UNIT_STATE_ON_OFF; data.timestamp = m_fired_time; data.values_num = 1; - data.values[0] = m_state; + data.values[0] = (float)(m_state); return 0; } @@ -272,23 +245,14 @@ bool proxi_sensor_hal::get_properties(sensor_properties_t &properties) bool proxi_sensor_hal::is_sensorhub_supported(void) { - DIR *main_dir = NULL; - main_dir = opendir(SENSORHUB_NODE); - - if (!main_dir) { - INFO("Sensor Hub is not supported"); - return false; - } - - INFO("It supports sensor hub"); - closedir(main_dir); - return true; + return false; } bool proxi_sensor_hal::check_hw_node(void) { string name_node; string hw_name; + string file_name; DIR *main_dir = NULL; struct dirent *dir_entry = NULL; bool find_node = false; @@ -296,83 +260,53 @@ bool proxi_sensor_hal::check_hw_node(void) INFO("======================start check_hw_node============================="); m_sensorhub_supported = is_sensorhub_supported(); - main_dir = opendir(SENSOR_NODE); + main_dir = opendir(IIO_DIR); - if (!main_dir) { - ERR("Directory open failed to collect data"); + if (!main_dir) + { + ERR("Could not open IIO directory\n"); return false; } - while ((!find_node) && (dir_entry = readdir(main_dir))) { - if ((strncasecmp(dir_entry->d_name , ".", 1 ) != 0) && (strncasecmp(dir_entry->d_name , "..", 2 ) != 0) && (dir_entry->d_ino != 0)) { - name_node = string(SENSOR_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_NAME); + while (!find_node) + { + dir_entry = readdir(main_dir); + if(dir_entry == NULL) + break; - ifstream infile(name_node.c_str()); + if ((strncasecmp(dir_entry->d_name , ".", 1 ) != 0) && (strncasecmp(dir_entry->d_name , "..", 2 ) != 0) && (dir_entry->d_ino != 0)) + { + file_name = string(IIO_DIR) + string(dir_entry->d_name) + string(NAME_NODE); + + ifstream infile(file_name.c_str()); if (!infile) continue; infile >> hw_name; - if (CConfig::get_instance().is_supported(SENSOR_TYPE_PROXI, hw_name) == true) { - m_name = m_model_id = hw_name; - INFO("m_model_id = %s", m_model_id.c_str()); - find_node = true; - break; - } - } - } + if (strncmp(dir_entry->d_name, IIO_DEV_BASE_NAME, IIO_DEV_STR_LEN) == 0) + { + if (CConfig::get_instance().is_supported(SENSOR_TYPE_PROXI, hw_name) == true) + { + m_name = m_model_id = hw_name; + m_proxi_dir = string(dir_entry->d_name); + m_enable_resource = string(IIO_DIR) + m_proxi_dir + string(EVENT_DIR) + string(EVENT_EN_NODE); + m_event_resource = string(DEV_DIR) + m_proxi_dir; - closedir(main_dir); - - if (find_node) { - main_dir = opendir(INPUT_DEVICE_NODE); - - if (!main_dir) { - ERR("Directory open failed to collect data"); - return false; - } - - find_node = false; - - while ((!find_node) && (dir_entry = readdir(main_dir))) { - if (strncasecmp(dir_entry->d_name, NODE_INPUT, 5) == 0) { - name_node = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_NAME); - ifstream infile(name_node.c_str()); - - if (!infile) - continue; - - infile >> hw_name; - - if (hw_name == string(INPUT_NAME)) { - INFO("name_node = %s", name_node.c_str()); - DBG("Find H/W for proxi_sensor"); + INFO("m_enable_resource = %s", m_enable_resource.c_str()); + INFO("m_model_id = %s", m_model_id.c_str()); + INFO("m_proxi_dir = %s", m_proxi_dir.c_str()); + INFO("m_event_resource = %s", m_event_resource.c_str()); find_node = true; - string dir_name; - dir_name = string(dir_entry->d_name); - unsigned found = dir_name.find_first_not_of(NODE_INPUT); - m_resource = string(DEV_INPUT_NODE) + dir_name.substr(found); - - if (m_sensorhub_supported) - m_enable_resource = string(SENSORHUB_NODE) + string(NODE_ENABLE); - else - m_enable_resource = string(INPUT_DEVICE_NODE) + string(dir_entry->d_name) + string("/") + string(NODE_ENABLE); - break; } } } - - closedir(main_dir); - } - - if (find_node) { - INFO("m_resource = %s", m_resource.c_str()); - INFO("m_enable_resource = %s", m_enable_resource.c_str()); } + closedir(main_dir); return find_node; } @@ -380,9 +314,12 @@ extern "C" void *create(void) { proxi_sensor_hal *inst; - try { + try + { inst = new proxi_sensor_hal(); - } catch (int err) { + } + catch (int err) + { ERR("Failed to create proxi_sensor_hal class, errno : %d, errstr : %s", err, strerror(err)); return NULL; } diff --git a/src/proxi/proxi_sensor_hal.h b/src/proxi/proxi_sensor_hal.h index d483814..bd273fa 100755 --- a/src/proxi/proxi_sensor_hal.h +++ b/src/proxi/proxi_sensor_hal.h @@ -23,17 +23,24 @@ #include #include +#define IIO_DIR "/sys/bus/iio/devices/" +#define NAME_NODE "/name" +#define EVENT_DIR "/events" +#define EVENT_EN_NODE "/in_proximity_thresh_either_en" +#define DEV_DIR "/dev/" + +#define IIO_DEV_BASE_NAME "iio:device" +#define IIO_DEV_STR_LEN 10 + +#define PROXIMITY_NODE_STATE_NEAR 1 +#define PROXIMITY_NODE_STATE_FAR 2 +#define PROXIMITY_TYPE 8 + using std::string; class proxi_sensor_hal : public sensor_hal { public: - enum proxi_node_state_event_t { - PROXIMITY_NODE_STATE_NEAR = 0, - PROXIMITY_NODE_STATE_FAR = 1, - PROXIMITY_NODE_STATE_UNKNOWN = 2, - }; - proxi_sensor_hal(); virtual ~proxi_sensor_hal(); string get_model_id(void); @@ -56,13 +63,17 @@ private: string m_vendor; string m_chip_name; - string m_resource; + string m_proxi_dir; + string m_enable_resource; + string m_event_resource; cmutex m_value_mutex; - bool enable_resource(string &resource_node, bool enable); + int m_event_fd; + + bool enable_resource(bool enable); bool update_value(bool wait); bool is_sensorhub_supported(void); }; -#endif /*_PROXI_SENSOR_HAL_H_*/ \ No newline at end of file +#endif /*_PROXI_SENSOR_HAL_H_*/ -- 2.7.4 From 5dde4aa1b8eff4e74a432ed4b68927c236bbef24 Mon Sep 17 00:00:00 2001 From: Amit Dharmapurikar Date: Mon, 4 Aug 2014 12:20:04 +0530 Subject: [PATCH 15/16] Adding IIO event related data structures for non-trigger based IIO drivers IIO event data structure and other event decoding data types have been defined for sensor drivers without IIO trigger. Change-Id: I38e6d78ee907f47f3544fdacfcffc41d7e5d1ae6 Signed-off-by: Amit Dharmapurikar --- src/shared/iio_common.h | 36 +++++++++++++++++++++++++++++++++++- 1 file changed, 35 insertions(+), 1 deletion(-) diff --git a/src/shared/iio_common.h b/src/shared/iio_common.h index 0f456dc..9f00c13 100644 --- a/src/shared/iio_common.h +++ b/src/shared/iio_common.h @@ -20,7 +20,20 @@ #ifndef IIO_COMMON_H_ #define IIO_COMMON_H_ -struct channel_parameters { +#include + +#define NO_OF_ULL_BYTES 8 +#define NO_OF_SHORT_VAL 4 +#define CH0_INDEX 0 +#define CH1_INDEX 1 + +#define GET_DIFF_BIT(val) (((unsigned short)(val) >> 7) & 0x01) +#define GET_DIR_VAL(val) ((val) & 0x0F) + +#define IOCTL_IIO_EVENT_FD _IOR('i', 0x90, int) + +struct channel_parameters +{ char *prefix_str; float scale; float offset; @@ -35,6 +48,27 @@ struct channel_parameters { unsigned int buf_index; }; +typedef struct iio_event_struct +{ + unsigned long long int event_id; + long long int timestamp; +} iio_event_t; + +typedef enum event_id_field +{ + CH_TYPE = 4, + MODIFIER, + DIRECTION, + EVENT_TYPE, +} event_id_field_t; + +typedef union ull_bytes +{ + unsigned long long num; + short int channels[NO_OF_SHORT_VAL]; + unsigned char bytes[NO_OF_ULL_BYTES]; +} ull_bytes_t; + void sort_channels_by_index(struct channel_parameters *channels, int count); int decode_channel_data_type(const char *device_dir, const char *ch_name, struct channel_parameters *ch_info); int add_channel_to_array(const char *device_dir, const char *ch_name, struct channel_parameters *channel); -- 2.7.4 From d12da5029a27ab8c2bda699fa2129cb035c5f862 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Mon, 15 Sep 2014 11:21:20 +0530 Subject: [PATCH 16/16] Updating octave design files - Updating octave design files based on testing of RD-PQ target data. - Adding customization data for when code is being ported to multiple devices. Change-Id: I7c6b352102c3686a6b90f10103b668d2852c262a --- src/sensor_fusion/design/lib/estimate_orientation.m | 12 ++++++++---- src/sensor_fusion/design/sf_gravity.m | 16 ++++++++++++---- src/sensor_fusion/design/sf_linear_acceleration.m | 14 +++++++++++--- src/sensor_fusion/design/sf_orientation.m | 14 +++++++++++--- 4 files changed, 42 insertions(+), 14 deletions(-) diff --git a/src/sensor_fusion/design/lib/estimate_orientation.m b/src/sensor_fusion/design/lib/estimate_orientation.m index dfbbc18..6e96c89 100755 --- a/src/sensor_fusion/design/lib/estimate_orientation.m +++ b/src/sensor_fusion/design/lib/estimate_orientation.m @@ -25,6 +25,10 @@ function [OR_driv, OR_aid, OR_err] = estimate_orientation(Accel_data, Gyro_data LOW_PASS_FILTERING_ON = 0; PLOT_SCALED_SENSOR_COMPARISON_DATA = 0; PLOT_INDIVIDUAL_SENSOR_INPUT_DATA = 0; + MAGNETIC_ALIGNMENT_FACTOR = -1; + PITCH_PHASE_CORRECTION = -1; + ROLL_PHASE_CORRECTION = -1; + YAW_PHASE_CORRECTION = -1; GRAVITY = 9.80665; PI = 3.141593; @@ -124,7 +128,7 @@ function [OR_driv, OR_aid, OR_err] = estimate_orientation(Accel_data, Gyro_data M_T(1) = 100000; acc_e = [0.0;0.0;1.0]; % gravity vector in earth frame - mag_e = [0.0;1.0;0.0]; % magnetic field vector in earth frame + mag_e = [0.0;MAGNETIC_ALIGNMENT_FACTOR;0.0]; % magnetic field vector in earth frame H = [eye(3) zeros(3,3); zeros(3,6)]; x = zeros(6,BUFFER_SIZE); @@ -303,9 +307,9 @@ function [OR_driv, OR_aid, OR_err] = estimate_orientation(Accel_data, Gyro_data OR_aid(3,:) = yaw * RAD2DEG; euler = quat2euler(quat_driv); - OR_driv(1,:) = euler(:,2)' * RAD2DEG; - OR_driv(2,:) = euler(:,1)' * RAD2DEG; - OR_driv(3,:) = -euler(:,3)' * RAD2DEG; + OR_driv(1,:) = ROLL_PHASE_CORRECTION * euler(:,2)' * RAD2DEG; + OR_driv(2,:) = PITCH_PHASE_CORRECTION * euler(:,1)' * RAD2DEG; + OR_driv(3,:) = YAW_PHASE_CORRECTION * euler(:,3)' * RAD2DEG; euler = quat2euler(quat_error); OR_err(1,:) = euler(:,2)' * RAD2DEG; diff --git a/src/sensor_fusion/design/sf_gravity.m b/src/sensor_fusion/design/sf_gravity.m index 19f249c..07e47d6 100755 --- a/src/sensor_fusion/design/sf_gravity.m +++ b/src/sensor_fusion/design/sf_gravity.m @@ -39,6 +39,14 @@ Bias_Gx = -5.3539; Bias_Gy = 0.24325; Bias_Gz = 2.3391; +Bias_Mx = 0; +Bias_My = 0; +Bias_Mz = 0; + +Sign_Mx = 1; +Sign_My = 1; +Sign_Mz = 1; + BUFFER_SIZE = 100; Accel_data = zeros(4,BUFFER_SIZE); @@ -69,9 +77,9 @@ Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,1))'))(1:BUFFER_SIZE); -Mag_data(2,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,2))'))(1:BUFFER_SIZE); -Mag_data(3,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,3))'))(1:BUFFER_SIZE); +Mag_data(1,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/gravity/single_roll_throw/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation @@ -92,7 +100,7 @@ hold on; grid on; UA = Accel_data(3,:); p3 = plot(1:length(UA),UA(1,1:length(UA)),'r'); -title(['Accelerometer Raw Data']); +title(['Raw Accelerometer Data']); legend([p1 p2 p3],'x-axis', 'y-axis', 'z-axis'); % Gravity Plot Results diff --git a/src/sensor_fusion/design/sf_linear_acceleration.m b/src/sensor_fusion/design/sf_linear_acceleration.m index edb78e0..21555b4 100755 --- a/src/sensor_fusion/design/sf_linear_acceleration.m +++ b/src/sensor_fusion/design/sf_linear_acceleration.m @@ -39,6 +39,14 @@ Bias_Gx = -5.3539; Bias_Gy = 0.24325; Bias_Gz = 2.3391; +Bias_Mx = 0; +Bias_My = 0; +Bias_Mz = 0; + +Sign_Mx = 1; +Sign_My = 1; +Sign_Mz = 1; + BUFFER_SIZE = 125; Accel_data = zeros(4,BUFFER_SIZE); @@ -69,9 +77,9 @@ Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,1))'))(1:BUFFER_SIZE); -Mag_data(2,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,2))'))(1:BUFFER_SIZE); -Mag_data(3,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,3))'))(1:BUFFER_SIZE); +Mag_data(1,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/linear_acceleration/move_x_y_z/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation diff --git a/src/sensor_fusion/design/sf_orientation.m b/src/sensor_fusion/design/sf_orientation.m index d1c927c..be1d107 100755 --- a/src/sensor_fusion/design/sf_orientation.m +++ b/src/sensor_fusion/design/sf_orientation.m @@ -39,6 +39,14 @@ Bias_Gx = -5.3539; Bias_Gy = 0.24325; Bias_Gz = 2.3391; +Bias_Mx = 0; +Bias_My = 0; +Bias_Mz = 0; + +Sign_Mx = 1; +Sign_My = 1; +Sign_Mz = 1; + BUFFER_SIZE = 1095; Accel_data = zeros(4,BUFFER_SIZE); @@ -69,9 +77,9 @@ Gyro_data(2,:) = Gyro_data(2,:)/scale_Gyro; Gyro_data(3,:) = Gyro_data(3,:)/scale_Gyro; % get magnetometer x,y,z axis data from stored file -Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))'))(1:BUFFER_SIZE); -Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))'))(1:BUFFER_SIZE); -Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))'))(1:BUFFER_SIZE); +Mag_data(1,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,1))') + Bias_Mx)(1:BUFFER_SIZE) * Sign_Mx; +Mag_data(2,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,2))') + Bias_My)(1:BUFFER_SIZE) * Sign_My; +Mag_data(3,:) = (((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,3))') + Bias_Mz)(1:BUFFER_SIZE) * Sign_Mz; Mag_data(4,:) = ((dlmread("data/100ms/orientation/roll_pitch_yaw/magnetic.txt")(:,4))')(1:BUFFER_SIZE); % estimate orientation -- 2.7.4