set(PROJECT_MINOR_VERSION "2")
set(PROJECT_RELEASE_VERSION "1")
set(CMAKE_VERBOSE_MAKEFILE OFF)
-
add_definitions(-Wall -O3 -omit-frame-pointer)
add_definitions(-DUSE_DLOG_LOG)
#add_definitions(-Wall -g -D_DEBUG)
INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/LICENSE.APLv2 DESTINATION share/license RENAME sensord)
INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/LICENSE.APLv2 DESTINATION share/license RENAME libsensord)
+IF("${TEST_SUITE}" STREQUAL "ON")
+INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/LICENSE.APLv2 DESTINATION share/license RENAME test)
+ENDIF()
+
add_subdirectory(src)
+
+IF("${TEST_SUITE}" STREQUAL "ON")
+add_subdirectory(test)
+ENDIF()
+
%define accel_state ON
%define gyro_state ON
-%define proxi_state OFF
+%define proxi_state ON
%define light_state OFF
%define geo_state ON
%define gravity_state OFF
%define linear_accel_state OFF
%define motion_state OFF
+%define sensor_fusion_state ON
BuildRequires: cmake
BuildRequires: vconf-keys-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}
<MODULE path = "/usr/lib/sensord/libproxi_sensor.so"/>
<MODULE path = "/usr/lib/sensord/liblight_sensor.so"/>
<MODULE path = "/usr/lib/sensord/libmotion_sensor.so"/>
- <MODULE path = "/usr/lib/sensord/libsensor_fusion.so"/>
+ <MODULE path = "/usr/lib/sensord/liborientation_sensor.so"/>
<MODULE path = "/usr/lib/sensord/libgravity_sensor.so"/>
<MODULE path = "/usr/lib/sensord/liblinear_accel_sensor.so"/>
</SENSOR>
IF("${GEO}" STREQUAL "ON")
add_subdirectory(geo)
ENDIF()
+IF("${ORIENTATION}" STREQUAL "ON")
+add_subdirectory(sensor_fusion)
+add_subdirectory(orientation)
+ENDIF()
IF("${GRAVITY}" STREQUAL "ON")
add_subdirectory(gravity)
ENDIF()
IF("${MOTION}" STREQUAL "ON")
add_subdirectory(motion)
ENDIF()
-IF("${SENSOR_FUSION}" STREQUAL "ON")
-add_subdirectory(sensor_fusion)
-ENDIF()
add_subdirectory(server)
add_subdirectory(libsensord)
#include <common.h>
#include <sf_common.h>
#include <sensor_base.h>
-#include <virtual_sensor.h>
#include <gravity_sensor.h>
#include <sensor_plugin_loader.h>
#define INITIAL_VALUE -1
#define INITIAL_TIME 0
-#define TIME_CONSTANT 0.18
#define GRAVITY 9.80665
#define SENSOR_NAME "GRAVITY_SENSOR"
gravity_sensor::gravity_sensor()
-: m_accel_sensor(NULL)
+: m_orientation_sensor(NULL)
, m_x(INITIAL_VALUE)
, m_y(INITIAL_VALUE)
, m_z(INITIAL_VALUE)
-, m_time(INITIAL_TIME)
+, m_timestamp(INITIAL_TIME)
{
m_name = string(SENSOR_NAME);
bool gravity_sensor::init()
{
- m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
+ m_orientation_sensor = sensor_plugin_loader::get_instance().get_sensor(ORIENTATION_SENSOR);
- if (!m_accel_sensor) {
- ERR("cannot load accel sensor_hal[%s]", sensor_base::get_name());
+ if (!m_orientation_sensor) {
+ ERR("Failed to load orientation sensor: 0x%x", m_orientation_sensor);
return false;
}
{
AUTOLOCK(m_mutex);
- m_accel_sensor->add_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
- m_accel_sensor->start();
+ m_orientation_sensor->add_client(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME);
+ m_orientation_sensor->start();
activate();
return true;
{
AUTOLOCK(m_mutex);
- m_accel_sensor->delete_client(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME);
- m_accel_sensor->stop();
+ m_orientation_sensor->delete_client(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME);
+ m_orientation_sensor->stop();
deactivate();
return true;
}
-bool gravity_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
+bool gravity_sensor::add_interval(int client_id, unsigned int interval)
{
- m_accel_sensor->add_interval(client_id, interval, true);
+ AUTOLOCK(m_mutex);
+ m_orientation_sensor->add_interval(client_id , interval, true);
+
return sensor_base::add_interval(client_id, interval, true);
}
-bool gravity_sensor::delete_interval(int client_id, bool is_processor)
+bool gravity_sensor::delete_interval(int client_id)
{
- m_accel_sensor->delete_interval(client_id, true);
+ AUTOLOCK(m_mutex);
+ m_orientation_sensor->delete_interval(client_id , true);
+
return sensor_base::delete_interval(client_id, true);
}
void gravity_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
{
- if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
- float x, y, z;
- calibrate_gravity(event, x, y, z);
-
- sensor_event_t event;
- event.event_type = GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME;
- event.data.data_accuracy = SENSOR_ACCURACY_GOOD;
- event.data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED;
- event.data.timestamp = m_time;
- event.data.values_num = 3;
- event.data.values[0] = x;
- event.data.values[1] = y;
- event.data.values[2] = z;
- outs.push_back(event);
-
- AUTOLOCK(m_value_mutex);
- m_x = x;
- m_y = y;
- m_z = z;
-
+ sensor_event_t gravity_event;
+ vector<sensor_event_t> orientation_event;
+ ((virtual_sensor *)m_orientation_sensor)->synthesize(event, orientation_event);
+
+ if (!orientation_event.empty()) {
+ AUTOLOCK(m_mutex);
+
+ m_timestamp = orientation_event[0].data.timestamp;
+ gravity_event.data.values[0] = GRAVITY * sin(orientation_event[0].data.values[1]);
+ gravity_event.data.values[1] = GRAVITY * sin(orientation_event[0].data.values[0]);
+ gravity_event.data.values[2] = GRAVITY * cos(orientation_event[0].data.values[1] *
+ orientation_event[0].data.values[0]);
+ gravity_event.data.values_num = 3;
+ gravity_event.data.timestamp = m_timestamp;
+ gravity_event.data.data_accuracy = SENSOR_ACCURACY_GOOD;
+ gravity_event.data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED;
+
+ outs.push_back(gravity_event);
return;
}
}
-int gravity_sensor::get_sensor_data(const unsigned int data_id, sensor_data_t &data)
+int gravity_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
{
- if (data_id != GRAVITY_BASE_DATA_SET)
+ sensor_data_t orientation_data;
+
+ if (event_type != GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME)
return -1;
- AUTOLOCK(m_value_mutex);
+ m_orientation_sensor->get_sensor_data(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME, orientation_data);
data.data_accuracy = SENSOR_ACCURACY_GOOD;
data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED;
- data.time_stamp = m_time;
+ data.timestamp = orientation_data.timestamp;
+ data.values[0] = GRAVITY * sin(orientation_data.values[1]);
+ data.values[1] = GRAVITY * sin(orientation_data.values[0]);
+ data.values[2] = GRAVITY * cos(orientation_data.values[1] * orientation_data.values[0]);
data.values_num = 3;
- data.values[0] = m_x;
- data.values[1] = m_y;
- data.values[2] = m_z;
return 0;
}
bool gravity_sensor::get_properties(const unsigned int type, sensor_properties_t &properties)
{
- m_accel_sensor->get_properties(type, properties);
-
- if (type != GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME)
- return true;
-
- properties.sensor_min_range = properties.sensor_min_range / GRAVITY;
- properties.sensor_max_range = properties.sensor_max_range / GRAVITY;
- properties.sensor_resolution = properties.sensor_resolution / GRAVITY;
- strncpy(properties.sensor_name, "Gravity Sensor", MAX_KEY_LENGTH);
+ properties.sensor_unit_idx = SENSOR_UNIT_DEGREE;
+ properties.sensor_min_range = -GRAVITY;
+ properties.sensor_max_range = GRAVITY;
+ properties.sensor_resolution = 1;
+ strncpy(properties.sensor_vendor, "Samsung", MAX_KEY_LENGTH);
+ strncpy(properties.sensor_name, SENSOR_NAME, MAX_KEY_LENGTH);
return true;
}
-void gravity_sensor::calibrate_gravity(const sensor_event_t &raw, float &x, float &y, float &z)
-{
- unsigned long long timestamp;
- float dt;
- float alpha;
- float last_x = 0, last_y = 0, last_z = 0;
-
- {
- AUTOLOCK(m_value_mutex);
- last_x = m_x;
- last_y = m_y;
- last_z = m_z;
- }
-
- timestamp = get_timestamp();
- dt = (timestamp - m_time) / US_TO_SEC;
- m_time = timestamp;
- alpha = TIME_CONSTANT / (TIME_CONSTANT + dt);
-
- if (dt > 1.0)
- alpha = 0.0;
-
- x = (alpha * last_x) + ((1 - alpha) * raw.data.values[0] / GRAVITY);
- y = (alpha * last_y) + ((1 - alpha) * raw.data.values[1] / GRAVITY);
- z = (alpha * last_z) + ((1 - alpha) * raw.data.values[2] / GRAVITY);
-}
-
extern "C" void *create(void)
{
gravity_sensor *inst;
try {
inst = new gravity_sensor();
- } catch (int ErrNo) {
+ } catch (int err) {
ERR("Failed to create gravity_sensor class, errno : %d, errstr : %s", err, strerror(err));
return NULL;
}
void synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs);
- virtual bool add_interval(int client_id, unsigned int interval, bool is_processor = false);
- virtual bool delete_interval(int client_id, bool is_processor = false);
+ bool add_interval(int client_id, unsigned int interval);
+ bool delete_interval(int client_id);
- int get_sensor_data(const unsigned int data_id, sensor_data_t &data);
+ int get_sensor_data(const unsigned int event_type, sensor_data_t &data);
bool get_properties(const unsigned int type, sensor_properties_t &properties);
private:
- sensor_base *m_accel_sensor;
- cmutex m_value_mutex;
+ sensor_base *m_orientation_sensor;
float m_x;
float m_y;
float m_z;
- unsigned long long m_time;
-
- void calibrate_gravity(const sensor_event_t &raw, float &x, float &y, float &z);
+ unsigned long long m_timestamp;
};
#endif /*_GRAVITY_SENSOR_H_*/
FILL_LOG_ELEMENT(LOG_ID_DATA, GEOMAGNETIC_ATTITUDE_DATA_SET, 0, 25),
FILL_LOG_ELEMENT(LOG_ID_DATA, LIGHT_BASE_DATA_SET, 0, 25),
FILL_LOG_ELEMENT(LOG_ID_DATA, LIGHT_LUX_DATA_SET, 0, 25),
- FILL_LOG_ELEMENT(LOG_ID_DATA, GRAVITY_BASE_DATA_SET, 0, 25),
- FILL_LOG_ELEMENT(LOG_ID_DATA, LINEAR_ACCEL_BASE_DATA_SET, 0, 25),
- FILL_LOG_ELEMENT(LOG_ID_DATA, ORIENTATION_BASE_DATA_SET, 0, 25),
FILL_LOG_ELEMENT(LOG_ID_ROTATE, ROTATION_EVENT_0, 0, 1),
FILL_LOG_ELEMENT(LOG_ID_ROTATE, ROTATION_EVENT_90, 0, 1),
switch (sensor) {
case GEOMAGNETIC_SENSOR:
return GEOMAGNETIC_EVENT_CALIBRATION_NEEDED;
- case ORIENTATION_SENSOR:
- return ORIENTATION_EVENT_CALIBRATION_NEEDED;
default:
return 0;
}
* @{
*/
-enum gravity_data_id {
- GRAVITY_BASE_DATA_SET = (GRAVITY_SENSOR << 16) | 0x0001,
-};
-
enum gravity_event_type {
- GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME = (GRAVITY_SENSOR << 16) | 0x0002,
+ GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME = (GRAVITY_SENSOR << 16) | 0x0001,
};
/**
* @{
*/
-enum linear_accel_data_id {
- LINEAR_ACCEL_BASE_DATA_SET = (LINEAR_ACCEL_SENSOR << 16) | 0x0001,
-};
-
enum linear_accel_event_type {
LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME = (LINEAR_ACCEL_SENSOR << 16) | 0x0001,
};
* @{
*/
-enum orientation_data_id {
- ORIENTATION_BASE_DATA_SET = (ORIENTATION_SENSOR << 16) | 0x0001,
-};
-
enum orientation_event_type {
- ORIENTATION_EVENT_CALIBRATION_NEEDED = (ORIENTATION_SENSOR << 16) | 0x0001,
- ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME = (ORIENTATION_SENSOR << 16) | 0x0002,
+ ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME = (ORIENTATION_SENSOR << 16) | 0x0001,
};
/**
#include <time.h>
#include <sys/types.h>
#include <dlfcn.h>
+#include <sensor.h>
#include <common.h>
#include <sf_common.h>
#include <virtual_sensor.h>
bool linear_accel_sensor::init()
{
m_gravity_sensor = sensor_plugin_loader::get_instance().get_sensor(GRAVITY_SENSOR);
+ m_accel_sensor = sensor_plugin_loader::get_instance().get_sensor(ACCELEROMETER_SENSOR);
- if (!m_gravity_sensor) {
- ERR("cannot load gravity sensor_hal[%s]", sensor_base::get_name());
+ if (!m_accel_sensor || !m_gravity_sensor) {
+ ERR("Failed to load sensors, accel: 0x%x, gravity: 0x%x",
+ m_accel_sensor, m_gravity_sensor);
return false;
}
return true;
}
-bool linear_accel_sensor::add_interval(int client_id, unsigned int interval, bool is_processor)
+bool linear_accel_sensor::add_interval(int client_id, unsigned int interval)
{
- m_gravity_sensor->add_interval((int)this , interval, true);
- return sensor_base::add_interval(client_id, interval, is_processor);
+ AUTOLOCK(m_mutex);
+ m_gravity_sensor->add_interval(client_id, interval, true);
+ return sensor_base::add_interval(client_id, interval, true);
}
-bool linear_accel_sensor::delete_interval(int client_id, bool is_processor)
+bool linear_accel_sensor::delete_interval(int client_id)
{
- m_gravity_sensor->delete_interval((int)this , true);
- return sensor_base::delete_interval(client_id, is_processor);
+ AUTOLOCK(m_mutex);
+ m_gravity_sensor->delete_interval(client_id, true);
+ return sensor_base::delete_interval(client_id, true);
}
void linear_accel_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
{
vector<sensor_event_t> gravity_event;
+ sensor_event_t lin_accel_event;
((virtual_sensor *)m_gravity_sensor)->synthesize(event, gravity_event);
if (!gravity_event.empty() && event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
AUTOLOCK(m_value_mutex);
- m_time = gravity_event[0].data.timestamp;
- gravity_event[0].event_type = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME;
- m_x = event.data.values[0] - gravity_event[0].data.values[0] * GRAVITY;
- m_y = event.data.values[1] - gravity_event[0].data.values[1] * GRAVITY;
- m_z = event.data.values[2] - gravity_event[0].data.values[2] * GRAVITY;
-
- gravity_event[0].data.values[0] = m_x;
- gravity_event[0].data.values[1] = m_y;
- gravity_event[0].data.values[2] = m_z;
- outs.push_back(gravity_event[0]);
- return;
+ lin_accel_event.data.values_num = 3;
+ lin_accel_event.data.timestamp = gravity_event[0].data.timestamp;
+ lin_accel_event.event_type = LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME;
+ lin_accel_event.data.data_accuracy = SENSOR_ACCURACY_GOOD;
+ lin_accel_event.data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED;
+ lin_accel_event.data.values[0] = event.data.values[0] - gravity_event[0].data.values[0];
+ lin_accel_event.data.values[1] = event.data.values[1] - gravity_event[0].data.values[1];
+ lin_accel_event.data.values[2] = event.data.values[2] - gravity_event[0].data.values[2];
+ outs.push_back(lin_accel_event);
}
+
+ return;
}
-int linear_accel_sensor::get_sensor_data(const unsigned int data_id, sensor_data_t &data)
+int linear_accel_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
{
- if (data_id != LINEAR_ACCEL_BASE_DATA_SET)
+ sensor_data_t gravity_data, accel_data;
+ ((virtual_sensor *)m_gravity_sensor)->get_sensor_data(GRAVITY_EVENT_RAW_DATA_REPORT_ON_TIME, gravity_data);
+ m_accel_sensor->get_sensor_data(ACCELEROMETER_BASE_DATA_SET, accel_data);
+
+ if (event_type != LINEAR_ACCEL_EVENT_RAW_DATA_REPORT_ON_TIME)
return -1;
- AUTOLOCK(m_value_mutex);
data.data_accuracy = SENSOR_ACCURACY_GOOD;
data.data_unit_idx = SENSOR_UNIT_METRE_PER_SECOND_SQUARED;
- data.time_stamp = m_time;
- data.values[0] = m_x;
- data.values[1] = m_y;
- data.values[2] = m_z;
+ data.timestamp = gravity_data.timestamp;
+ data.values[0] = accel_data.values[0] - gravity_data.values[0];
+ data.values[1] = accel_data.values[1] - gravity_data.values[1];
+ data.values[2] = accel_data.values[2] - gravity_data.values[2];
data.values_num = 3;
return 0;
}
try {
inst = new linear_accel_sensor();
- } catch (int ErrNo) {
+ } catch (int err) {
ERR("Failed to create linear_accel_sensor class, errno : %d, errstr : %s", err, strerror(err));
return NULL;
}
#include <sensor.h>
#include <vconf.h>
#include <string>
+#include <virtual_sensor.h>
using std::string;
void synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs);
- virtual bool add_interval(int client_id, unsigned int interval, bool is_processor = false);
- virtual bool delete_interval(int client_id, bool is_processor = false);
+ bool add_interval(int client_id, unsigned int interval);
+ bool delete_interval(int client_id);
- int get_sensor_data(const unsigned int data_id, sensor_data_t &data);
+ int get_sensor_data(const unsigned int event_type, sensor_data_t &data);
bool get_properties(const unsigned int type, sensor_properties_t &properties);
private:
+ sensor_base *m_accel_sensor;
sensor_base *m_gravity_sensor;
cmutex m_value_mutex;
--- /dev/null
+cmake_minimum_required(VERSION 2.6)
+project(orientation CXX)
+
+# to install pkgconfig setup file.
+SET(EXEC_PREFIX "\${prefix}")
+SET(LIBDIR "\${prefix}/lib")
+SET(VERSION 1.0)
+
+SET(SENSOR_NAME orientation_sensor)
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+include_directories(${CMAKE_SOURCE_DIR}/src/libsensord)
+include_directories(${CMAKE_SOURCE_DIR}/src/sensor_fusion)
+
+include(FindPkgConfig)
+pkg_check_modules(rpkgs REQUIRED vconf)
+
+set(PROJECT_MAJOR_VERSION "0")
+set(PROJECT_MINOR_VERSION "0")
+set(PROJECT_RELEASE_VERSION "1")
+set(CMAKE_VERBOSE_MAKEFILE OFF)
+
+
+FIND_PROGRAM(UNAME NAMES uname)
+EXEC_PROGRAM("${UNAME}" ARGS "-m" OUTPUT_VARIABLE "ARCH")
+IF("${ARCH}" MATCHES "^arm.*")
+ ADD_DEFINITIONS("-DTARGET -DHWREV_CHECK")
+ MESSAGE("add -DTARGET -DHWREV_CHECK")
+ELSE("${ARCH}" MATCHES "^arm.*")
+ ADD_DEFINITIONS("-DSIMULATOR")
+ MESSAGE("add -DSIMULATOR")
+ENDIF("${ARCH}" MATCHES "^arm.*")
+
+add_definitions(-Wall -O3 -omit-frame-pointer)
+#add_definitions(-Wall -g -D_DEBUG)
+add_definitions(-DUSE_DLOG_LOG)
+add_definitions(-Iinclude)
+
+add_library(${SENSOR_NAME} SHARED
+ orientation_sensor.cpp
+ )
+
+target_link_libraries(${SENSOR_NAME} ${rpkgs_LDFLAGS} ${GLES_LDFLAGS} "-lm")
+
+install(TARGETS ${SENSOR_NAME} DESTINATION lib/sensord)
--- /dev/null
+/*
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <time.h>
+#include <sys/types.h>
+#include <dlfcn.h>
+#include <common.h>
+#include <sf_common.h>
+#include <orientation_sensor.h>
+#include <sensor_plugin_loader.h>
+#include <orientation_filter.h>
+
+#define SENSOR_NAME "ORIENTATION_SENSOR"
+
+#define ACCELEROMETER_ENABLED 0x01
+#define GYROSCOPE_ENABLED 0x02
+#define GEOMAGNETIC_ENABLED 0x04
+#define ORIENTATION_ENABLED 7
+
+#define INITIAL_VALUE -1
+#define INITIAL_TIME 0
+
+// Below Defines const variables to be input from sensor config files once code is stabilized
+#define SAMPLING_TIME 100
+#define MS_TO_US 1000
+
+float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)};
+float bias_gyro[] = {-5.3539, 0.24325, 2.3391};
+float bias_magnetic[] = {0, -37.6, +37.6};
+int sign_accel[] = {+1, +1, +1};
+int sign_gyro[] = {+1, +1, +1};
+int sign_magnetic[] = {+1, -1, +1};
+float scale_accel = 1;
+float scale_gyro = 580 * 2;
+float scale_magnetic = 1;
+
+int pitch_phase_compensation = 1;
+int roll_phase_compensation = 1;
+int yaw_phase_compensation = -1;
+int magnetic_alignment_factor = 1;
+
+void pre_process_data(sensor_data<float> &data_out, sensor_data_t &data_in, float *bias, int *sign, float scale)
+{
+ data_out.m_data.m_vec[0] = sign[0] * (data_in.values[0] - bias[0]) / scale;
+ data_out.m_data.m_vec[1] = sign[1] * (data_in.values[1] - bias[1]) / scale;
+ data_out.m_data.m_vec[2] = sign[2] * (data_in.values[2] - bias[2]) / scale;
+
+ data_out.m_time_stamp = data_in.timestamp;
+}
+
+orientation_sensor::orientation_sensor()
+: m_accel_sensor(NULL)
+, m_gyro_sensor(NULL)
+, m_magnetic_sensor(NULL)
+, m_roll(INITIAL_VALUE)
+, m_pitch(INITIAL_VALUE)
+, m_yaw(INITIAL_VALUE)
+{
+ m_name = string(SENSOR_NAME);
+ register_supported_event(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME);
+ m_interval = SAMPLING_TIME * MS_TO_US;
+ m_timestamp = get_timestamp();
+ m_enable_orientation = 0;
+}
+
+orientation_sensor::~orientation_sensor()
+{
+ INFO("orientation_sensor is destroyed!\n");
+}
+
+bool orientation_sensor::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("Failed 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;
+}
+
+
+sensor_type_t orientation_sensor::get_type(void)
+{
+ return ORIENTATION_SENSOR;
+}
+
+bool orientation_sensor::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();
+
+ activate();
+ return true;
+}
+
+bool orientation_sensor::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();
+
+ deactivate();
+ return true;
+}
+
+bool orientation_sensor::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 sensor_base::add_interval(client_id, interval, true);
+}
+
+bool orientation_sensor::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 sensor_base::delete_interval(client_id, true);
+}
+
+void orientation_sensor::synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs)
+{
+ const float MIN_DELIVERY_DIFF_FACTOR = 0.75f;
+ unsigned long long diff_time;
+
+ sensor_data<float> accel;
+ sensor_data<float> gyro;
+ sensor_data<float> magnetic;
+
+ sensor_data_t accel_data;
+ sensor_data_t gyro_data;
+ sensor_data_t mag_data;
+
+ sensor_event_t orientation_event;
+ euler_angles<float> euler_orientation;
+
+ if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ diff_time = event.data.timestamp - m_timestamp;
+
+ if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
+ return;
+
+ pre_process_data(accel, accel_data, bias_accel, sign_accel, scale_accel);
+
+ m_enable_orientation |= ACCELEROMETER_ENABLED;
+ }
+ else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ diff_time = event.data.timestamp - m_timestamp;
+
+ if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
+ return;
+
+ pre_process_data(gyro, gyro_data, bias_gyro, sign_gyro, scale_gyro);
+
+ m_enable_orientation |= GYROSCOPE_ENABLED;
+ }
+ else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
+ diff_time = event.data.timestamp - m_timestamp;
+
+ if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
+ return;
+
+ pre_process_data(magnetic, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
+
+ m_enable_orientation |= GEOMAGNETIC_ENABLED;
+ }
+
+ if (m_enable_orientation == ORIENTATION_ENABLED) {
+ m_enable_orientation = 0;
+ m_timestamp = get_timestamp();
+
+ m_orientation.m_pitch_phase_compensation = pitch_phase_compensation;
+ m_orientation.m_roll_phase_compensation = roll_phase_compensation;
+ m_orientation.m_yaw_phase_compensation = yaw_phase_compensation;
+ m_orientation.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
+ euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic);
+
+ orientation_event.data.data_accuracy = SENSOR_ACCURACY_GOOD;
+ orientation_event.data.data_unit_idx = SENSOR_UNIT_DEGREE;
+ orientation_event.event_type = ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME;
+ orientation_event.data.timestamp = m_timestamp;
+ orientation_event.data.values_num = 3;
+ orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[0];
+ orientation_event.data.values[1] = euler_orientation.m_ang.m_vec[1];
+ orientation_event.data.values[2] = euler_orientation.m_ang.m_vec[2];
+
+ outs.push_back(orientation_event);
+ }
+
+ return;
+}
+
+int orientation_sensor::get_sensor_data(const unsigned int event_type, sensor_data_t &data)
+{
+ sensor_data<float> accel;
+ sensor_data<float> gyro;
+ sensor_data<float> magnetic;
+
+ sensor_data_t accel_data;
+ sensor_data_t gyro_data;
+ sensor_data_t magnetic_data;
+
+ euler_angles<float> euler_orientation;
+
+ if (event_type != ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME)
+ return -1;
+
+ m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data);
+ m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data);
+ m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data);
+
+ pre_process_data(accel, accel_data, bias_accel, sign_accel, scale_accel);
+ pre_process_data(gyro, gyro_data, bias_gyro, sign_gyro, scale_gyro);
+ pre_process_data(magnetic, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
+
+ m_orientation.m_pitch_phase_compensation = pitch_phase_compensation;
+ m_orientation.m_roll_phase_compensation = roll_phase_compensation;
+ m_orientation.m_yaw_phase_compensation = yaw_phase_compensation;
+ m_orientation.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
+ euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic);
+
+ data.data_accuracy = SENSOR_ACCURACY_GOOD;
+ data.data_unit_idx = SENSOR_UNIT_DEGREE;
+ data.timestamp = get_timestamp();
+ data.values[0] = euler_orientation.m_ang.m_vec[0];
+ data.values[1] = euler_orientation.m_ang.m_vec[1];
+ data.values[2] = euler_orientation.m_ang.m_vec[2];
+ data.values_num = 3;
+
+ return 0;
+}
+
+bool orientation_sensor::get_properties(sensor_properties_t &properties)
+{
+ properties.sensor_unit_idx = SENSOR_UNIT_DEGREE;
+ properties.sensor_min_range = -180;
+ properties.sensor_max_range = 180;
+ properties.sensor_resolution = 1;
+
+ strncpy(properties.sensor_vendor, "Samsung", MAX_KEY_LENGTH);
+ strncpy(properties.sensor_name, SENSOR_NAME, MAX_KEY_LENGTH);
+
+ return true;
+}
+
+extern "C" void *create(void)
+{
+ orientation_sensor *inst;
+
+ try {
+ inst = new orientation_sensor();
+ } catch (int err) {
+ ERR("orientation_sensor class create fail , errno : %d , errstr : %s", err, strerror(err));
+ return NULL;
+ }
+
+ return (void *)inst;
+}
+
+extern "C" void destroy(void *inst)
+{
+ delete (orientation_sensor *)inst;
+}
*
*/
-#ifndef _LIB_SENSOR_FUSION_H_
-#define _LIB_SENSOR_FUSION_H_
+#ifndef _ORIENTATION_SENSOR_H_
+#define _ORIENTATION_SENSOR_H_
-#include <sensor_fusion.h>
+#include <sensor.h>
+#include <virtual_sensor.h>
+#include <orientation_filter.h>
-class lib_sensor_fusion : public sensor_fusion
-{
+class orientation_sensor : public virtual_sensor {
public:
- lib_sensor_fusion();
- ~lib_sensor_fusion();
+ orientation_sensor();
+ ~orientation_sensor();
bool init(void);
bool on_start(void);
bool on_stop(void);
+ void synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs);
+
bool add_interval(int client_id, unsigned int interval);
bool delete_interval(int client_id);
bool get_properties(sensor_properties_t &properties);
+ sensor_type_t get_type(void);
+
+ int get_sensor_data(const unsigned int data_id, sensor_data_t &data);
- 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;
+
+ orientation_filter<float> m_orientation;
+
+ unsigned int m_enable_orientation;
+
+ float m_roll;
+ float m_pitch;
+ float m_yaw;
+ unsigned long long m_timestamp;
+ unsigned int m_interval;
};
-#endif /*_LIB_SENSOR_FUSION_H_*/
+#endif /* _ORIENTATION_SENSOR_H_ */
#include <linux/input.h>
#include <cconfig.h>
#include <proxi_sensor_hal.h>
+#include <iio_common.h>
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"
#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;
}
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!");
}
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;
}
{
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;
}
{
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;
}
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;
}
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;
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;
}
{
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;
}
#include <sensor_hal.h>
#include <string>
+#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);
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_*/
SET(SENSOR_FUSION_NAME sensor_fusion)
add_library(${SENSOR_FUSION_NAME} SHARED
- lib_sensor_fusion.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")
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;
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);
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;
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);
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
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
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);
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
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);
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
{
TYPE euler_data[EULER_SIZE] = {roll, pitch, yaw};
- vector<TYPE> v(EULER_SIZE, euler_data);
+ vect<TYPE> v(EULER_SIZE, euler_data);
m_ang = v;
}
template <typename TYPE>
-euler_angles<TYPE>::euler_angles(const vector<TYPE> v)
+euler_angles<TYPE>::euler_angles(const vect<TYPE> v)
{
m_ang = v;
}
template <typename TYPE>
class euler_angles {
public:
- vector<TYPE> m_ang;
+ vect<TYPE> m_ang;
euler_angles();
euler_angles(const TYPE roll, const TYPE pitch, const TYPE yaw);
- euler_angles(const vector<TYPE> v);
+ euler_angles(const vect<TYPE> v);
euler_angles(const euler_angles<TYPE>& e);
~euler_angles();
+++ /dev/null
-/*
- * 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 <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <errno.h>
-#include <math.h>
-#include <time.h>
-#include <sys/types.h>
-#include <dlfcn.h>
-#include <sensor.h>
-#include <common.h>
-#include <sf_common.h>
-#include <lib_sensor_fusion.h>
-#include <sensor_plugin_loader.h>
-
-#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;
-}
#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
std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL);
- vector<TYPE> vec(MOVING_AVERAGE_WINDOW_LENGTH, arr);
- vector<TYPE> vec1x3(V1x3S);
- vector<TYPE> vec1x6(V1x6S);
+ vect<TYPE> vec(MOVING_AVERAGE_WINDOW_LENGTH, arr);
+ vect<TYPE> vec1x3(V1x3S);
+ vect<TYPE> vec1x6(V1x6S);
matrix<TYPE> mat6x6(M6X6R, M6X6C);
m_var_gyr_x = vec;
m_state_new = vec1x6;
m_state_old = vec1x6;
m_state_error = vec1x6;
+
+ m_pitch_phase_compensation = 1;
+ m_roll_phase_compensation = 1;
+ m_yaw_phase_compensation = 1;
+ m_magnetic_alignment_factor = 1;
}
template <typename TYPE>
const TYPE iir_b[] = {0.98, 0};
const TYPE iir_a[] = {1.0000000, 0.02};
- TYPE a_bias[] = {BIAS_AX, BIAS_AY, BIAS_AZ};
- TYPE g_bias[] = {BIAS_GX, BIAS_GY, BIAS_GZ};
-
- vector<TYPE> acc_data(V1x3S);
- vector<TYPE> gyr_data(V1x3S);
- vector<TYPE> acc_bias(V1x3S, a_bias);
- vector<TYPE> gyr_bias(V1x3S, g_bias);
-
- acc_data = accel.m_data - acc_bias;
- gyr_data = (gyro.m_data - gyr_bias) / (TYPE) SCALE_GYRO;
+ vect<TYPE> acc_data(V1x3S);
+ vect<TYPE> gyr_data(V1x3S);
m_filt_accel[0] = m_filt_accel[1];
m_filt_gyro[0] = m_filt_gyro[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_accel[1].m_data = accel.m_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1];
+ m_filt_gyro[1].m_data = gyro.m_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_accel[1].m_data = accel.m_data;
+ m_filt_gyro[1].m_data = gyro.m_data;
m_filt_magnetic[1].m_data = magnetic.m_data;
}
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;
}
inline void orientation_filter<TYPE>::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, (TYPE) m_magnetic_alignment_factor, 0.0};
- vector<TYPE> acc_e(V1x3S, arr_acc_e);
- vector<TYPE> mag_e(V1x3S, arr_mag_e);
+ vect<TYPE> acc_e(V1x3S, arr_acc_e);
+ vect<TYPE> mag_e(V1x3S, arr_mag_e);
- vector<TYPE> acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data);
- vector<TYPE> acc_e_x_mag_e = cross(acc_e, mag_e);
+ vect<TYPE> acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data);
+ vect<TYPE> acc_e_x_mag_e = cross(acc_e, mag_e);
- vector<TYPE> cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data);
- vector<TYPE> cross2 = cross(acc_e_x_mag_e, acc_e);
+ vect<TYPE> cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data);
+ vect<TYPE> cross2 = cross(acc_e_x_mag_e, acc_e);
matrix<TYPE> mat_b(M3X3R, M3X3C);
matrix<TYPE> mat_e(M3X3R, M3X3C);
orientation = quat2euler(m_quat_driv);
- m_orientation = orientation.m_ang * (TYPE) DRIVING_SYSTEM_PHASE_COMPENSATION;
+ m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_roll_phase_compensation;
+ m_orientation.m_ang.m_vec[1] = orientation.m_ang.m_vec[1] * m_pitch_phase_compensation;
+ m_orientation.m_ang.m_vec[2] = orientation.m_ang.m_vec[2] * m_yaw_phase_compensation;
m_rot_matrix = quat2rot_mat(m_quat_driv);
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<TYPE> vec(V1x3S, arr_bias);
+ vect<TYPE> vec(V1x3S, arr_bias);
m_bias_correction = vec;
}
sensor_data<TYPE> m_filt_accel[2];
sensor_data<TYPE> m_filt_gyro[2];
sensor_data<TYPE> m_filt_magnetic[2];
- vector<TYPE> m_var_gyr_x;
- vector<TYPE> m_var_gyr_y;
- vector<TYPE> m_var_gyr_z;
- vector<TYPE> m_var_roll;
- vector<TYPE> m_var_pitch;
- vector<TYPE> m_var_yaw;
+ vect<TYPE> m_var_gyr_x;
+ vect<TYPE> m_var_gyr_y;
+ vect<TYPE> m_var_gyr_z;
+ vect<TYPE> m_var_roll;
+ vect<TYPE> m_var_pitch;
+ vect<TYPE> m_var_yaw;
matrix<TYPE> m_driv_cov;
matrix<TYPE> m_aid_cov;
matrix<TYPE> m_tran_mat;
matrix<TYPE> m_measure_mat;
matrix<TYPE> m_pred_cov;
- vector<TYPE> m_state_new;
- vector<TYPE> m_state_old;
- vector<TYPE> m_state_error;
- vector<TYPE> m_bias_correction;
+ vect<TYPE> m_state_new;
+ vect<TYPE> m_state_old;
+ vect<TYPE> m_state_error;
+ vect<TYPE> m_bias_correction;
quaternion<TYPE> m_quat_aid;
quaternion<TYPE> m_quat_driv;
rotation_matrix<TYPE> m_rot_matrix;
euler_angles<TYPE> m_orientation;
+ int m_pitch_phase_compensation;
+ int m_roll_phase_compensation;
+ int m_yaw_phase_compensation;
+ int m_magnetic_alignment_factor;
+
orientation_filter();
~orientation_filter();
{
TYPE vec_data[QUAT_SIZE] = {w, x, y, z};
- vector<TYPE> v(QUAT_SIZE, vec_data);
+ vect<TYPE> v(QUAT_SIZE, vec_data);
m_quat = v;
}
template <typename TYPE>
-quaternion<TYPE>::quaternion(const vector<TYPE> v)
+quaternion<TYPE>::quaternion(const vect<TYPE> v)
{
m_quat = v;
}
template <typename TYPE>
class quaternion {
public:
- vector<TYPE> m_quat;
+ vect<TYPE> m_quat;
quaternion();
quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z);
- quaternion(const vector<TYPE> v);
+ quaternion(const vect<TYPE> v);
quaternion(const quaternion<TYPE>& q);
~quaternion();
{
TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z};
- vector<TYPE> v(SENSOR_DATA_SIZE, vec_data);
+ vect<TYPE> v(SENSOR_DATA_SIZE, vec_data);
m_data = v;
m_time_stamp = time_stamp;
}
template <typename TYPE>
-sensor_data<TYPE>::sensor_data(const vector<TYPE> v,
+sensor_data<TYPE>::sensor_data(const vect<TYPE> v,
const unsigned long long time_stamp = 0)
{
m_data = v;
template <typename TYPE>
class sensor_data {
public:
- vector<TYPE> m_data;
+ vect<TYPE> 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<TYPE> v,
+ sensor_data(const vect<TYPE> v,
const unsigned long long time_stamp);
sensor_data(const sensor_data<TYPE>& s);
~sensor_data();
#ifdef _GRAVITY_SENSOR_H
+#define GRAVITY 9.80665
+
sensor_data<float> gravity_sensor::get_gravity(const sensor_data<float> accel,
const sensor_data<float> gyro, const sensor_data<float> magnetic)
{
- return comp_grav.orientation2gravity(accel, gyro, magnetic);
+ euler_angles<float> orientation;
+ sensor_data<float> gravity;
+
+ orientation = orien_sensor.get_orientation(accel, gyro, magnetic);
+
+ gravity.m_data.m_vec[0] = GRAVITY * sin(orientation.m_ang.m_vec[0]);
+ gravity.m_data.m_vec[1] = GRAVITY * sin(orientation.m_ang.m_vec[1]);
+ gravity.m_data.m_vec[2] = GRAVITY * cos(orientation.m_ang.m_vec[0]) *
+ cos(orientation.m_ang.m_vec[1]);
+
+ return gravity;
}
#endif
#ifndef _GRAVITY_SENSOR_H
#define _GRAVITY_SENSOR_H
-#include "./util/compute_gravity.h"
+#include "orientation_sensor.h"
class gravity_sensor
{
public:
- compute_gravity<float> comp_grav;
+ orientation_sensor orien_sensor;
sensor_data<float> get_gravity(const sensor_data<float> accel,
const sensor_data<float> gyro, const sensor_data<float> magnetic);
sensor_data<float> gravity_data;
float la_x, la_y, la_z;
- gravity_data = comp_grav.orientation2gravity(accel, gyro, magnetic);
+ gravity_data = grav_sensor.get_gravity(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];
#ifndef _LINEAR_ACCELERATION_SENSOR_H
#define _LINEAR_ACCELERATION_SENSOR_H
-#include "./util/compute_gravity.h"
+#include "gravity_sensor.h"
class linear_acceleration_sensor
{
public:
- compute_gravity<float> comp_grav;
+ gravity_sensor grav_sensor;
sensor_data<float> get_linear_acceleration(const sensor_data<float> accel,
const sensor_data<float> gyro, const sensor_data<float> magnetic);
#ifdef _ORIENTATION_SENSOR_H
-euler_angles<float> orientation_sensor::get_orientation(const sensor_data<float> accel,
- const sensor_data<float> gyro, const sensor_data<float> magnetic)
+float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)};
+float bias_gyro[] = {-5.3539, 0.24325, 2.3391};
+float bias_magnetic[] = {0, 0, 0};
+int sign_accel[] = {+1, +1, +1};
+int sign_gyro[] = {+1, +1, +1};
+int sign_magnetic[] = {+1, +1, +1};
+float scale_accel = 1;
+float scale_gyro = 575;
+float scale_magnetic = 1;
+
+int pitch_phase_compensation = -1;
+int roll_phase_compensation = -1;
+int yaw_phase_compensation = -1;
+int magnetic_alignment_factor = -1;
+
+void pre_process_data(sensor_data<float> &data_out, sensor_data<float> &data_in, float *bias, int *sign, float scale)
+{
+ data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale;
+ data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale;
+ data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale;
+
+ data_out.m_time_stamp = data_in.m_time_stamp;
+}
+
+euler_angles<float> orientation_sensor::get_orientation(sensor_data<float> accel_data,
+ sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
{
- return orien_filter.get_orientation(accel, gyro, magnetic);
+
+ pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
+ normalize(accel_data);
+ pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
+ pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
+ normalize(magnetic_data);
+
+ orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
+ orien_filter.m_roll_phase_compensation = roll_phase_compensation;
+ orien_filter.m_yaw_phase_compensation = yaw_phase_compensation;
+ orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
+ return orien_filter.get_orientation(accel_data, gyro_data, magnetic_data);
}
-rotation_matrix<float> orientation_sensor::get_rotation_matrix(const sensor_data<float> accel,
- const sensor_data<float> gyro, const sensor_data<float> magnetic)
+rotation_matrix<float> orientation_sensor::get_rotation_matrix(sensor_data<float> accel_data,
+ sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
{
- return orien_filter.get_rotation_matrix(accel, gyro, magnetic);
+
+ pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
+ normalize(accel_data);
+ pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
+ pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
+ normalize(magnetic_data);
+
+ orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
+ orien_filter.m_roll_phase_compensation = roll_phase_compensation;
+ orien_filter.m_yaw_phase_compensation = yaw_phase_compensation;
+ orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
+ return orien_filter.get_rotation_matrix(accel_data, gyro_data, magnetic_data);
}
#endif
#ifndef _ORIENTATION_SENSOR_H
#define _ORIENTATION_SENSOR_H
-#include "./util/orientation_filter.h"
+#include "../orientation_filter.h"
class orientation_sensor
{
public:
orientation_filter<float> orien_filter;
- euler_angles<float> get_orientation(const sensor_data<float> accel,
- const sensor_data<float> gyro, const sensor_data<float> magnetic);
- rotation_matrix<float> get_rotation_matrix(const sensor_data<float> accel,
- const sensor_data<float> gyro, const sensor_data<float> magnetic);
+ euler_angles<float> get_orientation(sensor_data<float> accel,
+ sensor_data<float> gyro, sensor_data<float> magnetic);
+ rotation_matrix<float> get_rotation_matrix(sensor_data<float> accel,
+ sensor_data<float> gyro, sensor_data<float> magnetic);
};
#include "orientation_sensor.cpp"
*
*/
-#include "../../euler_angles.h"
+#include "../../../euler_angles.h"
int main()
{
float arr2[4] = {0.6, 0.6, -.18, -.44};
float arr3[4] = {-0.5, -0.36, .43, .03};
- vector<float> v0(3, arr0);
- vector<float> v1(3, arr1);
- vector<float> v2(4, arr2);
- vector<float> v3(4, arr3);
+ vect<float> v0(3, arr0);
+ vect<float> v1(3, arr1);
+ vect<float> v2(4, arr2);
+ vect<float> v3(4, arr3);
quaternion<float> q1(v2);
quaternion<float> q2(v3);
*
*/
-#include "../../../gravity_sensor.h"
+#include "../../gravity_sensor.h"
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <string>
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()
*
*/
-#include "../../../linear_acceleration_sensor.h"
+#include "../../linear_acceleration_sensor.h"
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <string>
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()
*
*/
-#include "../../matrix.h"
+#include "../../../matrix.h"
int main()
{
*
*/
-#include "../../orientation_filter.h"
+#include "../../../orientation_filter.h"
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <string>
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
+float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)};
+float bias_gyro[] = {-5.3539, 0.24325, 2.3391};
+float bias_magnetic[] = {0, 0, 0};
+int sign_accel[] = {+1, +1, +1};
+int sign_gyro[] = {+1, +1, +1};
+int sign_magnetic[] = {+1, +1, +1};
+float scale_accel = 1;
+float scale_gyro = 575;
+float scale_magnetic = 1;
+
+int pitch_phase_compensation = -1;
+int roll_phase_compensation = -1;
+int yaw_phase_compensation = -1;
+int magnetic_alignment_factor = -1;
+
+void pre_process_data(sensor_data<float> &data_out, sensor_data<float> &data_in, float *bias, int *sign, float scale)
+{
+ data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale;
+ data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale;
+ data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale;
+
+ data_out.m_time_stamp = data_in.m_time_stamp;
+}
+
int main()
{
int data_available = ORIENTATION_DATA_SIZE;
time_stamp = strtoull (token, NULL, 10);
sensor_data<float> 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);
time_stamp = strtoull (token, NULL, 10);
sensor_data<float> 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);
time_stamp = strtoull (token, NULL, 10);
sensor_data<float> magnetic_data(sdata[0], sdata[1], sdata[2], time_stamp);
+ pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
+ normalize(accel_data);
+ pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
+ pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
+ normalize(magnetic_data);
+
+ cout << "Accel Data\t" << accel_data.m_data << "\t Time Stamp\t" << accel_data.m_time_stamp << "\n\n";
+ cout << "Gyro Data\t" << gyro_data.m_data << "\t Time Stamp\t" << gyro_data.m_time_stamp << "\n\n";
cout << "Magnetic Data\t" << magnetic_data.m_data << "\t Time Stamp\t" << magnetic_data.m_time_stamp << "\n\n";
+ orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
+ orien_filter.m_roll_phase_compensation = roll_phase_compensation;
+ orien_filter.m_yaw_phase_compensation = yaw_phase_compensation;
+ orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
orientation = orien_filter.get_orientation(accel_data, gyro_data, magnetic_data);
orien_file << orientation.m_ang;
*
*/
-#include "../../../orientation_sensor.h"
+#include "../../orientation_sensor.h"
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <string>
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()
*
*/
-#include "../../quaternion.h"
+#include "../../../quaternion.h"
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<float> v0(4, arr0);
- vector<float> v1(4, arr1);
+ vect<float> v0(4, arr0);
+ vect<float> v1(4, arr1);
quaternion<float> q0(v0);
quaternion<float> q1(v1);
*
*/
-#include "../../rotation_matrix.h"
+#include "../../../rotation_matrix.h"
int main()
{
*
*/
-#include "../../sensor_data.h"
+#include "../../../sensor_data.h"
int main()
{
float arr1[3] = {1.04, -4.678, -2.34};
- vector<float> v1(3, arr1);
+ vect<float> v1(3, arr1);
sensor_data<float> sd1(2.0, 3.0, 4.0, 140737488355328);
sensor_data<float> sd2(1.04, -4.678, -2.34);
cout<< "\nSum:\n" << sd9.m_data << endl;
cout<< "\n\n\nNormalization:\n";
- sensor_data<float> sd6 = normalize(sd3);
cout<< "\n" << sd3.m_data;
- cout<< "\nResult:\n" << sd6.m_data << endl;
- sensor_data<float> 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";
*
*/
-#include "../../vector.h"
+#include "../../../vector.h"
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<float> v1(5, arr0);
- vector<float> v2(5, arr8);
- vector<float> v10(4, arr3);
- vector<float> v12(4, arr4);
- vector<float> v15(6, arr1);
- vector<float> v20(6, arr43);
- vector<float> v21(3, arr2);
- vector<float> v22(3, arr15);
- vector<float> v3(4);
- vector<float> v6(3);
- vector<float> v13(5);
- vector<float> v95(6);
- vector<float> v35(5, arr5);
+ vect<float> v1(5, arr0);
+ vect<float> v2(5, arr8);
+ vect<float> v10(4, arr3);
+ vect<float> v12(4, arr4);
+ vect<float> v15(6, arr1);
+ vect<float> v20(6, arr43);
+ vect<float> v21(3, arr2);
+ vect<float> v22(3, arr15);
+ vect<float> v3(4);
+ vect<float> v6(3);
+ vect<float> v13(5);
+ vect<float> v95(6);
+ vect<float> 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<float> m12(3, 3, (float *) arr57);
cout<< "\nProduct:\n"<< m102 << endl;
cout<< "\n\n\nVector x Multiplication matrix:\n";
- vector<float> v102 = (v22 * m12);
+ vect<float> v102 = (v22 * m12);
cout<< "\n" << v22 << "\n" << m12;
cout<< "\nProduct:\n" << v102 << endl;
float val = mul(v22, m32);
cout<< "\nOutput:\n" << v3 << endl;
- vector<float> v111 = cross(v21, v22);
+ vect<float> v111 = cross(v21, v22);
cout<< "\n\n\nCross Product:";
cout << "\n\n" << v21 << "\n\n" << v22;
cout << "\nResult:\n\n" << v111;
+++ /dev/null
-/*
- * 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 <typename TYPE>
-compute_gravity<TYPE>::compute_gravity()
-{
-
-}
-
-template <typename TYPE>
-compute_gravity<TYPE>::~compute_gravity()
-{
-
-}
-
-template <typename TYPE>
-sensor_data<TYPE> compute_gravity<TYPE>::orientation2gravity(const sensor_data<TYPE> accel,
- const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
-{
- orientation_filter<TYPE> orien_filter;
- euler_angles<TYPE> orientation;
- sensor_data<TYPE> 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
+++ /dev/null
-/*
- * 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 <typename TYPE>
-class compute_gravity {
-public:
- orientation_filter<TYPE> estimate_orientation;
-
- compute_gravity();
- ~compute_gravity();
-
- sensor_data<TYPE> orientation2gravity(const sensor_data<TYPE> accel,
- const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
-};
-
-#include "compute_gravity.cpp"
-
-#endif /* _COMPUTE_GRAVITY_H */
+++ /dev/null
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
- <storageModule moduleId="org.eclipse.cdt.core.settings">
- <cconfiguration id="cdt.managedbuild.config.gnu.exe.debug.1091813786">
- <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.config.gnu.exe.debug.1091813786" moduleId="org.eclipse.cdt.core.settings" name="Debug">
- <externalSettings/>
- <extensions>
- <extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
- <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- </extensions>
- </storageModule>
- <storageModule moduleId="cdtBuildSystem" version="4.0.0">
- <configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.debug.1091813786" name="Debug" parent="cdt.managedbuild.config.gnu.exe.debug">
- <folderInfo id="cdt.managedbuild.config.gnu.exe.debug.1091813786." name="/" resourcePath="">
- <toolChain id="cdt.managedbuild.toolchain.gnu.exe.debug.255002136" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.exe.debug">
- <targetPlatform id="cdt.managedbuild.target.gnu.platform.exe.debug.1445043365" name="Debug Platform" superClass="cdt.managedbuild.target.gnu.platform.exe.debug"/>
- <builder buildPath="${workspace_loc:/compute_gravity_test/Debug}" id="cdt.managedbuild.target.gnu.builder.exe.debug.256420247" managedBuildOn="true" name="Gnu Make Builder.Debug" superClass="cdt.managedbuild.target.gnu.builder.exe.debug"/>
- <tool id="cdt.managedbuild.tool.gnu.archiver.base.1149894111" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
- <tool id="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.1566195436" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug">
- <option id="gnu.cpp.compiler.exe.debug.option.optimization.level.227126937" superClass="gnu.cpp.compiler.exe.debug.option.optimization.level" value="gnu.cpp.compiler.optimization.level.none" valueType="enumerated"/>
- <option id="gnu.cpp.compiler.exe.debug.option.debugging.level.1438087954" superClass="gnu.cpp.compiler.exe.debug.option.debugging.level" value="gnu.cpp.compiler.debugging.level.max" valueType="enumerated"/>
- <inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.2105929867" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
- </tool>
- <tool id="cdt.managedbuild.tool.gnu.c.compiler.exe.debug.893436230" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.exe.debug">
- <option defaultValue="gnu.c.optimization.level.none" id="gnu.c.compiler.exe.debug.option.optimization.level.350012261" superClass="gnu.c.compiler.exe.debug.option.optimization.level" valueType="enumerated"/>
- <option id="gnu.c.compiler.exe.debug.option.debugging.level.414345757" superClass="gnu.c.compiler.exe.debug.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
- <inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1305446274" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
- </tool>
- <tool id="cdt.managedbuild.tool.gnu.c.linker.exe.debug.153888855" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.exe.debug"/>
- <tool id="cdt.managedbuild.tool.gnu.cpp.linker.exe.debug.234553236" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.exe.debug">
- <inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.571726140" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
- <additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
- <additionalInput kind="additionalinput" paths="$(LIBS)"/>
- </inputType>
- </tool>
- <tool id="cdt.managedbuild.tool.gnu.assembler.exe.debug.1983093387" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.exe.debug">
- <inputType id="cdt.managedbuild.tool.gnu.assembler.input.1607068893" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
- </tool>
- </toolChain>
- </folderInfo>
- </configuration>
- </storageModule>
- <storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
- </cconfiguration>
- <cconfiguration id="cdt.managedbuild.config.gnu.exe.release.1717646668">
- <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.config.gnu.exe.release.1717646668" moduleId="org.eclipse.cdt.core.settings" name="Release">
- <externalSettings/>
- <extensions>
- <extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
- <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
- </extensions>
- </storageModule>
- <storageModule moduleId="cdtBuildSystem" version="4.0.0">
- <configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.release.1717646668" name="Release" parent="cdt.managedbuild.config.gnu.exe.release">
- <folderInfo id="cdt.managedbuild.config.gnu.exe.release.1717646668." name="/" resourcePath="">
- <toolChain id="cdt.managedbuild.toolchain.gnu.exe.release.55401756" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.exe.release">
- <targetPlatform id="cdt.managedbuild.target.gnu.platform.exe.release.703544742" name="Debug Platform" superClass="cdt.managedbuild.target.gnu.platform.exe.release"/>
- <builder buildPath="${workspace_loc:/compute_gravity_test/Release}" id="cdt.managedbuild.target.gnu.builder.exe.release.495854831" managedBuildOn="true" name="Gnu Make Builder.Release" superClass="cdt.managedbuild.target.gnu.builder.exe.release"/>
- <tool id="cdt.managedbuild.tool.gnu.archiver.base.1092424788" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
- <tool id="cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.1807942092" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.exe.release">
- <option id="gnu.cpp.compiler.exe.release.option.optimization.level.1858864018" superClass="gnu.cpp.compiler.exe.release.option.optimization.level" value="gnu.cpp.compiler.optimization.level.most" valueType="enumerated"/>
- <option id="gnu.cpp.compiler.exe.release.option.debugging.level.1779660512" superClass="gnu.cpp.compiler.exe.release.option.debugging.level" value="gnu.cpp.compiler.debugging.level.none" valueType="enumerated"/>
- <inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.920009923" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
- </tool>
- <tool id="cdt.managedbuild.tool.gnu.c.compiler.exe.release.1167146357" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.exe.release">
- <option defaultValue="gnu.c.optimization.level.most" id="gnu.c.compiler.exe.release.option.optimization.level.122519507" superClass="gnu.c.compiler.exe.release.option.optimization.level" valueType="enumerated"/>
- <option id="gnu.c.compiler.exe.release.option.debugging.level.916943887" superClass="gnu.c.compiler.exe.release.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
- <inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.847199460" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
- </tool>
- <tool id="cdt.managedbuild.tool.gnu.c.linker.exe.release.2027729811" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.exe.release"/>
- <tool id="cdt.managedbuild.tool.gnu.cpp.linker.exe.release.858576018" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.exe.release">
- <inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.1526133730" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
- <additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
- <additionalInput kind="additionalinput" paths="$(LIBS)"/>
- </inputType>
- </tool>
- <tool id="cdt.managedbuild.tool.gnu.assembler.exe.release.1336440508" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.exe.release">
- <inputType id="cdt.managedbuild.tool.gnu.assembler.input.150720839" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
- </tool>
- </toolChain>
- </folderInfo>
- </configuration>
- </storageModule>
- </cconfiguration>
- </storageModule>
- <storageModule moduleId="cdtBuildSystem" version="4.0.0">
- <project id="compute_gravity_test.cdt.managedbuild.target.gnu.exe.528773176" name="Executable" projectType="cdt.managedbuild.target.gnu.exe"/>
- </storageModule>
- <storageModule moduleId="scannerConfiguration">
- <autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
- <scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.1717646668;cdt.managedbuild.config.gnu.exe.release.1717646668.;cdt.managedbuild.tool.gnu.c.compiler.exe.release.1167146357;cdt.managedbuild.tool.gnu.c.compiler.input.847199460">
- <autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"/>
- </scannerConfigBuildInfo>
- <scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.1091813786;cdt.managedbuild.config.gnu.exe.debug.1091813786.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.1566195436;cdt.managedbuild.tool.gnu.cpp.compiler.input.2105929867">
- <autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"/>
- </scannerConfigBuildInfo>
- <scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.1091813786;cdt.managedbuild.config.gnu.exe.debug.1091813786.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.893436230;cdt.managedbuild.tool.gnu.c.compiler.input.1305446274">
- <autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"/>
- </scannerConfigBuildInfo>
- <scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.1717646668;cdt.managedbuild.config.gnu.exe.release.1717646668.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.1807942092;cdt.managedbuild.tool.gnu.cpp.compiler.input.920009923">
- <autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"/>
- </scannerConfigBuildInfo>
- </storageModule>
-</cproject>
+++ /dev/null
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
- <name>compute_gravity_test</name>
- <comment></comment>
- <projects>
- </projects>
- <buildSpec>
- <buildCommand>
- <name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
- <triggers>clean,full,incremental,</triggers>
- <arguments>
- <dictionary>
- <key>?name?</key>
- <value></value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.append_environment</key>
- <value>true</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.autoBuildTarget</key>
- <value>all</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.buildArguments</key>
- <value></value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.buildCommand</key>
- <value>make</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.buildLocation</key>
- <value>${workspace_loc:/compute_gravity_test/Debug}</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
- <value>clean</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.contents</key>
- <value>org.eclipse.cdt.make.core.activeConfigSettings</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.enableAutoBuild</key>
- <value>false</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.enableCleanBuild</key>
- <value>true</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.enableFullBuild</key>
- <value>true</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.fullBuildTarget</key>
- <value>all</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.stopOnError</key>
- <value>true</value>
- </dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
- <value>true</value>
- </dictionary>
- </arguments>
- </buildCommand>
- <buildCommand>
- <name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
- <triggers>full,incremental,</triggers>
- <arguments>
- </arguments>
- </buildCommand>
- <buildCommand>
- <name>org.tizen.nativecpp.apichecker.core.builder</name>
- <arguments>
- </arguments>
- </buildCommand>
- </buildSpec>
- <natures>
- <nature>org.eclipse.cdt.core.cnature</nature>
- <nature>org.eclipse.cdt.core.ccnature</nature>
- <nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
- <nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
- <nature>org.tizen.nativecpp.apichecker.core.tizenCppNature</nature>
- </natures>
-</projectDescription>
+++ /dev/null
-/*
- * 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 <stdlib.h>
-#include <iostream>
-#include <fstream>
-#include <string>
-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<float> gravity;
- compute_gravity<float> 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<float> 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<float> 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<float> 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;
-}
-
+++ /dev/null
-/*
- * 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 _VECTOR_H
-#define _VECTOR_H
-
-#include "matrix.h"
-
-template <typename TYPE>
-class vector {
-public:
- int m_size;
- TYPE *m_vec;
-
- vector(void);
- vector(const int size);
- vector(const int size, TYPE *vec_data);
- vector(const vector<TYPE>& v);
- ~vector();
-
- vector<TYPE> operator =(const vector<TYPE>& v);
-
- template<typename T> friend ostream& operator << (ostream& dout,
- vector<T>& v);
- template<typename T> friend vector<T> operator +(const vector<T> v1,
- const vector<T> v2);
- template<typename T> friend vector<T> operator +(const vector<T> v,
- const T val);
- template<typename T> friend vector<T> operator -(const vector<T> v1,
- const vector<T> v2);
- template<typename T> friend vector<T> operator -(const vector<T> v,
- const T val);
- template<typename T> friend matrix<T> operator *(const matrix<T> v1,
- const vector<T> v2);
- template<typename T> friend vector<T> operator *(const vector<T> v,
- const matrix<T> m);
- template<typename T> friend vector<T> operator *(const vector<T> v,
- const T val);
- template<typename T> friend vector<T> operator /(const vector<T> v1,
- const T val);
- template<typename T> friend bool operator ==(const vector<T> v1,
- const vector<T> v2);
- template<typename T> friend bool operator !=(const vector<T> v1,
- const vector<T> v2);
-
- template<typename T> friend T mul(const vector<T> v, const matrix<T> m);
- template<typename T> friend void insert_end(vector<T>& v, T val);
- template<typename T> friend matrix<T> transpose(const vector<T> v);
- template <typename T> friend vector<T> transpose(const matrix<T> m);
- template<typename T> friend vector<T> cross(const vector<T> v1,
- const vector<T> v2);
- template <typename T> friend T var(const vector<T> v);
- template <typename T> friend bool is_initialized(const vector<T> v);
-};
-
-#include "vector.cpp"
-
-#endif /* _VECTOR_H */
#if defined (_VECTOR_H) && defined (_MATRIX_H)
template <typename TYPE>
-vector<TYPE>::vector(void)
+vect<TYPE>::vect(void)
{
m_vec = NULL;
}
template <typename TYPE>
-vector<TYPE>::vector(const int size)
+vect<TYPE>::vect(const int size)
{
m_size = size;
m_vec = NULL;
}
template <typename TYPE>
-vector<TYPE>::vector(const int size, TYPE *vec_data)
+vect<TYPE>::vect(const int size, TYPE *vec_data)
{
m_size = size;
m_vec = NULL;
}
template <typename TYPE>
-vector<TYPE>::vector(const vector<TYPE>& v)
+vect<TYPE>::vect(const vect<TYPE>& v)
{
m_size = v.m_size;
m_vec = NULL;
}
template <typename TYPE>
-vector<TYPE>::~vector()
+vect<TYPE>::~vect()
{
if (m_vec != NULL)
delete[] m_vec;
}
template <typename TYPE>
-vector<TYPE> vector<TYPE>::operator =(const vector<TYPE>& v)
+vect<TYPE> vect<TYPE>::operator =(const vect<TYPE>& v)
{
if (this == &v)
{
}
template <typename TYPE>
-ostream& operator <<(ostream& dout, vector<TYPE>& v)
+ostream& operator <<(ostream& dout, vect<TYPE>& v)
{
for (int j = 0; j < v.m_size; j++)
{
}
template <typename T>
-vector<T> operator +(const vector<T> v1, const vector<T> v2)
+vect<T> operator +(const vect<T> v1, const vect<T> v2)
{
assert(v1.m_size == v2.m_size);
- vector<T> v3(v1.m_size);
+ vect<T> 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];
}
template <typename T>
-vector<T> operator +(const vector<T> v, const T val)
+vect<T> operator +(const vect<T> v, const T val)
{
- vector<T> v1(v.m_size);
+ vect<T> v1(v.m_size);
for (int j = 0; j < v.m_size; j++)
v1.m_vec[j] = v.m_vec[j] + val;
}
template <typename T>
-vector<T> operator -(const vector<T> v1, const vector<T> v2)
+vect<T> operator -(const vect<T> v1, const vect<T> v2)
{
assert(v1.m_size == v2.m_size);
- vector<T> v3(v1.m_size);
+ vect<T> 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];
}
template <typename T>
-vector<T> operator -(const vector<T> v, const T val)
+vect<T> operator -(const vect<T> v, const T val)
{
- vector<T> v1(v.m_size);
+ vect<T> v1(v.m_size);
for (int j = 0; j < v.m_size; j++)
v1.m_vec[j] = v.m_vec[j] - val;
}
template <typename T>
-matrix<T> operator *(const matrix<T> m, const vector<T> v)
+matrix<T> operator *(const matrix<T> m, const vect<T> v)
{
assert(m.m_rows == v.m_size);
assert(m.m_cols == 1);
}
template <typename T>
-vector<T> operator *(const vector<T> v, const matrix<T> m)
+vect<T> operator *(const vect<T> v, const matrix<T> m)
{
assert(m.m_rows == v.m_size);
assert(m.m_cols != 1);
- vector<T> v1(m.m_cols);
+ vect<T> v1(m.m_cols);
for (int j = 0; j < m.m_cols; j++)
{
}
template <typename T>
-vector<T> operator *(const vector<T> v, const T val)
+vect<T> operator *(const vect<T> v, const T val)
{
- vector<T> v1(v.m_size);
+ vect<T> v1(v.m_size);
for (int j = 0; j < v.m_size; j++)
v1.m_vec[j] = v.m_vec[j] * val;
}
template <typename T>
-vector<T> operator /(const vector<T> v, const T val)
+vect<T> operator /(const vect<T> v, const T val)
{
- vector<T> v1(v.m_size);
+ vect<T> v1(v.m_size);
for (int j = 0; j < v.m_size; j++)
v1.m_vec[j] = v.m_vec[j] / val;
}
template <typename T>
-bool operator ==(const vector<T> v1, const vector<T> v2)
+bool operator ==(const vect<T> v1, const vect<T> v2)
{
if (v1.m_size == v2.m_size)
{
}
template <typename T>
-bool operator !=(const vector<T> v1, const vector<T> v2)
+bool operator !=(const vect<T> v1, const vect<T> v2)
{
return (!(v1 == v2));
}
template <typename T>
-matrix<T> transpose(const vector<T> v)
+matrix<T> transpose(const vect<T> v)
{
matrix<T> m(v.m_size, 1);
}
template <typename T>
-vector<T> transpose(const matrix<T> m)
+vect<T> transpose(const matrix<T> m)
{
- vector<T> v(m.m_rows);
+ vect<T> v(m.m_rows);
for (int i = 0; i < m.m_rows; i++)
v.m_vec[i] = m.m_mat[i][0];
}
template <typename T>
-T mul(const vector<T> v, const matrix<T> m)
+T mul(const vect<T> v, const matrix<T> m)
{
assert(m.m_rows == v.m_size);
assert(m.m_cols == 1);
template <typename T>
-void insert_end(vector<T>& v, T val)
+void insert_end(vect<T>& v, T val)
{
for (int i = 0; i < (v.m_size - 1); i++)
v.m_vec[i] = v.m_vec[i+1];
}
template <typename T>
-vector<T> cross(const vector<T> v1, const vector<T> v2)
+vect<T> cross(const vect<T> v1, const vect<T> v2)
{
- vector<T> v3(v1.m_size);
+ vect<T> 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]));
}
template <typename T>
-bool is_initialized(const vector<T> v)
+bool is_initialized(const vect<T> v)
{
- vector<T> v1(v.m_size);
+ vect<T> v1(v.m_size);
bool retval;
retval = (v == v1) ? false : true;
}
template <typename T>
-T var(const vector<T> v)
+T var(const vect<T> v)
{
T val = 0;
T mean, var, diff;
--- /dev/null
+/*
+ * 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 _VECTOR_H
+#define _VECTOR_H
+
+#include "matrix.h"
+
+template <typename TYPE>
+class vect {
+public:
+ int m_size;
+ TYPE *m_vec;
+
+ vect(void);
+ vect(const int size);
+ vect(const int size, TYPE *vec_data);
+ vect(const vect<TYPE>& v);
+ ~vect();
+
+ vect<TYPE> operator =(const vect<TYPE>& v);
+
+ template<typename T> friend ostream& operator << (ostream& dout,
+ vect<T>& v);
+ template<typename T> friend vect<T> operator +(const vect<T> v1,
+ const vect<T> v2);
+ template<typename T> friend vect<T> operator +(const vect<T> v,
+ const T val);
+ template<typename T> friend vect<T> operator -(const vect<T> v1,
+ const vect<T> v2);
+ template<typename T> friend vect<T> operator -(const vect<T> v,
+ const T val);
+ template<typename T> friend matrix<T> operator *(const matrix<T> v1,
+ const vect<T> v2);
+ template<typename T> friend vect<T> operator *(const vect<T> v,
+ const matrix<T> m);
+ template<typename T> friend vect<T> operator *(const vect<T> v,
+ const T val);
+ template<typename T> friend vect<T> operator /(const vect<T> v1,
+ const T val);
+ template<typename T> friend bool operator ==(const vect<T> v1,
+ const vect<T> v2);
+ template<typename T> friend bool operator !=(const vect<T> v1,
+ const vect<T> v2);
+
+ template<typename T> friend T mul(const vect<T> v, const matrix<T> m);
+ template<typename T> friend void insert_end(vect<T>& v, T val);
+ template<typename T> friend matrix<T> transpose(const vect<T> v);
+ template <typename T> friend vect<T> transpose(const matrix<T> m);
+ template<typename T> friend vect<T> cross(const vect<T> v1,
+ const vect<T> v2);
+ template <typename T> friend T var(const vect<T> v);
+ template <typename T> friend bool is_initialized(const vect<T> v);
+};
+
+#include "vector.cpp"
+
+#endif /* _VECTOR_H */
sensor_base.cpp
physical_sensor.cpp
virtual_sensor.cpp
- sensor_fusion.cpp
iio_common.cpp
)
install(FILES sensord-server.pc DESTINATION lib/pkgconfig)
install(FILES ${PROJECT_NAME}.pc DESTINATION lib/pkgconfig)
install(FILES
- sensor_fusion.h
crw_lock.h
worker_thread.h
cconfig.h
sensor_base.h
physical_sensor.h
virtual_sensor.h
- sensor_fusion.h
sf_common.h
cpacket.h
csocket.h
csensor_event_dispatcher::csensor_event_dispatcher()
: m_lcd_on(true)
{
- m_sensor_fusion = sensor_plugin_loader::get_instance().get_fusion();
}
csensor_event_dispatcher::~csensor_event_dispatcher() { }
unsigned int event_cnt = 0;
sensor_events[event_cnt++] = *((sensor_event_t *)seed_event);
- if (m_sensor_fusion) {
- if (m_sensor_fusion->is_started())
- m_sensor_fusion->fuse(*((sensor_event_t *)seed_event));
- }
-
virtual_sensors v_sensors = get_active_virtual_sensors();
virtual_sensors::iterator it_v_sensor;
it_v_sensor = v_sensors.begin();
#include <sf_common.h>
#include <csensor_event_queue.h>
#include <cclient_info_manager.h>
-#include <sensor_fusion.h>
#include <csocket.h>
#include <virtual_sensor.h>
#include <vconf.h>
event_type_last_event_map m_last_events;
virtual_sensors m_active_virtual_sensors;
cmutex m_active_virtual_sensors_mutex;
- sensor_fusion *m_sensor_fusion;
csensor_event_dispatcher();
~csensor_event_dispatcher();
#ifndef IIO_COMMON_H_
#define IIO_COMMON_H_
-struct channel_parameters {
+#include <linux/ioctl.h>
+
+#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;
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);
+++ /dev/null
-/*
- * libsensord-share
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- */
-
-#include <sensor_fusion.h>
-
-sensor_fusion::sensor_fusion()
-{
-}
-
-sensor_fusion::~sensor_fusion()
-{
-}
-
-bool sensor_fusion::is_data_ready(void)
-{
- return true;
-}
-
-bool sensor_fusion::get_properties(sensor_properties_t &properties)
-{
- return false;
-}
-
-bool sensor_fusion::add_interval(int client_id, unsigned int interval)
-{
- return true;
-}
-
-bool sensor_fusion::delete_interval(int client_id)
-{
- return true;
-}
-
-bool sensor_fusion::get_rotation_matrix(arr33_t &rot)
-{
- return false;
-}
-
-bool sensor_fusion::get_attitude(float &x, float &y, float &z, float &w)
-{
- return false;
-}
-
-bool sensor_fusion::get_gyro_bias(float &x, float &y, float &z)
-{
- return false;
-}
-
-bool sensor_fusion::get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy)
-{
- return false;
-}
-
-bool sensor_fusion::get_linear_acceleration(float &x, float &y, float &z)
-{
- return false;
-}
-
-bool sensor_fusion::get_gravity(float &x, float &y, float &z)
-{
- return false;
-}
-
-bool sensor_fusion::get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy)
-{
- return false;
-}
-
-bool sensor_fusion::get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w)
-{
- return false;
-}
-
-bool sensor_fusion::get_orientation(float &azimuth, float &pitch, float &roll)
-{
- return false;
-}
-
-bool sensor_fusion::is_fusion(void)
-{
- return true;
-}
+++ /dev/null
-/*
- * libsensord-share
- *
- * Copyright (c) 2014 Samsung Electronics Co., Ltd.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
- */
-
-#ifndef _SENSOR_FUSION_H_
-#define _SENSOR_FUSION_H_
-
-#include <array>
-#include <sensor_base.h>
-
-typedef std::array<std::array<float, 3> , 3> arr33_t;
-
-class sensor_fusion : public sensor_base
-{
-public:
- sensor_fusion();
- virtual ~sensor_fusion();
-
- virtual void fuse(const sensor_event_t &event) = 0;
- virtual bool is_data_ready(void);
- virtual bool add_interval(int client_id, unsigned int interval);
- virtual bool delete_interval(int client_id);
- virtual bool get_properties(sensor_properties_t &properties);
-
- virtual bool get_rotation_matrix(arr33_t &rot);
- virtual bool get_attitude(float &x, float &y, float &z, float &w);
- virtual bool get_gyro_bias(float &x, float &y, float &z);
- virtual bool get_rotation_vector(float &x, float &y, float &z, float &w, float &heading_accuracy);
- virtual bool get_linear_acceleration(float &x, float &y, float &z);
- virtual bool get_gravity(float &x, float &y, float &z);
- virtual bool get_rotation_vector_6axis(float &x, float &y, float &z, float &w, float &heading_accuracy);
- virtual bool get_geomagnetic_rotation_vector(float &x, float &y, float &z, float &w);
- virtual bool get_orientation(float &azimuth, float &pitch, float &roll);
-
- bool is_fusion(void);
-};
-
-#endif /*_SENSOR_FUSION_H_*/
#include <libxml/parser.h>
#include <sensor_hal.h>
#include <sensor_base.h>
-#include <sensor_fusion.h>
#include <dlfcn.h>
#include <common.h>
DBG("init [%s] module", module->get_name());
sensor_type_t sensor_type = module->get_type();
- if (module->is_fusion())
- m_fusions.push_back((sensor_fusion *) module);
- else
- m_sensors.insert(make_pair(sensor_type, module));
+ m_sensors.insert(make_pair(sensor_type, module));
}
return true;
return virtual_list;
}
-sensor_fusion *sensor_plugin_loader::get_fusion(void)
-{
- if (m_fusions.empty())
- return NULL;
-
- return m_fusions.front();
-}
-
-vector<sensor_fusion *> sensor_plugin_loader::get_fusions(void)
-{
- return m_fusions;
-}
-
bool sensor_plugin_loader::destroy()
{
sensor_base *sensor;
class sensor_hal;
class sensor_base;
-class sensor_fusion;
using std::pair;
using std::vector;
*
*/
-typedef vector<sensor_fusion *> fusion_plugins;
-/*
-* a fusion_plugins is a group of fusion plugin
-* <FUSION>
-* ...
-* </FUSION>
-*
-*/
-
class sensor_plugin_loader
{
private:
sensor_hal_plugins m_sensor_hals;
sensor_plugins m_sensors;
- fusion_plugins m_fusions;
public:
static sensor_plugin_loader &get_instance() {
vector<sensor_base *> get_virtual_sensors(void);
- sensor_fusion *get_fusion(void);
- vector<sensor_fusion *> get_fusions(void);
-
bool destroy();
};
#endif /*_SENSOR_PLUGIN_LOADER_H_*/
virtual ~virtual_sensor();
virtual void synthesize(const sensor_event_t &event, vector<sensor_event_t> &outs) = 0;
+ virtual int get_sensor_data(const unsigned int event_type, sensor_data_t &data) = 0;
bool is_virtual(void);
protected:
--- /dev/null
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+project(sensor-tc C)
+
+SET(PREFIX ${CMAKE_INSTALL_PREFIX})
+SET(EXEC_PREFIX "\${prefix}")
+SET(LIBDIR "\${prefix}/lib")
+SET(INCLUDEDIR "\${prefix}/include")
+SET(VERSION 1.0)
+
+INCLUDE(FindPkgConfig)
+pkg_check_modules(pkgs REQUIRED glib-2.0 dlog)
+
+add_definitions(${rpkgs_CFLAGS})
+add_definitions(-DPREFIX="${CMAKE_INSTALL_PREFIX}")
+
+configure_file(${PROJECT_NAME}.pc.in ${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}.pc @ONLY)
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+include_directories(${CMAKE_SOURCE_DIR}/src/libsensord)
+include_directories(${CMAKE_SOURCE_DIR}/src/shared)
+
+FOREACH(flag ${pkgs_CFLAGS})
+ SET(EXTRA_CFLAGS "${EXTRA_CFLAGS} ${flag}")
+ENDFOREACH(flag)
+
+SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${EXTRA_CFLAGS}")
+
+link_directories(../src/libsensord/)
+
+add_executable(accelerometer src/accelerometer.c)
+
+SET_TARGET_PROPERTIES(accelerometer PROPERTIES LINKER_LANGUAGE C)
+
+target_link_libraries(accelerometer glib-2.0 dlog sensor)
+
+INSTALL(TARGETS accelerometer DESTINATION /usr/bin/)
+
+
+
+
--- /dev/null
+# Package Information for pkg-config
+
+prefix=/usr/bin
+exec_prefix=${prefix}
+libdir=${prefix}/lib
+includedir=${prefix}/include/sensor-tc
+
+Name: sensor-tc
+Description: Sensor functional testing library
+Version: 1.0
+Requires:
+Libs: -L${libdir} -lsensor-tc
+Cflags: -I${includedir}
--- /dev/null
+# Package Information for pkg-config
+
+prefix=@PREFIX@
+exec_prefix=@EXEC_PREFIX@
+libdir=@LIBDIR@
+includedir=@INCLUDEDIR@/sensor-tc
+
+Name: sensor-tc
+Description: Sensor functional testing library
+Version: @VERSION@
+Requires:
+Libs: -L${libdir} -lsensor-tc
+Cflags: -I${includedir}
--- /dev/null
+#include <time.h>
+#include <glib.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <sensor.h>
+#include <stdbool.h>
+
+static GMainLoop *mainloop;
+
+void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data)
+{
+ sensor_data_t *data = (sensor_data_t *)event->event_data;
+ printf("Accelerometer [%6.6f] [%6.6f] [%6.6f] [%lld]\n\n", data->values[0], data->values[1], data->values[2], data->timestamp);
+}
+
+void printformat()
+{
+ printf("Usage : ./accelerometer <event> <interval>(optional)\n\n");
+ printf("event:\n");
+ printf("ROTATION_CHECK\n");
+ printf("RAW_DATA_REPORT_ON_TIME\n");
+ printf("CALIBRATION_NEEDED\n");
+ printf("SET_HORIZON\n");
+ printf("SET_WAKEUP\n");
+ printf("ORIENTATION_DATA_REPORT_ON_TIME\n");
+ printf("LINEAR_ACCELERATION_DATA_REPORT_ON_TIME\n");
+ printf("GRAVITY_DATA_REPORT_ON_TIME\n\n");
+ printf("interval:\n");
+ printf("The time interval should be entered based on the sampling frequency supported by accelerometer driver on the device in ms.If no value for sensor is entered default value by the driver will be used.\n");
+}
+
+int main(int argc,char **argv)
+{
+ int result, handle;
+ unsigned int event;
+ bool error_state = FALSE;
+
+ mainloop = g_main_loop_new(NULL, FALSE);
+ sensor_type_t type = ACCELEROMETER_SENSOR;
+ event_condition_t *event_condition = (event_condition_t*) malloc(sizeof(event_condition_t));
+ event_condition->cond_op = CONDITION_EQUAL;
+ event_condition->cond_value1=100;
+
+ if (argc != 2 && argc != 3) {
+ printformat();
+ error_state = TRUE;
+ }
+ else {
+ if (strcmp(argv[1], "ROTATION_CHECK") == 0)
+ event = ACCELEROMETER_EVENT_ROTATION_CHECK;
+ else if (strcmp(argv[1], "RAW_DATA_REPORT_ON_TIME") == 0)
+ event = ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME;
+ else if (strcmp(argv[1], "CALIBRATION_NEEDED") == 0)
+ event = ACCELEROMETER_EVENT_CALIBRATION_NEEDED;
+ else if (strcmp(argv[1], "SET_HORIZON") == 0)
+ event = ACCELEROMETER_EVENT_SET_HORIZON;
+ else if (strcmp(argv[1], "SET_WAKEUP") == 0)
+ event = ACCELEROMETER_EVENT_SET_WAKEUP;
+ else if (strcmp(argv[1], "ORIENTATION_DATA_REPORT_ON_TIME") == 0)
+ event = ACCELEROMETER_EVENT_ORIENTATION_DATA_REPORT_ON_TIME;
+ else if (strcmp(argv[1], "LINEAR_ACCELERATION_DATA_REPORT_ON_TIME") == 0)
+ event = ACCELEROMETER_EVENT_LINEAR_ACCELERATION_DATA_REPORT_ON_TIME;
+ else if (strcmp(argv[1], "GRAVITY_DATA_REPORT_ON_TIME") == 0)
+ event = ACCELEROMETER_EVENT_GRAVITY_DATA_REPORT_ON_TIME;
+ else {
+ printformat();
+ error_state = TRUE;
+ }
+ if (argc == 3)
+ event_condition->cond_value1 = atof(argv[2]);
+ }
+
+ if (!error_state) {
+ handle = sf_connect(type);
+ result = sf_register_event(handle, event, event_condition, callback, NULL);
+
+ if (result < 0)
+ printf("Can't register accelerometer\n");
+
+ if (!(sf_start(handle,0) < 0)) {
+ printf("Success start \n");
+ }
+ else {
+ printf("Error\n\n\n\n");
+ sf_unregister_event(handle, event);
+ sf_disconnect(handle);
+ return -1;
+ }
+
+ g_main_loop_run(mainloop);
+ g_main_loop_unref(mainloop);
+
+ sf_unregister_event(handle, event);
+
+ if (!(sf_stop(handle) < 0))
+ printf("Success stop \n");
+
+ sf_disconnect(handle);
+ }
+
+ free(event_condition);
+
+ return 0;
+}
+
projects / platform / core / system / sensord.git / commitdiff