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 */ 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 28 SensorBase::~SensorBase() 29 { 30 } 31 32 int SensorBase::getType(void) 33 { 34 return this->type; 35 } 36 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 51 int SensorBase::getConfig(SensorConfig *config) 52 { 53 *config = this->config; 54 return 0; 55 } 56 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 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 82 SensorManager::SensorManager() 83 { 84 } 85 86 SensorManager::~SensorManager() 87 { 88 } 89 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 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 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 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 164 int SensorManager::sensorEventReady(SensorBase *sensor) 165 { 166 return 0; 167 } 168 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 188 rt_sensor_t rt_sensor_get_default(int type) 189 { 190 return (rt_sensor_t)SensorManager::getDefaultSensor(type); 191 } 192 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 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 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 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