1 /*
2 * Copyright (c) 2006-2018, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date Author Notes
8 * 2014-08-03 Bernard the first version
9 */
10
11 #include <stddef.h>
12 #include <string.h>
13
14 #include "sensor.h"
15
16 /**
17 * SensorBase
18 */
SensorBase(int type)19 SensorBase::SensorBase(int type)
20 {
21 memset(&(this->config), 0x0, sizeof(SensorConfig));
22
23 this->type = type;
24 this->next = this->prev = NULL;
25 subscribe(NULL, NULL);
26 }
27
~SensorBase()28 SensorBase::~SensorBase()
29 {
30 }
31
getType(void)32 int SensorBase::getType(void)
33 {
34 return this->type;
35 }
36
setConfig(SensorConfig * config)37 int SensorBase::setConfig(SensorConfig *config)
38 {
39 int result;
40
41 /* configure to the low level sensor */
42 result = this->configure(config);
43 if (result == 0)
44 {
45 this->config = *config;
46 }
47
48 return result;
49 }
50
getConfig(SensorConfig * config)51 int SensorBase::getConfig(SensorConfig *config)
52 {
53 *config = this->config;
54 return 0;
55 }
56
subscribe(SensorEventHandler_t handler,void * user_data)57 int SensorBase::subscribe(SensorEventHandler_t handler, void *user_data)
58 {
59 this->evtHandler = handler;
60 this->userData = user_data;
61
62 return 0;
63 }
64
publish(void)65 int SensorBase::publish(void)
66 {
67 if (this->evtHandler != NULL)
68 {
69 /* invoke subscribed handler */
70 (*evtHandler)(this->userData);
71 }
72
73 return 0;
74 }
75
76 /**
77 * Sensor Manager
78 */
79 /* sensors list */
80 static SensorBase *sensor_list = NULL;
81
SensorManager()82 SensorManager::SensorManager()
83 {
84 }
85
~SensorManager()86 SensorManager::~SensorManager()
87 {
88 }
89
registerSensor(SensorBase * sensor)90 int SensorManager::registerSensor(SensorBase *sensor)
91 {
92 RT_ASSERT(sensor != RT_NULL);
93
94 /* add sensor into the list */
95 if (sensor_list == NULL)
96 {
97 sensor->prev = sensor->next = sensor;
98 }
99 else
100 {
101 sensor_list->prev->next = sensor;
102 sensor->prev = sensor_list->prev;
103
104 sensor_list->prev = sensor;
105 sensor->next = sensor_list;
106 }
107
108 /* point the sensorList to this sensor */
109 sensor_list = sensor;
110
111 return 0;
112 }
113
unregisterSensor(SensorBase * sensor)114 int SensorManager::unregisterSensor(SensorBase *sensor)
115 {
116 /* disconnect sensor list */
117 sensor->next->prev = sensor->prev;
118 sensor->prev->next = sensor->next;
119
120 /* check the sensorList */
121 if (sensor == sensor_list)
122 {
123 if (sensor->next == sensor) sensor_list = NULL; /* empty list */
124 else sensor_list = sensor->next;
125 }
126
127 /* re-initialize sensor node */
128 sensor->next = sensor->prev = sensor;
129
130 return 0;
131 }
132
getDefaultSensor(int type)133 SensorBase *SensorManager::getDefaultSensor(int type)
134 {
135 SensorBase *sensor = sensor_list;
136
137 if (sensor == NULL) return NULL;
138
139 do
140 {
141 /* find the same type */
142 if (sensor->getType() == type) return sensor;
143
144 sensor = sensor->next;
145 }while (sensor != sensor_list);
146
147 return NULL;
148 }
149
subscribe(int type,SensorEventHandler_t handler,void * user_data)150 int SensorManager::subscribe(int type, SensorEventHandler_t handler, void *user_data)
151 {
152 SensorBase *sensor;
153
154 sensor = SensorManager::getDefaultSensor(type);
155 if (sensor != NULL)
156 {
157 sensor->subscribe(handler, user_data);
158 return 0;
159 }
160
161 return -1;
162 }
163
sensorEventReady(SensorBase * sensor)164 int SensorManager::sensorEventReady(SensorBase *sensor)
165 {
166 return 0;
167 }
168
pollSensor(SensorBase * sensor,sensors_event_t * events,int number,int duration)169 int SensorManager::pollSensor(SensorBase *sensor, sensors_event_t *events, int number, int duration)
170 {
171 rt_tick_t tick;
172 int result, index;
173
174 if (sensor == NULL) return -1;
175
176 tick = rt_tick_get();
177 for (index = 0; index < number; index ++)
178 {
179 result = sensor->poll(&events[index]);
180 if (result < 0) break;
181
182 if (rt_tick_get() - tick > duration) break;
183 }
184
185 return index;
186 }
187
rt_sensor_get_default(int type)188 rt_sensor_t rt_sensor_get_default(int type)
189 {
190 return (rt_sensor_t)SensorManager::getDefaultSensor(type);
191 }
192
rt_sensor_subscribe(rt_sensor_t sensor,SensorEventHandler_t handler,void * user_data)193 int rt_sensor_subscribe(rt_sensor_t sensor, SensorEventHandler_t handler, void *user_data)
194 {
195 SensorBase *sensor_base;
196 if (sensor == NULL) return -1;
197
198 sensor_base = (SensorBase*)sensor;
199
200 return sensor_base->subscribe(handler, user_data);
201 }
202
rt_sensor_poll(rt_sensor_t sensor,sensors_event_t * event)203 int rt_sensor_poll(rt_sensor_t sensor, sensors_event_t *event)
204 {
205 SensorBase *sensor_base;
206 if (sensor == NULL || event == NULL) return -1;
207
208 sensor_base = (SensorBase*)sensor;
209 return sensor_base->poll(event);
210 }
211
rt_sensor_configure(rt_sensor_t sensor,SensorConfig * config)212 int rt_sensor_configure(rt_sensor_t sensor, SensorConfig *config)
213 {
214 SensorBase *sensor_base;
215 if (sensor == NULL || config == NULL) return -1;
216
217 sensor_base = (SensorBase*)sensor;
218 return sensor_base->setConfig(config);
219 }
220
rt_sensor_activate(rt_sensor_t sensor,int enable)221 int rt_sensor_activate(rt_sensor_t sensor, int enable)
222 {
223 SensorBase *sensor_base;
224 if (sensor == NULL) return -1;
225
226 sensor_base = (SensorBase*)sensor;
227 return sensor_base->activate(enable);
228 }
229