2 * Copyright (c) 2018 Samsung Electronics Co., Ltd.
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
8 * http://www.apache.org/licenses/LICENSE-2.0
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.
22 #include "servo-type.h"
23 #include "resource_servo_motor_sg90.h"
25 #define IDLE_PRIORITY (G_PRIORITY_HIGH_IDLE + 30)
26 #define VALUE_DEFAULT (50.0f)
27 #define SERVO_V_CHANNEL 0
29 struct servo_v_data_s {
31 GHashTable *callback_hash;
34 struct servo_v_callback_s {
35 servo_state_changed_cb callback;
39 struct servo_v_pass_data_s {
44 static struct servo_v_data_s *g_servo_v;
45 static pthread_mutex_t g_servo_v_mutex;
47 static int __free_servo_v_handle(struct servo_v_data_s *servo_v)
49 if (servo_v->callback_hash) {
50 g_hash_table_destroy(servo_v->callback_hash);
51 g_hash_table_unref(servo_v->callback_hash);
58 int servo_v_finalize(void)
61 resource_close_servo_motor(SERVO_V_CHANNEL);
62 pthread_mutex_destroy(&g_servo_v_mutex);
63 __free_servo_v_handle(g_servo_v);
70 int servo_v_initialize(void)
73 _D("The servo_v is already initialized!");
77 g_servo_v = malloc(sizeof(struct servo_v_data_s));
78 retv_if(!g_servo_v, -1);
80 g_servo_v->callback_hash =
81 g_hash_table_new_full(g_str_hash, g_str_equal, g_free, g_free);
83 pthread_mutex_init(&g_servo_v_mutex, NULL);
84 g_servo_v->value = VALUE_DEFAULT;
86 if (resource_rotate_servo_motor_by_percent(SERVO_V_CHANNEL, VALUE_DEFAULT)) {
87 _E("faiiled to set servo h value to 0");
95 static void __cb_item_foreach(gpointer key, gpointer value, gpointer user_data)
97 char *item_name = key;
98 struct servo_v_callback_s *cb_item = value;
99 struct servo_v_pass_data_s *pass_item = user_data;
105 if (pass_item->pass_key &&
106 (0 == g_strcmp0(item_name, pass_item->pass_key))) {
107 // _D("pass item - %s", item_name);
111 _D("callback called for [%s]", item_name);
112 cb_item->callback(pass_item->value, cb_item->cb_data);
115 static gboolean __call_cb_idle(gpointer data)
117 struct servo_v_pass_data_s *pass_item = data;
119 g_hash_table_foreach(g_servo_v->callback_hash, __cb_item_foreach, pass_item);
120 g_free(pass_item->pass_key);
123 return G_SOURCE_REMOVE;
126 static inline int __double_is_same(double a, double b)
128 const double eps = 0.0001;
135 int servo_v_state_set(double value, const char *pass_key)
137 double old_value = 0.0;
138 retv_if(!g_servo_v, -1);
140 pthread_mutex_lock(&g_servo_v_mutex);
141 old_value = g_servo_v->value;
142 pthread_mutex_unlock(&g_servo_v_mutex);
144 if (!__double_is_same(old_value, value)) {
145 if (resource_rotate_servo_motor_by_percent(SERVO_V_CHANNEL, value)) {
146 _E("faiiled to set servo v value to [%lf]", value);
149 _D("set value : %lf", value);
150 pthread_mutex_lock(&g_servo_v_mutex);
151 g_servo_v->value = value;
152 pthread_mutex_unlock(&g_servo_v_mutex);
154 if (g_hash_table_size(g_servo_v->callback_hash) > 0) {
155 struct servo_v_pass_data_s *pass_item = NULL;
157 pass_item = g_new(struct servo_v_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);
164 _D("a value[%lf] is same as old one[%lf]" , value, old_value);
169 int servo_v_state_get(double *value)
171 retv_if(!g_servo_v, -1);
174 pthread_mutex_lock(&g_servo_v_mutex);
175 *value = g_servo_v->value;
176 pthread_mutex_unlock(&g_servo_v_mutex);
178 _D("get value : %lf", *value);
183 int servo_v_state_changed_cb_set(
184 const char *callback_key, servo_state_changed_cb callback, void *cb_data)
186 retv_if(!g_servo_v, -1);
187 retv_if(!g_servo_v->callback_hash, -1);
188 retv_if(!callback_key, -1);
191 struct servo_v_callback_s *cb_item = NULL;
192 cb_item = g_try_new(struct servo_v_callback_s, 1);
193 retv_if(!cb_item, -1);
195 cb_item->callback = callback;
196 cb_item->cb_data = cb_data;
198 g_hash_table_insert(g_servo_v->callback_hash,
199 g_strdup(callback_key), cb_item);
201 g_hash_table_remove(g_servo_v->callback_hash, callback_key);