Filtering the direction value received from sensor

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

2 files changed, 50 insertions, 14 deletions

diff --git a/src/model/model_sensors.c b/src/model/model_sensors.c
index 4ce4a75..1f335e6 100644
--- 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
--- 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);
 }