src/servo-h.c

   1  /*
   2  * Copyright (c) 2018 Samsung Electronics Co., Ltd.
   3  *
   4  * Licensed under the Apache License, Version 2.0 (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://www.apache.org/licenses/LICENSE-2.0
   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
  17 #include <stdlib.h>
  18 #include <pthread.h>
  19 #include <glib.h>
  20 #include <math.h>
  21 #include "log.h"
  22 #include "servo-type.h"
  23 #include "resource_servo_motor_sg90.h"
  24
  25 #define IDLE_PRIORITY (G_PRIORITY_HIGH_IDLE + 30)
  26 #define VALUE_DEFAULT (50.0f)
  27 #define SERVO_H_CHANNEL 2
  28
  29 struct servo_h_data_s {
  30         double value;
  31         GHashTable *callback_hash;
  32 };
  33
  34 struct servo_h_callback_s {
  35         servo_state_changed_cb callback;
  36         void *cb_data;
  37 };
  38
  39 struct servo_h_pass_data_s {
  40         char *pass_key;
  41         double value;
  42 };
  43
  44 static struct servo_h_data_s *g_servo_h;
  45 static pthread_mutex_t g_servo_h_mutex;
  46
  47 static int __free_servo_h_handle(struct servo_h_data_s *servo_h)
  48 {
  49         if (servo_h->callback_hash) {
  50                 g_hash_table_destroy(servo_h->callback_hash);
  51                 g_hash_table_unref(servo_h->callback_hash);
  52         }
  53
  54         free(servo_h);
  55         return 0;
  56 }
  57
  58 int servo_h_finalize(void)
  59 {
  60         if (g_servo_h) {
  61                 resource_close_servo_motor(SERVO_H_CHANNEL);
  62                 pthread_mutex_destroy(&g_servo_h_mutex);
  63                 __free_servo_h_handle(g_servo_h);
  64                 g_servo_h = NULL;
  65         }
  66         return 0;
  67 }
  68
  69 int servo_h_initialize(void)
  70 {
  71         if (g_servo_h) {
  72                 _D("The servo_h is already initialized!");
  73                 return 0;
  74         }
  75
  76         g_servo_h = malloc(sizeof(struct servo_h_data_s));
  77         retv_if(!g_servo_h, -1);
  78
  79         g_servo_h->callback_hash =
  80                 g_hash_table_new_full(g_str_hash, g_str_equal, g_free, g_free);
  81
  82         pthread_mutex_init(&g_servo_h_mutex, NULL);
  83         g_servo_h->value = VALUE_DEFAULT;
  84
  85         if (resource_rotate_servo_motor_by_percent(SERVO_H_CHANNEL, VALUE_DEFAULT)) {
  86                 _E("faiiled to set servo h value to 0");
  87                 servo_h_finalize();
  88                 return -1;
  89         }
  90
  91         return 0;
  92 }
  93
  94 static void __cb_item_foreach(gpointer key, gpointer value, gpointer user_data)
  95 {
  96         char *item_name = key;
  97         struct servo_h_callback_s *cb_item = value;
  98         struct servo_h_pass_data_s *pass_item = user_data;
  99
 100         ret_if(!item_name);
 101         ret_if(!cb_item);
 102         ret_if(!pass_item);
 103
 104
 105         if (pass_item->pass_key &&
 106                 (0 == g_strcmp0(item_name, pass_item->pass_key))) {
 107                         // _D("pass item - %s", item_name);
 108                         return;
 109                 }
 110
 111         _D("callback called for [%s]", item_name);
 112         cb_item->callback(pass_item->value, cb_item->cb_data);
 113 }
 114
 115 static gboolean __call_cb_idle(gpointer data)
 116 {
 117         struct servo_h_pass_data_s *pass_item = data;
 118
 119         g_hash_table_foreach(g_servo_h->callback_hash, __cb_item_foreach, pass_item);
 120         g_free(pass_item->pass_key);
 121         g_free(pass_item);
 122
 123         return G_SOURCE_REMOVE;
 124 }
 125
 126 static inline int __double_is_same(double a, double b)
 127 {
 128         const double eps = 0.0001;
 129         if (fabs(a-b) < eps)
 130                 return 1;
 131         else
 132                 return 0;
 133 }
 134
 135 int servo_h_state_set(double value, const char *pass_key)
 136 {
 137         double old_value = 0.0;
 138         retv_if(!g_servo_h, -1);
 139
 140         pthread_mutex_lock(&g_servo_h_mutex);
 141         old_value = g_servo_h->value;
 142         pthread_mutex_unlock(&g_servo_h_mutex);
 143
 144         if (!__double_is_same(old_value, value)) {
 145                 if (resource_rotate_servo_motor_by_percent(SERVO_H_CHANNEL, value)) {
 146                         _E("faiiled to set servo h value to [%lf]", value);
 147                         return -1;
 148                 }
 149                 _D("set value : %lf", value);
 150                 pthread_mutex_lock(&g_servo_h_mutex);
 151                 g_servo_h->value = value;
 152                 pthread_mutex_unlock(&g_servo_h_mutex);
 153
 154                 if (g_hash_table_size(g_servo_h->callback_hash) > 0) {
 155                         struct servo_h_pass_data_s *pass_item = NULL;
 156
 157                         pass_item = g_new(struct servo_h_pass_data_s, 1);
 158                         pass_item->pass_key = g_strdup(pass_key);
 159                         pass_item->value = value;
 160                         g_idle_add_full(IDLE_PRIORITY,
 161                                 __call_cb_idle, pass_item, NULL);
 162                 }
 163         } else {
 164                 _D("a value[%lf] is same as old one[%lf]" , value, old_value);
 165         }
 166         return 0;
 167 }
 168
 169 int servo_h_state_get(double *value)
 170 {
 171         retv_if(!g_servo_h, -1);
 172         retv_if(!value, -1);
 173
 174         pthread_mutex_lock(&g_servo_h_mutex);
 175         *value = g_servo_h->value;
 176         pthread_mutex_unlock(&g_servo_h_mutex);
 177
 178         _D("get value : %lf", *value);
 179
 180         return 0;
 181 }
 182
 183 int servo_h_state_changed_cb_set(
 184         const char *callback_key, servo_state_changed_cb callback, void *cb_data)
 185 {
 186         retv_if(!g_servo_h, -1);
 187         retv_if(!g_servo_h->callback_hash, -1);
 188         retv_if(!callback_key, -1);
 189
 190         if (callback) {
 191                 struct servo_h_callback_s *cb_item = NULL;
 192                 cb_item = g_try_new(struct servo_h_callback_s, 1);
 193                 retv_if(!cb_item, -1);
 194
 195                 cb_item->callback = callback;
 196                 cb_item->cb_data = cb_data;
 197
 198                 g_hash_table_insert(g_servo_h->callback_hash,
 199                         g_strdup(callback_key), cb_item);
 200         } else {
 201                 g_hash_table_remove(g_servo_h->callback_hash, callback_key);
 202         }
 203
 204         return 0;
 205 }