From 20d449c883eaabf407aaa2ac3b5bb9ae11c0cc3b Mon Sep 17 00:00:00 2001 From: Boram Bae Date: Thu, 14 Jan 2021 18:45:44 +0900 Subject: [PATCH 01/16] Use hal-api-sensor instead of legacy interface * This change is for Tizen Next-HAL * Now, all implementations of sensor device inherit sensor device interface of the hal-api-sensor * Move sensor_hal and sensor_hal_types to hal-api-sensor * Name sensor_loader`s existing HAL as Legacy HAL Change-Id: I74693374db341475039d31071798e5a252dfc39f Signed-off-by: Boram Bae --- include/physical_sensor.h | 2 +- include/sensor_hal.h | 75 ----- include/sensor_hal_types.h | 424 ----------------------------- include/sensor_types.h | 2 +- packaging/sensord.spec | 18 +- src/fusion-sensor/rotation_vector/fusion.h | 2 +- src/server/CMakeLists.txt | 3 +- src/server/sensor_loader.cpp | 46 +++- src/server/sensor_loader.h | 4 +- src/server/sensor_manager.cpp | 11 +- src/shared/sensor_info.h | 2 +- 11 files changed, 52 insertions(+), 537 deletions(-) delete mode 100644 include/sensor_hal.h delete mode 100644 include/sensor_hal_types.h diff --git a/include/physical_sensor.h b/include/physical_sensor.h index 58c57da..cacbe56 100644 --- a/include/physical_sensor.h +++ b/include/physical_sensor.h @@ -25,7 +25,7 @@ #include #include -#include "sensor_hal.h" +#include #ifndef SENSOR_VERSION #define PHYSICAL_SENSOR_VERSION(maj, min) \ diff --git a/include/sensor_hal.h b/include/sensor_hal.h deleted file mode 100644 index d90053c..0000000 --- a/include/sensor_hal.h +++ /dev/null @@ -1,75 +0,0 @@ -/* - * Copyright (c) 2016 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef _SENSOR_HAL_H_ -#define _SENSOR_HAL_H_ - -#include -#include -#include "sensor_hal_types.h" - -/* - * Create devices - */ -typedef void *sensor_device_t; -typedef int (*create_t)(sensor_device_t **devices); - -/* - * Sensor device interface - * 1 device must be abstracted from 1 device event node - */ -class sensor_device { -public: - virtual ~sensor_device() {} - - uint32_t get_hal_version(void) - { - return SENSOR_HAL_VERSION(1, 0); - } - - virtual int get_poll_fd(void) = 0; - virtual int get_sensors(const sensor_info_t **sensors) = 0; - - virtual bool enable(uint32_t id) = 0; - virtual bool disable(uint32_t id) = 0; - - virtual int read_fd(uint32_t **ids) = 0; - virtual int get_data(uint32_t id, sensor_data_t **data, int *length) = 0; - - virtual bool set_interval(uint32_t id, unsigned long val) - { - return true; - } - virtual bool set_batch_latency(uint32_t id, unsigned long val) - { - return true; - } - virtual bool set_attribute_int(uint32_t id, int32_t attribute, int32_t value) - { - return true; - } - virtual bool set_attribute_str(uint32_t id, int32_t attribute, char *value, int len) - { - return true; - } - virtual bool flush(uint32_t id) - { - return true; - } -}; - -#endif /* _SENSOR_HAL_H_ */ diff --git a/include/sensor_hal_types.h b/include/sensor_hal_types.h deleted file mode 100644 index 5dbdaca..0000000 --- a/include/sensor_hal_types.h +++ /dev/null @@ -1,424 +0,0 @@ -/* - * Copyright (c) 2016 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef _SENSOR_HAL_TYPES_H_ -#define _SENSOR_HAL_TYPES_H_ - -#include -#include -#include - -#define SENSOR_HAL_VERSION(maj, min) \ - ((((maj) & 0xFFFF) << 24) | ((min) & 0xFFFF)) - -#ifdef __cplusplus -extern "C" -{ -#endif /* __cplusplus */ - -/* - * Sensor Types - * These types are used to controll the sensors - * - * - base unit - * acceleration values : meter per second^2 (m/s^2) - * magnetic values : micro-Tesla (uT) - * orientation values : degrees - * gyroscope values : degree/s - * temperature values : degrees centigrade - * proximity valeus : distance - * light values : lux - * pressure values : hectopascal (hPa) - * humidity : relative humidity (%) - */ -typedef enum { - SENSOR_DEVICE_UNKNOWN = -2, - SENSOR_DEVICE_ALL = -1, - SENSOR_DEVICE_ACCELEROMETER, - SENSOR_DEVICE_GRAVITY, - SENSOR_DEVICE_LINEAR_ACCELERATION, - SENSOR_DEVICE_GEOMAGNETIC, - SENSOR_DEVICE_ROTATION_VECTOR, - SENSOR_DEVICE_ORIENTATION, - SENSOR_DEVICE_GYROSCOPE, - SENSOR_DEVICE_LIGHT, - SENSOR_DEVICE_PROXIMITY, - SENSOR_DEVICE_PRESSURE, - SENSOR_DEVICE_ULTRAVIOLET, - SENSOR_DEVICE_TEMPERATURE, - SENSOR_DEVICE_HUMIDITY, - SENSOR_DEVICE_HRM, - SENSOR_DEVICE_HRM_LED_GREEN, - SENSOR_DEVICE_HRM_LED_IR, - SENSOR_DEVICE_HRM_LED_RED, - SENSOR_DEVICE_GYROSCOPE_UNCAL, - SENSOR_DEVICE_GEOMAGNETIC_UNCAL, - SENSOR_DEVICE_GYROSCOPE_RV, - SENSOR_DEVICE_GEOMAGNETIC_RV, - - SENSOR_DEVICE_SIGNIFICANT_MOTION = 0x100, - - SENSOR_DEVICE_HRM_BATCH = 0x200, - SENSOR_DEVICE_HRM_LED_GREEN_BATCH, - - SENSOR_DEVICE_HUMAN_PEDOMETER = 0x300, - SENSOR_DEVICE_HUMAN_SLEEP_MONITOR, - SENSOR_DEVICE_HUMAN_SLEEP_DETECTOR, - SENSOR_DEVICE_HUMAN_STRESS_MONITOR, - - SENSOR_DEVICE_EXERCISE_WALKING = 0x400, - SENSOR_DEVICE_EXERCISE_RUNNING, - SENSOR_DEVICE_EXERCISE_HIKING, - SENSOR_DEVICE_EXERCISE_CYCLING, - SENSOR_DEVICE_EXERCISE_ELLIPTICAL, - SENSOR_DEVICE_EXERCISE_INDOOR_CYCLING, - SENSOR_DEVICE_EXERCISE_ROWING, - SENSOR_DEVICE_EXERCISE_STEPPER, - - SENSOR_DEVICE_DATA_JOURNAL = 0x500, - - SENSOR_DEVICE_FUSION = 0x900, - SENSOR_DEVICE_AUTO_ROTATION, - SENSOR_DEVICE_AUTO_BRIGHTNESS, - SENSOR_DEVICE_MYOTEST, - - SENSOR_DEVICE_GESTURE_MOVEMENT = 0x1200, - SENSOR_DEVICE_GESTURE_WRIST_UP, - SENSOR_DEVICE_GESTURE_WRIST_DOWN, - SENSOR_DEVICE_GESTURE_MOVEMENT_STATE, - SENSOR_DEVICE_GESTURE_PICK_UP, - SENSOR_DEVICE_GESTURE_FACE_DOWN, - - SENSOR_DEVICE_ACTIVITY_TRACKER = 0x1A00, - SENSOR_DEVICE_ACTIVITY_LEVEL_MONITOR, - SENSOR_DEVICE_GPS_BATCH, - SENSOR_DEVICE_PPG_BATCH, - SENSOR_DEVICE_GPS_TIMESYNC, - - SENSOR_DEVICE_HRM_CTRL = 0x1A80, - SENSOR_DEVICE_REG_CTRL, - SENSOR_DEVICE_GPS_CTRL, - - SENSOR_DEVICE_WEAR_STATUS = 0x2000, - SENSOR_DEVICE_WEAR_ON_MONITOR, - SENSOR_DEVICE_NO_MOVE_DETECTOR, - SENSOR_DEVICE_RESTING_HR, - SENSOR_DEVICE_STEP_LEVEL_MONITOR, - SENSOR_DEVICE_EXERCISE_STANDALONE, - SENSOR_DEVICE_EXERCISE_HR, - SENSOR_DEVICE_WORKOUT, - SENSOR_DEVICE_CYCLE_MONITOR, - SENSOR_DEVICE_STAIR_TRACKER, - SENSOR_DEVICE_PRESSURE_INDICATOR, - SENSOR_DEVICE_PRESSURE_ALERT, - SENSOR_DEVICE_HR_CALORIE, - SENSOR_DEVICE_SWIMMING_TRACKER, - SENSOR_DEVICE_STRESS_TRACKER, - SENSOR_DEVICE_FAKE_MOTION, - SENSOR_DEVICE_GEOFENCE, - SENSOR_DEVICE_SWIMMING_OUTDOOR, - SENSOR_DEVICE_AUTO_SWIMMING, - SENSOR_DEVICE_INACTIVITY_DETECTOR, - SENSOR_DEVICE_HRM_BP, - SENSOR_DEVICE_ECG, - SENSOR_DEVICE_FALL_DETECTION, - - SENSOR_DEVICE_CONTEXT = 0x7000, - SENSOR_DEVICE_MOTION, - SENSOR_DEVICE_PIR, - SENSOR_DEVICE_PIR_LONG, - SENSOR_DEVICE_DUST, - SENSOR_DEVICE_THERMOMETER, - SENSOR_DEVICE_PEDOMETER, - SENSOR_DEVICE_FLAT, - SENSOR_DEVICE_HRM_RAW, - SENSOR_DEVICE_TILT, - SENSOR_DEVICE_ROTATION_VECTOR_RAW, - SENSOR_DEVICE_GSR, - SENSOR_DEVICE_SIMSENSE, - SENSOR_DEVICE_PPG, -} sensor_device_type; - -/* - * A platform sensor handler is generated based on this handle - * This id can be assigned from HAL developer. so it has to be unique in 1 sensor_device. - */ -typedef struct sensor_info_t { - uint32_t id; - const char *name; - sensor_device_type type; - unsigned int event_type; // for Internal API - const char *model_name; - const char *vendor; - float min_range; - float max_range; - float resolution; - int min_interval; - int max_batch_count; - bool wakeup_supported; -} sensor_info_t; - -enum sensor_accuracy_t { - SENSOR_ACCURACY_UNDEFINED = -1, - SENSOR_ACCURACY_BAD = 0, - SENSOR_ACCURACY_NORMAL = 1, - SENSOR_ACCURACY_GOOD = 2, - SENSOR_ACCURACY_VERYGOOD = 3 -}; - -#define SENSOR_DATA_VALUE_SIZE 16 - -/* sensor_data_t */ -typedef struct sensor_data_t { - int accuracy; - unsigned long long timestamp; - int value_count; - float values[SENSOR_DATA_VALUE_SIZE]; -} sensor_data_t; - -#define SENSORHUB_DATA_VALUE_SIZE 4096 - -/* sensorhub_data_t */ -typedef struct sensorhub_data_t { - int accuracy; - unsigned long long timestamp; - - /* - * Use "value_count" instead of "hub_data_size" - * which is going to be removed soon - */ - union { - int value_count; - int hub_data_size; /* deprecated */ - }; - - /* - * Use "values" instead of "hub_data" - * which is going to be removed soon - */ - union { - char values[SENSORHUB_DATA_VALUE_SIZE]; - char hub_data[SENSORHUB_DATA_VALUE_SIZE]; /* deprecated */ - }; -} sensorhub_data_t; - -#define SENSOR_PEDOMETER_DATA_DIFFS_SIZE 20 - -typedef struct { - int accuracy; - unsigned long long timestamp; - int value_count; /* value_count == 8 */ - float values[SENSOR_DATA_VALUE_SIZE]; - /* values = {step count, walk step count, run step count, - moving distance, calorie burned, last speed, - last stepping frequency (steps per sec), - last step status (walking, running, ...)} */ - /* Additional data attributes (not in sensor_data_t)*/ - int diffs_count; - struct differences { - int timestamp; - int steps; - int walk_steps; - int run_steps; - int walk_up_steps; - int walk_down_steps; - int run_up_steps; - int run_down_steps; - float distance; - float calories; - float speed; - } diffs[SENSOR_PEDOMETER_DATA_DIFFS_SIZE]; -} sensor_pedometer_data_t; - -#define CONVERT_TYPE_ATTR(type, index) ((type) << 8 | 0x80 | (index)) - -enum sensor_attribute { - SENSOR_ATTR_ACCELEROMETER_INJECTION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_ACCELEROMETER, 0xFF), - SENSOR_ATTR_ACTIVITY = CONVERT_TYPE_ATTR(SENSOR_DEVICE_ACTIVITY_TRACKER, 0x1), - - SENSOR_ATTR_HRM_BATCH_OPR_MODE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_BATCH, 0x1), - SENSOR_ATTR_HRM_BATCH_ACTIVITY_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_BATCH, 0x2), - SENSOR_ATTR_HRM_BATCH_BATCH_DUR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_BATCH, 0x3), - SENSOR_ATTR_HRM_BATCH_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_BATCH, 0x4), - SENSOR_ATTR_HRM_BATCH_ELEVATED_DUR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_BATCH, 0x5), - SENSOR_ATTR_HRM_BATCH_ELEVATED_THR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_BATCH, 0x6), - SENSOR_ATTR_HRM_BATCH_BPM_RAW = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_BATCH, 0x7), - SENSOR_ATTR_HRM_BATCH_LOW_ALERT_DUR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_BATCH, 0x8), - SENSOR_ATTR_HRM_BATCH_LOW_ALERT_THR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_BATCH, 0x9), - - SENSOR_ATTR_HRM_LED_GREEN_BATCH_POLLING_INTERVAL = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_LED_GREEN_BATCH, 0x1), - SENSOR_ATTR_HRM_LED_GREEN_BATCH_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_LED_GREEN_BATCH, 0x2), - SENSOR_ATTR_HRM_LED_GREEN_BATCH_BATCH_PERIOD = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HRM_LED_GREEN_BATCH, 0x3), - - SENSOR_ATTR_PEDOMETER_HEIGHT = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_PEDOMETER, 0x1), - SENSOR_ATTR_PEDOMETER_WEIGHT = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_PEDOMETER, 0x2), - SENSOR_ATTR_PEDOMETER_GENDER = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_PEDOMETER, 0x3), - SENSOR_ATTR_PEDOMETER_AGE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_PEDOMETER, 0x4), - SENSOR_ATTR_PEDOMETER_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_PEDOMETER, 0x2F), - - SENSOR_ATTR_STRESS_MONITOR_AGE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_STRESS_MONITOR, 0x1), - - SENSOR_ATTR_EXERCISE_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_STANDALONE, 0x1), - SENSOR_ATTR_EXERCISE_GPS = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_STANDALONE, 0x2), - SENSOR_ATTR_EXERCISE_BATCH_INTERVAL = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_STANDALONE, 0x3), - SENSOR_ATTR_EXERCISE_PSERVICE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_STANDALONE, 0x4), - - SENSOR_ATTR_CYCLE_DURATION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_CYCLE_MONITOR, 0x1), - SENSOR_ATTR_CYCLE_HOLDING_POSITION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_CYCLE_MONITOR, 0x2), - SENSOR_ATTR_CYCLE_VELOCITY = CONVERT_TYPE_ATTR(SENSOR_DEVICE_CYCLE_MONITOR, 0x3), - SENSOR_ATTR_CYCLE_GET_DATA_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_CYCLE_MONITOR, 0x4), - SENSOR_ATTR_CYCLE_DATA_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_CYCLE_MONITOR, 0x5), - SENSOR_ATTR_CYCLE_GPS_AGREEMENT = CONVERT_TYPE_ATTR(SENSOR_DEVICE_CYCLE_MONITOR, 0x6), - - SENSOR_ATTR_WORKOUT_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_WORKOUT, 0x1), - SENSOR_ATTR_WORKOUT_DURATION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_WORKOUT, 0x2), - - SENSOR_ATTR_RESTING_HR_OPR_MODE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x1), - SENSOR_ATTR_RESTING_HR_MAX_RHR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x2), - SENSOR_ATTR_RESTING_HR_MIN_RHR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x3), - SENSOR_ATTR_RESTING_HR_AVG_RHR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x4), - SENSOR_ATTR_RESTING_HR_HOUR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x5), - SENSOR_ATTR_RESTING_HR_MIN = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x6), - SENSOR_ATTR_RESTING_HR_SEC = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x7), - SENSOR_ATTR_RESTING_HR_PROPERTY_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x10), - SENSOR_ATTR_RESTING_HR_PROPERTY_HR_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x11), - SENSOR_ATTR_RESTING_HR_PROPERTY_HR_VALUE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x12), - SENSOR_ATTR_RESTING_HR_PROPERTY_DURATION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x13), - SENSOR_ATTR_RESTING_HR_PROPERTY_ACT_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x14), - SENSOR_ATTR_RESTING_HR_PROPERTY_ACT_DUR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x15), - SENSOR_ATTR_RESTING_HR_PROPERTY_CONT_DUR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x16), - SENSOR_ATTR_RESTING_HR_DATA_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x20), - SENSOR_ATTR_RESTING_HR_DATA_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_RESTING_HR, 0x21), - - SENSOR_ATTR_STEP_LEVEL_MONITOR_DW_DURATION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_STEP_LEVEL_MONITOR, 0x1), - - SENSOR_ATTR_EXERCISE_HR_OPR_MODE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_HR, 0x1), - SENSOR_ATTR_EXERCISE_HR_ACTIVITY_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_HR, 0x2), - SENSOR_ATTR_EXERCISE_HR_BATCH_DUR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_HR, 0x3), - SENSOR_ATTR_EXERCISE_HR_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_HR, 0x4), - SENSOR_ATTR_EXERCISE_HR_ELEVATED_DUR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_HR, 0x5), - SENSOR_ATTR_EXERCISE_HR_ELEVATED_THR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_HR, 0x6), - SENSOR_ATTR_EXERCISE_HR_BPM_RAW = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_HR, 0x7), - SENSOR_ATTR_EXERCISE_HR_LOW_ALERT_DUR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_HR, 0x8), - SENSOR_ATTR_EXERCISE_HR_LOW_ALERT_THR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_EXERCISE_HR, 0x9), - - SENSOR_ATTR_PRESSURE_INDICATOR_START = CONVERT_TYPE_ATTR(SENSOR_DEVICE_PRESSURE_INDICATOR, 0x1), - SENSOR_ATTR_PRESSURE_INDICATOR_STOP = CONVERT_TYPE_ATTR(SENSOR_DEVICE_PRESSURE_INDICATOR, 0x2), - SENSOR_ATTR_PRESSURE_INDICATOR_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_PRESSURE_INDICATOR, 0x3), - SENSOR_ATTR_PRESSURE_INDICATOR_RESTORE_TIME = CONVERT_TYPE_ATTR(SENSOR_DEVICE_PRESSURE_INDICATOR, 0x4), - SENSOR_ATTR_PRESSURE_INDICATOR_RESTORE_VALUE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_PRESSURE_INDICATOR, 0x5), - SENSOR_ATTR_PRESSURE_INDICATOR_CURRENT_TIME = CONVERT_TYPE_ATTR(SENSOR_DEVICE_PRESSURE_INDICATOR, 0x6), - - SENSOR_ATTR_PRESSURE_ALERT_START = CONVERT_TYPE_ATTR(SENSOR_DEVICE_PRESSURE_ALERT, 0x1), - SENSOR_ATTR_PRESSURE_ALERT_STOP = CONVERT_TYPE_ATTR(SENSOR_DEVICE_PRESSURE_ALERT, 0x2), - - SENSOR_ATTR_HR_CALORIE_AGE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HR_CALORIE, 0x1), - SENSOR_ATTR_HR_CALORIE_HEIGHT = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HR_CALORIE, 0x2), - SENSOR_ATTR_HR_CALORIE_WEIGHT = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HR_CALORIE, 0x3), - SENSOR_ATTR_HR_CALORIE_GENDER = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HR_CALORIE, 0x4), - SENSOR_ATTR_HR_CALORIE_INST = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HR_CALORIE, 0x10), - SENSOR_ATTR_HR_CALORIE_EXERCISE_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HR_CALORIE, 0x11), - SENSOR_ATTR_HR_CALORIE_TARGET_CAL = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HR_CALORIE, 0x12), - SENSOR_ATTR_HR_CALORIE_MAX_HEARTRATE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HR_CALORIE, 0x13), - SENSOR_ATTR_HR_CALORIE_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HR_CALORIE, 0x20), - - SENSOR_ATTR_SWIMMING_TRACKER_PLACE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_TRACKER, 0x1), - SENSOR_ATTR_SWIMMING_TRACKER_DISTANCE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_TRACKER, 0x2), - SENSOR_ATTR_SWIMMING_TRACKER_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_TRACKER, 0x3), - SENSOR_ATTR_SWIMMING_TRACKER_BATCH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_TRACKER, 0x4), - SENSOR_ATTR_SWIMMING_TRACKER_GOAL_CALORIES = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_TRACKER, 0x5), - SENSOR_ATTR_SWIMMING_TRACKER_GOAL_LAPS = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_TRACKER, 0x6), - SENSOR_ATTR_SWIMMING_TRACKER_TIME_REST = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_TRACKER, 0x7), - SENSOR_ATTR_SWIMMING_TRACKER_TIME_TURN = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_TRACKER, 0x8), - - SENSOR_ATTR_SWIMMING_OUTDOOR_SET_BATCH_LAP_COUNT = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_OUTDOOR, 0x1), - SENSOR_ATTR_SWIMMING_OUTDOOR_SETGOAL_LENGTH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_OUTDOOR, 0x2), - SENSOR_ATTR_SWIMMING_OUTDOOR_SETGOAL_TIME = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_OUTDOOR, 0x3), - SENSOR_ATTR_SWIMMING_OUTDOOR_SET_GPS_PERMISSION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_OUTDOOR, 0x4), - SENSOR_ATTR_SWIMMING_OUTDOOR_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_SWIMMING_OUTDOOR, 0x6), - - SENSOR_ATTR_AUTO_SWIMMING_DURATION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_AUTO_SWIMMING, 0x1), - - SENSOR_ATTR_INACTIVITY_DETECTOR_LEVEL = CONVERT_TYPE_ATTR(SENSOR_DEVICE_INACTIVITY_DETECTOR, 0x1), - SENSOR_ATTR_INACTIVITY_DETECTOR_DURATION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_INACTIVITY_DETECTOR, 0x2), - - SENSOR_ATTR_STRESS_TRACKER_DUR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_STRESS_TRACKER, 0x1), - SENSOR_ATTR_STRESS_TRACKER_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_STRESS_TRACKER, 0x2), - - SENSOR_ATTR_STRESS_MONITOR_BASE_HR = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_STRESS_MONITOR, 0x1), - SENSOR_ATTR_STRESS_MONITOR_HISTO_INDEX = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_STRESS_MONITOR, 0x2), - SENSOR_ATTR_STRESS_MONITOR_HISTO_VALUE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_STRESS_MONITOR, 0x3), - SENSOR_ATTR_STRESS_MONITOR_TUNE_INDEX = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_STRESS_MONITOR, 0x4), - SENSOR_ATTR_STRESS_MONITOR_TUNE_VALUE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_STRESS_MONITOR, 0x5), - SENSOR_ATTR_STRESS_MONITOR_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_STRESS_MONITOR, 0x6), - SENSOR_ATTR_STRESS_MONITOR_OPERATION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_STRESS_MONITOR, 0x7), - - SENSOR_ATTR_SLEEP_MONITOR_STOP = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_SLEEP_MONITOR, 0x1), - SENSOR_ATTR_SLEEP_MONITOR_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_SLEEP_MONITOR, 0x2), - - SENSOR_ATTR_SLEEP_DETECTOR_STOP = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_SLEEP_DETECTOR, 0x1), - SENSOR_ATTR_SLEEP_DETECTOR_SETTING = CONVERT_TYPE_ATTR(SENSOR_DEVICE_HUMAN_SLEEP_DETECTOR, 0x2), - - SENSOR_ATTR_DATA_JOURNAL_START = CONVERT_TYPE_ATTR(SENSOR_DEVICE_DATA_JOURNAL, 0x1), - SENSOR_ATTR_DATA_JOURNAL_STOP = CONVERT_TYPE_ATTR(SENSOR_DEVICE_DATA_JOURNAL, 0x2), - - SENSOR_ATTR_GPS_BATCH_OPR_MODE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GPS_BATCH, 0x1), - - SENSOR_ATTR_PPG_BATCH_POLLING_INTERVAL = CONVERT_TYPE_ATTR(SENSOR_DEVICE_PPG_BATCH, 0x1), - SENSOR_ATTR_PPG_BATCH_FLUSH = CONVERT_TYPE_ATTR(SENSOR_DEVICE_PPG_BATCH, 0x2), - - SENSOR_ATTR_REG_CTRL_TYPE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_REG_CTRL, 0x1), - SENSOR_ATTR_REG_CTRL_ADDRESS = CONVERT_TYPE_ATTR(SENSOR_DEVICE_REG_CTRL, 0x2), - SENSOR_ATTR_REG_CTRL_VALUE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_REG_CTRL, 0x3), - SENSOR_ATTR_REG_CTRL_MODE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_REG_CTRL, 0x4), - - SENSOR_ATTR_GPS_CTRL_SETTING = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GPS_CTRL, 0x1), - - SENSOR_ATTR_FAKE_MOTION_EVENT_INTERVAL = CONVERT_TYPE_ATTR(SENSOR_DEVICE_FAKE_MOTION, 0x1), - SENSOR_ATTR_FAKE_MOTION_EVENT_COUNT = CONVERT_TYPE_ATTR(SENSOR_DEVICE_FAKE_MOTION, 0x2), - SENSOR_ATTR_FAKE_MOTION_PAUSE_INTERVAL = CONVERT_TYPE_ATTR(SENSOR_DEVICE_FAKE_MOTION, 0x3), - - SENSOR_ATTR_GEOFENCE_ID = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x1), - SENSOR_ATTR_GEOFENCE_LAT = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x2), - SENSOR_ATTR_GEOFENCE_LON = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x3), - SENSOR_ATTR_GEOFENCE_RADIUS = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x4), - SENSOR_ATTR_GEOFENCE_PROVIDER = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x5), - SENSOR_ATTR_GEOFENCE_ACCURACY = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x6), - SENSOR_ATTR_GEOFENCE_TIMESTAMP = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x7), - SENSOR_ATTR_GEOFENCE_CONNECT = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x8), - SENSOR_ATTR_GEOFENCE_START = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x10), - SENSOR_ATTR_GEOFENCE_STOP = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x11), - SENSOR_ATTR_GEOFENCE_SET_LOCATION = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x12), - SENSOR_ATTR_GEOFENCE_BT_CONNECT = CONVERT_TYPE_ATTR(SENSOR_DEVICE_GEOFENCE, 0x13), - - SENSOR_ATTR_FALL_DETECTION_OPR_MODE = CONVERT_TYPE_ATTR(SENSOR_DEVICE_FALL_DETECTION, 0x1), -}; - -enum sensor_activity { - SENSOR_ACTIVITY_UNKNOWN = 1, - SENSOR_ACTIVITY_STILL = 2, - SENSOR_ACTIVITY_WALKING = 4, - SENSOR_ACTIVITY_RUNNING = 8, - SENSOR_ACTIVITY_IN_VEHICLE = 16, - SENSOR_ACTIVITY_ON_BICYCLE = 32, -}; - -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif /* _SENSOR_HAL_TYPES_H_ */ diff --git a/include/sensor_types.h b/include/sensor_types.h index a41c985..b566dae 100644 --- a/include/sensor_types.h +++ b/include/sensor_types.h @@ -25,7 +25,7 @@ #include #include -#include +#include #ifdef __cplusplus extern "C" diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 9ea15c0..3b3bde2 100644 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -18,6 +18,8 @@ BuildRequires: pkgconfig(libtzplatform-config) BuildRequires: pkgconfig(cynara-creds-socket) BuildRequires: pkgconfig(cynara-client) BuildRequires: pkgconfig(cynara-session) +BuildRequires: pkgconfig(hal-api-sensor) +BuildRequires: pkgconfig(hal-api-common) Requires: %{name}-dummy = %{version}-%{release} Provides: %{name}-profile_mobile = %{version}-%{release} @@ -52,15 +54,6 @@ Requires: %{name}-dummy = %{version}-%{release} %description devel Internal Sensor API (Development) - -%package -n sensor-hal-devel -Summary: Sensord HAL interface -Group: System/Development - -%description -n sensor-hal-devel -Sensord HAL interface - - %package -n sensor-test Summary: Sensord library Group: System/Testing @@ -74,7 +67,7 @@ Sensor functional testing %build MAJORVER=`echo %{version} | awk 'BEGIN {FS="."}{print $1}'` -%cmake . -DMAJORVER=${MAJORVER} -DFULLVER=%{version} +%cmake . -DMAJORVER=${MAJORVER} -DFULLVER=%{version} -DCMAKE_HAL_LIBDIR_PREFIX=%{_hal_libdir} make %{?_smp_mflags} %install @@ -132,11 +125,6 @@ echo "You need to reinstall %{name}-dummy to keep using the APIs after uninstall %{_libdir}/pkgconfig/sensor.pc -%files -n sensor-hal-devel -%manifest packaging/sensord.manifest -%{_includedir}/sensor/sensor_hal*.h - - %files -n sensor-test %{_bindir}/sensorctl diff --git a/src/fusion-sensor/rotation_vector/fusion.h b/src/fusion-sensor/rotation_vector/fusion.h index 6c14439..4457bc4 100644 --- a/src/fusion-sensor/rotation_vector/fusion.h +++ b/src/fusion-sensor/rotation_vector/fusion.h @@ -19,7 +19,7 @@ #ifndef __FUSION_H__ #define __FUSION_H__ -#include +#include class fusion { public: diff --git a/src/server/CMakeLists.txt b/src/server/CMakeLists.txt index b11a84d..a3ee963 100644 --- a/src/server/CMakeLists.txt +++ b/src/server/CMakeLists.txt @@ -1,13 +1,14 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) PROJECT(sensord CXX) -SET(DEPENDENTS "glib-2.0 gio-2.0 dlog libsystemd-daemon cynara-client cynara-creds-socket cynara-session vconf") +SET(DEPENDENTS "glib-2.0 gio-2.0 dlog libsystemd-daemon cynara-client cynara-creds-socket cynara-session vconf hal-api-sensor hal-api-common") INCLUDE(FindPkgConfig) PKG_CHECK_MODULES(SERVER_PKGS REQUIRED ${DEPENDENTS}) ADD_DEFINITIONS(${SENSOR_DEFINITIONS}) ADD_DEFINITIONS(-DLIBDIR="${CMAKE_INSTALL_LIBDIR}") +ADD_DEFINITIONS(-DHAL_LIBDIR="${CMAKE_HAL_LIBDIR_PREFIX}") FOREACH(flag ${SERVER_PKGS_CFLAGS}) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${flag}") diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index 07c5de8..8b1ecd6 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -19,28 +19,50 @@ #include "sensor_loader.h" -#include -#include -#include -#include #include -#include -#include -#include +#include #include +#include +#include +#include +#include +#include +#include +#include + #include using namespace sensor; -sensor_loader::sensor_loader() -{ +sensor_loader::sensor_loader() { + if (hal_sensor_get_backend() != 0) { + _E("Failed to load hal sensor backend"); + } } -sensor_loader::~sensor_loader() -{ +sensor_loader::~sensor_loader() { + if (hal_sensor_put_backend() != 0) { + _E("Failed to clear hal sensor backend"); + } } -void sensor_loader::load_hal(const std::string &path, device_sensor_registry_t &devices) +void sensor_loader::load_hal(device_sensor_registry_t &devices) { + void **results = nullptr; + + int size = hal_sensor_create(&results); + if (size <= 0 || !results) { + _E("Failed to get sensor from hal sensor backend"); + return; + } + + for (int i = 0; i < size; ++i) { + devices.emplace_back(static_cast(results[i])); + } + _I("Success to load sensor from hal sensor backend"); +} + + +void sensor_loader::load_hal_legacy(const std::string &path, device_sensor_registry_t &devices) { load(path, devices); } diff --git a/src/server/sensor_loader.h b/src/server/sensor_loader.h index 6aee6fe..559d696 100644 --- a/src/server/sensor_loader.h +++ b/src/server/sensor_loader.h @@ -20,7 +20,6 @@ #ifndef __SENSOR_LOADER_H__ #define __SENSOR_LOADER_H__ -#include #include #include #include @@ -41,7 +40,8 @@ public: sensor_loader(); virtual ~sensor_loader(); - void load_hal(const std::string &path, device_sensor_registry_t &devices); + void load_hal(device_sensor_registry_t &devices); + void load_hal_legacy(const std::string &path, device_sensor_registry_t &devices); void load_physical_sensor(const std::string &path, physical_sensor_registry_t &sensors); void load_fusion_sensor(const std::string &path, fusion_sensor_registry_t &sensors); void load_external_sensor(const std::string &path, external_sensor_registry_t &sensors); diff --git a/src/server/sensor_manager.cpp b/src/server/sensor_manager.cpp index 6280b6a..6555ad9 100644 --- a/src/server/sensor_manager.cpp +++ b/src/server/sensor_manager.cpp @@ -37,8 +37,9 @@ using namespace sensor; -#define DEVICE_HAL_DIR_PATH_LEGACY LIBDIR "/sensor" -#define DEVICE_HAL_DIR_PATH LIBDIR "/sensor/hal" +#define DEVICE_HAL_DIR_PATH_LEGACY1 LIBDIR "/sensor" +#define DEVICE_HAL_DIR_PATH_LEGACY2 HAL_LIBDIR "/sensorhub/" // only for emulator +#define DEVICE_HAL_DIR_PATH_LEGACY3 LIBDIR "/sensor/hal" #define PHYSICAL_SENSOR_DIR_PATH LIBDIR "/sensor/physical" #define VIRTUAL_SENSOR_DIR_PATH LIBDIR "/sensor/fusion" #define EXTERNAL_SENSOR_DIR_PATH LIBDIR "/sensor/external" @@ -59,8 +60,10 @@ sensor_manager::~sensor_manager() bool sensor_manager::init(void) { - m_loader.load_hal(DEVICE_HAL_DIR_PATH_LEGACY, devices); - m_loader.load_hal(DEVICE_HAL_DIR_PATH, devices); + m_loader.load_hal(devices); + m_loader.load_hal_legacy(DEVICE_HAL_DIR_PATH_LEGACY1, devices); + m_loader.load_hal_legacy(DEVICE_HAL_DIR_PATH_LEGACY2, devices); + m_loader.load_hal_legacy(DEVICE_HAL_DIR_PATH_LEGACY3, devices); m_loader.load_physical_sensor(PHYSICAL_SENSOR_DIR_PATH, physical_sensors); m_loader.load_fusion_sensor(VIRTUAL_SENSOR_DIR_PATH, fusion_sensors); m_loader.load_external_sensor(EXTERNAL_SENSOR_DIR_PATH, external_sensors); diff --git a/src/shared/sensor_info.h b/src/shared/sensor_info.h index d07b5b8..0ef89f5 100644 --- a/src/shared/sensor_info.h +++ b/src/shared/sensor_info.h @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include namespace sensor { -- 2.7.4 From 41d11a5d305a22ed52bd542c6818f1c6abec1948 Mon Sep 17 00:00:00 2001 From: Boram Bae Date: Wed, 27 Jan 2021 16:40:25 +0900 Subject: [PATCH 02/16] Add a dependency to sensor.pc Change-Id: I115db40800a6bf9b1438e9657d20e5db22f274ce Signed-off-by: Boram Bae --- src/client-dummy/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/client-dummy/CMakeLists.txt b/src/client-dummy/CMakeLists.txt index 626d61e..76d393f 100644 --- a/src/client-dummy/CMakeLists.txt +++ b/src/client-dummy/CMakeLists.txt @@ -1,7 +1,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) PROJECT(sensor CXX) -SET(DEPENDENTS "glib-2.0 gio-2.0 dlog vconf") +SET(DEPENDENTS "glib-2.0 gio-2.0 dlog vconf hal-api-common hal-api-sensor") SET(VERSION ${FULLVER}) SET(PREFIX ${CMAKE_INSTALL_PREFIX}) -- 2.7.4 From a09e1606a10bd7719be0913a81b36e0ca25e17be Mon Sep 17 00:00:00 2001 From: Boram Bae Date: Tue, 2 Feb 2021 14:40:55 +0900 Subject: [PATCH 03/16] Use hal-sensor-types * Use hal-sensor-types instead of hal-sensor-interface * This change required by plan of Next HAL Change-Id: I913dc2ffda97f5cb11c59afc25e8227309e5808a Signed-off-by: Boram Bae --- include/physical_sensor.h | 2 +- include/sensor_types.h | 2 +- src/fusion-sensor/rotation_vector/fusion.h | 2 +- src/server/sensor_loader.cpp | 1 - src/shared/sensor_info.h | 2 +- 5 files changed, 4 insertions(+), 5 deletions(-) diff --git a/include/physical_sensor.h b/include/physical_sensor.h index cacbe56..fd1e609 100644 --- a/include/physical_sensor.h +++ b/include/physical_sensor.h @@ -25,7 +25,7 @@ #include #include -#include +#include #ifndef SENSOR_VERSION #define PHYSICAL_SENSOR_VERSION(maj, min) \ diff --git a/include/sensor_types.h b/include/sensor_types.h index b566dae..207dd98 100644 --- a/include/sensor_types.h +++ b/include/sensor_types.h @@ -25,7 +25,7 @@ #include #include -#include +#include #ifdef __cplusplus extern "C" diff --git a/src/fusion-sensor/rotation_vector/fusion.h b/src/fusion-sensor/rotation_vector/fusion.h index 4457bc4..a1dd680 100644 --- a/src/fusion-sensor/rotation_vector/fusion.h +++ b/src/fusion-sensor/rotation_vector/fusion.h @@ -19,7 +19,7 @@ #ifndef __FUSION_H__ #define __FUSION_H__ -#include +#include class fusion { public: diff --git a/src/server/sensor_loader.cpp b/src/server/sensor_loader.cpp index 8b1ecd6..4daaa3c 100644 --- a/src/server/sensor_loader.cpp +++ b/src/server/sensor_loader.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include #include diff --git a/src/shared/sensor_info.h b/src/shared/sensor_info.h index 0ef89f5..5a1f791 100644 --- a/src/shared/sensor_info.h +++ b/src/shared/sensor_info.h @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include namespace sensor { -- 2.7.4 From 60e8113e7fa080a9a2c3210fcc5ad3b7835611e3 Mon Sep 17 00:00:00 2001 From: INSUN PYO Date: Wed, 3 Feb 2021 09:38:29 +0900 Subject: [PATCH 04/16] Change systemd-devel package name Change-Id: Iac39dc53e727d1aed361ef359ec8440acf406cc4 --- packaging/sensord.spec | 2 +- src/server/CMakeLists.txt | 2 +- src/shared/CMakeLists.txt | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/packaging/sensord.spec b/packaging/sensord.spec index 3b3bde2..8ab9019 100644 --- a/packaging/sensord.spec +++ b/packaging/sensord.spec @@ -13,7 +13,7 @@ BuildRequires: libattr-devel BuildRequires: pkgconfig(dlog) BuildRequires: pkgconfig(glib-2.0) BuildRequires: pkgconfig(vconf) -BuildRequires: pkgconfig(libsystemd-daemon) +BuildRequires: pkgconfig(libsystemd) BuildRequires: pkgconfig(libtzplatform-config) BuildRequires: pkgconfig(cynara-creds-socket) BuildRequires: pkgconfig(cynara-client) diff --git a/src/server/CMakeLists.txt b/src/server/CMakeLists.txt index a3ee963..4b38e05 100644 --- a/src/server/CMakeLists.txt +++ b/src/server/CMakeLists.txt @@ -1,7 +1,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) PROJECT(sensord CXX) -SET(DEPENDENTS "glib-2.0 gio-2.0 dlog libsystemd-daemon cynara-client cynara-creds-socket cynara-session vconf hal-api-sensor hal-api-common") +SET(DEPENDENTS "glib-2.0 gio-2.0 dlog libsystemd cynara-client cynara-creds-socket cynara-session vconf hal-api-sensor hal-api-common") INCLUDE(FindPkgConfig) PKG_CHECK_MODULES(SERVER_PKGS REQUIRED ${DEPENDENTS}) diff --git a/src/shared/CMakeLists.txt b/src/shared/CMakeLists.txt index 11f605b..481528e 100644 --- a/src/shared/CMakeLists.txt +++ b/src/shared/CMakeLists.txt @@ -1,7 +1,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) PROJECT(sensord-shared CXX) -SET(DEPENDENTS "dlog libsystemd-daemon glib-2.0 gio-2.0 vconf libtzplatform-config") +SET(DEPENDENTS "dlog libsystemd glib-2.0 gio-2.0 vconf libtzplatform-config") INCLUDE(FindPkgConfig) PKG_CHECK_MODULES(SHARED_PKGS REQUIRED ${DEPENDENTS}) -- 2.7.4 From e9b9f85ef4124dc2d9e9bf6305046a2ae373004f Mon Sep 17 00:00:00 2001 From: Boram Bae Date: Wed, 17 Feb 2021 11:37:02 +0900 Subject: [PATCH 05/16] Fix svace issues Change-Id: Ia19483be36444f2f2acb735b6d911f62d63d1f29 Signed-off-by: Boram Bae --- src/sensorctl/info.cpp | 22 +++++++++++----------- src/sensorctl/testcase/sensor_provider.cpp | 2 +- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/sensorctl/info.cpp b/src/sensorctl/info.cpp index db4eed5..40003f1 100644 --- a/src/sensorctl/info.cpp +++ b/src/sensorctl/info.cpp @@ -32,7 +32,7 @@ bool info_manager::run(int argc, char *argv[]) { sensor_type_t type; sensor_t *sensors = nullptr; - int count; + int count = 0; if (argc < INFO_ARGC) { usage(); @@ -51,16 +51,16 @@ bool info_manager::run(int argc, char *argv[]) void info_manager::show_info(sensor_t *sensors, int count) { - sensor_type_t type; - sensor_t sensor; - char *vendor; - char *name; - float min_range; - float max_range; - float resolution; - int min_interval; - int fifo_count; - int max_batch_count; + sensor_type_t type = UNKNOWN_SENSOR; + sensor_t sensor = nullptr; + char *vendor = nullptr; + char *name = nullptr; + float min_range = 0; + float max_range = 0; + float resolution = 0; + int min_interval = 0; + int fifo_count = 0; + int max_batch_count = 0; for (int i = 0; i < count; ++i) { sensor = sensors[i]; diff --git a/src/sensorctl/testcase/sensor_provider.cpp b/src/sensorctl/testcase/sensor_provider.cpp index 59e76de..1f5f7d4 100644 --- a/src/sensorctl/testcase/sensor_provider.cpp +++ b/src/sensorctl/testcase/sensor_provider.cpp @@ -116,7 +116,7 @@ static gboolean publish_batch_event(gpointer gdata) static void add_mysensor(void) { - sensord_provider_h provider; + sensord_provider_h provider = nullptr; sensord_create_provider(MYSENSOR_URI, &provider); sensord_provider_set_name(provider, MYSENSOR_NAME); -- 2.7.4 From dfc8ea97c31ec75ba46ad6a68764f85fa185d972 Mon Sep 17 00:00:00 2001 From: Boram Bae Date: Wed, 17 Feb 2021 14:45:28 +0900 Subject: [PATCH 06/16] Fix coverity issues Change-Id: Ia3bcbecc10af6bf2d095680fe6e62860b1fda8ea Signed-off-by: Boram Bae --- src/client/sensor_manager.cpp | 1 + src/sensorctl/loopback.cpp | 12 ++++++++---- src/sensorctl/testcase/sensor_provider.cpp | 27 +++++++++++++++++---------- src/server/sensor_policy_monitor.cpp | 20 ++++++++++++++++---- 4 files changed, 42 insertions(+), 18 deletions(-) diff --git a/src/client/sensor_manager.cpp b/src/client/sensor_manager.cpp index b72580a..10ec56c 100644 --- a/src/client/sensor_manager.cpp +++ b/src/client/sensor_manager.cpp @@ -50,6 +50,7 @@ int sensor_manager::get_sensor(const char *uri, sensor_t *sensor) { if (!is_supported(uri)) { *sensor = NULL; + _D("Not supported URI [%s]\n", uri); return -ENODATA; } diff --git a/src/sensorctl/loopback.cpp b/src/sensorctl/loopback.cpp index b890f47..aedf77a 100644 --- a/src/sensorctl/loopback.cpp +++ b/src/sensorctl/loopback.cpp @@ -64,8 +64,10 @@ bool loopback_manager::run(int argc, char *argv[]) for (int i = 4; i < argc; i++) test_data[i+2] = atoi(argv[i]); - sensord_set_attribute_str(handle, 0, test_data, sizeof(test_data)); - sensord_disconnect(handle); + int error = sensord_set_attribute_str(handle, 0, test_data, sizeof(test_data)); + WARN_IF(error != 0, "Failed to set attribute\n"); + bool ret = sensord_disconnect(handle); + WARN_IF(ret != true, "Failed to disconnect\n"); return true; } @@ -77,8 +79,10 @@ bool loopback_manager::run(int argc, char *argv[]) char test_data[4] = {SHUB_INST_LIB_REMOVE, SHUB_LOOP_BACK_LIB, }; - sensord_set_attribute_str(handle, 0, test_data, sizeof(test_data)); - sensord_disconnect(handle); + int error = sensord_set_attribute_str(handle, 0, test_data, sizeof(test_data)); + WARN_IF(error != 0, "Failed to set attribute\n"); + bool ret = sensord_disconnect(handle); + WARN_IF(ret != true, "Failed to disconnect\n"); return true; } diff --git a/src/sensorctl/testcase/sensor_provider.cpp b/src/sensorctl/testcase/sensor_provider.cpp index 1f5f7d4..8ff3631 100644 --- a/src/sensorctl/testcase/sensor_provider.cpp +++ b/src/sensorctl/testcase/sensor_provider.cpp @@ -116,18 +116,25 @@ static gboolean publish_batch_event(gpointer gdata) static void add_mysensor(void) { + int err = 0; sensord_provider_h provider = nullptr; - sensord_create_provider(MYSENSOR_URI, &provider); - sensord_provider_set_name(provider, MYSENSOR_NAME); - sensord_provider_set_vendor(provider, MYSENSOR_VENDOR); - sensord_provider_set_range(provider, 0.0f, 1.0f); - sensord_provider_set_resolution(provider, 0.01f); - - sensord_add_provider(provider); - - sensord_remove_provider(provider); - sensord_destroy_provider(provider); + err = sensord_create_provider(MYSENSOR_URI, &provider); + EXPECT_EQ(err, 0); + err = sensord_provider_set_name(provider, MYSENSOR_NAME); + EXPECT_EQ(err, 0); + err = sensord_provider_set_vendor(provider, MYSENSOR_VENDOR); + EXPECT_EQ(err, 0); + err = sensord_provider_set_range(provider, 0.0f, 1.0f); + EXPECT_EQ(err, 0); + err = sensord_provider_set_resolution(provider, 0.01f); + EXPECT_EQ(err, 0); + err = sensord_add_provider(provider); + EXPECT_EQ(err, 0); + err = sensord_remove_provider(provider); + EXPECT_EQ(err, 0); + err = sensord_destroy_provider(provider); + EXPECT_EQ(err, 0); } static void added_cb(const char *uri, void *user_data) diff --git a/src/server/sensor_policy_monitor.cpp b/src/server/sensor_policy_monitor.cpp index 9ab0f2b..8d66d33 100644 --- a/src/server/sensor_policy_monitor.cpp +++ b/src/server/sensor_policy_monitor.cpp @@ -62,14 +62,26 @@ sensor_policy_monitor& sensor_policy_monitor::get_instance(void) sensor_policy_monitor::sensor_policy_monitor() { - vconf_notify_key_changed(VCONFKEY_PM_STATE, power_save_state_cb, NULL); - vconf_notify_key_changed(VCONFKEY_SETAPPL_PSMODE, power_save_state_cb, NULL); + int ret = vconf_notify_key_changed(VCONFKEY_PM_STATE, power_save_state_cb, NULL); + if (ret != 0) { + _D("Could not add notify callback to VCONFKEY_PM_STATE"); + } + ret = vconf_notify_key_changed(VCONFKEY_SETAPPL_PSMODE, power_save_state_cb, NULL); + if (ret != 0) { + _D("Could not add notify callback to VCONFKEY_SETAPPL_PSMODE"); + } } sensor_policy_monitor::~sensor_policy_monitor() { - vconf_ignore_key_changed(VCONFKEY_PM_STATE, power_save_state_cb); - vconf_ignore_key_changed(VCONFKEY_SETAPPL_PSMODE, power_save_state_cb); + int ret = vconf_ignore_key_changed(VCONFKEY_PM_STATE, power_save_state_cb); + if (ret != 0) { + _D("Failed to vconf_ignore_key_changed[VCONFKEY_PM_STATE]"); + } + ret = vconf_ignore_key_changed(VCONFKEY_SETAPPL_PSMODE, power_save_state_cb); + if (ret != 0) { + _D("Failed to vconf_ignore_key_changed[VCONFKEY_SETAPPL_PSMODE]"); + } } void sensor_policy_monitor::add_listener(sensor_policy_listener *listener) -- 2.7.4 From d3477311066e749720d6fcdd2028c013f09c83b6 Mon Sep 17 00:00:00 2001 From: INSUN PYO Date: Mon, 15 Mar 2021 16:35:23 +0900 Subject: [PATCH 07/16] Modify sensord.socket and sensord.service 1. Remove systemd default value. sensord.socket 2. Added more robust exception handling. If sensord.socket fails, sensord.service is not executed. Change-Id: I706d248b05b5b21fc7b1c319054c3fb5ba6e7315 --- packaging/sensord.service | 2 +- packaging/sensord.socket | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/packaging/sensord.service b/packaging/sensord.service index be67d1b..e3c6e19 100644 --- a/packaging/sensord.service +++ b/packaging/sensord.service @@ -1,5 +1,6 @@ [Unit] Description=Sensor Daemon +Requires=sensord.socket [Service] User=sensor @@ -8,7 +9,6 @@ Type=notify SmackProcessLabel=System ExecStart=/usr/bin/sensord MemoryLimit=20M -Sockets=sensord.socket Nice=-5 [Install] diff --git a/packaging/sensord.socket b/packaging/sensord.socket index 31cff8d..a64cd4b 100644 --- a/packaging/sensord.socket +++ b/packaging/sensord.socket @@ -7,7 +7,5 @@ SocketGroup=input ListenStream=/run/.sensord.socket SocketMode=0777 PassCredentials=yes -Accept=false SmackLabelIPIn=* SmackLabelIPOut=@ -Service=sensord.service -- 2.7.4 From 6cff7cdc022670340cc688ba37c6d589c052b3d2 Mon Sep 17 00:00:00 2001 From: Yunmi Ha Date: Fri, 23 Apr 2021 19:26:56 +0900 Subject: [PATCH 08/16] Fix an error while restoring connection in case of server down - remove command channel before restoring connection Change-Id: I18967110bb293e7d9442628bf7b980e436c1d33d Signed-off-by: Yunmi Ha --- src/client/sensor_internal.cpp | 3 +++ src/client/sensor_listener.cpp | 5 +++++ src/client/sensor_manager.cpp | 4 ++++ 3 files changed, 12 insertions(+) diff --git a/src/client/sensor_internal.cpp b/src/client/sensor_internal.cpp index 7fa8f4d..f185e94 100644 --- a/src/client/sensor_internal.cpp +++ b/src/client/sensor_internal.cpp @@ -364,6 +364,9 @@ API bool sensord_disconnect(int handle) delete listener; listeners.erase(handle); + if (listeners.empty()) + manager.disconnect(); + return true; } diff --git a/src/client/sensor_listener.cpp b/src/client/sensor_listener.cpp index 51c0152..12e2d99 100644 --- a/src/client/sensor_listener.cpp +++ b/src/client/sensor_listener.cpp @@ -187,6 +187,11 @@ sensor_t sensor_listener::get_sensor(void) void sensor_listener::restore(void) { ret_if(!is_connected()); + + m_cmd_channel->disconnect(); + delete m_cmd_channel; + m_cmd_channel = NULL; + retm_if(!connect(), "Failed to restore listener"); _D("Restoring sensor listener"); diff --git a/src/client/sensor_manager.cpp b/src/client/sensor_manager.cpp index 10ec56c..bcb86d5 100644 --- a/src/client/sensor_manager.cpp +++ b/src/client/sensor_manager.cpp @@ -249,6 +249,10 @@ void sensor_manager::restore(void) { ret_if(!is_connected()); + m_cmd_channel->disconnect(); + delete m_cmd_channel; + m_cmd_channel = NULL; + m_connected.store(false); retm_if(!connect_channel(), "Failed to restore manager"); -- 2.7.4 From e96d6fee64f76b442329f9a8b7360ff4edcfb496 Mon Sep 17 00:00:00 2001 From: Hyotaek Shim Date: Tue, 25 May 2021 14:56:51 +0900 Subject: [PATCH 09/16] Add namespace sensor for Autolock Change-Id: Ie5df4a9471cb2dfac519ce37cb195206fe7f5761 Signed-off-by: Hyotaek Shim --- .gitignore | 5 ----- src/shared/cbase_lock.cpp | 2 ++ src/shared/cbase_lock.h | 3 +++ src/shared/channel.cpp | 1 + src/shared/channel.h | 2 +- src/shared/cmutex.cpp | 2 ++ src/shared/cmutex.h | 2 ++ src/shared/event_loop.cpp | 1 + src/shared/event_loop.h | 2 +- 9 files changed, 13 insertions(+), 7 deletions(-) delete mode 100644 .gitignore diff --git a/.gitignore b/.gitignore deleted file mode 100644 index fa43b1a..0000000 --- a/.gitignore +++ /dev/null @@ -1,5 +0,0 @@ -.project -.cproject -.*.swp -nbproject -.vscode \ No newline at end of file diff --git a/src/shared/cbase_lock.cpp b/src/shared/cbase_lock.cpp index a7d7586..6c39497 100644 --- a/src/shared/cbase_lock.cpp +++ b/src/shared/cbase_lock.cpp @@ -24,6 +24,8 @@ #include #include +using namespace sensor; + cbase_lock::cbase_lock() { m_history_mutex = PTHREAD_MUTEX_INITIALIZER; diff --git a/src/shared/cbase_lock.h b/src/shared/cbase_lock.h index 2de5c7e..5c76040 100644 --- a/src/shared/cbase_lock.h +++ b/src/shared/cbase_lock.h @@ -22,6 +22,8 @@ #include +namespace sensor { + enum lock_type { LOCK_TYPE_MUTEX, LOCK_TYPE_READ, @@ -83,5 +85,6 @@ public: Autolock(cbase_lock &m, lock_type type); ~Autolock(); }; +} #endif /* _CBASE_LOCK_H_ */ diff --git a/src/shared/channel.cpp b/src/shared/channel.cpp index 22b0eaa..10ed266 100644 --- a/src/shared/channel.cpp +++ b/src/shared/channel.cpp @@ -30,6 +30,7 @@ #define SYSTEMD_SOCK_BUF_SIZE (128*1024) using namespace ipc; +using namespace sensor; class send_event_handler : public event_handler { diff --git a/src/shared/channel.h b/src/shared/channel.h index bc8444f..5af06da 100644 --- a/src/shared/channel.h +++ b/src/shared/channel.h @@ -74,7 +74,7 @@ private: std::vector m_pending_event_id; std::atomic m_connected; - cmutex m_cmutex; + sensor::cmutex m_cmutex; }; } diff --git a/src/shared/cmutex.cpp b/src/shared/cmutex.cpp index 6245b6e..958835c 100644 --- a/src/shared/cmutex.cpp +++ b/src/shared/cmutex.cpp @@ -20,6 +20,8 @@ #include #include +using namespace sensor; + cmutex::cmutex() { pthread_mutexattr_t mutex_attr; diff --git a/src/shared/cmutex.h b/src/shared/cmutex.h index 94aa2b6..343a260 100644 --- a/src/shared/cmutex.h +++ b/src/shared/cmutex.h @@ -21,6 +21,7 @@ #define _CMUTEX_H_ #include "cbase_lock.h" +namespace sensor { class cmutex : public cbase_lock { public: @@ -39,4 +40,5 @@ private: pthread_mutex_t m_mutex; }; +} #endif /* _CMUTEX_H_ */ diff --git a/src/shared/event_loop.cpp b/src/shared/event_loop.cpp index a49a2e4..e0e9799 100644 --- a/src/shared/event_loop.cpp +++ b/src/shared/event_loop.cpp @@ -33,6 +33,7 @@ #define BAD_HANDLE 0 using namespace ipc; +using namespace sensor; static gboolean g_io_handler(GIOChannel *ch, GIOCondition condition, gpointer data) { diff --git a/src/shared/event_loop.h b/src/shared/event_loop.h index 405f628..40fcb2e 100644 --- a/src/shared/event_loop.h +++ b/src/shared/event_loop.h @@ -98,7 +98,7 @@ private: std::map m_handlers; int m_term_fd; - cmutex m_cmutex; + sensor::cmutex m_cmutex; }; } -- 2.7.4 From f5bed42b6258cdcf6976321eab24e580242c4fa9 Mon Sep 17 00:00:00 2001 From: Hyotaek Shim Date: Tue, 25 May 2021 16:17:35 +0900 Subject: [PATCH 10/16] Add .gitignore Change-Id: I14d9e446f4fd795f0fa314a30536890b8490cd23 Signed-off-by: Hyotaek Shim --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9306ae6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +cscope.files +cscope.out +tags -- 2.7.4 From 511186bdcc306dd863ed7e1536f5ec334126e619 Mon Sep 17 00:00:00 2001 From: "taemin.yeom" Date: Wed, 30 Jun 2021 16:31:37 +0900 Subject: [PATCH 11/16] Fix min range values in rotation vector Change-Id: I544d6dc116fbe92be9c32ff26ccdd20978708988 Signed-off-by: taemin.yeom --- src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp | 2 +- src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp | 2 +- src/fusion-sensor/rotation_vector/rv_sensor.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp b/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp index 8f2d9bb..e954ddd 100644 --- a/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp +++ b/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp @@ -37,7 +37,7 @@ static sensor_info2_t sensor_info = { type: GYROSCOPE_RV_SENSOR, uri: NAME_SENSOR, vendor: NAME_VENDOR, - min_range: 0, + min_range: -1, max_range: 1, resolution: 1, min_interval: 10, diff --git a/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp b/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp index 8a40705..892ef10 100644 --- a/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp +++ b/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp @@ -37,7 +37,7 @@ static sensor_info2_t sensor_info = { type: GEOMAGNETIC_RV_SENSOR, uri: NAME_SENSOR, vendor: NAME_VENDOR, - min_range: 0, + min_range: -1, max_range: 1, resolution: 1, min_interval: 10, diff --git a/src/fusion-sensor/rotation_vector/rv_sensor.cpp b/src/fusion-sensor/rotation_vector/rv_sensor.cpp index 20b0226..05d8ee4 100644 --- a/src/fusion-sensor/rotation_vector/rv_sensor.cpp +++ b/src/fusion-sensor/rotation_vector/rv_sensor.cpp @@ -40,7 +40,7 @@ static sensor_info2_t sensor_info = { type: ROTATION_VECTOR_SENSOR, uri: NAME_SENSOR, vendor: NAME_VENDOR, - min_range: 0, + min_range: -1, max_range: 1, resolution: 1, min_interval: 10, -- 2.7.4 From 2e2aa7872f5455f4a4d707131f87c3150dda220f Mon Sep 17 00:00:00 2001 From: Yunmi Ha Date: Wed, 1 Sep 2021 13:38:24 +0900 Subject: [PATCH 12/16] Add orientation sensor type - GYROSCOPE_ORIENTATION_SENSOR : based on gyroscope_rotation_vector - GEOMAGNETIC_ORIENTATION_SENSOR : based on geomagnetic_rotration_vector Change-Id: I7440c5c153c30221dddab8b5eaacb55bf7d6adb2 Signed-off-by: Yunmi Ha --- include/sensor_types.h | 29 ++++++++++++++++------------- src/shared/sensor_utils.cpp | 2 ++ 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/include/sensor_types.h b/include/sensor_types.h index 207dd98..6a4756a 100644 --- a/include/sensor_types.h +++ b/include/sensor_types.h @@ -92,19 +92,22 @@ typedef enum sensor_type_t { GYROSCOPE_RV_SENSOR, GEOMAGNETIC_RV_SENSOR, - SIGNIFICANT_MOTION_SENSOR = 0x100, + GYROSCOPE_ORIENTATION_SENSOR = 100, + GEOMAGNETIC_ORIENTATION_SENSOR = 105, - HRM_BATCH_SENSOR = 0x200, + SIGNIFICANT_MOTION_SENSOR = 0x100, //256 + + HRM_BATCH_SENSOR = 0x200, //512 HRM_LED_GREEN_BATCH_SENSOR, - HUMAN_PEDOMETER_SENSOR = 0x300, + HUMAN_PEDOMETER_SENSOR = 0x300, //768 HUMAN_SLEEP_MONITOR_SENSOR, HUMAN_SLEEP_DETECTOR_SENSOR, SLEEP_DETECTOR_SENSOR = HUMAN_SLEEP_DETECTOR_SENSOR, HUMAN_STRESS_MONITOR_SENSOR, STRESS_MONITOR_SENSOR = HUMAN_STRESS_MONITOR_SENSOR, - EXERCISE_WALKING_SENSOR = 0x400, + EXERCISE_WALKING_SENSOR = 0x400, //1024 EXERCISE_RUNNING_SENSOR, EXERCISE_HIKING_SENSOR, EXERCISE_CYCLING_SENSOR, @@ -114,34 +117,34 @@ typedef enum sensor_type_t { EXERCISE_STEPPER_SENSOR, DATA_JOURNAL_SENSOR = 0x500, - // 0x500~0x600 Reserved + // 0x500~0x600 Reserved (1280 ~ 1536) - EXTERNAL_EXERCISE_SENSOR = 0x800, + EXTERNAL_EXERCISE_SENSOR = 0x800, //2048 EXERCISE_SENSOR = EXTERNAL_EXERCISE_SENSOR, - FUSION_SENSOR = 0x900, + FUSION_SENSOR = 0x900, //2304 AUTO_ROTATION_SENSOR, AUTO_BRIGHTNESS_SENSOR, MYOTEST_SENSOR, - GESTURE_MOVEMENT_SENSOR = 0x1200, + GESTURE_MOVEMENT_SENSOR = 0x1200, //4608 GESTURE_WRIST_UP_SENSOR, GESTURE_WRIST_DOWN_SENSOR, GESTURE_MOVEMENT_STATE_SENSOR, GESTURE_PICK_UP_SENSOR, GESTURE_FACE_DOWN_SENSOR, - ACTIVITY_TRACKER_SENSOR = 0x1A00, + ACTIVITY_TRACKER_SENSOR = 0x1A00, //6656 ACTIVITY_LEVEL_MONITOR_SENSOR, GPS_BATCH_SENSOR, PPG_BATCH_SENSOR, GPS_TIMESYNC_SENSOR, - HRM_CTRL_SENSOR = 0x1A80, + HRM_CTRL_SENSOR = 0x1A80, //6784 REG_CTRL_SENSOR, GPS_CTRL_SENSOR, - WEAR_STATUS_SENSOR = 0x2000, + WEAR_STATUS_SENSOR = 0x2000, //8192 WEAR_ON_MONITOR_SENSOR, NO_MOVE_DETECTOR_SENSOR, RESTING_HR_SENSOR, @@ -167,7 +170,7 @@ typedef enum sensor_type_t { ECG_SENSOR, FALL_DETECTION_SENSOR, - CONTEXT_SENSOR = 0x7000, + CONTEXT_SENSOR = 0x7000, //28,672 MOTION_SENSOR, PIR_SENSOR, PIR_LONG_SENSOR, @@ -183,7 +186,7 @@ typedef enum sensor_type_t { SIMSENSE_SENSOR, PPG_SENSOR, - CUSTOM_SENSOR = 0X9000, + CUSTOM_SENSOR = 0X9000, //36,864 } sensor_type_t; typedef struct sensor_info2_t { diff --git a/src/shared/sensor_utils.cpp b/src/shared/sensor_utils.cpp index 6636346..000f2ed 100644 --- a/src/shared/sensor_utils.cpp +++ b/src/shared/sensor_utils.cpp @@ -61,6 +61,8 @@ static std::map types = { {GEOMAGNETIC_UNCAL_SENSOR, "http://tizen.org/sensor/general/geomagnetic.uncalibrated"}, {GYROSCOPE_RV_SENSOR, "http://tizen.org/sensor/general/gyroscope_rotation_vector"}, {GEOMAGNETIC_RV_SENSOR, "http://tizen.org/sensor/general/geomagnetic_rotation_vector"}, + {GYROSCOPE_ORIENTATION_SENSOR, "http://tizen.org/sensor/general/gyroscope_orientation"}, + {GEOMAGNETIC_ORIENTATION_SENSOR, "http://tizen.org/sensor/general/geomagnetic_orientation"}, {SIGNIFICANT_MOTION_SENSOR, "http://tizen.org/sensor/general/significant_motion"}, -- 2.7.4 From 1d2983bbf54799785b650f9a8022b88429a23d26 Mon Sep 17 00:00:00 2001 From: INSUN PYO Date: Tue, 7 Sep 2021 16:49:46 +0900 Subject: [PATCH 13/16] Fix fd leak Change-Id: I4d74acb337b0fd4aaac6fb911055ac702dd055b3 --- src/shared/event_loop.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/shared/event_loop.cpp b/src/shared/event_loop.cpp index e0e9799..a60ba38 100644 --- a/src/shared/event_loop.cpp +++ b/src/shared/event_loop.cpp @@ -102,6 +102,9 @@ event_loop::event_loop(GMainLoop *mainloop) event_loop::~event_loop() { + if (m_term_fd != -1) + close(m_term_fd); + _D("Destoryed"); } @@ -268,7 +271,7 @@ bool event_loop::run(int timeout) g_source_unref(src); } - m_term_fd = eventfd(0, 0); + m_term_fd = eventfd(0, EFD_CLOEXEC); retv_if(m_term_fd == -1, false); terminator *handler = new(std::nothrow) terminator(this); -- 2.7.4 From a402f11acc33b2c820dd8bf5c58a8b7a0f33275e Mon Sep 17 00:00:00 2001 From: "taemin.yeom" Date: Thu, 2 Sep 2021 17:46:49 +0900 Subject: [PATCH 14/16] Upgrade calculating algorithm of rotation vector and orientation Change-Id: Ic564b55f3951ba7e44d6c780a5f683a4e0fafed8 Signed-off-by: taemin.yeom --- src/fusion-sensor/fusion_util.cpp | 69 +- src/fusion-sensor/gravity/gravity_comp_sensor.cpp | 1 - .../gravity/gravity_lowpass_sensor.cpp | 1 - .../linear_accel/linear_accel_sensor.cpp | 1 - src/fusion-sensor/pedometer/pedometer_sensor.cpp | 1 - src/fusion-sensor/rotation_vector/fusion_base.cpp | 63 +- src/fusion-sensor/rotation_vector/fusion_base.h | 12 +- .../rotation_vector/fusion_utils/errors.h | 81 +++ .../rotation_vector/fusion_utils/euler_angles.cpp | 126 ---- .../rotation_vector/fusion_utils/euler_angles.h | 50 -- .../rotation_vector/fusion_utils/mat.h | 395 ++++++++++ .../rotation_vector/fusion_utils/matrix.cpp | 186 ----- .../rotation_vector/fusion_utils/matrix.h | 58 -- .../fusion_utils/orientation_filter.cpp | 807 +++++++++++++-------- .../fusion_utils/orientation_filter.h | 145 ++-- .../rotation_vector/fusion_utils/quat.h | 100 +++ .../rotation_vector/fusion_utils/quaternion.cpp | 160 ---- .../rotation_vector/fusion_utils/quaternion.h | 56 -- .../fusion_utils/rotation_matrix.cpp | 120 --- .../rotation_vector/fusion_utils/rotation_matrix.h | 48 -- .../rotation_vector/fusion_utils/sensor_data.cpp | 127 ---- .../rotation_vector/fusion_utils/sensor_data.h | 58 -- .../rotation_vector/fusion_utils/traits.h | 120 +++ .../rotation_vector/fusion_utils/vec.h | 440 +++++++++++ .../rotation_vector/fusion_utils/vector.cpp | 249 ------- .../rotation_vector/fusion_utils/vector.h | 64 -- src/fusion-sensor/rotation_vector/gyro_fusion.cpp | 14 +- src/fusion-sensor/rotation_vector/gyro_fusion.h | 2 - .../rotation_vector/gyro_magnetic_fusion.cpp | 15 +- .../rotation_vector/gyro_magnetic_fusion.h | 2 - .../rotation_vector/gyro_rv_sensor.cpp | 9 +- .../rotation_vector/magnetic_fusion.cpp | 12 +- .../rotation_vector/magnetic_fusion.h | 2 - .../rotation_vector/magnetic_rv_sensor.cpp | 1 - src/fusion-sensor/rotation_vector/rv_sensor.cpp | 1 - 35 files changed, 1772 insertions(+), 1824 deletions(-) create mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/errors.h delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/euler_angles.cpp delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/euler_angles.h create mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/mat.h delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/matrix.cpp delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/matrix.h create mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/quat.h delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/quaternion.cpp delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/quaternion.h delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/rotation_matrix.cpp delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/rotation_matrix.h delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/sensor_data.cpp delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/sensor_data.h create mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/traits.h create mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/vec.h delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/vector.cpp delete mode 100644 src/fusion-sensor/rotation_vector/fusion_utils/vector.h diff --git a/src/fusion-sensor/fusion_util.cpp b/src/fusion-sensor/fusion_util.cpp index 7314fb9..256376c 100644 --- a/src/fusion-sensor/fusion_util.cpp +++ b/src/fusion-sensor/fusion_util.cpp @@ -132,7 +132,6 @@ int calculate_rotation_matrix(float *accel, float *geo, float *R, float *I) int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float &roll) { int error; - float g[3]; float R[9]; error = quat_to_matrix(quat, R); @@ -140,69 +139,11 @@ int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float & if (error < 0) return error; - float xyz_z = ARCTAN(R[3], R[0]); - float yxz_x = asinf(R[7]); - float yxz_y = ARCTAN(-R[6], R[8]); - float yxz_z = ARCTAN(-R[1], R[4]); - - float a = fabs(yxz_x / HALF); - a = a * a; - - float p = (fabs(yxz_y) / HALF - 1.0); - - if (p < 0) - p = 0; - - float v = 1 + (1 - a) / a * p; - - if (v > 20) - v = 20; - - if (yxz_x * yxz_y > 0) { - if (yxz_z > 0 && xyz_z < 0) - xyz_z += M_PI * 2; - } else { - if (yxz_z < 0 && xyz_z > 0) - xyz_z -= M_PI * 2; - } - - g[0] = (1 - a * v) * yxz_z + (a * v) * xyz_z; - g[0] *= -1; - - float tmp = R[7]; - - if (tmp > 1.0f) - tmp = 1.0f; - else if (tmp < -1.0f) - tmp = -1.0f; - - g[1] = -asinf(tmp); - if (R[8] < 0) - g[1] = M_PI - g[1]; - - if (g[1] > M_PI) - g[1] -= M_PI * 2; - - if ((fabs(R[7]) > QUAT)) - g[2] = (float) atan2f(R[6], R[7]); - else - g[2] = (float) atan2f(R[6], R[8]); - - if (g[2] > HALF) - g[2] = M_PI - g[2]; - else if (g[2] < -HALF) - g[2] = -M_PI - g[2]; - - g[0] *= RAD2DEGREE; - g[1] *= RAD2DEGREE; - g[2] *= RAD2DEGREE; - - if (g[0] < 0) - g[0] += 360; - - azimuth = g[0]; - pitch = g[1]; - roll = g[2]; + azimuth = atan2f(-R[3], R[0]) * RAD2DEGREE; + pitch = atan2f(-R[7], R[8]) * RAD2DEGREE; + roll = asinf (R[6]) * RAD2DEGREE; + if (azimuth < 0) + azimuth += 360; return 0; } diff --git a/src/fusion-sensor/gravity/gravity_comp_sensor.cpp b/src/fusion-sensor/gravity/gravity_comp_sensor.cpp index 1fbf747..43ba1dd 100644 --- a/src/fusion-sensor/gravity/gravity_comp_sensor.cpp +++ b/src/fusion-sensor/gravity/gravity_comp_sensor.cpp @@ -21,7 +21,6 @@ #include #include -#include #include #define NAME_SENSOR "http://tizen.org/sensor/general/gravity/tizen_complementary" diff --git a/src/fusion-sensor/gravity/gravity_lowpass_sensor.cpp b/src/fusion-sensor/gravity/gravity_lowpass_sensor.cpp index eb3a48f..93cc2ed 100644 --- a/src/fusion-sensor/gravity/gravity_lowpass_sensor.cpp +++ b/src/fusion-sensor/gravity/gravity_lowpass_sensor.cpp @@ -21,7 +21,6 @@ #include #include -#include #include #define NAME_SENSOR "http://tizen.org/sensor/general/gravity/tizen_lowpass" diff --git a/src/fusion-sensor/linear_accel/linear_accel_sensor.cpp b/src/fusion-sensor/linear_accel/linear_accel_sensor.cpp index 99b7b1e..fc1ad8e 100644 --- a/src/fusion-sensor/linear_accel/linear_accel_sensor.cpp +++ b/src/fusion-sensor/linear_accel/linear_accel_sensor.cpp @@ -21,7 +21,6 @@ #include #include -#include #define NAME_SENSOR "http://tizen.org/sensor/general/linear_acceleration/tizen_default" #define NAME_VENDOR "tizen.org" diff --git a/src/fusion-sensor/pedometer/pedometer_sensor.cpp b/src/fusion-sensor/pedometer/pedometer_sensor.cpp index 5f46dff..abf2cb5 100644 --- a/src/fusion-sensor/pedometer/pedometer_sensor.cpp +++ b/src/fusion-sensor/pedometer/pedometer_sensor.cpp @@ -19,7 +19,6 @@ #include #include -#include #include #define NAME_SENSOR "http://samsung.com/sensor/healthinfo/pedometer/samsung_pedometer" diff --git a/src/fusion-sensor/rotation_vector/fusion_base.cpp b/src/fusion-sensor/rotation_vector/fusion_base.cpp index fbd887d..dab8fe5 100644 --- a/src/fusion-sensor/rotation_vector/fusion_base.cpp +++ b/src/fusion-sensor/rotation_vector/fusion_base.cpp @@ -25,9 +25,8 @@ #include #include "fusion_base.h" -#define ACCEL_COMPENSATION -1 -#define GYRO_COMPENSATION 1 -#define MAG_COMPENSATION -1 +const float RAD2DEG = 57.29577951; +const float US2S = 1000000.0f; fusion_base::fusion_base() : m_enable_accel(false) @@ -38,6 +37,9 @@ fusion_base::fusion_base() , m_z(0) , m_w(0) , m_timestamp(0) +, m_timestamp_accel(0) +, m_timestamp_gyro(0) +, m_timestamp_mag(0) { } @@ -55,31 +57,45 @@ void fusion_base::clear(void) void fusion_base::push_accel(sensor_data_t &data) { //_I("[fusion_sensor] : Pushing accel"); - pre_process_data(m_accel, data.values, ACCEL_COMPENSATION, ACCEL_SCALE); - m_accel.m_time_stamp = data.timestamp; + android::vec3_t v(data.values); + + float dT = (data.timestamp - m_timestamp_accel) / US2S; + m_timestamp_accel = data.timestamp; + m_timestamp = data.timestamp; + m_enable_accel = true; - if (get_orientation()) - store_orientation(); + m_orientation_filter.handleAcc(v, dT); + store_orientation(); } void fusion_base::push_gyro(sensor_data_t &data) { - //_I("[fusion_sensor] : Pushing mag"); - pre_process_data(m_gyro, data.values, GYRO_COMPENSATION, GYRO_SCALE); - m_gyro.m_time_stamp = data.timestamp; + //_I("[fusion_sensor] : Pushing gyro"); + android::vec3_t v(data.values); + v[0] /= RAD2DEG; + v[1] /= RAD2DEG; + v[2] /= RAD2DEG; + + float dT = (data.timestamp - m_timestamp_gyro) / US2S; + m_timestamp_gyro = data.timestamp; + m_timestamp = data.timestamp; + m_enable_gyro = true; - if (get_orientation()) - store_orientation(); + m_orientation_filter.handleGyro(v, dT); + store_orientation(); } void fusion_base::push_mag(sensor_data_t &data) { - //_I("[fusion_sensor] : Pushing gyro"); - pre_process_data(m_magnetic, data.values, MAG_COMPENSATION, GEOMAGNETIC_SCALE); - m_magnetic.m_time_stamp = data.timestamp; + //_I("[fusion_sensor] : Pushing mag"); + android::vec3_t v(data.values); + + m_timestamp_mag = data.timestamp; + m_timestamp = data.timestamp; + m_enable_magnetic = true; - if (get_orientation()) - store_orientation(); + m_orientation_filter.handleMag(v); + store_orientation(); } bool fusion_base::get_rv(unsigned long long ×tamp, float &x, float &y, float &z, float &w) @@ -96,14 +112,15 @@ bool fusion_base::get_rv(unsigned long long ×tamp, float &x, float &y, floa void fusion_base::store_orientation(void) { - m_x = m_orientation_filter.m_quaternion.m_quat.m_vec[0]; - m_y = m_orientation_filter.m_quaternion.m_quat.m_vec[1]; - m_z = m_orientation_filter.m_quaternion.m_quat.m_vec[2]; - m_w = m_orientation_filter.m_quaternion.m_quat.m_vec[3]; + android::quat_t q = m_orientation_filter.getAttitude(); + m_x = q[0]; + m_y = q[1]; + m_z = q[2]; + m_w = q[3]; if (std::isnan(m_x) || std::isnan(m_y) || std::isnan(m_z) || std::isnan(m_w)) { - m_timestamp = 0; - m_orientation_filter = orientation_filter(); + m_timestamp = m_timestamp_accel = m_timestamp_gyro = m_timestamp_mag = 0; + m_orientation_filter = android::orientation_filter(); } clear(); } diff --git a/src/fusion-sensor/rotation_vector/fusion_base.h b/src/fusion-sensor/rotation_vector/fusion_base.h index 96a3301..8dbe8b4 100644 --- a/src/fusion-sensor/rotation_vector/fusion_base.h +++ b/src/fusion-sensor/rotation_vector/fusion_base.h @@ -33,12 +33,7 @@ public: virtual bool get_rv(unsigned long long ×tamp, float &w, float &x, float &y, float &z); protected: - - sensor_data m_accel; - sensor_data m_gyro; - sensor_data m_magnetic; - - orientation_filter m_orientation_filter; + android::orientation_filter m_orientation_filter; bool m_enable_accel; bool m_enable_gyro; @@ -48,11 +43,14 @@ protected: float m_y; float m_z; float m_w; + float m_timestamp; + float m_timestamp_accel; + float m_timestamp_gyro; + float m_timestamp_mag; void clear(); void store_orientation(void); - virtual bool get_orientation(void) = 0; }; diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/errors.h b/src/fusion-sensor/rotation_vector/fusion_utils/errors.h new file mode 100644 index 0000000..95e8092 --- /dev/null +++ b/src/fusion-sensor/rotation_vector/fusion_utils/errors.h @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2007 The Android Open Source Project + * + * 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. + */ + +// released in android-11.0.0_r9 + +#ifndef ANDROID_ERRORS_H +#define ANDROID_ERRORS_H +#include +#include +namespace android { +// use this type to return error codes +#ifdef HAVE_MS_C_RUNTIME +typedef int status_t; +#else +typedef int32_t status_t; +#endif +/* the MS C runtime lacks a few error codes */ +/* + * Error codes. + * All error codes are negative values. + */ +// Win32 #defines NO_ERROR as well. It has the same value, so there's no +// real conflict, though it's a bit awkward. +#ifdef _WIN32 +# undef NO_ERROR +#endif + +enum { + OK = 0, // Everything's swell. + NO_ERROR = 0, // No errors. + + UNKNOWN_ERROR = 0x80000000, + NO_MEMORY = -ENOMEM, + INVALID_OPERATION = -ENOSYS, + BAD_VALUE = -EINVAL, + BAD_TYPE = 0x80000001, + NAME_NOT_FOUND = -ENOENT, + PERMISSION_DENIED = -EPERM, + NO_INIT = -ENODEV, + ALREADY_EXISTS = -EEXIST, + DEAD_OBJECT = -EPIPE, + FAILED_TRANSACTION = 0x80000002, + JPARKS_BROKE_IT = -EPIPE, +#if !defined(HAVE_MS_C_RUNTIME) + BAD_INDEX = -EOVERFLOW, + NOT_ENOUGH_DATA = -ENODATA, + WOULD_BLOCK = -EWOULDBLOCK, + TIMED_OUT = -ETIMEDOUT, + UNKNOWN_TRANSACTION = -EBADMSG, +#else + BAD_INDEX = -E2BIG, + NOT_ENOUGH_DATA = 0x80000003, + WOULD_BLOCK = 0x80000004, + TIMED_OUT = 0x80000005, + UNKNOWN_TRANSACTION = 0x80000006, +#endif + FDS_NOT_ALLOWED = 0x80000007, +}; +// Restore define; enumeration is in "android" namespace, so the value defined +// there won't work for Win32 code in a different namespace. +#ifdef _WIN32 +# define NO_ERROR 0L +#endif +}; // namespace android + +// --------------------------------------------------------------------------- + +#endif // ANDROID_ERRORS_H \ No newline at end of file diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/euler_angles.cpp b/src/fusion-sensor/rotation_vector/fusion_utils/euler_angles.cpp deleted file mode 100644 index e028c65..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/euler_angles.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ -#if defined (_EULER_ANGLES_H_) && defined (_VECTOR_H_) - -#include - -#define RAD2DEG 57.2957795 -#define DEG2RAD 0.0174532925 - -template -euler_angles::euler_angles() : m_ang() -{ -} - -template -euler_angles::euler_angles(const TYPE roll, const TYPE pitch, const TYPE azimuth) -{ - TYPE euler_data[EULER_SIZE] = {roll, pitch, azimuth}; - - vect v(euler_data); - m_ang = v; -} - -template -euler_angles::euler_angles(const vect v) -{ - m_ang = v; -} - -template -euler_angles::euler_angles(const euler_angles& e) -{ - m_ang = e.m_ang; -} - -template -euler_angles::~euler_angles() -{ -} - -template -euler_angles euler_angles::operator =(const euler_angles& e) -{ - m_ang = e.m_ang; - - return *this; -} - -template -euler_angles quat2euler(const quaternion q) -{ - T w, x, y, z; - T R11, R21, R31, R32, R33; - T phi, theta, psi; - - w = q.m_quat.m_vec[0]; - x = q.m_quat.m_vec[1]; - y = q.m_quat.m_vec[2]; - z = q.m_quat.m_vec[3]; - - R11 = 2 * (w * w) - 1 + 2 * (x * x); - R21 = 2 * ((x * y) - (w * z)); - R31 = 2 * ((x * z) + (w * y)); - R32 = 2 * ((y * z) - (w * x)); - R33 = 2 * (w * w) - 1 + 2 * (z * z); - - phi = atan2(R32, R33); - theta = atan2(-R31 , sqrt((R32 * R32) + (R33 * R33))); - psi = atan2(R21, R11); - - euler_angles e(phi, theta, psi); - - return e; -} - -template -euler_angles rad2deg(const euler_angles e) -{ - return (e.m_ang * (T) RAD2DEG); -} - -template -euler_angles deg2rad(const euler_angles e) -{ - return (e.m_ang * (T) DEG2RAD); -} - -template -quaternion euler2quat(euler_angles euler) { - T theta = euler.m_ang.m_vec[0]; - T phi = euler.m_ang.m_vec[1]; - T psi = euler.m_ang.m_vec[2]; - - T R[ROT_MAT_ROWS][ROT_MAT_COLS]; - R[0][0] = cos(psi)*cos(theta); - R[0][1] = -sin(psi)*cos(phi) + cos(psi)*sin(theta)*sin(phi); - R[0][2] = sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi); - R[1][0] = sin(psi)*cos(theta); - R[1][1] = cos(psi)*cos(phi) + sin(psi)*sin(theta)*sin(phi); - R[1][2] = -cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi); - R[2][0] = -sin(theta); - R[2][1] = cos(theta)*sin(phi); - R[2][2] = cos(theta)*cos(phi); - - rotation_matrix rot_mat(R); - quaternion q = rot_mat2quat(rot_mat); - return q; -} - -#endif //_EULER_ANGLES_H_ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/euler_angles.h b/src/fusion-sensor/rotation_vector/fusion_utils/euler_angles.h deleted file mode 100644 index 9e2f607..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/euler_angles.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef _EULER_ANGLES_H_ -#define _EULER_ANGLES_H_ - -#include "vector.h" -#include "quaternion.h" -#include "rotation_matrix.h" - -#define EULER_SIZE 3 - -template -class euler_angles { -public: - vect m_ang; - - euler_angles(); - euler_angles(const TYPE roll, const TYPE pitch, const TYPE azimuth); - euler_angles(const vect v); - euler_angles(const euler_angles& e); - ~euler_angles(); - - euler_angles operator =(const euler_angles& e); - - template friend euler_angles quat2euler(const quaternion q); - template friend quaternion euler2quat(euler_angles euler); - template friend euler_angles rad2deg(const euler_angles e); - template friend euler_angles deg2rad(const euler_angles e); -}; - -#include "euler_angles.cpp" - -#endif //_EULER_ANGLES_H_ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/mat.h b/src/fusion-sensor/rotation_vector/fusion_utils/mat.h new file mode 100644 index 0000000..24647ad --- /dev/null +++ b/src/fusion-sensor/rotation_vector/fusion_utils/mat.h @@ -0,0 +1,395 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * 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. + */ + +// released in android-11.0.0_r9 + +#ifndef ANDROID_MAT_H +#define ANDROID_MAT_H + +#include "vec.h" +#include "traits.h" + +// ----------------------------------------------------------------------- + +namespace android { + +template +class mat; + +namespace helpers { + +template +mat& doAssign( + mat& lhs, + typename TypeTraits::ParameterType rhs) { + for (size_t i=0 ; i +mat PURE doMul( + const mat& lhs, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +vec PURE doMul( + const mat& lhs, + const vec& rhs) +{ + vec res; + for (size_t r=0 ; r +mat PURE doMul( + const vec& lhs, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +mat PURE doMul( + const mat& rhs, + typename TypeTraits::ParameterType v) +{ + mat res; + for (size_t c=0 ; c +mat PURE doMul( + typename TypeTraits::ParameterType v, + const mat& rhs) +{ + mat res; + for (size_t c=0 ; c +class mat : public vec< vec, C > { + typedef typename TypeTraits::ParameterType pTYPE; + typedef vec< vec, C > base; +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + size_type size() const { return R*C; } + enum { ROWS = R, COLS = C }; + + + // ----------------------------------------------------------------------- + // default constructors + + mat() { } + mat(const mat& rhs) : base(rhs) { } + mat(const base& rhs) : base(rhs) { } // NOLINT(google-explicit-constructor) + + // ----------------------------------------------------------------------- + // conversion constructors + + // sets the diagonal to the value, off-diagonal to zero + mat(pTYPE rhs) { // NOLINT(google-explicit-constructor) + helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // Assignment + + mat& operator=(const mat& rhs) { + base::operator=(rhs); + return *this; + } + + mat& operator=(const base& rhs) { + base::operator=(rhs); + return *this; + } + + mat& operator=(pTYPE rhs) { + return helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // non-member function declaration and definition + + friend inline mat PURE operator + (const mat& lhs, const mat& rhs) { + return helpers::doAdd( + static_cast(lhs), + static_cast(rhs)); + } + friend inline mat PURE operator - (const mat& lhs, const mat& rhs) { + return helpers::doSub( + static_cast(lhs), + static_cast(rhs)); + } + + // matrix*matrix + template + friend mat PURE operator * ( + const mat& lhs, + const mat& rhs) { + return helpers::doMul(lhs, rhs); + } + + // matrix*vector + friend vec PURE operator * ( + const mat& lhs, const vec& rhs) { + return helpers::doMul(lhs, rhs); + } + + // vector*matrix + friend mat PURE operator * ( + const vec& lhs, const mat& rhs) { + return helpers::doMul(lhs, rhs); + } + + // matrix*scalar + friend inline mat PURE operator * (const mat& lhs, pTYPE v) { + return helpers::doMul(lhs, v); + } + + // scalar*matrix + friend inline mat PURE operator * (pTYPE v, const mat& rhs) { + return helpers::doMul(v, rhs); + } + + // ----------------------------------------------------------------------- + // streaming operator to set the columns of the matrix: + // example: + // mat33_t m; + // m << v0 << v1 << v2; + + // column_builder<> stores the matrix and knows which column to set + template + struct column_builder { + mat& matrix; + explicit column_builder(mat& matrix) : matrix(matrix) { } + }; + + // operator << is not a method of column_builder<> so we can + // overload it for unauthorized values (partial specialization + // not allowed in class-scope). + // we just set the column and return the next column_builder<> + template + friend column_builder operator << ( + const column_builder& lhs, + const vec& rhs) { + lhs.matrix[PREV_COLUMN+1] = rhs; + return column_builder(lhs.matrix); + } + + // we return void here so we get a compile-time error if the + // user tries to set too many columns + friend void operator << ( + const column_builder& lhs, + const vec& rhs) { + lhs.matrix[C-1] = rhs; + } + + // this is where the process starts. we set the first columns and + // return the next column_builder<> + column_builder<0> operator << (const vec& rhs) { + (*this)[0] = rhs; + return column_builder<0>(*this); + } +}; + +// Specialize column matrix so they're exactly equivalent to a vector +template +class mat : public vec { + typedef vec base; +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + size_type size() const { return R; } + enum { ROWS = R, COLS = 1 }; + + mat() { } + explicit mat(const base& rhs) : base(rhs) { } + mat(const mat& rhs) : base(rhs) { } + explicit mat(const TYPE& rhs) { helpers::doAssign(*this, rhs); } + mat& operator=(const mat& rhs) { base::operator=(rhs); return *this; } + mat& operator=(const base& rhs) { base::operator=(rhs); return *this; } + mat& operator=(const TYPE& rhs) { return helpers::doAssign(*this, rhs); } + // we only have one column, so ignore the index + const base& operator[](size_t) const { return *this; } + base& operator[](size_t) { return *this; } + void operator << (const vec& rhs) { base::operator[](0) = rhs; } +}; + +// ----------------------------------------------------------------------- +// matrix functions + +// transpose. this handles matrices of matrices +inline int PURE transpose(int v) { return v; } +inline float PURE transpose(float v) { return v; } +inline double PURE transpose(double v) { return v; } + +// Transpose a matrix +template +mat PURE transpose(const mat& m) { + mat r; + for (size_t i=0 ; i static TYPE trace(const mat& m) { + TYPE t; + for (size_t i=0 ; i +static bool isPositiveSemidefinite(const mat& m, TYPE tolerance) { + for (size_t i=0 ; i tolerance) + return false; + + return true; +} + +// Transpose a vector +template < + template class VEC, + typename TYPE, + size_t SIZE +> +mat PURE transpose(const VEC& v) { + mat r; + for (size_t i=0 ; i +mat PURE invert(const mat& src) { + T t; + size_t swap; + mat tmp(src); + mat inverse(1); + + for (size_t i=0 ; i fabs(tmp[i][i])) { + swap = j; + } + } + + if (swap != i) { + /* swap rows. */ + for (size_t k=0 ; k mat22_t; +typedef mat mat33_t; +typedef mat mat44_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_MAT_H */ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/matrix.cpp b/src/fusion-sensor/rotation_vector/fusion_utils/matrix.cpp deleted file mode 100644 index ad584da..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/matrix.cpp +++ /dev/null @@ -1,186 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifdef _MATRIX_H_ - -TYPE_ROW_COL matrix::matrix(void) -{ - for (int i = 0; i < ROW; i++) - for (int j = 0; j < COL; j++) - m_mat[i][j] = 0; -} - -TYPE_ROW_COL matrix::matrix(const matrix &m) -{ - for (int p = 0; p < ROW; p++) - for (int q = 0; q < COL; q++) - m_mat[p][q] = m.m_mat[p][q]; -} - -TYPE_ROW_COL matrix::matrix(TYPE mat_data[ROW][COL]) -{ - for (int i = 0; i < ROW; i++) - for (int j = 0; j < COL; j++) - m_mat[i][j] = mat_data[i][j]; -} - -TYPE_ROW_COL matrix::~matrix() -{ -} - -TYPE_ROW_COL matrix matrix::operator =(const matrix &m) -{ - if (this == &m) - { - return *this; - } - - for (int i = 0; i < ROW; i++) - for (int j = 0; j < COL; j++) - m_mat[i][j] = m.m_mat[i][j]; - - return *this; -} - -T_R_C ostream& operator <<(ostream& dout, matrix &m) -{ - for (int i = 0; i < R; i++) - { - for (int j = 0; j < C; j++) - { - dout << m.m_mat[i][j] << "\t"; - } - dout << endl; - } - return dout; -} - -T_R_C matrix operator +(const matrix &m1, const matrix &m2) -{ - matrix m3; - - for (int i = 0; i < R; i++) - for (int j = 0; j < C; j++) - m3.m_mat[i][j] = m1.m_mat[i][j] + m2.m_mat[i][j]; - - return m3; -} - -T_R_C matrix operator +(const matrix &m, const T &val) -{ - matrix m1; - - for (int i = 0; i < R; i++) - for (int j = 0; j < C; j++) - m1.m_mat[i][j] = m.m_mat[i][j] + val; - - return m1; -} - -T_R_C matrix operator -(const matrix &m1, const matrix &m2) -{ - matrix m3; - - for (int i = 0; i < R; i++) - for (int j = 0; j < C; j++) - m3.m_mat[i][j] = m1.m_mat[i][j] - m2.m_mat[i][j]; - - return m3; -} - -T_R_C matrix operator -(const matrix &m, const T &val) -{ - matrix m1; - - for (int i = 0; i < R; i++) - for (int j = 0; j < C; j++) - m1.m_mat[i][j] = m.m_mat[i][j] - val; - - return m1; -} - -T_R_C_C2 matrix operator *(const matrix &m1, const matrix &m2) -{ - matrix m3; - - for (int i = 0; i < R; i++) - { - for (int j = 0; j < C2; j++) - { - m3.m_mat[i][j] = 0; - for (int k = 0; k < C; k++) - m3.m_mat[i][j] += m1.m_mat[i][k] * m2.m_mat[k][j]; - } - } - - return m3; -} - -T_R_C matrix operator *(const matrix &m, const T &val) -{ - matrix m1; - - for (int i = 0; i < R; i++) - for (int j = 0; j < C; j++) - m1.m_mat[i][j] = m.m_mat[i][j] * val; - - return m1; -} - -T_R_C matrix operator /(const matrix &m1, const T &val) -{ - matrix m3; - - for (int i = 0; i < R; i++) - for (int j = 0; j < C; j++) - m3.m_mat[i][j] = m1.m_mat[i][j] / val; - - return m3; -} - -T_R1_C1_R2_C2 bool operator ==(const matrix &m1, const matrix &m2) -{ - if ((R1 == R2) && (C1 == C2)) { - for (int i = 0; i < R1; i++) - for (int j = 0; j < C2; j++) - if (m1.m_mat[i][j] != m2.m_mat[i][j]) - return false; - return true; - } - - return false; -} - -T_R1_C1_R2_C2 bool operator !=(const matrix &m1, const matrix &m2) -{ - return (!(m1 == m2)); -} - -T_R_C matrix tran(const matrix &m) -{ - matrix m1; - - for (int i = 0; i < R; i++) - for (int j = 0; j < C; j++) - m1.m_mat[j][i] = m.m_mat[i][j]; - - return m1; -} - -#endif //_MATRIX_H_ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/matrix.h b/src/fusion-sensor/rotation_vector/fusion_utils/matrix.h deleted file mode 100644 index bed7754..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/matrix.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef _MATRIX_H_ -#define _MATRIX_H_ - -#include -#include -using namespace std; - -#define TYPE_ROW_COL template -#define T_R_C template -#define T_R_C_C2 template -#define T_R1_C1_R2_C2 template - -TYPE_ROW_COL class matrix { -public: - TYPE m_mat[ROW][COL]; - - matrix(void); - matrix(TYPE mat_data[ROW][COL]); - matrix(const matrix& m); - ~matrix(); - - matrix operator =(const matrix &m); - - T_R_C friend ostream& operator << (ostream& dout, matrix &m); - T_R_C friend matrix operator +(const matrix &m1, const matrix &m2); - T_R_C friend matrix operator +(const matrix &m, const T &val); - T_R_C friend matrix operator -(const matrix &m1, const matrix &m2); - T_R_C friend matrix operator -(const matrix &m, const T &val); - T_R_C_C2 friend matrix operator *(const matrix &m1, const matrix &m2); - T_R_C friend matrix operator *(const matrix &m, const T &val); - T_R_C friend matrix operator /(const matrix &m1, const T &val); - T_R1_C1_R2_C2 friend bool operator ==(const matrix &m1, const matrix &m2); - T_R1_C1_R2_C2 friend bool operator !=(const matrix &m1, const matrix &m2); - T_R_C friend matrix tran(const matrix &m); -}; - -#include "matrix.cpp" - -#endif //_MATRIX_H_ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp index 2a27fd6..5df2f08 100644 --- a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp +++ b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp @@ -1,367 +1,560 @@ /* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * Copyright (C) 2011 The Android Open Source Project * * 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 + * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. - * */ - -#ifdef _ORIENTATION_FILTER_H_ +// released in android-11.0.0_r9 #include "orientation_filter.h" -//Windowing is used for buffering of previous samples for statistical analysis -#define MOVING_AVERAGE_WINDOW_LENGTH 20 -//Earth's Gravity -#define GRAVITY 9.80665 -#define PI 3.141593 -//Needed for non-zero initialization for statistical analysis -#define NON_ZERO_VAL 0.1 -//microseconds to seconds -#define US2S (1.0 / 1000000.0) -//Initialize sampling interval to 100000microseconds -#define SAMPLE_INTV 100000 -#define ACCEL_THRESHOLD 0.2 -#define GYRO_THRESHOLD (0.01 * PI) - -// constants for computation of covariance and transition matrices -#define ZIGMA_W (0.05 * DEG2RAD) -#define TAU_W 1000 -#define QWB_CONST ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W) -#define F_CONST (-1.0 / TAU_W) -#define SQUARE(T) (T * T) - -#define NEGLIGIBLE_VAL 0.0000001 - -#define ABS(val) (((val) < 0) ? -(val) : (val)) - - -template -orientation_filter::orientation_filter() -{ - TYPE arr[MOVING_AVERAGE_WINDOW_LENGTH]; - - std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL); - - vect vec(arr); - - m_var_gyr_x = vec; - m_var_gyr_y = vec; - m_var_gyr_z = vec; - m_var_roll = vec; - m_var_pitch = vec; - m_var_azimuth = vec; - m_gyro_dt = TYPE(); -} - -template -orientation_filter::~orientation_filter() -{ -} - -template -inline void orientation_filter::initialize_sensor_data(const sensor_data *accel, - const sensor_data *gyro, const sensor_data *magnetic) -{ - m_accel.m_data = accel->m_data; - m_accel.m_time_stamp = accel->m_time_stamp; - normalize(m_accel); - - if (gyro != NULL) { - unsigned long long sample_interval_gyro = SAMPLE_INTV; +using namespace android; - if (m_gyro.m_time_stamp != 0 && gyro->m_time_stamp != 0) - sample_interval_gyro = gyro->m_time_stamp - m_gyro.m_time_stamp; +// ----------------------------------------------------------------------- - m_gyro_dt = sample_interval_gyro * US2S; - m_gyro.m_time_stamp = gyro->m_time_stamp; +/*==================== BEGIN FUSION SENSOR PARAMETER =========================*/ - m_gyro.m_data = gyro->m_data * (TYPE) PI; +/* Note: + * If a platform uses software fusion, it is necessary to tune the following + * parameters to fit the hardware sensors prior to release. + * + * The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode. + * The GEOMAG_ parameters will be used in FUSION_NOGYRO mode. + */ - m_gyro.m_data = m_gyro.m_data - m_bias_correction; - } +/* + * GYRO_VAR gives the measured variance of the gyro's output per + * Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro, + * which is independent of the sampling frequency. + * + * The variance of gyro's output at a given sampling period can be + * calculated as: + * variance(T) = GYRO_VAR / T + * + * The variance of the INTEGRATED OUTPUT at a given sampling period can be + * calculated as: + * variance_integrate_output(T) = GYRO_VAR * T + */ +static const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz +static const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed) +static const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz +static const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed) - if (magnetic != NULL) { - m_magnetic.m_data = magnetic->m_data; - m_magnetic.m_time_stamp = magnetic->m_time_stamp; - } -} +/* + * Standard deviations of accelerometer and magnetometer + */ +static const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05) +static const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5) +static const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05) +static const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5) -template -inline void orientation_filter::orientation_triad_algorithm() -{ - TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0}; - TYPE arr_mag_e[V1x3S] = {0.0, 1.0, 0.0}; - vect acc_e(arr_acc_e); - vect mag_e(arr_mag_e); +/* ====================== END FUSION SENSOR PARAMETER ========================*/ - vect acc_b_x_mag_b = cross(m_accel.m_data, m_magnetic.m_data); - vect acc_e_x_mag_e = cross(acc_e, mag_e); +static const float SYMMETRY_TOLERANCE = 1e-10f; - vect cross1 = cross(acc_b_x_mag_b, m_accel.m_data); - vect cross2 = cross(acc_e_x_mag_e, acc_e); +/* + * Accelerometer updates will not be performed near free fall to avoid + * ill-conditioning and div by zeros. + * Threshhold: 10% of g, in m/s^2 + */ +static const float NOMINAL_GRAVITY = 9.81f; +static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY); - matrix mat_b; - matrix mat_e; +/* + * The geomagnetic-field should be between 30uT and 60uT. + * Fields strengths greater than this likely indicate a local magnetic + * disturbance which we do not want to update into the fused frame. + */ +static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT +static const float MAX_VALID_MAGNETIC_FIELD_SQ = + MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD; - for(int i = 0; i < M3X3R; i++) - { - mat_b.m_mat[i][0] = m_accel.m_data.m_vec[i]; - mat_b.m_mat[i][1] = acc_b_x_mag_b.m_vec[i]; - mat_b.m_mat[i][2] = cross1.m_vec[i]; - mat_e.m_mat[i][0] = acc_e.m_vec[i]; - mat_e.m_mat[i][1] = acc_e_x_mag_e.m_vec[i]; - mat_e.m_mat[i][2] = cross2.m_vec[i]; - } +/* + * Values of the field smaller than this should be ignored in fusion to avoid + * ill-conditioning. This state can happen with anomalous local magnetic + * disturbances canceling the Earth field. + */ +static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT +static const float MIN_VALID_MAGNETIC_FIELD_SQ = + MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD; - matrix mat_b_t = tran(mat_b); - rotation_matrix rot_mat(mat_e * mat_b_t); +/* + * If the cross product of two vectors has magnitude squared less than this, + * we reject it as invalid due to alignment of the vectors. + * This threshold is used to check for the case where the magnetic field sample + * is parallel to the gravity field, which can happen in certain places due + * to magnetic field disturbances. + */ +static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3; +static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ = + MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG; + +static const float SQRT_3 = 1.732f; +static const float WVEC_EPS = 1e-4f/SQRT_3; +// ----------------------------------------------------------------------- + +template +static mat scaleCovariance( + const mat& A, + const mat& P) { + // A*P*transpose(A); + mat APAt; + for (size_t r=0 ; r +static mat crossMatrix(const vec& p, OTHER_TYPE diag) { + mat r; + r[0][0] = diag; + r[1][1] = diag; + r[2][2] = diag; + r[0][1] = p.z; + r[1][0] =-p.z; + r[0][2] =-p.y; + r[2][0] = p.y; + r[1][2] = p.x; + r[2][1] =-p.x; + return r; } -template -inline void orientation_filter::compute_accel_orientation() -{ - TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0}; - vect acc_e(arr_acc_e); +template +class Covariance { + mat mSumXX; + vec mSumX; + size_t mN; +public: + Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { } + void update(const vec& x) { + mSumXX += x*transpose(x); + mSumX += x; + mN++; + } + mat operator()() const { + const float N = 1.0f / mN; + return mSumXX*N - (mSumX*transpose(mSumX))*(N*N); + } + void reset() { + mN = 0; + mSumXX = 0; + mSumX = 0; + } + size_t getCount() const { + return mN; + } +}; + +// ----------------------------------------------------------------------- + +orientation_filter::orientation_filter() { + Phi[0][1] = 0; + Phi[1][1] = 1; + + Ba.x = 0; + Ba.y = 0; + Ba.z = 1; + + Bm.x = 0; + Bm.y = 1; + Bm.z = 0; + + x0 = 0; + x1 = 0; + + init(); +} - m_quat_aid = sensor_data2quat(m_accel, acc_e); +void orientation_filter::init(int mode) { + mInitState = 0; + + mGyroRate = 0; + + mCount[0] = 0; + mCount[1] = 0; + mCount[2] = 0; + + mData = 0; + mMode = mode; + + if (mMode != FUSION_NOGYRO) { //normal or game rotation + mParam.gyroVar = DEFAULT_GYRO_VAR; + mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR; + mParam.accStdev = DEFAULT_ACC_STDEV; + mParam.magStdev = DEFAULT_MAG_STDEV; + } else { + mParam.gyroVar = GEOMAG_GYRO_VAR; + mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR; + mParam.accStdev = GEOMAG_ACC_STDEV; + mParam.magStdev = GEOMAG_MAG_STDEV; + } } -template -inline void orientation_filter::compute_covariance() +void orientation_filter::initFusion(const vec4_t& q, float dT) { - TYPE var_gyr_x, var_gyr_y, var_gyr_z; - TYPE var_roll, var_pitch, var_azimuth; - quaternion quat_diff, quat_error; - - if(!is_initialized(m_quat_driv.m_quat)) - m_quat_driv = m_quat_aid; - - quaternion quat_rot_inc(0, m_gyro.m_data.m_vec[0], m_gyro.m_data.m_vec[1], - m_gyro.m_data.m_vec[2]); - - quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5; - - m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI); - m_quat_driv.quat_normalize(); - - m_quat_output = phase_correction(m_quat_driv, m_quat_aid); - - m_orientation = quat2euler(m_quat_output); - - quat_error = m_quat_aid * m_quat_driv; - - m_euler_error = (quat2euler(quat_error)).m_ang; - - m_gyro.m_data = m_gyro.m_data - m_euler_error.m_ang; - - m_euler_error.m_ang = m_euler_error.m_ang / (TYPE) PI; - - m_gyro_bias = m_euler_error.m_ang * (TYPE) PI; + // initial estimate: E{ x(t0) } + x0 = q; + x1 = 0; + + // process noise covariance matrix: G.Q.Gt, with + // + // G = | -1 0 | Q = | q00 q10 | + // | 0 1 | | q01 q11 | + // + // q00 = sv^2.dt + 1/3.su^2.dt^3 + // q10 = q01 = 1/2.su^2.dt^2 + // q11 = su^2.dt + // + + const float dT2 = dT*dT; + const float dT3 = dT2*dT; + + // variance of integrated output at 1/dT Hz (random drift) + const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3; + + // variance of drift rate ramp + const float q11 = mParam.gyroBiasVar * dT; + const float q10 = 0.5f * mParam.gyroBiasVar * dT2; + const float q01 = q10; + + GQGt[0][0] = q00; // rad^2 + GQGt[1][0] = -q10; + GQGt[0][1] = -q01; + GQGt[1][1] = q11; // (rad/s)^2 + + // initial covariance: Var{ x(t0) } + // TODO: initialize P correctly + P = 0; +} - insert_end(m_var_gyr_x, m_gyro.m_data.m_vec[0]); - insert_end(m_var_gyr_y, m_gyro.m_data.m_vec[1]); - insert_end(m_var_gyr_z, m_gyro.m_data.m_vec[2]); - insert_end(m_var_roll, m_orientation.m_ang.m_vec[0]); - insert_end(m_var_pitch, m_orientation.m_ang.m_vec[1]); - insert_end(m_var_azimuth, m_orientation.m_ang.m_vec[2]); +bool orientation_filter::hasEstimate() const { + return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) && + ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) && + (mInitState & ACC); +} - var_gyr_x = var(m_var_gyr_x); - var_gyr_y = var(m_var_gyr_y); - var_gyr_z = var(m_var_gyr_z); - var_roll = var(m_var_roll); - var_pitch = var(m_var_pitch); - var_azimuth = var(m_var_azimuth); +bool orientation_filter::checkInitComplete(int what, const vec3_t& d, float dT) { + if (hasEstimate()) + return true; + + if (what == ACC) { + mData[0] += d * (1/length(d)); + mCount[0]++; + mInitState |= ACC; + if (mMode == FUSION_NOGYRO ) { + mGyroRate = dT; + } + } else if (what == MAG) { + mData[1] += d * (1/length(d)); + mCount[1]++; + mInitState |= MAG; + } else if (what == GYRO) { + mGyroRate = dT; + mData[2] += d*dT; + mCount[2]++; + mInitState |= GYRO; + } + + if (hasEstimate()) { + // Average all the values we collected so far + mData[0] *= 1.0f/mCount[0]; + if (mMode != FUSION_NOMAG) { + mData[1] *= 1.0f/mCount[1]; + } + mData[2] *= 1.0f/mCount[2]; + + // calculate the MRPs from the data collection, this gives us + // a rough estimate of our initial state + mat33_t R; + vec3_t up(mData[0]); + vec3_t east; + + if (mMode != FUSION_NOMAG) { + east = normalize(cross_product(mData[1], up)); + } else { + east = getOrthogonal(up); + } + + vec3_t north(cross_product(up, east)); + R << east << north << up; + const vec4_t q = matrixToQuat(R); + + initFusion(q, mGyroRate); + } + + return false; +} - m_driv_cov.m_mat[0][0] = var_gyr_x; - m_driv_cov.m_mat[1][1] = var_gyr_y; - m_driv_cov.m_mat[2][2] = var_gyr_z; - m_driv_cov.m_mat[3][3] = (TYPE) QWB_CONST; - m_driv_cov.m_mat[4][4] = (TYPE) QWB_CONST; - m_driv_cov.m_mat[5][5] = (TYPE) QWB_CONST; +void orientation_filter::handleGyro(const vec3_t& w, float dT) { + if (!checkInitComplete(GYRO, w, dT)) + return; - m_aid_cov.m_mat[0][0] = var_roll; - m_aid_cov.m_mat[1][1] = var_pitch; - m_aid_cov.m_mat[2][2] = var_azimuth; + predict(w, dT); } -template -inline void orientation_filter::time_update() -{ - euler_angles orientation; - - m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2]; - m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1]; - m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2]; - m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0]; - m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1]; - m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0]; - m_tran_mat.m_mat[3][3] = (TYPE) F_CONST; - m_tran_mat.m_mat[4][4] = (TYPE) F_CONST; - m_tran_mat.m_mat[5][5] = (TYPE) F_CONST; - - m_measure_mat.m_mat[0][0] = 1; - m_measure_mat.m_mat[1][1] = 1; - m_measure_mat.m_mat[2][2] = 1; - - if (is_initialized(m_state_old)) - m_state_new = transpose(m_tran_mat * transpose(m_state_old)); - - m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov; - - for (int j = 0; j < M6X6C; ++j) { - for (int i = 0; i < M6X6R; ++i) { - if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL) - m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL; - } - if (ABS(m_state_new.m_vec[j]) < NEGLIGIBLE_VAL) - m_state_new.m_vec[j] = NEGLIGIBLE_VAL; - } - - m_quat_9axis = m_quat_output; - m_quat_gaming_rv = m_quat_9axis; - - m_rot_matrix = quat2rot_mat(m_quat_driv); - - quaternion quat_eu_er(1, m_euler_error.m_ang.m_vec[0], m_euler_error.m_ang.m_vec[1], - m_euler_error.m_ang.m_vec[2]); - - m_quat_driv = (m_quat_driv * quat_eu_er) * (TYPE) PI; - m_quat_driv.quat_normalize(); - - if (is_initialized(m_state_new)) { - m_state_error.m_vec[0] = m_euler_error.m_ang.m_vec[0]; - m_state_error.m_vec[1] = m_euler_error.m_ang.m_vec[1]; - m_state_error.m_vec[2] = m_euler_error.m_ang.m_vec[2]; - m_state_error.m_vec[3] = m_state_new.m_vec[3]; - m_state_error.m_vec[4] = m_state_new.m_vec[4]; - m_state_error.m_vec[5] = m_state_new.m_vec[5]; - } +status_t orientation_filter::handleAcc(const vec3_t& a, float dT) { + if (!checkInitComplete(ACC, a, dT)) + return BAD_VALUE; + + // ignore acceleration data if we're close to free-fall + const float l = length(a); + if (l < FREE_FALL_THRESHOLD) { + return BAD_VALUE; + } + + const float l_inv = 1.0f/l; + + if ( mMode == FUSION_NOGYRO ) { + //geo mag + vec3_t w_dummy; + w_dummy = x1; //bias + predict(w_dummy, dT); + } + + if ( mMode == FUSION_NOMAG) { + vec3_t m; + m = getRotationMatrix()*Bm; + update(m, Bm, mParam.magStdev); + } + + vec3_t unityA = a * l_inv; + const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY)); + const float p = l_inv * mParam.accStdev*expf(d); + + update(unityA, Ba, p); + return NO_ERROR; } -template -inline void orientation_filter::time_update_gaming_rv() -{ - euler_angles orientation; - euler_angles euler_aid; - euler_angles euler_driv; - - m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2]; - m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1]; - m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2]; - m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0]; - m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1]; - m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0]; - m_tran_mat.m_mat[3][3] = (TYPE) F_CONST; - m_tran_mat.m_mat[4][4] = (TYPE) F_CONST; - m_tran_mat.m_mat[5][5] = (TYPE) F_CONST; - - m_measure_mat.m_mat[0][0] = 1; - m_measure_mat.m_mat[1][1] = 1; - m_measure_mat.m_mat[2][2] = 1; - - if (is_initialized(m_state_old)) - m_state_new = transpose(m_tran_mat * transpose(m_state_old)); - - m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov; - - euler_aid = quat2euler(m_quat_aid); - euler_driv = quat2euler(m_quat_output); - - euler_angles euler_gaming_rv(euler_aid.m_ang.m_vec[0], euler_aid.m_ang.m_vec[1], - euler_driv.m_ang.m_vec[2]); - m_quat_gaming_rv = euler2quat(euler_gaming_rv); - - if (is_initialized(m_state_new)) { - m_state_error.m_vec[0] = m_euler_error.m_ang.m_vec[0]; - m_state_error.m_vec[1] = m_euler_error.m_ang.m_vec[1]; - m_state_error.m_vec[2] = m_euler_error.m_ang.m_vec[2]; - m_state_error.m_vec[3] = m_state_new.m_vec[3]; - m_state_error.m_vec[4] = m_state_new.m_vec[4]; - m_state_error.m_vec[5] = m_state_new.m_vec[5]; - } +status_t orientation_filter::handleMag(const vec3_t& m) { + if (!checkInitComplete(MAG, m)) + return BAD_VALUE; + + // the geomagnetic-field should be between 30uT and 60uT + // reject if too large to avoid spurious magnetic sources + const float magFieldSq = length_squared(m); + if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) { + return BAD_VALUE; + } else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) { + // Also reject if too small since we will get ill-defined (zero mag) + // cross-products below + return BAD_VALUE; + } + + // Orthogonalize the magnetic field to the gravity field, mapping it into + // tangent to Earth. + const vec3_t up( getRotationMatrix() * Ba ); + const vec3_t east( cross_product(m, up) ); + + // If the m and up vectors align, the cross product magnitude will + // approach 0. + // Reject this case as well to avoid div by zero problems and + // ill-conditioning below. + if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) { + return BAD_VALUE; + } + + // If we have created an orthogonal magnetic field successfully, + // then pass it in as the update. + vec3_t north( cross_product(up, east) ); + + const float l_inv = 1 / length(north); + north *= l_inv; + + update(north, Bm, mParam.magStdev*l_inv); + return NO_ERROR; } -template -inline void orientation_filter::measurement_update() -{ - matrix gain; - matrix iden; - iden.m_mat[0][0] = iden.m_mat[1][1] = iden.m_mat[2][2] = 1; - iden.m_mat[3][3] = iden.m_mat[4][4] = iden.m_mat[5][5] = 1; +void orientation_filter::checkState() { + // P needs to stay positive semidefinite or the fusion diverges. When we + // detect divergence, we reset the fusion. + // TODO(braun): Instead, find the reason for the divergence and fix it. - for (int j = 0; j < M6X6C; ++j) { - for (int i = 0; i < M6X6R; ++i) { - gain.m_mat[i][j] = m_pred_cov.m_mat[j][i] / (m_pred_cov.m_mat[j][j] + m_aid_cov.m_mat[j][j]); - m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j]; - } + if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) || + !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) { + P = 0; + } +} - matrix temp = iden; +vec4_t orientation_filter::getAttitude() const { + return x0; +} - for (int i = 0; i < M6X6R; ++i) - temp.m_mat[i][j] = iden.m_mat[i][j] - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i]); +vec3_t orientation_filter::getBias() const { + return x1; +} - m_pred_cov = temp * m_pred_cov; - } +mat33_t orientation_filter::getRotationMatrix() const { + return quatToMatrix(x0); +} - for (int j = 0; j < M6X6C; ++j) { - for (int i = 0; i < M6X6R; ++i) { - if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL) - m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL; - } - } +mat34_t orientation_filter::getF(const vec4_t& q) { + mat34_t F; - m_state_old = m_state_new; + // This is used to compute the derivative of q + // F = | [q.xyz]x | + // | -q.xyz | - TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]}; - vect vec(arr_bias); + F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y; + F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x; + F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w; + F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z; + return F; +} - m_bias_correction = vec; +void orientation_filter::predict(const vec3_t& w, float dT) { + const vec4_t q = x0; + const vec3_t b = x1; + vec3_t we = w - b; + + if (length(we) < WVEC_EPS) { + we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS; + } + // q(k+1) = O(we)*q(k) + // -------------------- + // + // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi | + // | -psi' cos(0.5*||w||*dT) | + // + // psi = sin(0.5*||w||*dT)*w / ||w|| + // + // + // P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G' + // ---------------------------------------- + // + // G = | -I33 0 | + // | 0 I33 | + // + // Phi = | Phi00 Phi10 | + // | 0 1 | + // + // Phi00 = I33 + // - [w]x * sin(||w||*dt)/||w|| + // + [w]x^2 * (1-cos(||w||*dT))/||w||^2 + // + // Phi10 = [w]x * (1 - cos(||w||*dt))/||w||^2 + // - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3 + // - I33*dT + + const mat33_t I33(1); + const mat33_t I33dT(dT); + const mat33_t wx(crossMatrix(we, 0)); + const mat33_t wx2(wx*wx); + const float lwedT = length(we)*dT; + const float hlwedT = 0.5f*lwedT; + const float ilwe = 1.f/length(we); + const float k0 = (1-cosf(lwedT))*(ilwe*ilwe); + const float k1 = sinf(lwedT); + const float k2 = cosf(hlwedT); + const vec3_t psi(sinf(hlwedT)*ilwe*we); + const mat33_t O33(crossMatrix(-psi, k2)); + mat44_t O; + O[0].xyz = O33[0]; O[0].w = -psi.x; + O[1].xyz = O33[1]; O[1].w = -psi.y; + O[2].xyz = O33[2]; O[2].w = -psi.z; + O[3].xyz = psi; O[3].w = k2; + + Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0; + Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1); + + x0 = O*q; + + if (x0.w < 0) + x0 = -x0; + + P = Phi*P*transpose(Phi) + GQGt; + + checkState(); +} - m_gyro_bias = m_gyro_bias + vec; +void orientation_filter::update(const vec3_t& z, const vec3_t& Bi, float sigma) { + vec4_t q(x0); + // measured vector in body space: h(p) = A(p)*Bi + const mat33_t A(quatToMatrix(q)); + const vec3_t Bb(A*Bi); + + // Sensitivity matrix H = dh(p)/dp + // H = [ L 0 ] + const mat33_t L(crossMatrix(Bb, 0)); + + // gain... + // K = P*Ht / [H*P*Ht + R] + vec K; + const mat33_t R(sigma*sigma); + const mat33_t S(scaleCovariance(L, P[0][0]) + R); + const mat33_t Si(invert(S)); + const mat33_t LtSi(transpose(L)*Si); + K[0] = P[0][0] * LtSi; + K[1] = transpose(P[1][0])*LtSi; + + // update... + // P = (I-K*H) * P + // P -= K*H*P + // | K0 | * | L 0 | * P = | K0*L 0 | * | P00 P10 | = | K0*L*P00 K0*L*P10 | + // | K1 | | K1*L 0 | | P01 P11 | | K1*L*P00 K1*L*P10 | + // Note: the Joseph form is numerically more stable and given by: + // P = (I-KH) * P * (I-KH)' + K*R*R' + const mat33_t K0L(K[0] * L); + const mat33_t K1L(K[1] * L); + P[0][0] -= K0L*P[0][0]; + P[1][1] -= K1L*P[1][0]; + P[1][0] -= K0L*P[1][0]; + P[0][1] = transpose(P[1][0]); + + const vec3_t e(z - Bb); + const vec3_t dq(K[0]*e); + + q += getF(q)*(0.5f*dq); + x0 = normalize_quat(q); + + if (mMode != FUSION_NOMAG) { + const vec3_t db(K[1]*e); + x1 += db; + } + + checkState(); } -template -void orientation_filter::get_device_orientation(const sensor_data *accel, - const sensor_data *gyro, const sensor_data *magnetic) -{ - initialize_sensor_data(accel, gyro, magnetic); - - if (gyro != NULL && magnetic != NULL) { - orientation_triad_algorithm(); - compute_covariance(); - time_update(); - measurement_update(); - m_quaternion = m_quat_9axis; - } else if (!gyro && !magnetic) { - compute_accel_orientation(); - m_quaternion = m_quat_aid; - } else if (!gyro) { - orientation_triad_algorithm(); - m_quaternion = m_quat_aid; - } else if (!magnetic) { - compute_accel_orientation(); - compute_covariance(); - time_update_gaming_rv(); - measurement_update(); - m_quaternion = m_quat_gaming_rv; - } +vec3_t orientation_filter::getOrthogonal(const vec3_t &v) { + vec3_t w; + if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2])) { + w[0]=0.f; + w[1] = v[2]; + w[2] = -v[1]; + } else if (fabsf(v[1]) <= fabsf(v[2])) { + w[0] = v[2]; + w[1] = 0.f; + w[2] = -v[0]; + }else { + w[0] = v[1]; + w[1] = -v[0]; + w[2] = 0.f; + } + return normalize(w); } -#endif //_ORIENTATION_FILTER_H_ + +// ----------------------------------------------------------------------- + diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h index eaea224..0ff3ea5 100644 --- a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h +++ b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h @@ -1,94 +1,103 @@ /* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * Copyright (C) 2011 The Android Open Source Project * * 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 + * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. - * */ -#ifndef _ORIENTATION_FILTER_H_ -#define _ORIENTATION_FILTER_H_ +// released in android-11.0.0_r9 + +#ifndef ANDROID_FUSION_H +#define ANDROID_FUSION_H + +#include "quat.h" +#include "mat.h" +#include "vec.h" +#include "errors.h" -#include "matrix.h" -#include "vector.h" -#include "sensor_data.h" -#include "quaternion.h" -#include "euler_angles.h" -#include "rotation_matrix.h" +namespace android { -#define MOVING_AVERAGE_WINDOW_LENGTH 20 +typedef mat mat34_t; -#define M3X3R 3 -#define M3X3C 3 -#define M6X6R 6 -#define M6X6C 6 -#define V1x3S 3 -#define V1x4S 4 -#define V1x6S 6 -#define ACCEL_SCALE 1 -#define GYRO_SCALE 1146 -#define GEOMAGNETIC_SCALE 1 +enum FUSION_MODE{ + FUSION_9AXIS, // use accel gyro mag + FUSION_NOMAG, // use accel gyro (game rotation, gravity) + FUSION_NOGYRO, // use accel mag (geomag rotation) + NUM_FUSION_MODE +}; -template class orientation_filter { + /* + * the state vector is made of two sub-vector containing respectively: + * - modified Rodrigues parameters + * - the estimated gyro bias + */ + quat_t x0; + vec3_t x1; + + /* + * the predicated covariance matrix is made of 4 3x3 sub-matrices and it is + * semi-definite positive. + * + * P = | P00 P10 | = | P00 P10 | + * | P01 P11 | | P10t P11 | + * + * Since P01 = transpose(P10), the code below never calculates or + * stores P01. + */ + mat P; + + /* + * the process noise covariance matrix + */ + mat GQGt; + public: - sensor_data m_accel; - sensor_data m_gyro; - sensor_data m_magnetic; - vect m_var_gyr_x; - vect m_var_gyr_y; - vect m_var_gyr_z; - vect m_var_roll; - vect m_var_pitch; - vect m_var_azimuth; - matrix m_driv_cov; - matrix m_aid_cov; - matrix m_tran_mat; - matrix m_measure_mat; - matrix m_pred_cov; - vect m_state_new; - vect m_state_old; - vect m_state_error; - vect m_bias_correction; - vect m_gyro_bias; - quaternion m_quat_aid; - quaternion m_quat_driv; - rotation_matrix m_rot_matrix; - euler_angles m_orientation; - quaternion m_quat_9axis; - quaternion m_quat_gaming_rv; - quaternion m_quaternion; - quaternion m_quat_output; - euler_angles m_euler_error; - TYPE m_gyro_dt; + orientation_filter(); + void init(int mode = FUSION_9AXIS); + void handleGyro(const vec3_t& w, float dT); + status_t handleAcc(const vec3_t& a, float dT); + status_t handleMag(const vec3_t& m); + vec4_t getAttitude() const; + vec3_t getBias() const; + mat33_t getRotationMatrix() const; + bool hasEstimate() const; - orientation_filter(void); - ~orientation_filter(void); +private: + struct Parameter { + float gyroVar; + float gyroBiasVar; + float accStdev; + float magStdev; + } mParam; - inline void initialize_sensor_data(const sensor_data *accel, - const sensor_data *gyro, const sensor_data *magnetic); - inline void orientation_triad_algorithm(void); - inline void compute_accel_orientation(void); - inline void compute_covariance(void); - inline void time_update(void); - inline void time_update_gaming_rv(void); - inline void measurement_update(void); + mat Phi; + vec3_t Ba, Bm; + uint32_t mInitState; + float mGyroRate; + vec mData; + size_t mCount[3]; + int mMode; - void get_device_orientation(const sensor_data *accel, - const sensor_data *gyro, const sensor_data *magnetic); + enum { ACC=0x1, MAG=0x2, GYRO=0x4 }; + bool checkInitComplete(int, const vec3_t& w, float d = 0); + void initFusion(const vec4_t& q0, float dT); + void checkState(); + void predict(const vec3_t& w, float dT); + void update(const vec3_t& z, const vec3_t& Bi, float sigma); + static mat34_t getF(const vec4_t& p); + static vec3_t getOrthogonal(const vec3_t &v); }; -#include "orientation_filter.cpp" +} -#endif /* _ORIENTATION_FILTER_H_ */ +#endif // ANDROID_FUSION_H diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/quat.h b/src/fusion-sensor/rotation_vector/fusion_utils/quat.h new file mode 100644 index 0000000..0b3b1bb --- /dev/null +++ b/src/fusion-sensor/rotation_vector/fusion_utils/quat.h @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * 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. + */ + +// released in android-11.0.0_r9 + +#ifndef ANDROID_QUAT_H +#define ANDROID_QUAT_H + +#include + +#include "vec.h" +#include "mat.h" + +// ----------------------------------------------------------------------- +namespace android { +// ----------------------------------------------------------------------- + +template +mat quatToMatrix(const vec& q) { + mat R; + TYPE q0(q.w); + TYPE q1(q.x); + TYPE q2(q.y); + TYPE q3(q.z); + TYPE sq_q1 = 2 * q1 * q1; + TYPE sq_q2 = 2 * q2 * q2; + TYPE sq_q3 = 2 * q3 * q3; + TYPE q1_q2 = 2 * q1 * q2; + TYPE q3_q0 = 2 * q3 * q0; + TYPE q1_q3 = 2 * q1 * q3; + TYPE q2_q0 = 2 * q2 * q0; + TYPE q2_q3 = 2 * q2 * q3; + TYPE q1_q0 = 2 * q1 * q0; + R[0][0] = 1 - sq_q2 - sq_q3; + R[0][1] = q1_q2 - q3_q0; + R[0][2] = q1_q3 + q2_q0; + R[1][0] = q1_q2 + q3_q0; + R[1][1] = 1 - sq_q1 - sq_q3; + R[1][2] = q2_q3 - q1_q0; + R[2][0] = q1_q3 - q2_q0; + R[2][1] = q2_q3 + q1_q0; + R[2][2] = 1 - sq_q1 - sq_q2; + return R; +} + +template +vec matrixToQuat(const mat& R) { + // matrix to quaternion + + struct { + inline TYPE operator()(TYPE v) { + return v < 0 ? 0 : v; + } + } clamp; + + vec q; + const float Hx = R[0].x; + const float My = R[1].y; + const float Az = R[2].z; + q.x = sqrtf( clamp( Hx - My - Az + 1) * 0.25f ); + q.y = sqrtf( clamp(-Hx + My - Az + 1) * 0.25f ); + q.z = sqrtf( clamp(-Hx - My + Az + 1) * 0.25f ); + q.w = sqrtf( clamp( Hx + My + Az + 1) * 0.25f ); + q.x = copysignf(q.x, R[2].y - R[1].z); + q.y = copysignf(q.y, R[0].z - R[2].x); + q.z = copysignf(q.z, R[1].x - R[0].y); + // guaranteed to be unit-quaternion + return q; +} + +template +vec normalize_quat(const vec& q) { + vec r(q); + if (r.w < 0) { + r = -r; + } + return normalize(r); +} + +// ----------------------------------------------------------------------- + +typedef vec4_t quat_t; + +// ----------------------------------------------------------------------- +}; // namespace android + +#endif /* ANDROID_QUAT_H */ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/quaternion.cpp b/src/fusion-sensor/rotation_vector/fusion_utils/quaternion.cpp deleted file mode 100644 index 9d28f58..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/quaternion.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#if defined (_QUATERNION_H_) && defined (_VECTOR_H_) - -#include - -template int sgn(T val) { - if (val >= 0) - return 1; - else - return -1; -} - -template T mag(T val) { - if (val < 0) - return val * static_cast(-1); - else - return val; -} - -template -quaternion::quaternion() : m_quat() -{ -} - -template -quaternion::quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z) -{ - TYPE vec_data[QUAT_SIZE] = {w, x, y, z}; - - vect v(vec_data); - m_quat = v; -} - -template -quaternion::quaternion(const vect v) -{ - m_quat = v; -} - -template -quaternion::quaternion(const quaternion& q) -{ - m_quat = q.m_quat; -} - -template -quaternion::~quaternion() -{ -} - -template -quaternion quaternion::operator =(const quaternion& q) -{ - m_quat = q.m_quat; - - return *this; -} - -template -void quaternion::quat_normalize() -{ - TYPE w, x, y, z; - TYPE val; - - w = m_quat.m_vec[0] * m_quat.m_vec[0]; - x = m_quat.m_vec[1] * m_quat.m_vec[1]; - y = m_quat.m_vec[2] * m_quat.m_vec[2]; - z = m_quat.m_vec[3] * m_quat.m_vec[3]; - - val = sqrt(w + x + y + z); - - m_quat = m_quat / val; -} - -template -quaternion operator *(const quaternion q, const T val) -{ - return (q.m_quat * val); -} - -template -quaternion operator *(const quaternion q1, const quaternion q2) -{ - T w, x, y, z; - T w1, x1, y1, z1; - T w2, x2, y2, z2; - - w1 = q1.m_quat.m_vec[0]; - x1 = q1.m_quat.m_vec[1]; - y1 = q1.m_quat.m_vec[2]; - z1 = q1.m_quat.m_vec[3]; - - w2 = q2.m_quat.m_vec[0]; - x2 = q2.m_quat.m_vec[1]; - y2 = q2.m_quat.m_vec[2]; - z2 = q2.m_quat.m_vec[3]; - - x = x1*w2 + y1*z2 - z1*y2 + w1*x2; - y = -x1*z2 + y1*w2 + z1*x2 + w1*y2; - z = x1*y2 - y1*x2 + z1*w2 + w1*z2; - w = -x1*x2 - y1*y2 - z1*z2 + w1*w2; - - quaternion q(w, x, y, z); - - return q; -} - -template -quaternion operator +(const quaternion q1, const quaternion q2) -{ - return (q1.m_quat + q2.m_quat); -} - - -template -quaternion phase_correction(const quaternion q1, const quaternion q2) -{ - T w, x, y, z; - w = mag(q1.m_quat.m_vec[0]) * sgn(q2.m_quat.m_vec[0]); - x = mag(q1.m_quat.m_vec[1]) * sgn(q2.m_quat.m_vec[1]); - y = mag(q1.m_quat.m_vec[2]) * sgn(q2.m_quat.m_vec[2]); - z = mag(q1.m_quat.m_vec[3]) * sgn(q2.m_quat.m_vec[3]); - - quaternion q(w, x, y, z); - - return q; -} - -template -quaternion axis2quat(const vect axis, const T angle) -{ - T w; - vect imag; - - w = cos(angle/2); - imag = axis * (T)(-sin(angle/2)); - - quaternion q(w, imag.m_vec[0], imag.m_vec[1], imag.m_vec[2]); - - return q; -} -#endif //_QUATERNION_H_ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/quaternion.h b/src/fusion-sensor/rotation_vector/fusion_utils/quaternion.h deleted file mode 100644 index cbe9bf4..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/quaternion.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef _QUATERNION_H_ -#define _QUATERNION_H_ - -#include "vector.h" - -#define QUAT_SIZE 4 -#define REF_VEC_SIZE 3 - -template -class quaternion { -public: - vect m_quat; - - quaternion(); - quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z); - quaternion(const vect v); - quaternion(const quaternion& q); - ~quaternion(); - - quaternion operator =(const quaternion& q); - void quat_normalize(void); - - template friend quaternion operator *(const quaternion q, - const T val); - template friend quaternion operator *(const quaternion q1, - const quaternion q2); - template friend quaternion operator +(const quaternion q1, - const quaternion q2); - template friend quaternion phase_correction(const quaternion q1, - const quaternion q2); - template friend quaternion axis2quat(const vect axis, - const T angle); -}; - -#include "quaternion.cpp" - -#endif //_QUATERNION_H_ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/rotation_matrix.cpp b/src/fusion-sensor/rotation_vector/fusion_utils/rotation_matrix.cpp deleted file mode 100644 index fa89881..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/rotation_matrix.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#if defined (_ROTATION_MATRIX_H_) && defined (_MATRIX_H_) - -#define QUAT_LEN 4 - -template T get_sign(T val) -{ - return (val >= (T) 0) ? (T) 1 : (T) -1; -} - -template -rotation_matrix::rotation_matrix() : m_rot_mat() -{ -} - -template -rotation_matrix::rotation_matrix(const matrix m) -{ - m_rot_mat = m; -} - -template -rotation_matrix::rotation_matrix(TYPE mat_data[ROT_MAT_ROWS][ROT_MAT_COLS]) -{ - matrix m(mat_data); - m_rot_mat = m; -} - -template -rotation_matrix::rotation_matrix(const rotation_matrix& rm) -{ - m_rot_mat = rm.m_rot_mat; -} - -template -rotation_matrix::~rotation_matrix() -{ -} - -template -rotation_matrix rotation_matrix::operator =(const rotation_matrix& rm) -{ - m_rot_mat = rm.m_rot_mat; - - return *this; -} - -template -rotation_matrix quat2rot_mat(quaternion q) -{ - T w, x, y, z; - T R[ROT_MAT_ROWS][ROT_MAT_COLS]; - - w = q.m_quat.m_vec[0]; - x = q.m_quat.m_vec[1]; - y = q.m_quat.m_vec[2]; - z = q.m_quat.m_vec[3]; - - R[0][0] = (2 * w * w) - 1 + (2 * x * x); - R[0][1] = 2 * ((x * y) + (w * z)); - R[0][2] = 2 * ((x * z) - (w * y)); - R[1][0] = 2 * ((x * y) - (w * z)); - R[1][1] = (2 * w * w) - 1 + (2 * y * y); - R[1][2] = 2 * ((y * z) + (w * x)); - R[2][0] = 2 * ((x * z) + (w * y)); - R[2][1] = 2 * ((y * z) - (w * x)); - R[2][2] = (2 * w * w) - 1 + (2 * z * z); - - rotation_matrix rm(R); - - return rm; -} - -template -quaternion rot_mat2quat(rotation_matrix rm) -{ - T q0, q1, q2, q3; - T phi, theta, psi; - - phi = atan2(rm.m_rot_mat.m_mat[2][1], rm.m_rot_mat.m_mat[2][2]); - theta = atan2(-rm.m_rot_mat.m_mat[2][0], - sqrt((rm.m_rot_mat.m_mat[2][1] * rm.m_rot_mat.m_mat[2][1]) + - (rm.m_rot_mat.m_mat[2][2] * rm.m_rot_mat.m_mat[2][2]))); - psi = atan2(rm.m_rot_mat.m_mat[1][0], rm.m_rot_mat.m_mat[0][0]); - - q0 = (cos(phi/2) * cos(theta/2) * cos(psi/2)) + - (sin(phi/2) * sin(theta/2) * sin(psi/2)); - q1 = (-cos(phi/2) * sin(theta/2) * sin(psi/2)) + - (sin(phi/2) * cos(theta/2) * cos(psi/2)); - q2 = (cos(phi/2) * sin(theta/2) * cos(psi/2)) + - (sin(phi/2) * cos(theta/2) * sin(psi/2)); - q3 = (cos(phi/2) * cos(theta/2) * sin(psi/2)) - - (sin(phi/2) * sin(theta/2) * cos(psi/2)); - - quaternion q(q0, q1, q2, q3); - - q.quat_normalize(); - - return q; -} - -#endif /* _ROTATION_MATRIX_H_ */ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/rotation_matrix.h b/src/fusion-sensor/rotation_vector/fusion_utils/rotation_matrix.h deleted file mode 100644 index 5cdf430..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/rotation_matrix.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef _ROTATION_MATRIX_H_ -#define _ROTATION_MATRIX_H_ - -#include "matrix.h" -#include "quaternion.h" - -#define ROT_MAT_ROWS 3 -#define ROT_MAT_COLS 3 - -template -class rotation_matrix { -public: - matrix m_rot_mat; - - rotation_matrix(); - rotation_matrix(const matrix m); - rotation_matrix(TYPE mat_data[ROT_MAT_ROWS][ROT_MAT_COLS]); - rotation_matrix(const rotation_matrix& rm); - ~rotation_matrix(); - - rotation_matrix operator =(const rotation_matrix& rm); - - template friend rotation_matrix quat2rot_mat(quaternion q); - template friend quaternion rot_mat2quat(rotation_matrix rm); -}; - -#include "rotation_matrix.cpp" - -#endif /* _ROTATION_MATRIX_H_ */ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/sensor_data.cpp b/src/fusion-sensor/rotation_vector/fusion_utils/sensor_data.cpp deleted file mode 100644 index abd1181..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/sensor_data.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#if defined (_SENSOR_DATA_H_) && defined (_VECTOR_H_) - -#include "math.h" - -template -sensor_data::sensor_data() : m_data(), m_time_stamp(0) -{ -} - -template -sensor_data::sensor_data(const TYPE x, const TYPE y, - const TYPE z, const unsigned long long time_stamp) -{ - TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z}; - - vect v(vec_data); - m_data = v; - m_time_stamp = time_stamp; -} - -template -sensor_data::sensor_data(const vect v, - const unsigned long long time_stamp) -{ - m_data = v; - m_time_stamp = time_stamp; -} - -template -sensor_data::sensor_data(const sensor_data& s) -{ - m_data = s.m_data; - m_time_stamp = s.m_time_stamp; -} - -template -sensor_data::~sensor_data() -{ -} - -template -sensor_data sensor_data::operator =(const sensor_data& s) -{ - m_data = s.m_data; - m_time_stamp = s.m_time_stamp; - - return *this; -} - -template -sensor_data operator +(sensor_data data1, sensor_data data2) -{ - sensor_data result(data1.m_data + data2.m_data, 0); - return result; -} - -template -void normalize(sensor_data& data) -{ - T x, y, z; - - x = data.m_data.m_vec[0]; - y = data.m_data.m_vec[1]; - z = data.m_data.m_vec[2]; - - T val = sqrt(x*x + y*y + z*z); - - data.m_data.m_vec[0] = x / val; - data.m_data.m_vec[1] = y / val; - data.m_data.m_vec[2] = z / val; -} - -template -sensor_data scale_data(sensor_data data, T scaling_factor) -{ - T x, y, z; - - x = data.m_data.m_vec[0] / scaling_factor; - y = data.m_data.m_vec[1] / scaling_factor; - z = data.m_data.m_vec[2] / scaling_factor; - - sensor_data s(x, y, z, data.m_time_stamp); - - return s; -} - - -template -quaternion sensor_data2quat(const sensor_data data, const vect ref_vec) -{ - vect axis; - T angle; - - axis = cross(data.m_data, ref_vec); - angle = acos(dot(data.m_data, ref_vec)); - - return axis2quat(axis, angle); -} - -template -void pre_process_data(sensor_data &data_out, const T *data_in, int sign, int scale) -{ - data_out.m_data.m_vec[0] = sign * data_in[0] / scale; - data_out.m_data.m_vec[1] = sign * data_in[1] / scale; - data_out.m_data.m_vec[2] = sign * data_in[2] / scale; -} - -#endif /* _SENSOR_DATA_H_ */ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/sensor_data.h b/src/fusion-sensor/rotation_vector/fusion_utils/sensor_data.h deleted file mode 100644 index 151d655..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/sensor_data.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef _SENSOR_DATA_H_ -#define _SENSOR_DATA_H_ - -#include "vector.h" -#include "quaternion.h" - -#define SENSOR_DATA_SIZE 3 - -template -class sensor_data { -public: - vect m_data; - unsigned long long m_time_stamp { 0 }; - - sensor_data(); - sensor_data(const TYPE x, const TYPE y, const TYPE z, - const unsigned long long time_stamp); - sensor_data(const vect v, - const unsigned long long time_stamp); - sensor_data(const sensor_data& s); - ~sensor_data(); - - sensor_data operator =(const sensor_data& s); - - template friend sensor_data operator +(sensor_data data1, - sensor_data data2); - - template friend void normalize(sensor_data& data); - template friend sensor_data scale_data(sensor_data data, - T scaling_factor); - template friend quaternion sensor_data2quat(const sensor_data data, - const vect ref_vec); - template friend void pre_process_data(sensor_data &data_out, - const T *data_in, int sign, int scale); -}; - -#include "sensor_data.cpp" - -#endif /* _SENSOR_DATA_H_ */ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/traits.h b/src/fusion-sensor/rotation_vector/fusion_utils/traits.h new file mode 100644 index 0000000..a371b74 --- /dev/null +++ b/src/fusion-sensor/rotation_vector/fusion_utils/traits.h @@ -0,0 +1,120 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * 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. + */ + +// released in android-11.0.0_r9 + +#ifndef ANDROID_TRAITS_H +#define ANDROID_TRAITS_H + +// ----------------------------------------------------------------------- +// Typelists + +namespace android { + +// end-of-list marker +class NullType {}; + +// type-list node +template +struct TypeList { + typedef T Head; + typedef U Tail; +}; + +// helpers to build typelists +#define TYPELIST_1(T1) TypeList +#define TYPELIST_2(T1, T2) TypeList +#define TYPELIST_3(T1, T2, T3) TypeList +#define TYPELIST_4(T1, T2, T3, T4) TypeList + +// typelists algorithms +namespace TL { +template struct IndexOf; + +template +struct IndexOf { + enum { value = -1 }; +}; + +template +struct IndexOf, T> { + enum { value = 0 }; +}; + +template +struct IndexOf, T> { +private: + enum { temp = IndexOf::value }; +public: + enum { value = temp == -1 ? -1 : 1 + temp }; +}; + +}; // namespace TL + +// type selection based on a boolean +template +struct Select { + typedef T Result; +}; +template +struct Select { + typedef U Result; +}; + +// ----------------------------------------------------------------------- +// Type traits + +template +class TypeTraits { + typedef TYPELIST_4( + unsigned char, unsigned short, + unsigned int, unsigned long int) UnsignedInts; + + typedef TYPELIST_4( + signed char, signed short, + signed int, signed long int) SignedInts; + + typedef TYPELIST_1( + bool) OtherInts; + + typedef TYPELIST_3( + float, double, long double) Floats; + + template struct PointerTraits { + enum { result = false }; + typedef NullType PointeeType; + }; + template struct PointerTraits { + enum { result = true }; + typedef U PointeeType; + }; + +public: + enum { isStdUnsignedInt = TL::IndexOf::value >= 0 }; + enum { isStdSignedInt = TL::IndexOf::value >= 0 }; + enum { isStdIntegral = TL::IndexOf::value >= 0 || isStdUnsignedInt || isStdSignedInt }; + enum { isStdFloat = TL::IndexOf::value >= 0 }; + enum { isPointer = PointerTraits::result }; + enum { isStdArith = isStdIntegral || isStdFloat }; + + // best parameter type for given type + typedef typename Select::Result ParameterType; +}; + +// ----------------------------------------------------------------------- +}; // namespace android + +#endif /* ANDROID_TRAITS_H */ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/vec.h b/src/fusion-sensor/rotation_vector/fusion_utils/vec.h new file mode 100644 index 0000000..ec689c9 --- /dev/null +++ b/src/fusion-sensor/rotation_vector/fusion_utils/vec.h @@ -0,0 +1,440 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * 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. + */ + +// released in android-11.0.0_r9 + +#ifndef ANDROID_VEC_H +#define ANDROID_VEC_H + +#include + +#include +#include + +#include "traits.h" + +// ----------------------------------------------------------------------- + +#define PURE __attribute__((pure)) + +namespace android { + +// ----------------------------------------------------------------------- +// non-inline helpers + +template +class vec; + +template +struct vbase; + +namespace helpers { + +template inline T min(T a, T b) { return a inline T max(T a, T b) { return a>b ? a : b; } + +template < template class VEC, + typename TYPE, size_t SIZE, size_t S> +vec& doAssign( + vec& lhs, const VEC& rhs) { + const size_t minSize = min(SIZE, S); + const size_t maxSize = max(SIZE, S); + for (size_t i=0 ; i class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +VLHS PURE doAdd( + const VLHS& lhs, + const VRHS& rhs) { + VLHS r; + for (size_t i=0 ; i class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +VLHS PURE doSub( + const VLHS& lhs, + const VRHS& rhs) { + VLHS r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +VEC PURE doMulScalar( + const VEC& lhs, + typename TypeTraits::ParameterType rhs) { + VEC r; + for (size_t i=0 ; i class VEC, + typename TYPE, + size_t SIZE +> +VEC PURE doScalarMul( + typename TypeTraits::ParameterType lhs, + const VEC& rhs) { + VEC r; + for (size_t i=0 ; i. Without this, an extra conversion to vec<> would be needed. +// +// example: +// vec4_t a; +// vec3_t b; +// vec3_t c = a.xyz + b; +// +// "a.xyz + b" is a mixed-operation between a vbase<> and a vec<>, requiring +// a conversion of vbase<> to vec<>. The template gunk below avoids this, +// by allowing the addition on these different vector types directly +// + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS PURE operator + ( + const VLHS& lhs, + const VRHS& rhs) { + return helpers::doAdd(lhs, rhs); +} + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +inline VLHS PURE operator - ( + const VLHS& lhs, + const VRHS& rhs) { + return helpers::doSub(lhs, rhs); +} + +template < + template class VEC, + typename TYPE, + size_t SIZE +> +inline VEC PURE operator * ( + const VEC& lhs, + typename TypeTraits::ParameterType rhs) { + return helpers::doMulScalar(lhs, rhs); +} + +template < + template class VEC, + typename TYPE, + size_t SIZE +> +inline VEC PURE operator * ( + typename TypeTraits::ParameterType lhs, + const VEC& rhs) { + return helpers::doScalarMul(lhs, rhs); +} + + +template < + template class VLHS, + template class VRHS, + typename TYPE, + size_t SIZE +> +TYPE PURE dot_product( + const VLHS& lhs, + const VRHS& rhs) { + TYPE r(0); + for (size_t i=0 ; i class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length(const V& v) { + return sqrt(dot_product(v, v)); +} + +template < + template class V, + typename TYPE, + size_t SIZE +> +TYPE PURE length_squared(const V& v) { + return dot_product(v, v); +} + +template < + template class V, + typename TYPE, + size_t SIZE +> +V PURE normalize(const V& v) { + return v * (1/length(v)); +} + +template < + template class VLHS, + template class VRHS, + typename TYPE +> +VLHS PURE cross_product( + const VLHS& u, + const VRHS& v) { + VLHS r; + r.x = u.y*v.z - u.z*v.y; + r.y = u.z*v.x - u.x*v.z; + r.z = u.x*v.y - u.y*v.x; + return r; +} + + +template +vec PURE operator - (const vec& lhs) { + vec r; + for (size_t i=0 ; i +struct vbase { + TYPE v[SIZE]; + inline const TYPE& operator[](size_t i) const { return v[i]; } + inline TYPE& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[2]; + struct { float x, y; }; + struct { float s, t; }; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[3]; + struct { float x, y, z; }; + struct { float s, t, r; }; + vbase xy; + vbase st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; +template<> struct vbase { + union { + float v[4]; + struct { float x, y, z, w; }; + struct { float s, t, r, q; }; + vbase xyz; + vbase str; + vbase xy; + vbase st; + }; + inline const float& operator[](size_t i) const { return v[i]; } + inline float& operator[](size_t i) { return v[i]; } +}; + +// ----------------------------------------------------------------------- + +template +class vec : public vbase +{ + typedef typename TypeTraits::ParameterType pTYPE; + typedef vbase base; + +public: + // STL-like interface. + typedef TYPE value_type; + typedef TYPE& reference; + typedef TYPE const& const_reference; + typedef size_t size_type; + + typedef TYPE* iterator; + typedef TYPE const* const_iterator; + iterator begin() { return base::v; } + iterator end() { return base::v + SIZE; } + const_iterator begin() const { return base::v; } + const_iterator end() const { return base::v + SIZE; } + size_type size() const { return SIZE; } + + // ----------------------------------------------------------------------- + // default constructors + + vec() { } + vec(const vec& rhs) : base(rhs) { } + vec(const base& rhs) : base(rhs) { } // NOLINT(google-explicit-constructor) + + // ----------------------------------------------------------------------- + // conversion constructors + + vec(pTYPE rhs) { // NOLINT(google-explicit-constructor) + for (size_t i=0 ; i class VEC, size_t S> + explicit vec(const VEC& rhs) { + helpers::doAssign(*this, rhs); + } + + explicit vec(TYPE const* array) { + for (size_t i=0 ; i class VEC, size_t S> + vec& operator = (const VEC& rhs) { + return helpers::doAssign(*this, rhs); + } + + // ----------------------------------------------------------------------- + // operation-assignment + + vec& operator += (const vec& rhs); + vec& operator -= (const vec& rhs); + vec& operator *= (pTYPE rhs); + + // ----------------------------------------------------------------------- + // non-member function declaration and definition + // NOTE: we declare the non-member function as friend inside the class + // so that they are known to the compiler when the class is instantiated. + // This helps the compiler doing template argument deduction when the + // passed types are not identical. Essentially this helps with + // type conversion so that you can multiply a vec by an scalar int + // (for instance). + + friend inline vec PURE operator + (const vec& lhs, const vec& rhs) { + return helpers::doAdd(lhs, rhs); + } + friend inline vec PURE operator - (const vec& lhs, const vec& rhs) { + return helpers::doSub(lhs, rhs); + } + friend inline vec PURE operator * (const vec& lhs, pTYPE v) { + return helpers::doMulScalar(lhs, v); + } + friend inline vec PURE operator * (pTYPE v, const vec& rhs) { + return helpers::doScalarMul(v, rhs); + } + friend inline TYPE PURE dot_product(const vec& lhs, const vec& rhs) { + return android::dot_product(lhs, rhs); + } +}; + +// ----------------------------------------------------------------------- + +template +vec& vec::operator += (const vec& rhs) { + vec& lhs(*this); + for (size_t i=0 ; i +vec& vec::operator -= (const vec& rhs) { + vec& lhs(*this); + for (size_t i=0 ; i +vec& vec::operator *= (vec::pTYPE rhs) { + vec& lhs(*this); + for (size_t i=0 ; i vec2_t; +typedef vec vec3_t; +typedef vec vec4_t; + +// ----------------------------------------------------------------------- + +}; // namespace android + +#endif /* ANDROID_VEC_H */ diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/vector.cpp b/src/fusion-sensor/rotation_vector/fusion_utils/vector.cpp deleted file mode 100644 index d06ad7d..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/vector.cpp +++ /dev/null @@ -1,249 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#if defined (_VECTOR_H_) && defined (_MATRIX_H_) - -TYPE_SIZE vect::vect(void) -{ - for (int i = 0; i < SIZE; i++) - m_vec[i] = 0; -} - -TYPE_SIZE vect::vect(TYPE vec_data[SIZE]) -{ - for (int j = 0; j < SIZE; j++) - m_vec[j] = vec_data[j]; -} - -TYPE_SIZE vect::vect(const vect& v) -{ - for (int q = 0; q < SIZE; q++) - m_vec[q] = v.m_vec[q]; -} - - -TYPE_SIZE vect::~vect() -{ -} - -TYPE_SIZE vect vect::operator =(const vect& v) -{ - if (this == &v) - return *this; - - for (int q = 0; q < SIZE; q++) - m_vec[q] = v.m_vec[q]; - - return *this; -} - -T_S ostream& operator <<(ostream& dout, vect& v) -{ - for (int j = 0; j < S; j++) - dout << v.m_vec[j] << "\t"; - - dout << endl; - - return dout; -} - -T_S vect operator +(const vect v1, const vect v2) -{ - vect v3; - - for (int j = 0; j < S; j++) - v3.m_vec[j] = v1.m_vec[j] + v2.m_vec[j]; - - return v3; -} - -T_S vect operator +(const vect v, const T val) -{ - vect v1; - - for (int j = 0; j < S; j++) - v1.m_vec[j] = v.m_vec[j] + val; - - return v1; -} - -T_S vect operator -(const vect v1, const vect v2) -{ - vect v3; - - for (int j = 0; j < S; j++) - v3.m_vec[j] = v1.m_vec[j] - v2.m_vec[j]; - - return v3; -} - -T_S vect operator -(const vect v, const T val) -{ - vect v1; - - for (int j = 0; j < S; j++) - v1.m_vec[j] = v.m_vec[j] - val; - - return v1; -} - -T_S_R_C matrix operator *(const matrix m, const vect v) -{ - assert(R == S); - assert(C == 1); - - matrix m1; - - for (int i = 0; i < R; i++) - for (int j = 0; j < S; j++) - m1.m_mat[i][j] = m.m_mat[i][0] * v.m_vec[j]; - - return m1; -} - -T_S_R_C vect operator *(const vect v, const matrix m) -{ - assert(R == S); - assert(C != 1); - vect v1; - - for (int j = 0; j < C; j++) - { - v1.m_vec[j] = 0; - for (int k = 0; k < R; k++) - v1.m_vec[j] += v.m_vec[k] * m.m_mat[k][j]; - } - - return v1; -} - -T_S vect operator *(const vect v, const T val) -{ - vect v1; - - for (int j = 0; j < S; j++) - v1.m_vec[j] = v.m_vec[j] * val; - - return v1; -} - -T_S vect operator /(const vect v, const T val) -{ - vect v1; - - for (int j = 0; j < S; j++) - v1.m_vec[j] = v.m_vec[j] / val; - - return v1; -} - -T_S1_S2 bool operator ==(const vect v1, const vect v2) -{ - if (S1 == S2) { - for (int i = 0; i < S1; i++) - if (v1.m_vec[i] != v2.m_vec[i]) - return false; - return true; - } - - return false; -} - -T_S1_S2 bool operator !=(const vect v1, const vect v2) -{ - return (!(v1 == v2)); -} - -T_S matrix transpose(const vect v) -{ - matrix m; - - for (int i = 0; i < S; i++) - m.m_mat[i][0] = v.m_vec[i]; - - return m; -} - -T_S vect transpose(const matrix m) -{ - vect v; - - for (int i = 0; i < S; i++) - v.m_vec[i] = m.m_mat[i][0]; - - return v; -} - -T_S void insert_end(vect& v, T val) -{ - for (int i = 0; i < (S - 1); i++) - v.m_vec[i] = v.m_vec[i+1]; - - v.m_vec[S-1] = val; -} - -T_S vect cross(const vect v1, const vect v2) -{ - vect v3; - - 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])); - v3.m_vec[2] = ((v1.m_vec[0] * v2.m_vec[1]) - (v1.m_vec[1] * v2.m_vec[0])); - - return v3; -} - -T_S T dot(const vect v1, const vect v2) -{ - return (v1.m_vec[0] * v2.m_vec[0] + v1.m_vec[1] * v2.m_vec[1] + v1.m_vec[2] * v2.m_vec[2]); -} - -T_S bool is_initialized(const vect v) -{ - vect v1; - bool retval; - - retval = (v == v1) ? false : true; - - return retval; -} - -T_S T var(const vect v) -{ - T val = 0; - T mean, var, diff; - - for (int i = 0; i < S; i++) - val += v.m_vec[i]; - - mean = val / S; - - val = 0; - for (int i = 0; i < S; i++) - { - diff = (v.m_vec[i] - mean); - val += diff * diff; - } - - var = val / (S - 1); - - return var; -} -#endif - diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/vector.h b/src/fusion-sensor/rotation_vector/fusion_utils/vector.h deleted file mode 100644 index 4753984..0000000 --- a/src/fusion-sensor/rotation_vector/fusion_utils/vector.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - * sensord - * - * Copyright (c) 2014 Samsung Electronics Co., Ltd. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - -#ifndef _VECTOR_H_ -#define _VECTOR_H_ - -#include "matrix.h" - -#define TYPE_SIZE template -#define T_S template -#define T_S_R_C template -#define T_S1_S2 template - -TYPE_SIZE class vect { -public: - TYPE m_vec[SIZE]; - vect(void); - vect(TYPE vec_data[SIZE]); - vect(const vect& v); - ~vect(); - - vect operator =(const vect& v); - - T_S friend ostream& operator << (ostream& dout, vect& v); - T_S friend vect operator +(const vect v1, const vect v2); - T_S friend vect operator +(const vect v, const T val); - T_S friend vect operator -(const vect v1, const vect v2); - T_S friend vect operator -(const vect v, const T val); - T_S_R_C friend matrix operator *(const matrix v1, const vect v2); - T_S_R_C friend vect operator *(const vect v, const matrix m); - T_S friend vect operator *(const vect v, const T val); - T_S friend vect operator /(const vect v1, const T val); - T_S1_S2 friend bool operator ==(const vect v1, const vect v2); - T_S1_S2 friend bool operator !=(const vect v1, const vect v2); - - T_S friend void insert_end(vect& v, T val); - T_S friend matrix transpose(const vect v); - T_S friend vect transpose(const matrix m); - T_S friend vect cross(const vect v1, const vect v2); - T_S friend T dot(const vect v1, const vect v2); - T_S friend T var(const vect v); - T_S friend bool is_initialized(const vect v); -}; - -#include "vector.cpp" - -#endif /* _VECTOR_H_ */ - diff --git a/src/fusion-sensor/rotation_vector/gyro_fusion.cpp b/src/fusion-sensor/rotation_vector/gyro_fusion.cpp index a668e65..a813253 100644 --- a/src/fusion-sensor/rotation_vector/gyro_fusion.cpp +++ b/src/fusion-sensor/rotation_vector/gyro_fusion.cpp @@ -21,19 +21,9 @@ gyro_fusion::gyro_fusion() { + m_orientation_filter.init(1); } gyro_fusion::~gyro_fusion() { -} - -bool gyro_fusion::get_orientation(void) -{ - //_I("[fusion_sensor] : enable values are %d %d %d", m_enable_accel, m_enable_gyro, m_enable_magnetic); - if (!m_enable_accel || !m_enable_gyro) - return false; - - m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, NULL); - m_timestamp = fmax(m_accel.m_time_stamp, m_gyro.m_time_stamp); - return true; -} +} \ No newline at end of file diff --git a/src/fusion-sensor/rotation_vector/gyro_fusion.h b/src/fusion-sensor/rotation_vector/gyro_fusion.h index 9139e0d..c003e04 100644 --- a/src/fusion-sensor/rotation_vector/gyro_fusion.h +++ b/src/fusion-sensor/rotation_vector/gyro_fusion.h @@ -26,8 +26,6 @@ class gyro_fusion : public fusion_base { public: gyro_fusion(); ~gyro_fusion(); -private: - bool get_orientation(); }; #endif /* __GYRO_FUSION_H__ */ diff --git a/src/fusion-sensor/rotation_vector/gyro_magnetic_fusion.cpp b/src/fusion-sensor/rotation_vector/gyro_magnetic_fusion.cpp index 0d1f32d..d2a362f 100644 --- a/src/fusion-sensor/rotation_vector/gyro_magnetic_fusion.cpp +++ b/src/fusion-sensor/rotation_vector/gyro_magnetic_fusion.cpp @@ -21,20 +21,9 @@ gyro_magnetic_fusion::gyro_magnetic_fusion() { + m_orientation_filter.init(0); } gyro_magnetic_fusion::~gyro_magnetic_fusion() { -} - -bool gyro_magnetic_fusion::get_orientation(void) -{ - //_I("[fusion_sensor] : enable values are %d %d %d", m_enable_accel, m_enable_gyro, m_enable_magnetic); - if (!m_enable_accel || !m_enable_gyro || !m_enable_magnetic) - return false; - - m_orientation_filter.get_device_orientation(&m_accel, &m_gyro, &m_magnetic); - m_timestamp = fmax(m_accel.m_time_stamp, m_gyro.m_time_stamp); - m_timestamp = fmax(m_timestamp, m_magnetic.m_time_stamp); - return true; -} +} \ No newline at end of file diff --git a/src/fusion-sensor/rotation_vector/gyro_magnetic_fusion.h b/src/fusion-sensor/rotation_vector/gyro_magnetic_fusion.h index b95aafd..206750a 100644 --- a/src/fusion-sensor/rotation_vector/gyro_magnetic_fusion.h +++ b/src/fusion-sensor/rotation_vector/gyro_magnetic_fusion.h @@ -26,8 +26,6 @@ class gyro_magnetic_fusion : public fusion_base { public: gyro_magnetic_fusion(); ~gyro_magnetic_fusion(); -private: - bool get_orientation(); }; #endif /* __GYRO_MAGNETIC_FUSION_H__ */ diff --git a/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp b/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp index e954ddd..cc410da 100644 --- a/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp +++ b/src/fusion-sensor/rotation_vector/gyro_rv_sensor.cpp @@ -21,7 +21,6 @@ #include #include -#include #define NAME_SENSOR "http://tizen.org/sensor/general/gyroscope_rotation_vector/tizen_default" #define NAME_VENDOR "tizen.org" @@ -109,10 +108,10 @@ int gyro_rv_sensor::get_data(sensor_data_t **data, int *length) sensor_data->accuracy = m_accuracy; sensor_data->timestamp = m_time; sensor_data->value_count = 4; - sensor_data->values[0] = m_x; - sensor_data->values[1] = m_y; - sensor_data->values[2] = m_z; - sensor_data->values[3] = m_w; + sensor_data->values[0] = m_w; + sensor_data->values[1] = m_x; + sensor_data->values[2] = m_y; + sensor_data->values[3] = m_z; *data = sensor_data; *length = sizeof(sensor_data_t); diff --git a/src/fusion-sensor/rotation_vector/magnetic_fusion.cpp b/src/fusion-sensor/rotation_vector/magnetic_fusion.cpp index 966a247..a6bf61f 100644 --- a/src/fusion-sensor/rotation_vector/magnetic_fusion.cpp +++ b/src/fusion-sensor/rotation_vector/magnetic_fusion.cpp @@ -21,19 +21,9 @@ magnetic_fusion::magnetic_fusion() { + m_orientation_filter.init(2); } magnetic_fusion::~magnetic_fusion() { } - -bool magnetic_fusion::get_orientation(void) -{ - //_I("[fusion_sensor] : enable values are %d %d %d", m_enable_accel, m_enable_magnetic, m_enable_magnetic); - if (!m_enable_accel || !m_enable_magnetic) - return false; - - m_orientation_filter.get_device_orientation(&m_accel, NULL, &m_magnetic); - m_timestamp = fmax(m_accel.m_time_stamp, m_magnetic.m_time_stamp); - return true; -} diff --git a/src/fusion-sensor/rotation_vector/magnetic_fusion.h b/src/fusion-sensor/rotation_vector/magnetic_fusion.h index e5aca7e..8301a5e 100644 --- a/src/fusion-sensor/rotation_vector/magnetic_fusion.h +++ b/src/fusion-sensor/rotation_vector/magnetic_fusion.h @@ -26,8 +26,6 @@ class magnetic_fusion: public fusion_base { public: magnetic_fusion(); ~magnetic_fusion(); -private: - bool get_orientation(); }; #endif /* __MAGNETIC_FUSION_H__ */ diff --git a/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp b/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp index 892ef10..7c67727 100644 --- a/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp +++ b/src/fusion-sensor/rotation_vector/magnetic_rv_sensor.cpp @@ -21,7 +21,6 @@ #include #include -#include #define NAME_SENSOR "http://tizen.org/sensor/general/geomagnetic_rotation_vector/tizen_default" #define NAME_VENDOR "tizen.org" diff --git a/src/fusion-sensor/rotation_vector/rv_sensor.cpp b/src/fusion-sensor/rotation_vector/rv_sensor.cpp index 05d8ee4..9e447d6 100644 --- a/src/fusion-sensor/rotation_vector/rv_sensor.cpp +++ b/src/fusion-sensor/rotation_vector/rv_sensor.cpp @@ -21,7 +21,6 @@ #include #include -#include #define NAME_SENSOR "http://tizen.org/sensor/general/rotation_vector/tizen_default" #define NAME_VENDOR "tizen.org" -- 2.7.4 From 7591c7fe1e98195ad1f13038d1adbae732ab92ed Mon Sep 17 00:00:00 2001 From: "taemin.yeom" Date: Thu, 16 Sep 2021 16:53:04 +0900 Subject: [PATCH 15/16] Support new orientation sensor types - GYROSCOPE_ORIENTATION_SENSOR : based on gyroscope_rotation_vector - GEOMAGNETIC_ORIENTATION_SENSOR : based on geomagnetic_rotration_vector Change-Id: I604e4bb772274660be31077f30a3cdb051c9d342 Signed-off-by: taemin.yeom --- src/fusion-sensor/create.cpp | 4 + .../orientation/gyro_orientation_sensor.cpp | 111 ++++++++++++++++++++ .../orientation/gyro_orientation_sensor.h | 41 ++++++++ .../orientation/magnetic_orientation_sensor.cpp | 112 +++++++++++++++++++++ .../orientation/magnetic_orientation_sensor.h | 41 ++++++++ 5 files changed, 309 insertions(+) create mode 100644 src/fusion-sensor/orientation/gyro_orientation_sensor.cpp create mode 100644 src/fusion-sensor/orientation/gyro_orientation_sensor.h create mode 100644 src/fusion-sensor/orientation/magnetic_orientation_sensor.cpp create mode 100644 src/fusion-sensor/orientation/magnetic_orientation_sensor.h diff --git a/src/fusion-sensor/create.cpp b/src/fusion-sensor/create.cpp index 241a4cd..1d996fe 100644 --- a/src/fusion-sensor/create.cpp +++ b/src/fusion-sensor/create.cpp @@ -36,6 +36,8 @@ #endif #ifdef ENABLE_ORIENTATION #include "orientation/orientation_sensor.h" +#include "orientation/magnetic_orientation_sensor.h" +#include "orientation/gyro_orientation_sensor.h" #endif #ifdef ENABLE_PEDOMETER #include "pedometer/pedometer_sensor.h" @@ -87,6 +89,8 @@ extern "C" int create(fusion_sensor_t **fsensors) #ifdef ENABLE_ORIENTATION create_sensor("Orientation Sensor"); + create_sensor("Magnetic Orientation Sensor"); + create_sensor("Gyroscope Orientation Sensor"); #endif #ifdef ENABLE_PEDOMETER diff --git a/src/fusion-sensor/orientation/gyro_orientation_sensor.cpp b/src/fusion-sensor/orientation/gyro_orientation_sensor.cpp new file mode 100644 index 0000000..019162d --- /dev/null +++ b/src/fusion-sensor/orientation/gyro_orientation_sensor.cpp @@ -0,0 +1,111 @@ +/* + * sensord + * + * Copyright (c) 2016 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "gyro_orientation_sensor.h" + +#include +#include +#include + +#define NAME_SENSOR "http://tizen.org/sensor/general/gyroscope_orientation/tizen_default" +#define NAME_VENDOR "tizen.org" +#define SRC_ID_RV 0x1 +#define SRC_STR_RV "http://tizen.org/sensor/general/gyroscope_rotation_vector" + +static sensor_info2_t sensor_info = { + id: 0x1, + type: GYROSCOPE_ORIENTATION_SENSOR, + uri: NAME_SENSOR, + vendor: NAME_VENDOR, + min_range: -180, + max_range: 360, + resolution: 0.01, + min_interval: 10, + max_batch_count: 0, + wakeup_supported: false, + privilege:"", +}; + +static required_sensor_s required_sensors[] = { + {SRC_ID_RV, SRC_STR_RV}, +}; + + +gyro_orientation_sensor::gyro_orientation_sensor() +: m_azimuth(-1) +, m_pitch(-1) +, m_roll(-1) +, m_accuracy(-1) +, m_time(0) +, m_interval(0) +{ +} + +gyro_orientation_sensor::~gyro_orientation_sensor() +{ +} + +int gyro_orientation_sensor::get_sensor_info(const sensor_info2_t **info) +{ + *info = &sensor_info; + return OP_SUCCESS; +} + +int gyro_orientation_sensor::get_required_sensors(const required_sensor_s **sensors) +{ + *sensors = required_sensors; + return 1; +} + +int gyro_orientation_sensor::update(uint32_t id, sensor_data_t *data, int len) +{ + int error; + float azimuth, pitch, roll; + + error = quat_to_orientation(data->values, azimuth, pitch, roll); + retv_if(error, OP_ERROR); + + m_azimuth = azimuth; + m_pitch = pitch; + m_roll = roll; + m_time = data->timestamp; + m_accuracy = data->accuracy; + + _D("[gyro_orientation] : [%10f] [%10f] [%10f]", m_azimuth, m_pitch, m_roll); + return OP_SUCCESS; +} + +int gyro_orientation_sensor::get_data(sensor_data_t **data, int *length) +{ + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + retvm_if(!sensor_data, -ENOMEM, "Failed to allocate memory"); + + sensor_data->accuracy = m_accuracy; + sensor_data->timestamp = m_time; + sensor_data->value_count = 3; + sensor_data->values[0] = m_azimuth; + sensor_data->values[1] = m_pitch; + sensor_data->values[2] = m_roll; + + *data = sensor_data; + *length = sizeof(sensor_data_t); + + return 0; +} diff --git a/src/fusion-sensor/orientation/gyro_orientation_sensor.h b/src/fusion-sensor/orientation/gyro_orientation_sensor.h new file mode 100644 index 0000000..10438cc --- /dev/null +++ b/src/fusion-sensor/orientation/gyro_orientation_sensor.h @@ -0,0 +1,41 @@ +/* + * sensord + * + * Copyright (c) 2016 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +class gyro_orientation_sensor : public fusion_sensor { +public: + gyro_orientation_sensor(); + virtual ~gyro_orientation_sensor(); + + int get_sensor_info(const sensor_info2_t **info); + int get_required_sensors(const required_sensor_s **sensors); + + int update(uint32_t id, sensor_data_t *data, int len); + int get_data(sensor_data_t **data, int *len); + +private: + float m_azimuth; + float m_pitch; + float m_roll; + int m_accuracy; + unsigned long long m_time; + unsigned long m_interval; +}; \ No newline at end of file diff --git a/src/fusion-sensor/orientation/magnetic_orientation_sensor.cpp b/src/fusion-sensor/orientation/magnetic_orientation_sensor.cpp new file mode 100644 index 0000000..66d435f --- /dev/null +++ b/src/fusion-sensor/orientation/magnetic_orientation_sensor.cpp @@ -0,0 +1,112 @@ +/* + * sensord + * + * Copyright (c) 2016 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "magnetic_orientation_sensor.h" + +#include +#include +#include + +#define NAME_SENSOR "http://tizen.org/sensor/general/geomagnetic_orientation/tizen_default" +#define NAME_VENDOR "tizen.org" + +#define SRC_ID_RV 0x1 +#define SRC_STR_RV "http://tizen.org/sensor/general/geomagnetic_rotation_vector" + +static sensor_info2_t sensor_info = { + id: 0x1, + type: GEOMAGNETIC_ORIENTATION_SENSOR, + uri: NAME_SENSOR, + vendor: NAME_VENDOR, + min_range: -180, + max_range: 360, + resolution: 0.01, + min_interval: 10, + max_batch_count: 0, + wakeup_supported: false, + privilege:"", +}; + +static required_sensor_s required_sensors[] = { + {SRC_ID_RV, SRC_STR_RV}, +}; + + +magnetic_orientation_sensor::magnetic_orientation_sensor() +: m_azimuth(-1) +, m_pitch(-1) +, m_roll(-1) +, m_accuracy(-1) +, m_time(0) +, m_interval(0) +{ +} + +magnetic_orientation_sensor::~magnetic_orientation_sensor() +{ +} + +int magnetic_orientation_sensor::get_sensor_info(const sensor_info2_t **info) +{ + *info = &sensor_info; + return OP_SUCCESS; +} + +int magnetic_orientation_sensor::get_required_sensors(const required_sensor_s **sensors) +{ + *sensors = required_sensors; + return 1; +} + +int magnetic_orientation_sensor::update(uint32_t id, sensor_data_t *data, int len) +{ + int error; + float azimuth, pitch, roll; + + error = quat_to_orientation(data->values, azimuth, pitch, roll); + retv_if(error, OP_ERROR); + + m_azimuth = azimuth; + m_pitch = pitch; + m_roll = roll; + m_time = data->timestamp; + m_accuracy = data->accuracy; + + _D("[magnetic_orientation] : [%10f] [%10f] [%10f]", m_azimuth, m_pitch, m_roll); + return OP_SUCCESS; +} + +int magnetic_orientation_sensor::get_data(sensor_data_t **data, int *length) +{ + sensor_data_t *sensor_data; + sensor_data = (sensor_data_t *)malloc(sizeof(sensor_data_t)); + retvm_if(!sensor_data, -ENOMEM, "Failed to allocate memory"); + + sensor_data->accuracy = m_accuracy; + sensor_data->timestamp = m_time; + sensor_data->value_count = 3; + sensor_data->values[0] = m_azimuth; + sensor_data->values[1] = m_pitch; + sensor_data->values[2] = m_roll; + + *data = sensor_data; + *length = sizeof(sensor_data_t); + + return 0; +} diff --git a/src/fusion-sensor/orientation/magnetic_orientation_sensor.h b/src/fusion-sensor/orientation/magnetic_orientation_sensor.h new file mode 100644 index 0000000..982247d --- /dev/null +++ b/src/fusion-sensor/orientation/magnetic_orientation_sensor.h @@ -0,0 +1,41 @@ +/* + * sensord + * + * Copyright (c) 2016 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +class magnetic_orientation_sensor : public fusion_sensor { +public: + magnetic_orientation_sensor(); + virtual ~magnetic_orientation_sensor(); + + int get_sensor_info(const sensor_info2_t **info); + int get_required_sensors(const required_sensor_s **sensors); + + int update(uint32_t id, sensor_data_t *data, int len); + int get_data(sensor_data_t **data, int *len); + +private: + float m_azimuth; + float m_pitch; + float m_roll; + int m_accuracy; + unsigned long long m_time; + unsigned long m_interval; +}; -- 2.7.4 From 9320b5fa8bb0d9db650f23f6b29a20d91aa1e51f Mon Sep 17 00:00:00 2001 From: "taemin.yeom" Date: Wed, 13 Oct 2021 16:57:43 +0900 Subject: [PATCH 16/16] Improve correctness in orientation calculation -fix gravity constant with HAL -change reference not to be used in function args to prevent changing values by other threads Change-Id: I39c14d37490803c9712958d6f2dc464f026b07c8 Signed-off-by: taemin.yeom --- .../rotation_vector/fusion_utils/orientation_filter.cpp | 8 ++++---- .../rotation_vector/fusion_utils/orientation_filter.h | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp index 5df2f08..fb5e6c0 100644 --- a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp +++ b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.cpp @@ -68,7 +68,7 @@ static const float SYMMETRY_TOLERANCE = 1e-10f; * ill-conditioning and div by zeros. * Threshhold: 10% of g, in m/s^2 */ -static const float NOMINAL_GRAVITY = 9.81f; +static const float NOMINAL_GRAVITY = 9.80665f; static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY); /* @@ -308,14 +308,14 @@ bool orientation_filter::checkInitComplete(int what, const vec3_t& d, float dT) return false; } -void orientation_filter::handleGyro(const vec3_t& w, float dT) { +void orientation_filter::handleGyro(const vec3_t w, float dT) { if (!checkInitComplete(GYRO, w, dT)) return; predict(w, dT); } -status_t orientation_filter::handleAcc(const vec3_t& a, float dT) { +status_t orientation_filter::handleAcc(const vec3_t a, float dT) { if (!checkInitComplete(ACC, a, dT)) return BAD_VALUE; @@ -348,7 +348,7 @@ status_t orientation_filter::handleAcc(const vec3_t& a, float dT) { return NO_ERROR; } -status_t orientation_filter::handleMag(const vec3_t& m) { +status_t orientation_filter::handleMag(const vec3_t m) { if (!checkInitComplete(MAG, m)) return BAD_VALUE; diff --git a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h index 0ff3ea5..24b2077 100644 --- a/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h +++ b/src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h @@ -64,9 +64,9 @@ class orientation_filter { public: orientation_filter(); void init(int mode = FUSION_9AXIS); - void handleGyro(const vec3_t& w, float dT); - status_t handleAcc(const vec3_t& a, float dT); - status_t handleMag(const vec3_t& m); + void handleGyro(const vec3_t w, float dT); + status_t handleAcc(const vec3_t a, float dT); + status_t handleMag(const vec3_t m); vec4_t getAttitude() const; vec3_t getBias() const; mat33_t getRotationMatrix() const; -- 2.7.4