|
| 1 | +#include <stddef.h> |
| 2 | +#include "sensor.h" |
| 3 | + |
| 4 | +/** |
| 5 | + * Sensor |
| 6 | + */ |
| 7 | +Sensor::Sensor() |
| 8 | +{ |
| 9 | + this->next = this->prev = NULL; |
| 10 | + Subscribe(NULL, NULL); |
| 11 | +} |
| 12 | + |
| 13 | +Sensor::~Sensor() |
| 14 | +{ |
| 15 | +} |
| 16 | + |
| 17 | +int Sensor::GetType(void) |
| 18 | +{ |
| 19 | + return this->type; |
| 20 | +} |
| 21 | + |
| 22 | +int Sensor::Subscribe(SensorEventHandler_t *handler, void* user_data) |
| 23 | +{ |
| 24 | + this->evtHandler = handler; |
| 25 | + this->userData = user_data; |
| 26 | + |
| 27 | + return 0; |
| 28 | +} |
| 29 | + |
| 30 | +int Sensor::Publish(sensors_event_t* event) |
| 31 | +{ |
| 32 | + if (this->evtHandler != NULL) |
| 33 | + { |
| 34 | + /* invoke subscribed handler */ |
| 35 | + (*evtHandler)(this, event, this->userData); |
| 36 | + } |
| 37 | + |
| 38 | + return 0; |
| 39 | +} |
| 40 | + |
| 41 | +/** |
| 42 | + * Sensor Manager |
| 43 | + */ |
| 44 | +/* sensor manager instance */ |
| 45 | +static SensorManager _sensor_manager; |
| 46 | + |
| 47 | +SensorManager::SensorManager() |
| 48 | +{ |
| 49 | + sensorList = NULL; |
| 50 | +} |
| 51 | + |
| 52 | +SensorManager::~SensorManager() |
| 53 | +{ |
| 54 | +} |
| 55 | + |
| 56 | +int SensorManager::RegisterSensor(Sensor* sensor) |
| 57 | +{ |
| 58 | + SensorManager* self = &_sensor_manager; |
| 59 | + |
| 60 | + RT_ASSERT(sensor != RT_NULL); |
| 61 | + |
| 62 | + /* add sensor into the list */ |
| 63 | + if (self->sensorList = NULL) |
| 64 | + { |
| 65 | + sensor->prev = sensor->next = sensor; |
| 66 | + } |
| 67 | + else |
| 68 | + { |
| 69 | + sensor->prev = self->sensorList; |
| 70 | + sensor->next = self->sensorList->next; |
| 71 | + |
| 72 | + self->sensorList->next->prev = sensor; |
| 73 | + self->sensorList->next = sensor; |
| 74 | + } |
| 75 | + |
| 76 | + /* point the sensorList to this sensor */ |
| 77 | + self->sensorList = sensor; |
| 78 | + |
| 79 | + return 0; |
| 80 | +} |
| 81 | + |
| 82 | +int SensorManager::DeregisterSensor(Sensor* sensor) |
| 83 | +{ |
| 84 | + SensorManager* self = &_sensor_manager; |
| 85 | + |
| 86 | + /* disconnect sensor list */ |
| 87 | + sensor->next->prev = sensor->prev; |
| 88 | + sensor->prev->next = sensor->next; |
| 89 | + |
| 90 | + /* check the sensorList */ |
| 91 | + if (sensor == self->sensorList) |
| 92 | + { |
| 93 | + if (sensor->next == sensor) self->sensorList = NULL; /* empty list */ |
| 94 | + else self->sensorList = sensor->next; |
| 95 | + } |
| 96 | + |
| 97 | + /* re-initialize sensor node */ |
| 98 | + sensor->next = sensor->prev = sensor; |
| 99 | + |
| 100 | + return 0; |
| 101 | +} |
| 102 | + |
| 103 | +Sensor *SensorManager::GetDefaultSensor(int type) |
| 104 | +{ |
| 105 | + SensorManager* self = &_sensor_manager; |
| 106 | + Sensor *sensor = self->sensorList; |
| 107 | + |
| 108 | + if (sensor == NULL) return NULL; |
| 109 | + |
| 110 | + do |
| 111 | + { |
| 112 | + /* find the same type */ |
| 113 | + if (sensor->GetType() == type) return sensor; |
| 114 | + |
| 115 | + sensor = sensor->next; |
| 116 | + } |
| 117 | + while (sensor != self->sensorList); |
| 118 | + |
| 119 | + return NULL; |
| 120 | +} |
| 121 | + |
| 122 | +int SensorManager::Subscribe(int type, SensorEventHandler_t *handler, void* user_data) |
| 123 | +{ |
| 124 | + Sensor *sensor; |
| 125 | + |
| 126 | + sensor = SensorManager::GetDefaultSensor(type); |
| 127 | + if (sensor != NULL) |
| 128 | + { |
| 129 | + sensor->Subscribe(handler, user_data); |
| 130 | + return 0; |
| 131 | + } |
| 132 | + |
| 133 | + return -1; |
| 134 | +} |
| 135 | + |
0 commit comments