Camera controll

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

7 files changed, 88 insertions, 31 deletions

diff --git a/inc/controller/controller_racing.h b/inc/controller/controller_racing.h
index 20e4214..6380f7e 100644
--- a/inc/controller/controller_racing.h
+++ b/inc/controller/controller_racing.h
@@ -23,6 +23,7 @@
 typedef struct _s_controller_data {
 	float throttle;
 	float direction;
+	float cam_elevation;
 } s_controller_data;
 
 void controller_racing_init(t_view_update_cb view_update_cb);
@@ -30,5 +31,6 @@ void controller_racing_destroy(void);
 void controller_racing_back(void);
 void controller_racing_next();
 void controller_racing_set_stop(bool stop);
+void controller_racing_set_cam_azimuth(float cam_azimuth);
 
 #endif /* CONTROLLER_CONTROLLER_RACING_H_ */
diff --git a/inc/model/model_car_connection.h b/inc/model/model_car_connection.h
index a39c2e8..964b22e 100644
--- a/inc/model/model_car_connection.h
+++ b/inc/model/model_car_connection.h
@@ -38,5 +38,7 @@ void model_car_connection_send_direction(float direction);
 void model_car_connection_send_throttle(float throttle);
 void model_car_connection_ready_to_drive(bool is_ready);
 void model_car_connection_set_stop(bool stop);
+void model_car_connection_send_cam_elevation(float cam_elevation);
+void model_car_connection_send_cam_azimuth(float cam_azimuth);
 
 #endif /* MODEL_MODEL_CAR_CONNECTION_H_ */
diff --git a/inc/model/model_sensors.h b/inc/model/model_sensors.h
index e9868b0..4f5a0a0 100644
--- a/inc/model/model_sensors.h
+++ b/inc/model/model_sensors.h
@@ -23,6 +23,7 @@
 typedef struct _s_model_sensors_cb_data {
 	e_model_type type;
 	float velocity;
+	float cam_elevation;
 	float direction;
 } s_model_sensors_cb_data;
 
diff --git a/src/controller/controller_racing.c b/src/controller/controller_racing.c
index 8f039ca..03faeba 100644
--- a/src/controller/controller_racing.c
+++ b/src/controller/controller_racing.c
@@ -43,6 +43,7 @@ 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.cam_elevation = model_data->cam_elevation;
 	s_info.base_controller.view_update_cb(&s_info.controller_data);
 }
 
@@ -59,6 +60,11 @@ void controller_racing_set_stop(bool stop)
 
 }
 
