Filtering the direction value received from sensor

Change-Id: I2172110caadcff124986e25b9a9885fa0af0e52e
Signed-off-by: Michal Skorupinski <m.skorupinsk@samsung.com>

diff --git a/src/model/model_sensors.c b/src/model/model_sensors.c
index 4ce4a75..1f335e6 100644 (file)
--- a/src/model/model_sensors.c
+++ b/src/model/model_sensors.c
@@ -19,6 +19,15 @@
 #include "model/model_sensors.h"
 #include "log.h"
 
+#define VALUE_LIST_SIZE 10
+
+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;
 
@@ -26,15 +35,16 @@ typedef struct _s_model_sensors {
        sensor_listener_h listener;
 
        float initial_sensor_data[3];
-
-
+       s_data_history velocity;
+       s_data_history direction;
 } s_model_sensors;
 
 static s_model_sensors s_info = {
                .initial_sensor_data = { 0, },
+               .direction = { 0, },
 };
 
-static inline float _length(float *vector, int count)
+static inline float _vector_length(float *vector, int count)
 {
        float len = 0;
        int i = 0;
@@ -46,6 +56,14 @@ static inline float _length(float *vector, int count)
        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 inline float _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;
@@ -59,20 +77,41 @@ static inline float _range_map(float value, float input_min, float input_max, fl
        return result;
 }
 
+static void _update_data_history(float value, s_data_history *parameter)
+{
+       parameter->sum = parameter->sum - parameter->array[parameter->current_index] + value;
+       parameter->array[parameter->current_index] = value;
+       ++parameter->current_index;
+       parameter->current_index %= VALUE_LIST_SIZE;
+}
+
+static inline float _get_average_parameter_from_history(float value, s_data_history *parameter)
+{
+       _update_data_history(value, parameter);
+       return parameter->sum / VALUE_LIST_SIZE;
+}
+
 static void _sensor_event_cb(sensor_h sensor, sensor_event_s *event, void *data)
 {
        static s_model_sensors_cb_data model_data = {
                        .type = MODEL_TYPE_UPDATE,
        };
 
-       float len = _length(event->values, 3);
-       float direction = (event->values[0] - s_info.initial_sensor_data[0]) / len;
-       float velocity  = (event->values[1] - s_info.initial_sensor_data[1]) / len;
+       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);
+
+       float direction = vector[0];
+       float velocity  = vector[1];
+
+       direction = _get_average_parameter_from_history(direction, &s_info.direction);
 
-       model_data.direction = _range_map(direction, -0.80f, 0.80f, -1.0f, 1.0f);
+       model_data.direction = _range_map(direction, -8.00f, 8.00f, -1.0f, 1.0f);
        model_data.velocity  = velocity;
 
-       _D("MODEL VALUES: % .4f % .4f -> % .4f % .4f", direction, velocity, model_data.direction, model_data.velocity);
+       _D("MODEL VALUES{%f}: dir:% .4f ranged:% .4f", len, direction, model_data.direction);
 
        if (s_info.sensors_update_cb) {
                s_info.sensors_update_cb(&model_data);

diff --git a/src/view/view_racing.c b/src/view/view_racing.c
index 29735b2..38d99fb 100644 (file)
--- a/src/view/view_racing.c
+++ b/src/view/view_racing.c
@@ -45,10 +45,7 @@ static s_view_racing s_info = {
 
 static float _calculate_angle(float value, float min, float max)
 {
-       float x = sinf(value);
-
-       float y = (x-(-1)) * ((max - min) / (1 - (-1))) + min;
-
+       float y = (value-(-1)) * ((max - min) / (1 - (-1))) + min;
        return y;
 }
 
@@ -96,11 +93,11 @@ static void _controller_cb(void *data)
 {
        s_controller_data *controller_data = (s_controller_data *)data;
 
-       _D("VIEW VALUES: %f %f", controller_data->direction, controller_data->velocity);
-
        float angle = _calculate_angle(controller_data->direction, s_info.dir_min_angle, s_info.dir_max_angle);
        view_base_set_angle(s_info.direction, angle, 180.0, 180.0);
 
+       _D("VIEW VALUES: %f %f DIR: %f", controller_data->direction, controller_data->velocity, angle);
+
        angle = _calculate_angle(controller_data->velocity, s_info.vel_min_angle, s_info.vel_max_angle);
        view_base_set_angle(s_info.velocity, angle, 180.0, 180.0);
 }