src/model/model_car_connection.c

   1 /*
   2 * Copyright (c) 2018 Samsung Electronics Co., Ltd.
   3 *
   4 * Licensed under the Flora License, Version 1.1 (the License);
   5 * you may not use this file except in compliance with the License.
   6 * You may obtain a copy of the License at
   7 *
   8 * http://floralicense.org/license/
   9 *
  10 * Unless required by applicable law or agreed to in writing, software
  11 * distributed under the License is distributed on an AS IS BASIS,
  12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13 * See the License for the specific language governing permissions and
  14 * limitations under the License.
  15 */
  16 #include <stdlib.h>
  17 #include <stdio.h>
  18 #include <glib.h>
  19 #include <gio/gio.h>
  20 #include <sys/types.h>
  21 #include <sys/socket.h>
  22 #include <netinet/in.h>
  23
  24 #include "gear-racing-controller.h"
  25 #include "model/model_car_connection.h"
  26 #include "log.h"
  27
  28 #define BASE_VALUE 1000
  29 #define HELLO_TIMER_WAIT 3.0
  30
  31 typedef enum  _e_message_type {
  32         MESSAGE_HELLO,
  33         MESSAGE_CALIBRATION,
  34         MESSAGE_ALIVE,
  35         MESSAGE_DRIVE,
  36         MESSAGE_BYE,
  37 } e_message_type;
  38
  39 typedef struct _s_message {
  40         unsigned long long int seq_num;
  41         int cmd;
  42         int servo;
  43         int speed;
  44         unsigned long long int time;
  45 } s_message;
  46
  47 typedef struct _s_car_model_connection {
  48         t_model_car_connection_update_cb controller_update_cb;
  49
  50         GSocket *socket;
  51         GSocketAddress *car_address;
  52         gint port;
  53         guint io_watch_id;
  54
  55         Ecore_Timer *connection_wait_timer;
  56
  57         bool ready_to_drive;
  58
  59         float direction;
  60         float throttle;
  61         bool stop;
  62
  63         Ecore_Timer *send_timer;
  64 } s_model_car_connection;
  65
  66 s_model_car_connection s_info = { 0, };
  67
  68
  69 gboolean _read_socket_cb(GIOChannel *source, GIOCondition condition, gpointer data)
  70 {
  71         GIOChannel *channel = data;
  72         GString *buffer = g_string_new(NULL);
  73         GIOStatus state = G_IO_STATUS_NORMAL;
  74         GError *error = NULL;
  75
  76         while (state == G_IO_STATUS_NORMAL)
  77         {
  78                 gsize delim = 0;
  79
  80                 state = g_io_channel_read_line_string(channel, buffer, &delim, &error);
  81
  82                 if (error) {
  83                         g_warning("%s\n", error->message);
  84                 }
  85
  86                 switch(state)
  87                 {
  88                         case G_IO_STATUS_NORMAL:
  89                                 if (strlen(buffer->str) > 0)
  90                                 {
  91                                         _D("Got: %s\n", buffer->str);
  92                                         if (!strncmp(buffer->str, "PING!", strlen("PING!")))
  93                                         {
  94                                                         _D("Received reply\n");
  95                                         }
  96                                         else if (!strncmp(buffer->str, "Welcome to the echo service!", strlen("Welcome to the echo service!")))
  97                                         {
  98                                                         _D("Service online\n");
  99                                         }
 100
 101                                         /* reset the buffer for the next call */
 102                                         g_string_set_size(buffer, 0);
 103                                         delim = 0;
 104                                 }
 105                                 break;
 106                         case G_IO_STATUS_AGAIN:
 107                                 /* no data right now... try again later */
 108                                 break;
 109                         case G_IO_STATUS_ERROR:
 110                         case G_IO_STATUS_EOF:
 111                                 _W("Error reading: %s\n", error->message);
 112
 113                                 /* drop last reference on connection */
 114                                 g_io_channel_shutdown(channel, FALSE, &error);
 115
 116                                 /* don't need the input buffer anymore */
 117                                 g_string_free(buffer, TRUE);
 118
 119                                 /* remove the event source */
 120                                 return(FALSE);
 121                 }
 122         }
 123
 124          return true;
 125 }
 126
 127 static void _receiver_udp_start(void)
 128 {
 129         int socket_fd = g_socket_get_fd(s_info.socket);
 130         GIOChannel *channel = g_io_channel_unix_new(socket_fd);
 131         s_info.io_watch_id = g_io_add_watch(channel, G_IO_IN | G_IO_ERR | G_IO_HUP, _read_socket_cb, channel);
 132         g_io_channel_unref(channel);
 133 }
 134
 135 static void _send_data(int command, int servo, int speed)
 136 {
 137         GError *error = NULL;
 138         static s_message message = { 0, };
 139
 140         message.cmd = command;
 141         message.servo = servo;
 142         message.speed = speed;
 143         message.seq_num++;
 144         message.time = time(NULL);
 145
 146         _D("Send CMD[%d], Servo[%d], Speed[%d]", message.cmd, message.servo, message.speed);
 147
 148         ASSERT(!s_info.socket, "Socket not created");
 149         ASSERT(!s_info.car_address, "Destination not set");
 150
 151         GIOCondition condition = g_socket_condition_check(s_info.socket, G_IO_OUT);
 152
 153         if (condition != G_IO_OUT) {
 154                 _W("condition == %d", condition);
 155 //              return;
 156         }
 157
 158         bool connection_result = g_socket_check_connect_result(s_info.socket, &error);
 159         ASSERT(error, "g_socket_check_connect_result(). Error: %s", error->message);
 160         ASSERT(!connection_result, "connection_result == false")
 161
 162         gssize size = g_socket_send(s_info.socket, (const gchar *)&message, sizeof(s_message), NULL, &error);
 163         ASSERT(size < 0, "Failed to send to socket: %s", error->message)
 164
 165         if (error) {
 166                 g_error_free(error);
 167         }
 168 }
 169
 170 static Eina_Bool _connection_wait_timer_cb(void *data)
 171 {
 172         static s_model_car_connection_cb_data model_data = {
 173                         .type = MODEL_TYPE_FAIL,
 174         };
 175
 176         return ECORE_CALLBACK_CANCEL; //TODO Test only
 177
 178         if (s_info.controller_update_cb) {
 179                 s_info.controller_update_cb(&model_data);
 180         }
 181
 182         model_car_connection_end_connection();
 183
 184         bool renew = (bool)data;
 185         return renew;
 186 }
 187
 188 static Eina_Bool _send_timer_cb(void *data)
 189 {
 190         if (s_info.stop) {
 191                 return ECORE_CALLBACK_RENEW;
 192         }
 193
 194         _D("DATA: %f %f", s_info.direction, s_info.throttle);
 195         _send_data(MESSAGE_DRIVE, s_info.direction * -BASE_VALUE, s_info.throttle * 4095);
 196
 197         return ECORE_CALLBACK_RENEW;
 198 }
 199
 200
 201 void model_car_connection_init(void)
 202 {
 203         GError *error = NULL;
 204
 205         s_info.socket = g_socket_new(G_SOCKET_FAMILY_IPV4, G_SOCKET_TYPE_DATAGRAM, G_SOCKET_PROTOCOL_UDP, &error);
 206         ASSERT(!s_info.socket, "Failed to get new Socket - %s", error->message);
 207
 208         if (error) {
 209                 g_error_free(error);
 210         }
 211
 212         _receiver_udp_start();
 213 }
 214
 215 void model_car_connection_subscribe_event(t_model_car_connection_update_cb model_update_cb)
 216 {
 217         s_info.controller_update_cb = model_update_cb;
 218 }
 219
 220 void model_car_connection_model_state_change(void)
 221 {
 222         static s_model_car_connection_cb_data model_data = {
 223                         .type = MODEL_TYPE_END,
 224         };
 225
 226         s_info.controller_update_cb(&model_data);
 227 }
 228 void model_car_connection_unsubscirbe_event()
 229 {
 230         s_info.controller_update_cb = NULL;
 231 }
 232
 233 void model_car_connection_start_connection(char *address, int port)
 234 {
 235         s_info.port = port;
 236         s_info.car_address = g_inet_socket_address_new_from_string(address, port);
 237         ASSERT_MEMORY(!s_info.car_address);
 238
 239         GError *error = NULL;
 240         bool connected = g_socket_connect(s_info.socket, s_info.car_address, NULL, &error);
 241         ASSERT(error, "g_socket_connect(): %s", error->message);
 242         ASSERT(!connected, "Failed to connect to socket");
 243
 244         _send_data(MESSAGE_HELLO, 0, 0);
 245         s_info.connection_wait_timer = ecore_timer_add(HELLO_TIMER_WAIT, _connection_wait_timer_cb, (void *)ECORE_CALLBACK_CANCEL);
 246
 247 //      static s_model_car_connection_cb_data model_data = {
 248 //                      .type = MODEL_TYPE_END,
 249 //      };
 250
 251 //      s_info.controller_update_cb(&model_data);
 252
 253         s_info.send_timer = ecore_timer_add(1.0 / 60.0, _send_timer_cb, NULL);
 254
 255         if (error) {
 256                 g_error_free(error);
 257         }
 258 }
 259
 260 void model_car_connection_ready_to_drive(bool is_ready)
 261 {
 262         s_info.ready_to_drive = is_ready;
 263 }
 264
 265 void model_car_connection_end_connection(void)
 266 {
 267         if (!s_info.socket) {
 268                 return;
 269         }
 270
 271         GError *error = NULL;
 272         g_socket_shutdown(s_info.socket, true, true, &error);
 273         ASSERT(error, "Failed to shutdown the socket");
 274
 275         g_object_unref(s_info.car_address);
 276
 277         if (error) {
 278                 g_error_free(error);
 279         }
 280
 281         ecore_timer_del(s_info.send_timer);
 282 }
 283
 284 void model_car_connection_send_direction(float direction)
 285 {
 286         if (!s_info.ready_to_drive || s_info.stop) {
 287                 return;
 288         }
 289
 290         s_info.direction = direction;
 291 //      _send_data(MESSAGE_DRIVE, s_info.direction * -BASE_VALUE, s_info.throttle * -BASE_VALUE);
 292 }
 293
 294 void model_car_connection_send_throttle(float throttle)
 295 {
 296         if (!s_info.ready_to_drive || s_info.stop) {
 297                 return;
 298         }
 299
 300         s_info.throttle = throttle;
 301 //      _send_data(MESSAGE_DRIVE, s_info.direction * -BASE_VALUE, s_info.throttle * -BASE_VALUE);
 302 }
 303
 304 void model_car_connection_set_stop(bool stop)
 305 {
 306         s_info.stop = stop;
 307
 308         if (stop) {
 309                 _send_data(MESSAGE_DRIVE, 0, 0);
 310         }
 311 }
 312