+void controller_racing_set_cam_azimuth(float cam_azimuth)
+{
+	model_car_connection_send_cam_azimuth(cam_azimuth);
+}
+
 void controller_racing_init(t_view_update_cb view_update_cb)
 {
 	s_info.base_controller.view_update_cb = view_update_cb;
diff --git a/src/model/model_car_connection.c b/src/model/model_car_connection.c
index 293d782..6bb9684 100644
--- a/src/model/model_car_connection.c
+++ b/src/model/model_car_connection.c
@@ -25,7 +25,7 @@
 #include "model/model_car_connection.h"
 #include "log.h"
 
-#define DIRECTIO_BASE_VALUE 1000
+#define DIRECTION_BASE_VALUE 1000
 #define THROTTLE_BASE_VALUE 10000
 #define HELLO_TIMER_WAIT 3.0
 
@@ -34,6 +34,8 @@ typedef enum  _e_message_type {
 	MESSAGE_CALIBRATION,
 	MESSAGE_ALIVE,
 	MESSAGE_DRIVE,
+	MESSAGE_CAMERA,
+	MESSAGE_DRIVE_CAMERA,
 	MESSAGE_BYE,
 } e_message_type;
 
@@ -42,6 +44,8 @@ typedef struct _s_message {
 	int cmd;
 	int servo;
 	int speed;
+	int cam_azimuth;
+	int cam_elevation;
 	unsigned long long int time;
 } s_message;
 
@@ -59,6 +63,8 @@ typedef struct _s_car_model_connection {
 
 	float direction;
 	float throttle;
+	float cam_elevation;
+	float cam_azimuth;
 	bool stop;
 
 	Ecore_Timer *send_timer;
@@ -133,7 +139,7 @@ static void _receiver_udp_start(void)
 	g_io_channel_unref(channel);
 }
 
-static void _send_data(int command, int servo, int speed)
+static void _send_data(int command, int servo, int speed, int cam_azimuth, int cam_elevation)
 {
 	GError *error = NULL;
 	static s_message message = { 0, };
@@ -141,10 +147,12 @@ static void _send_data(int command, int servo, int speed)
 	message.cmd = command;
 	message.servo = servo;
 	message.speed = speed;
+	message.cam_azimuth = cam_azimuth;
+	message.cam_elevation = cam_elevation;
 	message.seq_num++;
 	message.time = time(NULL);
 
-	_D("Send CMD[%d], Servo[%d], Speed[%d]", message.cmd, message.servo, message.speed);
+	_D("Send CMD[%d], Servo[%d], Speed[%d]", message.cmd, message.servo, message.speed, message.cam_azimuth, message.cam_elevation);
 
 	ASSERT(!s_info.socket, "Socket not created");
 	ASSERT(!s_info.car_address, "Destination not set");
@@ -188,8 +196,8 @@ static Eina_Bool _connection_wait_timer_cb(void *data)
 
 static Eina_Bool _send_timer_cb(void *data)
 {
-//	_D("DATA: %f %f", s_info.direction, s_info.throttle);
-	_send_data(MESSAGE_DRIVE, s_info.direction * -DIRECTIO_BASE_VALUE, s_info.throttle * THROTTLE_BASE_VALUE);
+	_send_data(MESSAGE_DRIVE_CAMERA, s_info.direction * -DIRECTION_BASE_VALUE, s_info.throttle * THROTTLE_BASE_VALUE,
+			s_info.cam_azimuth * -DIRECTION_BASE_VALUE, s_info.cam_elevation * -DIRECTION_BASE_VALUE);
 
 	return ECORE_CALLBACK_RENEW;
 }
@@ -238,15 +246,9 @@ void model_car_connection_start_connection(char *address, int port)
 	ASSERT(error, "g_socket_connect(): %s", error->message);
 	ASSERT(!connected, "Failed to connect to socket");
 
-	_send_data(MESSAGE_HELLO, 0, 0);
+	_send_data(MESSAGE_HELLO, 0, 0, 0, 0);
 	s_info.connection_wait_timer = ecore_timer_add(HELLO_TIMER_WAIT, _connection_wait_timer_cb, (void *)ECORE_CALLBACK_CANCEL);
 
-//	static s_model_car_connection_cb_data model_data = {
-//			.type = MODEL_TYPE_END,
-//	};
-
-//	s_info.controller_update_cb(&model_data);
-
 	s_info.send_timer = ecore_timer_add(1.0 / 60.0, _send_timer_cb, NULL);
 
 	if (error) {
@@ -300,6 +302,24 @@ void model_car_connection_send_throttle(float throttle)
 	}
 }
 
+void model_car_connection_send_cam_elevation(float cam_elevation)
+{
+	if (!s_info.ready_to_drive) {
+		return;
+	}
+
+	s_info.cam_elevation = cam_elevation;
+}
+
+void model_car_connection_send_cam_azimuth(float cam_azimuth)
+{
+	if (!s_info.ready_to_drive) {
+		return;
+	}
+
+	s_info.cam_azimuth = cam_azimuth;
+}
+
 void model_car_connection_set_stop(bool stop)
 {
 	s_info.stop = stop;
diff --git a/src/model/model_sensors.c b/src/model/model_sensors.c
index ce625e1..75ce73c 100644
--- a/src/model/model_sensors.c
+++ b/src/model/model_sensors.c
@@ -22,9 +22,14 @@
 #include "math_helper.h"
 
 #define VALUE_LIST_SIZE 10
-#define DEADZONE 1.0f
-#define MAX_ROTATION 8.0f
-#define MAX_ROTATION_WITH_DEADZONE (MAX_ROTATION - DEADZONE)
+
+#define DIRECTION_DEADZONE 1.0f
+#define MAX_DIRECTION 8.0f
+#define MAX_DIRECTION_WITH_DEADZONE (MAX_DIRECTION - DIRECTION_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 {
@@ -41,7 +46,7 @@ typedef struct _s_model_sensors {
 	sensor_listener_h listener;
 
 	float initial_sensor_data[3];
-	s_data_history velocity;
+	s_data_history cam_elevation;
 	s_data_history direction;
 } s_model_sensors;
 
@@ -84,6 +89,19 @@ 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)
+{
+	if (fabsf(direction) <= deadzone) {
+		direction = 0;
+	} else if (direction > 0) {
+		direction = direction - deadzone;
+	} else {
+		direction = direction + deadzone;
+	}
+
+	return direction;
+}
+
 static void _sensor_event_cb(sensor_h sensor, sensor_event_s *event, void *data)
 {
 	static s_model_sensors_cb_data model_data = {
@@ -97,23 +115,25 @@ static void _sensor_event_cb(sensor_h sensor, sensor_event_s *event, void *data)
 	len = _vector_length(&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);
 
-	if (fabsf(direction) <= DEADZONE) {
-		direction = 0;
-	} else if (direction > 0) {
-		direction = direction - DEADZONE;
-	} else {
-		direction = direction + DEADZONE;
-	}
+	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_ROTATION_WITH_DEADZONE, MAX_ROTATION_WITH_DEADZONE, -1.0f, 1.0f);
-	model_data.velocity  = 0;
+	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);
 
-//	_D("MODEL VALUES{%f}: dir:% .4f ranged:% .4f", len, direction, model_data.direction);
+	if(model_data.cam_elevation < 0) {
+		model_data.cam_elevation = 0;
+	} else if (model_data.cam_elevation > 1) {
+		model_data.cam_elevation = 1;
+	}
 
 	model_car_connection_send_direction(model_data.direction);
+	model_car_connection_send_cam_elevation(-model_data.cam_elevation);
 
 	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 b588ede..f2468af 100644
--- a/src/view/view_racing.c
+++ b/src/view/view_racing.c
@@ -112,6 +112,9 @@ static void _controller_cb(void *data)
 	angle = math_helper_range_map(fabsf(controller_data->throttle), 0, 1, s_info.vel_min_angle, s_info.vel_max_angle);
 	view_base_set_angle(s_info.velocity, angle, 180.0, 180.0);
 
+	angle = math_helper_range_map(controller_data->cam_elevation, 0, 1, 0, 90);
+	view_base_set_angle(s_info.camera_elevation_indicator, 90 - angle, 180.0, 180.0);
+
 	if (controller_data->throttle < 0) {
 		evas_object_color_set(s_info.velocity, 0, 255, 255, 255);
 	} else {
@@ -165,17 +168,20 @@ static Eina_Bool _camera_azimuth_animator_cb(void *data)
 		s_info.cam_azimuth = 0;
 	}
 
-
 	if (s_info.cam_azimuth < -1) {
-		s_info.cam_azimuth = 1;
-	} else if (s_info.cam_azimuth > 1) {
 		s_info.cam_azimuth = -1;
+	} else if (s_info.cam_azimuth > 1) {
+		s_info.cam_azimuth = 1;
 	}
 
-	float angle = math_helper_range_map(s_info.cam_azimuth, -1, 1, -180.0f, 180.0f);
+	controller_racing_set_cam_azimuth(s_info.cam_azimuth);
+
+	float angle = math_helper_range_map(s_info.cam_azimuth, -1, 1, -90.0f, 90.0f);
 	view_base_set_angle(s_info.camera_azimuth_indicator, angle, 180.0, 180.0);
 	_D("[CAM] AZIMUTH: % f", s_info.cam_azimuth);
 
+
+
 	return ECORE_CALLBACK_RENEW;
 }
 
@@ -231,7 +237,7 @@ static void _view_racing_create_gui(Evas_Object *parent)
 	evas_object_color_set(s_info.camera_azimuth_indicator, 0, 255, 255, 255);
 
 	s_info.camera_elevation_indicator = _create_image(parent, 180, -5, 17, 17,
-			180.0f,
+			90.0f,
 			"images/circle.png", HORIZONTAL_ALIGN_CENTER, VERTICAL_ALIGN_TOP);
 	evas_object_color_set(s_info.camera_elevation_indicator, 255, 0, 0, 255);