From f152a9d20563f372b3d721433067814c3f14da5f Mon Sep 17 00:00:00 2001 From: Krzysztof Wieclaw Date: Fri, 24 Aug 2018 12:58:36 +0200 Subject: [PATCH] Added velocity control based on gravity sensor Change-Id: I09506c04029463f0eb86f4573b51d30ec1b98bb7 Signed-off-by: Krzysztof Wieclaw --- .cproject | 1778 +++++++++++--------------------- inc/math_helper.h | 12 + inc/model/model_sensors.h | 2 + src/controller/controller_name_input.c | 1 - src/controller/controller_racing.c | 8 +- src/math_helper.c | 163 ++- src/model/model_sensors.c | 208 +++- 7 files changed, 919 insertions(+), 1253 deletions(-) diff --git a/.cproject b/.cproject index e7dd90c..99facaa 100644 --- a/.cproject +++ b/.cproject @@ -1,1190 +1,594 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/inc/math_helper.h b/inc/math_helper.h index 7f80311..9eca756 100644 --- a/inc/math_helper.h +++ b/inc/math_helper.h @@ -19,5 +19,17 @@ #define MATH_HELPER_H_ float math_helper_range_map(float value, float input_min, float input_max, float output_min, float output_max); +float vector_length(float *vector, int size); +void vector_scalar_multiplication(float *vector_in, float scalar, float *vector_out, int size); +void vector_diff(float *vector_1, float *vector_2, float *vector_out, int size); +void vector_sum(float *vector_1, float *vector_2, float *vector_out, int size); +void vector_normalize(float *vector_in, float *vector_out, int size); +float vector_dot_product(float *vector_1, float *vector_2, int size); +void vector3_cross_product(float *vector_1, float *vector_2, float *vector_out); +void vector_matrix_multiply(float *matrix, float *vector_in, float *vector_out, int size); +void matrix_scalar_mulitply(float *matrix_in, float scalar, float *matrix_out, int rows, int columns); +void matrix_mulitply(float *matrix_1, float *matrix_2, float *matrix_out, int matrix1_rows, int common_dimension, int matrix2_columns); +void matrix_add(float *matrix_1, float *matrix_2, float *matrix_out, int rows, int columns); +void vector3_rotation_matrix(float *vector_1, float *vector_2, float matrix_out[3][3]); #endif /* MATH_HELPER_H_ */ diff --git a/inc/model/model_sensors.h b/inc/model/model_sensors.h index 4f5a0a0..73a2c36 100644 --- a/inc/model/model_sensors.h +++ b/inc/model/model_sensors.h @@ -33,5 +33,7 @@ void model_sensors_init(void); void model_sensors_unsubscirbe_event(void); void model_sensors_subscribe_event(t_model_sensors_update_cb model_update_cb); void model_sensors_set_initial_values(void); +void model_sensors_sensor_accelerometer_set_initial_values(void); +void model_sensors_sensor_gravity_set_initial_values(void); #endif /* MODEL_MODEL_SENSORS_H_ */ diff --git a/src/controller/controller_name_input.c b/src/controller/controller_name_input.c index 852c90c..2230cb6 100644 --- a/src/controller/controller_name_input.c +++ b/src/controller/controller_name_input.c @@ -15,7 +15,6 @@ */ - #include "gear-racing-controller.h" #include "model/model_cloud_connection.h" #include "controller/controller_name_input.h" diff --git a/src/controller/controller_racing.c b/src/controller/controller_racing.c index 03faeba..8940359 100644 --- a/src/controller/controller_racing.c +++ b/src/controller/controller_racing.c @@ -43,16 +43,11 @@ static void _model_connected_cb(s_model_car_connection_cb_data *model_data) static void _sensors_cb(s_model_sensors_cb_data *model_data) { s_info.controller_data.direction = model_data->direction; + s_info.controller_data.throttle = model_data->velocity; s_info.controller_data.cam_elevation = model_data->cam_elevation; s_info.base_controller.view_update_cb(&s_info.controller_data); } -static void _hw_cb(s_model_hw_cb_data *model_data) -{ - s_info.controller_data.throttle = model_data->throttle; - s_info.base_controller.view_update_cb(&s_info.controller_data); -} - void controller_racing_set_stop(bool stop) { model_car_connection_set_stop(stop); @@ -71,7 +66,6 @@ void controller_racing_init(t_view_update_cb view_update_cb) model_car_connection_subscribe_event(_model_connected_cb); model_sensors_subscribe_event(_sensors_cb); - model_hw_subscribe_event(_hw_cb); model_sensors_set_initial_values(); model_car_connection_ready_to_drive(true); diff --git a/src/math_helper.c b/src/math_helper.c index 169f96b..31342cb 100644 --- a/src/math_helper.c +++ b/src/math_helper.c @@ -16,9 +16,11 @@ #include "math_helper.h" +#include + float math_helper_range_map(float value, float input_min, float input_max, float output_min, float output_max) { - float result = ((value - input_min) / (input_max - input_min)) * (output_max - output_min) + output_min; + float result = ((value - input_min) / (input_max - input_min)) * (output_max - output_min) + output_min; if (result < output_min) { result = output_min; @@ -29,3 +31,162 @@ float math_helper_range_map(float value, float input_min, float input_max, float return result; } +float vector_length(float *vector, int size) +{ + float len = 0; + int i = 0; + + for (i = 0; i < size; ++i) { + len += vector[i] * vector[i]; + } + + return sqrtf(len); +} + +void vector_scalar_multiplication(float *vector_in, float scalar, float *vector_out, int size) +{ + for (int i = 0; i < size; ++i) { + vector_out[i] = vector_in[i] * scalar; + } +} + +void vector_diff(float *vector_1, float *vector_2, float *vector_out, int size) +{ + for (int i = 0; i < size; ++i) { + vector_out[i] = vector_1[i] - vector_2[i]; + } +} + +void vector_sum(float *vector_1, float *vector_2, float *vector_out, int size) +{ + for (int i = 0; i < size; ++i) { + vector_out[i] = vector_1[i] + vector_2[i]; + } +} + +void vector_normalize(float *vector_in, float *vector_out, int size) +{ + vector_scalar_multiplication(vector_in, 1.0f / vector_length(vector_in, size), vector_out, size); +} + +float vector_dot_product(float *vector_1, float *vector_2, int size) +{ + float sum = 0; + for (int i = 0; i < size; ++i) { + sum += vector_1[i] * vector_2[i]; + } + return sum; +} + +void vector3_cross_product(float *vector_1, float *vector_2, float *vector_out) +{ + vector_out[0] = vector_1[1] * vector_2[2] - vector_1[2] * vector_2[1]; + vector_out[1] = vector_1[2] * vector_2[0] - vector_1[0] * vector_2[2]; + vector_out[2] = vector_1[0] * vector_2[1] - vector_1[1] * vector_2[0]; +} + +static void __vector_matrix_multiply(float *vector_in, float *vector_out, int size, float matrix[size][size]) +{ + for (int i = 0; i < size; ++i) { + vector_out[i] = 0; + for (int j = 0; j < size; ++j) { + vector_out[i] += matrix[i][j] * vector_in[j]; + } + } +} + +void vector_matrix_multiply(float *matrix, float *vector_in, float *vector_out, int size) +{ + __vector_matrix_multiply(vector_in, vector_out, size, (float (*)[size]) matrix); +} + +static void __matrix_scalar_multiply(int rows, int columns, float matrix_in[][columns], float scalar, float matrix_out[][columns]) +{ + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < columns; ++j) { + matrix_out[i][j] = matrix_in[i][j] * scalar; + } + } +} + +void matrix_scalar_mulitply(float *matrix_in, float scalar, float *matrix_out, int rows, int columns) +{ + __matrix_scalar_multiply(rows, columns, (float (*)[columns]) matrix_in, scalar, (float (*)[columns]) matrix_out); +} + +static void __matrix_multiply(int matrix1_rows, int common_dimension, int matrix2_columns, float matrix_1[][common_dimension], float matrix_2[][matrix2_columns], float matrix_out[][matrix2_columns]) +{ + for (int i = 0; i < matrix1_rows; ++i) { + for (int j = 0; j < matrix2_columns; ++j) { + matrix_out[i][j] = 0; + for (int k = 0; k < common_dimension; ++k) { + matrix_out[i][j] += matrix_1[i][k] * matrix_2[k][j]; + } + } + } +} + +void matrix_mulitply(float *matrix_1, float *matrix_2, float *matrix_out, int matrix1_rows, int common_dimension, int matrix2_columns) +{ + __matrix_multiply(matrix1_rows, common_dimension, matrix2_columns, (float (*)[common_dimension]) matrix_1, (float (*)[matrix2_columns]) matrix_2, (float (*)[matrix2_columns]) matrix_out); +} + +static void _vector3_skew_matrix(float *vector_in, float matrix_out[3][3]) +{ + matrix_out[0][0] = 0; + matrix_out[0][1] = -vector_in[2]; + matrix_out[0][2] = vector_in[1]; + matrix_out[1][0] = vector_in[2]; + matrix_out[1][1] = 0; + matrix_out[1][2] = -vector_in[0]; + matrix_out[2][0] = -vector_in[1]; + matrix_out[2][1] = vector_in[0]; + matrix_out[2][2] = 0; +} + +static void __matrix_add(int rows, int columns, float matrix_1[rows][columns], float matrix_2[rows][columns], float matrix_out[rows][columns]) +{ + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < columns; ++j) { + matrix_out[i][j] = matrix_1[i][j] + matrix_2[i][j]; + } + } +} + +void matrix_add(float *matrix_1, float *matrix_2, float *matrix_out, int rows, int columns) +{ + __matrix_add(rows, columns, (float (*)[columns]) matrix_1, (float (*)[columns]) matrix_2, (float (*)[columns]) matrix_out); +} + +void vector3_rotation_matrix(float *vector_1, float *vector_2, float matrix_out[3][3]) +{ + float v[3]; + float v1[3]; + float v2[3]; + vector_normalize(vector_1, &v1[0], 3); + vector_normalize(vector_2, &v2[0], 3); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + matrix_out[i][j] = i == j ? 1.0f : 0.0f; + } + } + vector3_cross_product(v1, v2, &v[0]); + + float s = vector_length(&v[0], 3); + if (s == 0) { + if (v1[0] != v2[0]) { + for (int i = 0; i < 3; ++i) { + matrix_out[i][i] = -1; + } + } + return; + } + float c = vector_dot_product(v1, v2, 3); + float vs[3][3]; + float vs2[3][3]; + _vector3_skew_matrix(v, vs); + matrix_mulitply(&vs[0][0], &vs[0][0], &vs2[0][0], 3, 3, 3); + matrix_scalar_mulitply(&vs2[0][0], 1 / (1 + c), &vs2[0][0], 3, 3); + matrix_add(&matrix_out[0][0], &vs[0][0], &matrix_out[0][0], 3, 3); + matrix_add(&matrix_out[0][0], &vs2[0][0], &matrix_out[0][0], 3, 3); +} diff --git a/src/model/model_sensors.c b/src/model/model_sensors.c index 75ce73c..07b9719 100644 --- a/src/model/model_sensors.c +++ b/src/model/model_sensors.c @@ -21,60 +21,62 @@ #include "log.h" #include "math_helper.h" -#define VALUE_LIST_SIZE 10 +#define VALUE_LIST_SIZE 50 +#define GRAVITY_LISTENER_INTERVAL 10 +#define ACCELEROMETER_LISTENER_INTERVAL 10 +#define LINEAR_LISTENER_INTERVAL 10 + +#define GRAVITY_DEADZONE 0.2f +#define MAX_GRAVITY 0.7f +#define MAX_GRAVITY_WITH_DEADZONE (MAX_GRAVITY - GRAVITY_DEADZONE) #define DIRECTION_DEADZONE 1.0f #define MAX_DIRECTION 8.0f #define MAX_DIRECTION_WITH_DEADZONE (MAX_DIRECTION - DIRECTION_DEADZONE) +#define DEADZONE 1.0f +#define MAX_ROTATION 8.0f +#define MAX_ROTATION_WITH_DEADZONE (MAX_ROTATION - DEADZONE) + #define CAM_ELEVATION_DEADZONE 3.0f #define MAX_CAM_ELEVATION 5.0f #define MAX_CAM_ELEVATION_WITH_DEADZONE (MAX_CAM_ELEVATION - CAM_ELEVATION_DEADZONE) - typedef struct _s_data_history { int current_index; float sum; float array[VALUE_LIST_SIZE]; } s_data_history; - typedef struct _s_model_sensors { t_model_sensors_update_cb sensors_update_cb; - sensor_h sensor; - sensor_listener_h listener; + sensor_h sensor_accelerometer; + sensor_listener_h listener_accelerometer; - float initial_sensor_data[3]; - s_data_history cam_elevation; + sensor_h sensor_gravity; + sensor_listener_h listener_gravity; + + sensor_h sensor_linear; + sensor_listener_h listener_linear; + + float initial_sensor_accelerometer_data[3]; + float initial_sensor_gravity_data[3]; + float last_linear_event_values[3]; + float velocity_reference_vector[3]; + s_model_sensors_cb_data last_model_data; + s_data_history velocity; s_data_history direction; + s_data_history cam_elevation; } s_model_sensors; static s_model_sensors s_info = { - .initial_sensor_data = { 0, }, - .direction = { 0, }, + .initial_sensor_accelerometer_data = { 0, }, + .initial_sensor_gravity_data = { 0, }, + .last_linear_event_values = { 0, }, + .velocity_reference_vector = { 0, }, }; -static inline float _vector_length(float *vector, int count) -{ - float len = 0; - int i = 0; - - for (i = 0; i < count; ++i) { - len += vector[i] * vector[i]; - } - - return sqrtf(len); -} - -static inline void _vector_diff(float *vector_1, float *vector_2, float *vector_out, int count) -{ - int i; - for (i = 0; i < count; ++i) { - vector_out[i] = vector_1[i] - vector_2[i]; - } -} - static void _update_data_history(float value, s_data_history *parameter) { parameter->sum = parameter->sum - parameter->array[parameter->current_index] + value; @@ -89,44 +91,39 @@ static inline float _get_average_parameter_from_history(float value, s_data_hist return parameter->sum / VALUE_LIST_SIZE; } -float _get_position_in_deadzone(float direction, float deadzone) +float _get_position_in_deadzone(float value, float deadzone) { - if (fabsf(direction) <= deadzone) { - direction = 0; - } else if (direction > 0) { - direction = direction - deadzone; + if (fabsf(value) <= deadzone) { + value = 0; + } else if (value > 0) { + value = value - deadzone; } else { - direction = direction + deadzone; + value = value + deadzone; } - return direction; + return value; } -static void _sensor_event_cb(sensor_h sensor, sensor_event_s *event, void *data) +static void _sensor_accelerometer_event_cb(sensor_h sensor_accelerometer, sensor_event_s *event, void *data) { - static s_model_sensors_cb_data model_data = { - .type = MODEL_TYPE_UPDATE, - }; - float vector[3] = { 0,}; - float len = 0; - _vector_diff(event->values, s_info.initial_sensor_data, &vector[0], 3); - len = _vector_length(&vector[0], 3); + vector_diff(event->values, s_info.initial_sensor_accelerometer_data, &vector[0], 3); float direction = vector[0]; float cam_elevation = vector[1]; - direction = _get_average_parameter_from_history(direction, &s_info.direction); cam_elevation = _get_average_parameter_from_history(cam_elevation, &s_info.cam_elevation); direction = _get_position_in_deadzone(direction, DIRECTION_DEADZONE); cam_elevation = _get_position_in_deadzone(cam_elevation, CAM_ELEVATION_DEADZONE); - model_data.direction = math_helper_range_map(direction, -MAX_DIRECTION_WITH_DEADZONE, MAX_DIRECTION_WITH_DEADZONE, -1.0f, 1.0f); - model_data.cam_elevation += math_helper_range_map(cam_elevation, -MAX_CAM_ELEVATION_WITH_DEADZONE, MAX_CAM_ELEVATION_WITH_DEADZONE, -0.03f, 0.03f); + s_info.last_model_data.direction = math_helper_range_map(direction, -MAX_DIRECTION_WITH_DEADZONE, MAX_DIRECTION_WITH_DEADZONE, -1.0f, 1.0f); + s_info.last_model_data.cam_elevation += math_helper_range_map(cam_elevation, -MAX_CAM_ELEVATION_WITH_DEADZONE, MAX_CAM_ELEVATION_WITH_DEADZONE, -0.03f, 0.03f); - if(model_data.cam_elevation < 0) { + s_model_sensors_cb_data model_data = s_info.last_model_data; + + if (model_data.cam_elevation < 0) { model_data.cam_elevation = 0; } else if (model_data.cam_elevation > 1) { model_data.cam_elevation = 1; @@ -135,22 +132,90 @@ static void _sensor_event_cb(sensor_h sensor, sensor_event_s *event, void *data) model_car_connection_send_direction(model_data.direction); model_car_connection_send_cam_elevation(-model_data.cam_elevation); + _D("MODEL VALUES: dir:% .4f ranged:% .4f", direction, model_data.direction); + _D("MODEL VALUES: cam_elevation:% .4f ranged:% .4f", cam_elevation, model_data.cam_elevation); + if (s_info.sensors_update_cb) { + s_info.sensors_update_cb(&model_data); + } +} + +static void _sensor_gravity_event_cb(sensor_h sensor_gravity, sensor_event_s *event, void *data) +{ + float vector[3] = { 0, 0, 0}; + + //Adding linear acceleration to reduce noise appearing during move + vector_sum(&event->values[0], &s_info.last_linear_event_values[0], &vector[0], 3); + + vector_normalize(&vector[0], &vector[0], 3); + + float velocity = vector_dot_product(vector, s_info.velocity_reference_vector, 3); + velocity = _get_average_parameter_from_history(velocity, &s_info.velocity); + velocity = _get_position_in_deadzone(velocity, GRAVITY_DEADZONE); + s_info.last_model_data.velocity = math_helper_range_map(velocity, -MAX_GRAVITY_WITH_DEADZONE, MAX_GRAVITY_WITH_DEADZONE, -1.0f, 1.0f); + + s_model_sensors_cb_data model_data = s_info.last_model_data; + + model_car_connection_send_throttle(model_data.velocity); + _D("MODEL VALUES: velocity:% .4f ranged:% .4f", velocity, model_data.velocity); if (s_info.sensors_update_cb) { s_info.sensors_update_cb(&model_data); } } +static void _sensor_linear_event_cb(sensor_h sensor_linear, sensor_event_s *event, void *data) +{ + memcpy(s_info.last_linear_event_values, event->values, sizeof(s_info.last_linear_event_values)); +} + void model_sensors_set_initial_values(void) { + model_sensors_sensor_accelerometer_set_initial_values(); + model_sensors_sensor_gravity_set_initial_values(); + s_info.last_model_data.type = MODEL_TYPE_UPDATE; + s_info.last_model_data.direction = 0.0f; + s_info.last_model_data.velocity = 0.0f; + s_info.last_model_data.cam_elevation = 0.0f; +} + +void model_sensors_sensor_accelerometer_set_initial_values(void) +{ sensor_event_s event = { 0, }; - int ret = sensor_listener_read_data(s_info.listener, &event); + int ret = sensor_listener_read_data(s_info.listener_accelerometer, &event); ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); - memcpy(s_info.initial_sensor_data, event.values, sizeof(s_info.initial_sensor_data)); + memcpy(s_info.initial_sensor_accelerometer_data, event.values, sizeof(s_info.initial_sensor_accelerometer_data)); + + _D("Initial sensor_accelerometer data: {%f, %f, %f}", s_info.initial_sensor_accelerometer_data[0], s_info.initial_sensor_accelerometer_data[1], + s_info.initial_sensor_accelerometer_data[2]); +} + +void model_sensors_sensor_gravity_set_initial_values(void) +{ + sensor_event_s event = { 0, }; + + int ret = sensor_listener_read_data(s_info.listener_gravity, &event); + ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); - _D("Initial sensor data: {%f, %f, %f}", s_info.initial_sensor_data[0], s_info.initial_sensor_data[1], - s_info.initial_sensor_data[2]); + memcpy(s_info.initial_sensor_gravity_data, event.values, sizeof(s_info.initial_sensor_gravity_data)); + + //In further calculation there is assumption, that normalized initial gravity vector was [0,0,1] (device facing sky) + //However, user can start in different position, so we have to calculate rotation matrix to change base for reference vector + float reference_vector[3] = {0, 0, vector_length(s_info.initial_sensor_gravity_data, 3)}; + _D("Caluclating rotation matrix from [% .4f, % .4f, % .4f] to [% .4f,% .4f,% .4f]", s_info.initial_sensor_gravity_data[0], s_info.initial_sensor_gravity_data[1], + s_info.initial_sensor_gravity_data[2], reference_vector[0], reference_vector[1], reference_vector[2]); + float rotation_matrix[3][3]; + vector3_rotation_matrix(s_info.initial_sensor_gravity_data, reference_vector, rotation_matrix); + + //With previous assumption, the greatest speed should be achievable with vector [0,1,0] (device facing walls). + reference_vector[0] = 0; + reference_vector[1] = -1; + reference_vector[2] = 0; + vector_matrix_multiply(&rotation_matrix[0][0], &reference_vector[0], &s_info.velocity_reference_vector[0], 3); + + _D("Initial sensor_gravity data: {%f, %f, %f}", s_info.initial_sensor_gravity_data[0], s_info.initial_sensor_gravity_data[1], + s_info.initial_sensor_gravity_data[2]); + _D("Initial velovity refernce vector: {%f, %f, %f}", s_info.velocity_reference_vector[0], s_info.velocity_reference_vector[1], s_info.velocity_reference_vector[2]); } void model_sensors_init(void) @@ -161,16 +226,46 @@ void model_sensors_init(void) ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); ASSERT(!supported, "Sensor %d is not supported", SENSOR_ACCELEROMETER) - ret = sensor_get_default_sensor(SENSOR_ACCELEROMETER, &s_info.sensor); + ret = sensor_get_default_sensor(SENSOR_ACCELEROMETER, &s_info.sensor_accelerometer); + ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); + + ret = sensor_create_listener(s_info.sensor_accelerometer, &s_info.listener_accelerometer); + ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); + + ret = sensor_listener_set_event_cb(s_info.listener_accelerometer, ACCELEROMETER_LISTENER_INTERVAL, _sensor_accelerometer_event_cb, NULL); + ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); + + sensor_listener_start(s_info.listener_accelerometer); + + ret = sensor_is_supported(SENSOR_GRAVITY, &supported); + ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); + ASSERT(!supported, "Sensor %d is not supported", SENSOR_GRAVITY) + + ret = sensor_get_default_sensor(SENSOR_GRAVITY, &s_info.sensor_gravity); + ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); + + ret = sensor_create_listener(s_info.sensor_gravity, &s_info.listener_gravity); ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); - ret = sensor_create_listener(s_info.sensor, &s_info.listener); + ret = sensor_listener_set_event_cb(s_info.listener_gravity, GRAVITY_LISTENER_INTERVAL, _sensor_gravity_event_cb, NULL); ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); - ret = sensor_listener_set_event_cb(s_info.listener, 1000, _sensor_event_cb, NULL); + sensor_listener_start(s_info.listener_gravity); + + ret = sensor_is_supported(SENSOR_LINEAR_ACCELERATION, &supported); + ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); + ASSERT(!supported, "Sensor %d is not supported", SENSOR_LINEAR_ACCELERATION) + + ret = sensor_get_default_sensor(SENSOR_LINEAR_ACCELERATION, &s_info.sensor_linear); + ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); + + ret = sensor_create_listener(s_info.sensor_linear, &s_info.listener_linear); + ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); + + ret = sensor_listener_set_event_cb(s_info.listener_linear, LINEAR_LISTENER_INTERVAL, _sensor_linear_event_cb, NULL); ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE); - sensor_listener_start(s_info.listener); + sensor_listener_start(s_info.listener_linear); model_sensors_set_initial_values(); } @@ -184,4 +279,3 @@ void model_sensors_unsubscirbe_event(void) { s_info.sensors_update_cb = NULL; } - -- 2.7.4