Felix Rüdiger
BLE GATT-service implementation for high quantity sensor data from a MPU6050-accelerator/gyroscope
Dependencies: BLE_API mbed nRF51822 MPU6050_lib
Diff: MPU6050Service.h
- Revision:
- 10:b305e261e7d1
- Parent:
- 9:6a28d9c0e486
- Child:
- 12:dc46aa2edccd
--- a/MPU6050Service.h Wed Jul 22 16:03:03 2015 +0000
+++ b/MPU6050Service.h Tue Sep 08 13:42:58 2015 +0000
@@ -13,8 +13,9 @@
UUID_MPU6050_SERVICE = 0xA123,
UUID_MPU6050_SENSOR_DATA_CHAR = 0xA124,
- UUID_MPU6050_MEASURING_RANGE_CHAR = 0xA125,
- UUID_MPU6050_MASTER_CLOCK_CHAR = 0xA126
+ UUID_MPU6050_MOTION_INDICATOR_CHAR = 0xA125,
+ UUID_MPU6050_MEASURING_RANGE_CHAR = 0xA126,
+ UUID_MPU6050_MASTER_CLOCK_CHAR = 0xA127
};
enum MeasuringRange
@@ -24,40 +25,72 @@
ACCEL_RANGE_8G = 0x02, // 0b__10
ACCEL_RANGE_16G = 0x03, // 0b__11
+ ACCEL_RANGE_MASK = 0x03, // 0b__11
+
GYRO_RANGE_250 = 0x00, // 0b00__
GYRO_RANGE_500 = 0x04, // 0b01__
GYRO_RANGE_1000 = 0x08, // 0b10__
- GYRO_RANGE_2000 = 0x0C // 0b11__
+ GYRO_RANGE_2000 = 0x0C, // 0b11__
+
+ GYRO_RANGE_MASK = 0x0C // 0b11__
};
friend inline MeasuringRange operator|(MeasuringRange a, MeasuringRange b) { return static_cast<MeasuringRange>(static_cast<int>(a) | static_cast<int>(b)); }
+ friend inline MeasuringRange operator&(MeasuringRange a, MeasuringRange b) { return static_cast<MeasuringRange>(static_cast<int>(a) & static_cast<int>(b)); }
- typedef struct
+ enum MotionIndicator
{
- int16_t accelX, accelY, accelZ;
+ MOTION_IND_NEGATIVE_X = MPU6050::MOT_NEGATIVE_X,
+ MOTION_IND_POSITIVE_X = MPU6050::MOT_POSITIVE_X,
+
+ MOTION_IND_NEGATIVE_Y = MPU6050::MOT_NEGATIVE_Y,
+ MOTION_IND_POSITIVE_Y = MPU6050::MOT_POSITIVE_Y,
+
+ MOTION_IND_NEGATIVE_Z = MPU6050::MOT_NEGATIVE_Z,
+ MOTION_IND_POSITIVE_Z = MPU6050::MOT_POSITIVE_Z,
- int16_t temperature;
+ MOTION_IND_FREEFALL = 0x02, // 0b00000010
+ MOTION_IND_ZERO_MOTION = MPU6050::MOT_ZERO
+ };
+ friend inline MotionIndicator operator|(MotionIndicator a, MotionIndicator b) { return static_cast<MotionIndicator>(static_cast<int>(a) | static_cast<int>(b)); }
+ friend inline MotionIndicator operator&(MotionIndicator a, MotionIndicator b) { return static_cast<MotionIndicator>(static_cast<int>(a) & static_cast<int>(b)); }
+
+ typedef __packed struct
+ {
+ int16_t accelX, accelY, accelZ;
+
+ int16_t temperature;
- int16_t gyroX, gyroY, gyroZ;
+ int16_t gyroX, gyroY, gyroZ;
- uint32_t clock_us;
+ uint32_t clock_us;
} SensorData;
+ typedef __packed struct
+ {
+ MotionIndicator indicator;
+
+ uint32_t clock_us;
+
+ } MotionIndication;
+
MPU6050Service
(
BLE &ble,
MPU6050 &mpu6050,
+ PinName interruptPin,
const SensorData *initialData = NULL,
const MeasuringRange measuringRange = ACCEL_RANGE_2G | GYRO_RANGE_250,
const uint32_t masterClock_us = 0
) :
gattServer(ble.gattServer()),
mpu(mpu6050),
- timeAccumulationTicker(),
+ intIn(interruptPin),
+ timer(),
_isRunning(false),
updateSensorDataCharacteristicTrigger(false),
- time(masterClock_us),
+ timerOffset_us(masterClock_us),
sensorDataCharacteristic
(
@@ -66,6 +99,12 @@
GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY // improving latency by repurposing notifications and "writing" the client
// (i.e. indicating the client but not wait for a response)
),
+ motionIndicatorCharaceristic
+ (
+ UUID_MPU6050_MOTION_INDICATOR_CHAR,
+ NULL,
+ GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY
+ ),
measuringRangeCharacteristic
(
UUID_MPU6050_MEASURING_RANGE_CHAR,
@@ -82,12 +121,11 @@
static bool serviceAdded = false;
if (serviceAdded)
return;
-
- timeAccumulationTicker.attach(&MPU6050::timeAccumulationPeriodicCallback, 1.0f / (float)(1000 * 1000 * timerResolution_us));
GattCharacteristic *charTable[] =
{
&sensorDataCharacteristic,
+ &motionIndicatorCharaceristic,
&measuringRangeCharacteristic,
&masterClockCharacteristic
};
@@ -98,6 +136,8 @@
gattServer.onDataSent(this, &MPU6050Service::dataSentCallback);
gattServer.onDataWritten(this, &MPU6050Service::dataWrittenCallback);
+ intIn.rise(this, &MPU6050Service::interruptRiseCallback);
+
serviceAdded = true;
}
@@ -113,6 +153,10 @@
_isRunning = false;
return false;
}
+
+ timer.stop();
+ timer.reset();
+ timer.start();
_isRunning = true;
@@ -124,42 +168,42 @@
}
void stop()
- {
+ {
_isRunning = false;
}
void handleService()
{
- if (_isRunning && updateSensorDataCharacteristicTrigger)
- updateSensorDataCharacteristic();
+ if (_isRunning)
+ {
+ if (updateMeasuringRangeTrigger)
+ updateMeasuringRange();
+ if (updateSensorDataCharacteristicTrigger)
+ updateSensorDataCharacteristic();
+ if (updateMotionIndicatorCharacteristicTrigger)
+ updateMotionIndicatorCharacteristic();
+ }
}
- private:
- static const uint32_t timerResolution_us = 100;
+ private:
+ static const int MAX_SENSOR_POLLS_PER_UPDATE = 15;
GattServer &gattServer;
MPU6050 &mpu;
- Ticker timeAccumulationTicker;
+ InterruptIn intIn;
+ Timer timer;
volatile bool _isRunning;
bool updateSensorDataCharacteristicTrigger;
-
- uint32_t time;
+ bool updateMotionIndicatorCharacteristicTrigger;
+ bool updateMeasuringRangeTrigger;
- ReadOnlyGattCharacteristic<SensorData> sensorDataCharacteristic;
- ReadWriteGattCharacteristic<MeasuringRange> measuringRangeCharacteristic;
- WriteOnlyGattCharacteristic<uint32_t> masterClockCharacteristic;
+ uint32_t timerOffset_us;
- union
- {
- struct
- {
- MPU6050::SensorReading reading;
- uint32_t clock_us;
- } input;
-
- SensorData data;
- } converter;
+ ReadOnlyGattCharacteristic<SensorData> sensorDataCharacteristic;
+ ReadOnlyGattCharacteristic<MotionIndicator> motionIndicatorCharaceristic;
+ WriteOnlyGattCharacteristic<MeasuringRange> measuringRangeCharacteristic;
+ WriteOnlyGattCharacteristic<uint32_t> masterClockCharacteristic;
void dataSentCallback(unsigned count)
{
@@ -170,34 +214,124 @@
void dataWrittenCallback(const GattWriteCallbackParams *context)
{
- // some writable characteristics were updated by the client
+ // update master clock immediately
+ if (context->handle == masterClockCharacteristic.getValueAttribute().getHandle())
+ {
+ timer.stop();
+ timer.reset();
+
+ timerOffset_us = *((uint32_t*)context->data);
+
+ timer.start();
+ }
+ // delay measuring range update
+ else if (context->handle == measuringRangeCharacteristic.getValueAttribute().getHandle())
+ {
+ updateMeasuringRangeTrigger = true;
+ }
}
- void timeAccumulationPeriodicCallback()
+ void interruptRiseCallback()
{
- // we do this here to achieve a better resolution
- time += timerResolution_us;
+ updateMotionIndicatorCharacteristicTrigger = true;
}
void updateSensorDataCharacteristic()
- {
- // flood the transmission buffer of the BLE stack until it's full
- do
- {
- if (mpu.getMotionAndTemperature(&(converter.input.reading)))
- {
- if (!gattServer.write(sensorDataCharacteristic.getValueHandle(), reinterpret_cast<uint8_t*>(&(converter.data)), sizeof(SensorData)) == BLE_ERROR_NONE)
+ {
+ updateSensorDataCharacteristicTrigger = false;
+
+ int n = 0;
+ SensorData data;
+
+ for (; n < MAX_SENSOR_POLLS_PER_UPDATE; n++)
+ if (mpu.getMotionAndTemperature
+ (
+ (int16_t*)&(data.accelX), (int16_t*)&(data.accelY), (int16_t*)&(data.accelZ),
+ (int16_t*)&(data.temperature),
+ (int16_t*)&(data.gyroX), (int16_t*)&(data.gyroY), (int16_t*)&(data.gyroZ),
+ 0.0
+ ))
+ {
+ data.clock_us = timer.read_us() + timerOffset_us;
+
+ if (gattServer.write(sensorDataCharacteristic.getValueHandle(), (uint8_t*)&data, sizeof(SensorData)) != BLE_ERROR_NONE)
break;
- }
- // write data in characteristic and repeat
- } while (true);
+ }
+
+ if (n == 0)
+ updateSensorDataCharacteristicTrigger = true;
+ }
+
+ void updateMotionIndicatorCharacteristic()
+ {
+ updateMotionIndicatorCharacteristicTrigger = false;
- // error case or buffer is full (or break out of loop);
- // this also means the last data update will be lost, if not sent again
- // (we are ignoring this fact and just keep on going, because we're NOT interested in the QUALITY of data, but in the QUANTITY
+ MPU6050::Interrupt intSet = static_cast<MPU6050::Interrupt>(0);
+ MPU6050::Motion motSet = static_cast<MPU6050::Motion>(0);
+ MotionIndicator mot;
+
+ mpu.getInterruptStatuses(&intSet);
+ mpu.getMotionStatus(&motSet);
+
+ mot = *((MotionIndicator*)&motSet);
+
+ if ((intSet & MPU6050::INT_FREEFALL) != 0)
+ mot = mot | MOTION_IND_FREEFALL;
+ else
+ mot = mot & static_cast<MotionIndicator>(~0x02);
+
+ gattServer.write(motionIndicatorCharaceristic.getValueHandle(), (uint8_t*)&mot, sizeof(MotionIndicator));
}
- bool deviceReset()
+ void updateMeasuringRange()
+ {
+ static const uint16_t sizeOfMeasuringRange = sizeof(MeasuringRange);
+
+ updateMeasuringRangeTrigger = false;
+
+ union
+ {
+ MeasuringRange range;
+ uint8_t bytes[sizeOfMeasuringRange];
+ } conv;
+
+ if (gattServer.read(measuringRangeCharacteristic.getValueHandle(), conv.bytes, (uint16_t*)&sizeOfMeasuringRange) == BLE_ERROR_NONE)
+ {
+ switch (conv.range & ACCEL_RANGE_MASK)
+ {
+ case ACCEL_RANGE_2G:
+ mpu.setAccelRange(MPU6050::ACCEL_RANGE_2G);
+ break;
+ case ACCEL_RANGE_4G:
+ mpu.setAccelRange(MPU6050::ACCEL_RANGE_4G);
+ break;
+ case ACCEL_RANGE_8G:
+ mpu.setAccelRange(MPU6050::ACCEL_RANGE_8G);
+ break;
+ case ACCEL_RANGE_16G:
+ mpu.setAccelRange(MPU6050::ACCEL_RANGE_16G);
+ break;
+ }
+
+ switch (conv.range & GYRO_RANGE_MASK)
+ {
+ case GYRO_RANGE_250:
+ mpu.setGyroRange(MPU6050::GYRO_RANGE_250);
+ break;
+ case GYRO_RANGE_500:
+ mpu.setGyroRange(MPU6050::GYRO_RANGE_500);
+ break;
+ case GYRO_RANGE_1000:
+ mpu.setGyroRange(MPU6050::GYRO_RANGE_1000);
+ break;
+ case GYRO_RANGE_2000:
+ mpu.setGyroRange(MPU6050::GYRO_RANGE_2000);
+ break;
+ }
+ }
+ }
+
+ inline bool deviceReset()
{
uint8_t id;
@@ -205,8 +339,9 @@
mpu.getDeviceId(&id) &&
(id == MPU6050::MPU6050_ID) &&
mpu.reset() &&
- mpu.setSleepEnabled(false) &&
- mpu.setFIFOEnabled(false);
+ mpu.setClockSource(MPU6050::CLOCK_SRC_INTERNAL_8MHZ) &&
+ mpu.setInterruptsEnabled(MPU6050::INT_FREEFALL | MPU6050::INT_MOTION | MPU6050::INT_ZERO_MOTION) &&
+ mpu.setSleepEnabled(false);
}
};
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 26 Jun 2015 |
| Imports: | 95 |
| Forks: | 0 |
| Commits: | 14 |
| Dependents: | 0 |
| Dependencies: | 4 |
| Followers: | 5 |
| Issues: | 0 |
The code in this repository is Apache licensed.