diff --git a/components/drivers/sensors/sensor.cpp b/components/drivers/sensors/sensor.cpp index 8751efd36dd82e1fe470aef30d33eccfb62fbc7e..88342d6dac32eea48e29758d43373321dfb9919c 100644 --- a/components/drivers/sensors/sensor.cpp +++ b/components/drivers/sensors/sensor.cpp @@ -13,27 +13,52 @@ */ #include +#include + #include "sensor.h" /** - * Sensor + * SensorBase */ -Sensor::Sensor() +SensorBase::SensorBase(int type) { + memset(&(this->config), 0x0, sizeof(SensorConfig)); + + this->type = type; this->next = this->prev = NULL; - Subscribe(NULL, NULL); + subscribe(NULL, NULL); } -Sensor::~Sensor() +SensorBase::~SensorBase() { } -int Sensor::GetType(void) +int SensorBase::getType(void) { return this->type; } -int Sensor::Subscribe(SensorEventHandler_t *handler, void *user_data) +int SensorBase::setConfig(SensorConfig *config) +{ + int result; + + /* configure to the low level sensor */ + result = this->configure(config); + if (result == 0) + { + this->config = *config; + } + + return result; +} + +int SensorBase::getConfig(SensorConfig *config) +{ + *config = this->config; + return 0; +} + +int SensorBase::subscribe(SensorEventHandler_t *handler, void *user_data) { this->evtHandler = handler; this->userData = user_data; @@ -41,7 +66,7 @@ int Sensor::Subscribe(SensorEventHandler_t *handler, void *user_data) return 0; } -int Sensor::Publish(sensors_event_t *event) +int SensorBase::publish(sensors_event_t *event) { if (this->evtHandler != NULL) { @@ -55,57 +80,52 @@ int Sensor::Publish(sensors_event_t *event) /** * Sensor Manager */ -/* sensor manager instance */ -static SensorManager _sensor_manager; +/* sensors list */ +static SensorBase *sensor_list = NULL; SensorManager::SensorManager() { - sensorList = NULL; } SensorManager::~SensorManager() { } -int SensorManager::RegisterSensor(Sensor *sensor) +int SensorManager::registerSensor(SensorBase *sensor) { - SensorManager *self = &_sensor_manager; - RT_ASSERT(sensor != RT_NULL); /* add sensor into the list */ - if (self->sensorList == NULL) + if (sensor_list == NULL) { sensor->prev = sensor->next = sensor; } else { - sensor->prev = self->sensorList; - sensor->next = self->sensorList->next; + sensor_list->prev->next = sensor; + sensor->prev = sensor_list->prev; - self->sensorList->next->prev = sensor; - self->sensorList->next = sensor; + sensor_list->prev = sensor; + sensor->next = sensor_list; } /* point the sensorList to this sensor */ - self->sensorList = sensor; + sensor_list = sensor; return 0; } -int SensorManager::DeregisterSensor(Sensor *sensor) +int SensorManager::unregisterSensor(SensorBase *sensor) { - SensorManager *self = &_sensor_manager; - /* disconnect sensor list */ sensor->next->prev = sensor->prev; sensor->prev->next = sensor->next; /* check the sensorList */ - if (sensor == self->sensorList) + if (sensor == sensor_list) { - if (sensor->next == sensor) self->sensorList = NULL; /* empty list */ - else self->sensorList = sensor->next; + if (sensor->next == sensor) sensor_list = NULL; /* empty list */ + else sensor_list = sensor->next; } /* re-initialize sensor node */ @@ -114,35 +134,58 @@ int SensorManager::DeregisterSensor(Sensor *sensor) return 0; } -Sensor *SensorManager::GetDefaultSensor(int type) +SensorBase *SensorManager::getDefaultSensor(int type) { - SensorManager *self = &_sensor_manager; - Sensor *sensor = self->sensorList; + SensorBase *sensor = sensor_list; if (sensor == NULL) return NULL; do { /* find the same type */ - if (sensor->GetType() == type) return sensor; + if (sensor->getType() == type) return sensor; sensor = sensor->next; - }while (sensor != self->sensorList); + }while (sensor != sensor_list); return NULL; } -int SensorManager::Subscribe(int type, SensorEventHandler_t *handler, void *user_data) +int SensorManager::subscribe(int type, SensorEventHandler_t *handler, void *user_data) { - Sensor *sensor; + SensorBase *sensor; - sensor = SensorManager::GetDefaultSensor(type); + sensor = SensorManager::getDefaultSensor(type); if (sensor != NULL) { - sensor->Subscribe(handler, user_data); + sensor->subscribe(handler, user_data); return 0; } return -1; } +int SensorManager::sensorEventReady(SensorBase *sensor) +{ + return 0; +} + +int SensorManager::pollSensor(SensorBase *sensor, sensors_event_t *events, int number, int duration) +{ + rt_tick_t tick; + int result, index; + + if (sensor == NULL) return -1; + + tick = rt_tick_get(); + for (index = 0; index < number; index ++) + { + result = sensor->poll(&events[index]); + if (result < 0) break; + + if (rt_tick_get() - tick > duration) break; + } + + return index; +} + diff --git a/components/drivers/sensors/sensor.h b/components/drivers/sensors/sensor.h index 065422944d6245cf6ccdd2cfe65d6833f21b65fb..55452420561842fea2849389e35e70921f6dc0e5 100644 --- a/components/drivers/sensors/sensor.h +++ b/components/drivers/sensors/sensor.h @@ -36,6 +36,8 @@ #include #include +#pragma anon_unions + /** * Handles must be higher than SENSORS_HANDLE_BASE and must be unique. * A Handle identifies a given sensors. The handle is used to activate @@ -726,14 +728,24 @@ enum * Values returned by the accelerometer in various locations in the universe. * all values are in SI units (m/s^2) */ -#define GRAVITY_SUN (275.0f) -#define GRAVITY_EARTH (9.80665f) +#define SENSORS_GRAVITY_SUN (275.0f) +#define SENSORS_GRAVITY_MOON (1.6f) +#define SENSORS_GRAVITY_EARTH (9.80665f) +#define SENSORS_GRAVITY_STANDARD (SENSORS_GRAVITY_EARTH) /** Maximum magnetic field on Earth's surface */ -#define MAGNETIC_FIELD_EARTH_MAX (60.0f) +#define MAGNETIC_FIELD_EARTH_MAX (60.0f) /** Minimum magnetic field on Earth's surface */ -#define MAGNETIC_FIELD_EARTH_MIN (30.0f) +#define MAGNETIC_FIELD_EARTH_MIN (30.0f) + +/** Average sea level pressure is 1013.25 hPa */ +#define SENSORS_PRESSURE_SEALEVELHPA (1013.25F) + +/** Degrees/s to rad/s multiplier */ +#define SENSORS_DPS_TO_RADS (0.017453293F) +/** Gauss to micro-Tesla multiplier */ +#define SENSORS_GAUSS_TO_MICROTESLA (100) /** * status of orientation sensor @@ -768,6 +780,22 @@ typedef struct uint8_t reserved[3]; } sensors_vec_t; +/** + * sensor raw vector data + */ +typedef struct +{ + struct + { + int16_t x; + int16_t y; + int16_t z; + }; + + int8_t status; + uint8_t reserved[1]; +} sensors_raw_vec_t; + /** * uncalibrated gyroscope and magnetometer event data */ @@ -830,15 +858,21 @@ typedef struct sensors_event_t /* acceleration values are in meter per second per second (m/s^2) */ sensors_vec_t acceleration; + /* raw acceleration data */ + sensors_raw_vec_t raw_acceleration; /* magnetic vector values are in micro-Tesla (uT) */ sensors_vec_t magnetic; + /* raw magnetic data */ + sensors_raw_vec_t raw_magnetic; /* orientation values are in degrees */ sensors_vec_t orientation; /* gyroscope values are in rad/s */ sensors_vec_t gyro; + /* raw gyroscope data */ + sensors_raw_vec_t raw_gyro; /* temperature is in degrees centigrade (Celsius) */ float temperature; @@ -943,41 +977,109 @@ typedef struct sensor_t void *reserved[6]; } sensor_t; -class SensorConfigure +enum SensorMode +{ + SENSOR_MODE_RAW, + SENSOR_MODE_CALIBRATED, + SENSOR_MODE_NORMAL, +}; + +enum SensorAccelRange +{ + SENSOR_ACCEL_RANGE_2G, + SENSOR_ACCEL_RANGE_4G, + SENSOR_ACCEL_RANGE_8G, + SENSOR_ACCEL_RANGE_16G, +}; +#define SENSOR_ACCEL_SENSITIVITY_2G (0.001F) +#define SENSOR_ACCEL_SENSITIVITY_4G (0.002F) +#define SENSOR_ACCEL_SENSITIVITY_8G (0.004F) +#define SENSOR_ACCEL_SENSITIVITY_16G (0.012F) + +enum SensorGyroRange +{ + SENSOR_GYRO_RANGE_250DPS, + SENSOR_GYRO_RANGE_500DPS, + SENSOR_GYRO_RANGE_1000DPS, + SENSOR_GYRO_RANGE_2000DPS, +}; +#define SENSOR_GYRO_SENSITIVITY_250DPS (0.00875F) +#define SENSOR_GYRO_SENSITIVITY_500DPS (0.0175F) +#define SENSOR_GYRO_SENSITIVITY_1000DPS (0.035F) +#define SENSOR_GYRO_SENSITIVITY_2000DPS (0.070F) + +enum SensorDataRate { - int32_t delay; + SENSOR_DATARATE_3200HZ, + SENSOR_DATARATE_1600HZ, + SENSOR_DATARATE_800HZ, + SENSOR_DATARATE_400HZ, + SENSOR_DATARATE_200HZ, + SENSOR_DATARATE_100HZ, + SENSOR_DATARATE_50HZ, + SENSOR_DATARATE_25HZ, + SENSOR_DATARATE_12_5HZ, + SENSOR_DATARATE_6_25HZ, + SENSOR_DATARATE_3_13HZ, + SENSOR_DATARATE_1_56HZ, + SENSOR_DATARATE_0_78HZ, + SENSOR_DATARATE_0_39HZ, + SENSOR_DATARATE_0_20HZ, + SENSOR_DATARATE_0_10HZ, }; -class Sensor; +class SensorBase; class SensorManager; -typedef void (*SensorEventHandler_t)(Sensor *sensor, sensors_event_t *event, void *user_data); +typedef void (*SensorEventHandler_t)(SensorBase *sensor, sensors_event_t *event, void *user_data); + +/** + * Sensor Configuration + */ +typedef struct SensorConfig +{ + int mode; + + enum SensorDataRate data_rate; + + union range + { + enum SensorAccelRange accel_range; + enum SensorGyroRange gyro_range; + } range; +}SensorConfig; /** * Sensor Base Class */ -class Sensor +class SensorBase { private: int type; public: - Sensor(); - ~Sensor(); + SensorBase(int type); + ~SensorBase(); - virtual int Configure(SensorConfigure *config) = 0; - virtual int Activate(int enable) = 0; + virtual int configure(SensorConfig *config) = 0; + virtual int activate(int enable) = 0; - virtual int Poll(sensors_event_t *events, int number, int duration) = 0; - virtual void GetSensor(struct sensor_t *sensor) = 0; + virtual int poll(sensors_event_t *events) = 0; + virtual void getSensor(struct sensor_t *sensor) = 0; - int GetType(void); + int getType(void); - int Subscribe(SensorEventHandler_t *handler, void *user_data); - int Publish(sensors_event_t *event); + int setConfig(SensorConfig *config); + int getConfig(SensorConfig *config); + + int subscribe(SensorEventHandler_t *handler, void *user_data); + int publish(sensors_event_t *event); protected: - Sensor *next; - Sensor *prev; + SensorBase *next; + SensorBase *prev; + + /* sensor configuration */ + SensorConfig config; SensorEventHandler_t *evtHandler; void *userData; @@ -994,14 +1096,14 @@ public: SensorManager(); ~SensorManager(); - static int RegisterSensor(Sensor *sensor); - static int DeregisterSensor(Sensor *sensor); + static int registerSensor(SensorBase *sensor); + static int unregisterSensor(SensorBase *sensor); - static Sensor *GetDefaultSensor(int type); - static int Subscribe(int type, SensorEventHandler_t *handler, void *user_data); + static SensorBase *getDefaultSensor(int type); + static int subscribe(int type, SensorEventHandler_t *handler, void *user_data); -private: - Sensor *sensorList; + static int sensorEventReady(SensorBase *sensor); + static int pollSensor(SensorBase *sensor, sensors_event_t *events, int number, int duration); }; #endif