src/app.c

   1 /*
   2  * Copyright (c) 2017 Samsung Electronics Co., Ltd.
   3  *
   4  * Contact: Jeonghoon Park <jh1979.park@samsung.com>
   5  *
   6  * Licensed under the Flora License, Version 1.1 (the License);
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * http://floralicense.org/license/
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an AS IS BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  */
  18
  19 #include <stdio.h>
  20 #include <stdlib.h>
  21 #include <unistd.h>
  22 #include <math.h>
  23 #include <glib.h>
  24 #include <service_app.h>
  25 #include "log.h"
  26 #include "resource.h"
  27 #include "net-util.h"
  28 #include "config.h"
  29 #include "cloud/cloud_communication.h"
  30 #include "messages/message_manager.h"
  31 #include "controller_connection_manager.h"
  32 #include "lap_counter/lap_counter.h"
  33 #include "command.h"
  34
  35 #define ENABLE_MOTOR 1
  36 #define STERING_SERVO_CENTER 340
  37 #define STERING_SERVO_RANGE 125
  38
  39 #define MAX_UDP_INPUT 10000
  40
  41 #define CONFIG_GRP_CAR "Car"
  42 #define CONFIG_GRP_STEERING "Steering"
  43 #define CONFIG_GRP_CAMERA "Camera"
  44 #define CONFIG_GRP_ENGINE "Engine"
  45 #define CONFIG_GRP_RPI "Rpi"
  46
  47 #define CONFIG_KEY_ID "Id"
  48 #define CONFIG_KEY_NAME "Name"
  49
  50 #define CONFIG_KEY_STEERING_CENTER "Center"
  51 #define CONFIG_KEY_STEERING_RANGE "Range"
  52 #define CONFIG_KEY_CAMERA_AZIMUTH_CENTER "Azimuth.Center"
  53 #define CONFIG_KEY_CAMERA_AZIMUTH_RANGE "Azimuth.Range"
  54 #define CONFIG_KEY_CAMERA_ELEVATION_MIN "Elevation.Min"
  55 #define CONFIG_KEY_CAMERA_ELEVATION_MAX "Elevation.Max"
  56 #define CONFIG_KEY_CAMERA_ENGINE_MIN "Min"
  57 #define CONFIG_KEY_CAMERA_ENGINE_MAX "Max"
  58
  59 #define CONFIG_KEY_RPI_PIN_STERING "Stering"
  60 #define CONFIG_KEY_RPI_PIN_CAMERA_AZIMUTH "Azimuth"
  61 #define CONFIG_KEY_RPI_PIN_CAMERA_ELEVATION "Elevation"
  62
  63 #define CONFIG_KEY_RPI_PIN_ENGINE_1_FORWARD "1.forward"
  64 #define CONFIG_KEY_RPI_PIN_ENGINE_1_BACK "1.back"
  65 #define CONFIG_KEY_RPI_ENGINE_CHANNEL_1 "Channel.1"
  66 #define CONFIG_KEY_RPI_PIN_ENGINE_2_FORWARD "2.forward"
  67 #define CONFIG_KEY_RPI_PIN_ENGINE_2_BACK "2.back"
  68 #define CONFIG_KEY_RPI_ENGINE_CHANNEL_2 "Channel.2"
  69
  70 #define CLOUD_REQUESTS_FREQUENCY 15
  71
  72 #define ELEVATION_MIN 200
  73 #define ELEVATION_MAX 400
  74 #define AZIMUTH_MIN 200
  75 #define AZIMUTH_MAX 700
  76
  77 #define CONFIG_LED_STATE_KEY_INIT "init"
  78 #define CONFIG_DEFAULT_LED_3BIT_INIT 1, 1, 0
  79 #define CONFIG_DEFAULT_LED_24BIT_INIT 255, 32, 0
  80
  81 #define CONFIG_LED_STATE_KEY_OFF "off"
  82 #define CONFIG_DEFAULT_LED_3BIT_OFF 0, 0, 0
  83 #define CONFIG_DEFAULT_LED_24BIT_OFF 0, 0, 0
  84
  85 enum {
  86         DIR_STATE_S,
  87         DIR_STATE_F,
  88         DIR_STATE_B,
  89 };
  90
  91 typedef struct app_data_s {
  92         unsigned int f_value;
  93         unsigned int r_value;
  94         unsigned int dir_state;
  95         const char *user_name;
  96         guint idle_h;
  97
  98         int stering_center;
  99         int stering_range;
 100
 101         int camera_azimuth_center;
 102         int camera_azimuth_range;
 103
 104         int camera_elevation_min;
 105         int camera_elevation_max;
 106
 107         int engine_min;
 108         int engine_max;
 109
 110         int stering_pin;
 111
 112         int elevation_pin;
 113         int azimuth_pin;
 114
 115         int engine_1_forward_pin;
 116         int engine_1_back_pin;
 117         int engine_1_channel;
 118
 119         int engine_2_forward_pin;
 120         int engine_2_back_pin;
 121         int engine_2_channel;
 122 } app_data;
 123
 124 static app_data s_info = {
 125                 .stering_center = STERING_SERVO_CENTER,
 126                 .stering_range = STERING_SERVO_RANGE,
 127
 128                 .camera_azimuth_center = (AZIMUTH_MAX + AZIMUTH_MIN) /2,
 129                 .camera_azimuth_range = AZIMUTH_MAX - ((AZIMUTH_MAX + AZIMUTH_MIN) /2),
 130
 131                 .camera_elevation_min = ELEVATION_MIN,
 132                 .camera_elevation_max = ELEVATION_MAX,
 133
 134                 .engine_min = -4095,
 135                 .engine_max = 4095,
 136
 137                 .stering_pin = 0,
 138
 139                 .elevation_pin = 14,
 140                 .azimuth_pin = 15,
 141
 142                 .engine_1_forward_pin = 6,
 143                 .engine_1_back_pin = 5,
 144                 .engine_1_channel = 5,
 145
 146                 .engine_2_forward_pin = 21,
 147                 .engine_2_back_pin = 20,
 148                 .engine_2_channel = 4,
 149 };
 150
 151 static void _initialize_components(app_data *ad);
 152 static void _initialize_config();
 153
 154 static void service_app_lang_changed(app_event_info_h event_info, void *user_data)
 155 {
 156         return;
 157 }
 158
 159 static void service_app_region_changed(app_event_info_h event_info, void *user_data)
 160 {
 161         return;
 162 }
 163
 164 static void service_app_low_battery(app_event_info_h event_info, void *user_data)
 165 {
 166         _E("low battery! exit app");
 167         service_app_exit();
 168
 169         return;
 170 }
 171
 172 static void service_app_low_memory(app_event_info_h event_info, void *user_data)
 173 {
 174         return;
 175 }
 176
 177 static inline double __map_round(double val)
 178 {
 179         return floor(val + 0.5);
 180 }
 181
 182 static int __map_range_val(int v_min, int v_max, int d_min, int d_max, int val)
 183 {
 184         int rval = 0;
 185         double slope = 0;
 186         slope = 1.0 * (d_max - d_min) / (v_max - v_min);
 187
 188         rval = d_min + __map_round(slope * (val - v_min));
 189
 190         return rval;
 191 }
 192
 193 static inline int ___map_speed_val(int speed)
 194 {
 195         return __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT, s_info.engine_min, s_info.engine_max, speed);
 196 }
 197
 198 static inline int ___map_servo_val(int servo)
 199 {
 200         return __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT,
 201                         s_info.stering_center - s_info.stering_range,
 202                         s_info.stering_center + s_info.stering_range, servo);
 203 }
 204
 205 static int __driving_motors(int servo, int speed)
 206 {
 207         int val_speed;
 208         int val_servo;
 209
 210         val_servo = ___map_servo_val(servo);
 211         val_speed = ___map_speed_val(speed);
 212
 213         _D("control motor - servo[%4d : %4d], speed[%4d : %4d]",
 214                 servo, val_servo, speed, val_speed);
 215 #if ENABLE_MOTOR
 216         resource_set_servo_motor_value(0, val_servo);
 217         resource_set_motor_driver_L298N_speed(MOTOR_ID_1, val_speed);
 218         resource_set_motor_driver_L298N_speed(MOTOR_ID_2, val_speed);
 219 #endif
 220
 221         return 0;
 222 }
 223
 224 static void __camera(int azimuth, int elevation)
 225 {
 226         int val_azimuth = __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT,
 227                         s_info.camera_azimuth_center - s_info.camera_azimuth_range,
 228                         s_info.camera_azimuth_center + s_info.camera_azimuth_range, azimuth);
 229
 230         int val_elevation = __map_range_val(0, MAX_UDP_INPUT, s_info.camera_elevation_min, s_info.camera_elevation_max, elevation); // No need to look upside down
 231
 232         _D("camera - azimuth[%4d : %4d], elevation[%4d : %4d]", azimuth, val_azimuth, elevation, val_elevation);
 233 #if ENABLE_MOTOR
 234         resource_set_servo_motor_value(s_info.elevation_pin, val_elevation);
 235         resource_set_servo_motor_value(s_info.azimuth_pin, val_azimuth);
 236 #endif
 237 }
 238
 239 static void __command_received_cb(command_s command) {
 240         switch(command.type) {
 241         case COMMAND_TYPE_DRIVE:
 242                 __driving_motors(command.data.steering.direction, command.data.steering.speed);
 243                 break;
 244         case COMMAND_TYPE_CAMERA:
 245                 __camera(command.data.camera_position.camera_azimuth, command.data.camera_position.camera_elevation);
 246                 break;
 247         case COMMAND_TYPE_DRIVE_AND_CAMERA:
 248                 __driving_motors(command.data.steering_and_camera.direction, command.data.steering_and_camera.speed);
 249                 __camera(command.data.steering_and_camera.camera_azimuth, command.data.steering_and_camera.camera_elevation);
 250                 break;
 251         case COMMAND_TYPE_NONE:
 252                 break;
 253         default:
 254                 _E("Unknown command type");
 255                 break;
 256         }
 257 }
 258
 259 static void __user_name_received_cb(const char *name)
 260 {
 261         _D("User name received: %s", name);
 262         lap_counter_set_user_name(name);
 263         lap_counter_set_start_lap();
 264 }
 265
 266 static void _initialize_config()
 267 {
 268         net_util_init();
 269
 270         config_init();
 271
 272         char *id = NULL;
 273         char *name = NULL;
 274         gboolean modified = false;
 275
 276         if (config_get_string(CONFIG_GRP_CAR, CONFIG_KEY_ID, &id) != 0) {
 277                 char *uuid = g_uuid_string_random();
 278                 config_set_string(CONFIG_GRP_CAR, CONFIG_KEY_ID, uuid);
 279                 g_free(uuid);
 280                 modified = true;
 281         }
 282
 283         char *uuid = g_uuid_string_random();
 284         modified |= config_get_string_or_set_default(CONFIG_GRP_CAR, CONFIG_KEY_ID, uuid, &id);
 285         g_free(uuid);
 286
 287         modified |= config_get_string_or_set_default(CONFIG_GRP_CAR, CONFIG_KEY_NAME, "Pink Car", &name);
 288
 289         modified |= config_get_int_or_set_default(CONFIG_GRP_STEERING, CONFIG_KEY_STEERING_CENTER, s_info.stering_center, &s_info.stering_center);
 290         modified |= config_get_int_or_set_default(CONFIG_GRP_STEERING, CONFIG_KEY_STEERING_RANGE, s_info.stering_range, &s_info.stering_range);
 291
 292         modified |= config_get_int_or_set_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_AZIMUTH_CENTER, s_info.camera_azimuth_center, &s_info.camera_azimuth_center);
 293         modified |= config_get_int_or_set_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_AZIMUTH_RANGE, s_info.camera_azimuth_range, &s_info.camera_azimuth_range);
 294         modified |= config_get_int_or_set_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_ELEVATION_MIN, s_info.camera_elevation_min, &s_info.camera_elevation_min);
 295         modified |= config_get_int_or_set_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_ELEVATION_MAX, s_info.camera_elevation_max, &s_info.camera_elevation_max);
 296
 297         modified |= config_get_int_or_set_default(CONFIG_GRP_ENGINE, CONFIG_KEY_CAMERA_ENGINE_MIN, s_info.engine_min, &s_info.engine_min);
 298         modified |= config_get_int_or_set_default(CONFIG_GRP_ENGINE, CONFIG_KEY_CAMERA_ENGINE_MAX, s_info.engine_max, &s_info.engine_max);
 299
 300         modified |= config_get_int_or_set_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_STERING, s_info.stering_pin, &s_info.stering_pin);
 301         modified |= config_get_int_or_set_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_CAMERA_AZIMUTH, s_info.azimuth_pin, &s_info.azimuth_pin);
 302         modified |= config_get_int_or_set_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_CAMERA_ELEVATION, s_info.elevation_pin, &s_info.elevation_pin);
 303
 304         modified |= config_get_int_or_set_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_1_FORWARD, s_info.engine_1_forward_pin, &s_info.engine_1_forward_pin);
 305         modified |= config_get_int_or_set_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_1_BACK, s_info.engine_1_back_pin, &s_info.engine_1_back_pin);
 306         modified |= config_get_int_or_set_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_ENGINE_CHANNEL_1, s_info.engine_1_channel, &s_info.engine_1_channel);
 307
 308         modified |= config_get_int_or_set_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_2_FORWARD, s_info.engine_2_forward_pin, &s_info.engine_2_forward_pin);
 309         modified |= config_get_int_or_set_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_2_BACK, s_info.engine_2_back_pin, &s_info.engine_2_back_pin);
 310         modified |= config_get_int_or_set_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_ENGINE_CHANNEL_2, s_info.engine_2_channel, &s_info.engine_2_channel);
 311
 312         if (modified == true) {
 313                 config_save();
 314         }
 315
 316         free(id);
 317         free(name);
 318 }
 319
 320 static void _initialize_components(app_data *ad)
 321 {
 322         FUNCTION_START;
 323
 324         net_util_init();
 325         _initialize_config();
 326         cloud_communication_init();
 327         message_manager_init();
 328         controller_connection_manager_listen();
 329         lap_counter_init();
 330         resource_led_init();
 331
 332
 333         _D("Car settings: Stering[%d +/- %d] Engine[%d - %d], CamAzimuth[%d +/- %d], CamElev[%d - %d]",
 334                         s_info.stering_center, s_info.stering_range,
 335                         s_info.engine_min, s_info.engine_max,
 336                         s_info.camera_azimuth_center, s_info.camera_azimuth_range,
 337                         s_info.camera_elevation_min, s_info.camera_elevation_max);
 338
 339         _D("RPI: Stering[%d], Azimuth[%d], Elev[%d]; ENG_1[%d, %d, %d]; ENG_2[%d, %d, %d];",
 340                         s_info.stering_pin, s_info.azimuth_pin, s_info.elevation_pin,
 341                         s_info.engine_1_forward_pin, s_info.engine_1_back_pin, s_info.engine_1_channel,
 342                         s_info.engine_2_forward_pin, s_info.engine_2_back_pin, s_info.engine_2_channel);
 343
 344         FUNCTION_END;
 345 }
 346
 347 static bool service_app_create(void *data)
 348 {
 349         FUNCTION_START;
 350
 351         int ret = 0;
 352         app_data *ad = data;
 353
 354         _D("-----------------------=======================================================================-----------------------");
 355         _D("-----------------------=======================================================================-----------------------");
 356         _D("-----------------------============================== APP %s %s ==============================-----------------------", __DATE__, __TIME__);
 357         _D("-----------------------=======================================================================-----------------------");
 358         _D("-----------------------=======================================================================-----------------------");
 359
 360         _initialize_components(ad);
 361
 362         /*
 363          * if you want to use default configuration,
 364          * Do not need to call resource_set_motor_driver_L298N_configuration(),
 365          *
 366         */
 367 #if ENABLE_MOTOR
 368         ret = resource_set_motor_driver_L298N_configuration(MOTOR_ID_1, s_info.engine_1_forward_pin, s_info.engine_1_back_pin, s_info.engine_1_channel);
 369         if (ret) {
 370                 _E("resource_set_motor_driver_L298N_configuration()");
 371                 service_app_exit();
 372         }
 373         ret = resource_set_motor_driver_L298N_configuration(MOTOR_ID_2, s_info.engine_2_forward_pin, s_info.engine_2_back_pin, s_info.engine_2_channel);
 374         if (ret) {
 375                 _E("resource_set_motor_driver_L298N_configuration()");
 376                 service_app_exit();
 377         }
 378 #endif
 379
 380         resource_lap_counter_init();
 381         cloud_communication_start(CLOUD_REQUESTS_FREQUENCY);
 382
 383         controller_connection_manager_set_command_received_cb(__command_received_cb);
 384         controller_connection_manager_set_user_name_received_cb(__user_name_received_cb);
 385
 386         resource_led_set_rgb_colors(CONFIG_LED_STATE_KEY_INIT,
 387                         CONFIG_DEFAULT_LED_3BIT_INIT,
 388                         CONFIG_DEFAULT_LED_24BIT_INIT,
 389                         LED_COLOR_RED);
 390
 391         FUNCTION_END;
 392         return true;
 393 }
 394
 395 static void service_app_control(app_control_h app_control, void *data)
 396 {
 397         FUNCTION_START;
 398
 399 #if ENABLE_MOTOR
 400         /* set speed 0, to reduce delay of initializing motor driver */
 401         resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 0);
 402         resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 0);
 403         resource_set_servo_motor_value(s_info.stering_pin, s_info.stering_center);
 404         resource_set_servo_motor_value(s_info.elevation_pin, s_info.camera_elevation_min);
 405         resource_set_servo_motor_value(s_info.azimuth_pin, s_info.camera_azimuth_center);
 406 #endif
 407
 408         FUNCTION_END;
 409         return;
 410 }
 411
 412 static void service_app_terminate(void *data)
 413 {
 414         app_data *ad = data;
 415         resource_led_set_rgb_colors(CONFIG_LED_STATE_KEY_OFF,
 416                         CONFIG_DEFAULT_LED_3BIT_OFF,
 417                         CONFIG_DEFAULT_LED_24BIT_OFF,
 418                         LED_COLOR_NONE);
 419
 420         resource_set_servo_motor_value(s_info.stering_pin, s_info.stering_center);
 421         resource_set_servo_motor_value(s_info.elevation_pin, s_info.camera_elevation_min);
 422         resource_set_servo_motor_value(s_info.azimuth_pin, s_info.camera_azimuth_center);
 423
 424         resource_lap_counter_destroy();
 425         resource_led_destroy();
 426
 427         if (ad->idle_h)
 428                 g_source_remove(ad->idle_h);
 429
 430         lap_counter_shutdown();
 431         controller_connection_manager_release();
 432         message_manager_shutdown();
 433
 434         cloud_communication_stop();
 435         cloud_communication_fini();
 436         config_shutdown();
 437         net_util_fini();
 438
 439         resource_close_all();
 440         log_file_close();
 441
 442         _D("Bye ~");
 443
 444         return;
 445 }
 446
 447 int main(int argc, char* argv[])
 448 {
 449         app_data *ad = NULL;
 450         int ret = 0;
 451         service_app_lifecycle_callback_s event_callback;
 452         app_event_handler_h handlers[5] = {NULL, };
 453
 454         log_type_set(LOG_TYPE_DLOG);
 455
 456         ad = calloc(1, sizeof(app_data));
 457         retv_if(!ad, -1);
 458
 459         event_callback.create = service_app_create;
 460         event_callback.terminate = service_app_terminate;
 461         event_callback.app_control = service_app_control;
 462
 463         service_app_add_event_handler(&handlers[APP_EVENT_LOW_BATTERY],
 464                 APP_EVENT_LOW_BATTERY, service_app_low_battery, &ad);
 465         service_app_add_event_handler(&handlers[APP_EVENT_LOW_MEMORY],
 466                 APP_EVENT_LOW_MEMORY, service_app_low_memory, &ad);
 467         service_app_add_event_handler(&handlers[APP_EVENT_LANGUAGE_CHANGED],
 468                 APP_EVENT_LANGUAGE_CHANGED, service_app_lang_changed, &ad);
 469         service_app_add_event_handler(&handlers[APP_EVENT_REGION_FORMAT_CHANGED],
 470                 APP_EVENT_REGION_FORMAT_CHANGED, service_app_region_changed, &ad);
 471
 472         ret = service_app_main(argc, argv, &event_callback, ad);
 473         if (ret)
 474                 _E("failed to start app");
 475
 476         return 0;
 477 }