Revision 0:4dc21c013b2a, committed 2018-03-02

Comitter:

MarijnJ

Date:

Fri Mar 02 10:44:47 2018 +0000

Commit message:

bla

Changed in this revision

diff -r 000000000000 -r 4dc21c013b2a BLE_API.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BLE_API.lib	Fri Mar 02 10:44:47 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/Bluetooth-Low-Energy/code/BLE_API/#65474dc93927

diff -r 000000000000 -r 4dc21c013b2a MPU9250.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.lib	Fri Mar 02 10:44:47 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/anyThing-Connected/code/MPU9250/#264ee5acfe00

diff -r 000000000000 -r 4dc21c013b2a SEGGER_RTT.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SEGGER_RTT.lib	Fri Mar 02 10:44:47 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/anyThing-Connected/code/SEGGER_RTT/#7dcd871d726b

diff -r 000000000000 -r 4dc21c013b2a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 02 10:44:47 2018 +0000
@@ -0,0 +1,646 @@
+/*
+
+	MPU9250 Basic Example Code
+	by: Kris Winer
+	date: April 1, 2014
+	license: Beerware - Use this code however you'd like. If you
+	find it useful you can buy me a beer some time.
+
+	Demonstrate basic MPU-9250 functionality including parameterizing the
+	register addresses, initializing the sensor, getting properly scaled
+	accelerometer, gyroscope, and magnetometer data out. Added display functions
+	to allow display to on breadboard monitor. Addition of 9 DoF sensor fusion
+	using open source Madgwick and Mahony filter algorithms. Sketch runs on
+	the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
+
+	-----------------------------------------------------------------------
+
+	Adapted by Marijn Jeurissen for the anyThing Connected Sensor Sticker based on Nordic nRF51822
+	date: February 16, 2018
+
+
+*/
+
+
+// If you define DEBUG_MODE, then RTT debug prints will be enabled
+#define DEBUG_MODE
+
+
+#include "mbed.h"
+#include "ble/BLE.h"
+#include "ble/services/DeviceInformationService.h"
+#include "MPU9250.h"
+#include "softdevice_handler.h"
+
+#ifdef DEBUG_MODE
+#include "SEGGER_RTT.h"
+#include "SEGGER_RTT.c"
+#include "SEGGER_RTT_printf.c"
+#endif
+
+
+// Sensor config
+#define SENSOR_SDA P0_8
+#define SENSOR_SCL P0_9
+
+// Declination at Delft is 1 degrees 3 minutes on 2018-02-16
+#define SENSOR_DECLINATION (-1.05f)
+
+
+// BLE TxPower values for advertising and connection, highest possible
+// TODO: Remove this shit as it DOESN'T FUCKING DO ANYTHING!
+static const int TX_POWER_ADVERTISING	= 10;
+static const int TX_POWER_CONNECTION	= 10;
+
+
+// Used for triggering a sensor polling, updating all sensor values
+// TODO: Use multiple values/bit masking to allow fine-grained control of what
+// sensor value is read/polled
+// Always read sensor value at startup
+static volatile bool triggerSensorPolling = true;
+
+// Array used to keep track of sensor data, 10 floats in the format:
+// Acceleration x, y, z, Gyrometer x, y, z, Magnetometer x, y, z, Temperature
+static const uint16_t NUM_MEASUREMENTS = 10;
+static float measurements[NUM_MEASUREMENTS] = {0.0f};
+
+
+// Buffer for float printing
+static const uint16_t BUFFER_SIZE = 50;
+static char buffer[BUFFER_SIZE];
+
+
+// Define UUIDs for service and characteristics
+static const uint16_t customServiceUUID	= 0xFFFF;	// Custom UUID, FFFF is reserved for development
+static const uint16_t readCharUUID		= 0xA001;
+static const uint16_t writeCharUUID		= 0xA002;
+
+// Define BLE configuration things
+static const char DEVICE_NAME[]			= "AnyConSensor";
+static const uint16_t uuid16List[]		= {customServiceUUID,
+											GattService::UUID_DEVICE_INFORMATION_SERVICE};
+static const int ADVERTISING_INTERVAL	= 1000;		// in ms
+
+// Set Up custom Characteristics
+static const uint16_t READ_CHAR_SIZE = NUM_MEASUREMENTS * sizeof(float);
+static const uint16_t WRITE_CHAR_SIZE = NUM_MEASUREMENTS * sizeof(float);
+
+static uint8_t readValue[READ_CHAR_SIZE] = {0};
+ReadOnlyArrayGattCharacteristic<uint8_t, READ_CHAR_SIZE> readChar(readCharUUID, readValue);
+
+static uint8_t writeValue[WRITE_CHAR_SIZE] = {0};
+WriteOnlyArrayGattCharacteristic<uint8_t, WRITE_CHAR_SIZE> writeChar(writeCharUUID, writeValue);
+
+// Set up custom service
+GattCharacteristic  *characteristics[] = {&readChar, &writeChar};
+GattService customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic*));
+
+DeviceInformationService  *deviceInfoService;
+
+//Initialize tap variables
+bool zTap = 0, zTapStarted = 0;
+int zDiff = 0, zTapLimit = 0, zTapDelay = 0;
+int tapDiff = 0, tapCount = 0, lastTapFrame = 0, tapCycleStart = 0 , tapCycleDuration = 0, now = 0, timerCycle = 0;
+float zMin = 0, zMax = 0, zTapStartValue = 0;
+
+// Tap Settings
+int tapZThresh = 3000 ; //x and y axis acceleration threshold to trigger tap in mg
+int tapPulseLimit = 50 ; //Not a tap if acceleration duration over this limit
+int tapDelayDuration = 500; //Minimum time between taps in ms
+int tapCycleLimit = 3000; //Maximum cycle duration to detect multitap from first tap in ms
+int tapAction = 3; //Number of taps needed to trigger multitap action      
+
+int toInt(float x) {
+	return (int) ((x >= 0.0f) ? x + 0.5f : x - 0.5f);
+}
+
+int getTime(int counter, int shift) {
+	return (int)((counter / 60000.0f) + shift);
+}
+
+void resetTap()
+{
+    zTapLimit = now + tapPulseLimit;
+    zMax = measurements[2];
+    zMin = measurements[2];
+    zDiff = 0;
+    zTap = 0;
+    zTapStarted = 0;
+    zTapStartValue = 0; 
+}
+
+void detectTap()
+{
+    now = t.read_ms();
+    if (now < zTapLimit)
+    {
+        if (measurements[2] < zMin) zMin = measurements[2];
+        if (measurements[2] > zMax)  zMax = measurements[2];
+        tapDiff = toInt(1000*(zMax - zMin));
+        if (tapDiff > zDiff) zDiff = tapDiff;
+    }
+    else  //Reset tap difference values every 50ms max tap detection duration time
+    {
+        resetTap();
+    }
+    
+    if ((zDiff > tapZThresh) //if acceleration is above threshold
+        && (now > zTapDelay)  //only register tap again after tap delay
+        && (zTapStarted == 0)) //wait for acceleration to end
+    {
+        resetTap();
+        zTapStarted = 1;
+        zTapStartValue = measurements[2];
+    }
+    if (now < zTapLimit && zTapStarted == 1) //Check if acceleration stops within pulse window
+    {
+        if (measurements[2] < zTapStartValue) //if acceleration falls again within limit, tap detected
+        {
+            resetTap();
+            zTap = 1;
+            zTapDelay = now + tapDelayDuration;
+            #ifdef DEBUG_MODE
+				SEGGER_RTT_WriteString(0, "---------------------------------Tap detected\n"); 
+			#endif
+        }
+    }
+    else
+    {
+        resetTap();
+    }
+    // Detects taps on the x axis and resets some of the flags
+    if (zTap)    // check for tap
+    {
+        now = t.read_ms();
+        if (tapCount == 0) tapCycleStart = now;  // if first tap reset tap cycle
+        tapCount++;                            // increment tap counter
+        if ((now - tapCycleStart) > tapCycleLimit || tapCount >= tapAction) tapCount = 0;    //Reset tap count after cycle ends
+        if (tapCount == tapAction) 
+        {
+            #ifdef DEBUG_MODE
+				SEGGER_RTT_WriteString(0, "---------------------------------Triple tap\n"); // do we have 3 taps during cycle?
+			#endif
+        }
+    }
+}
+
+/*
+	Restart advertising when client disconnects
+*/
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *) {
+	BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
+}
+
+/*
+	Callback when a device disconnects
+*/
+void timeoutCallback(Gap::TimeoutSource_t source) {
+	#ifdef DEBUG_MODE
+	SEGGER_RTT_printf(0, "Fuck, got a timeout on BLE connection with error %d...", source);
+	#endif
+}
+/*
+	Callback function to check if we need to update sensor values
+*/
+void periodicCallback() {
+	// Note that the periodicCallback() executes in interrupt context, so it is
+	// safer to do heavy-weight sensor polling from the main thread
+	triggerSensorPolling = true;
+}
+
+
+/*
+	Handle writes to writeCharacteristic
+	TODO: Remove this or find a use for it
+*/
+void writeCharCallback(const GattWriteCallbackParams  *params) {
+	// Check to see what characteristic was written, by handle
+	if (params->handle == writeChar.getValueHandle()) {
+		// toggle LED if only 1 byte is written
+		if (params->len == 1) {
+			(params->data[0] == 0x00) ? printf("led on\n\r") : printf("led off\n\r");
+		} else {
+			// Print the data if more than 1 byte is written
+			#ifdef DEBUG_MODE
+			SEGGER_RTT_printf(0, "Data received: length = %d, data = 0x", params->len);
+			#endif
+
+			for(int x=0; x < params->len; x++) {
+				printf("%x", params->data[x]);
+			}
+
+			printf("\n\r");
+		}
+	}
+}
+
+/*
+	Initialization callback
+*/
+void bleInitComplete(BLE::InitializationCompleteCallbackContext  *params) {
+	BLE &ble = params->ble;
+	ble_error_t error = params->error;
+
+	if (error != BLE_ERROR_NONE) {
+		return;
+	}
+
+	ble.gap().onDisconnection(disconnectionCallback);
+	// ble.gattServer().onDataWritten(writeCharCallback);
+	ble.gap().onTimeout(timeoutCallback);
+
+	// Setup information service
+	deviceInfoService = new DeviceInformationService(ble, "ARM", "Model1", "SN1",
+		"hw-rev1", "fw-rev1", "soft-rev1");
+
+	// Setup advertising, BLE only, no classic BT
+	ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+	ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
+		(uint8_t *)uuid16List, sizeof(uuid16List));
+	ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME,
+		(uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
+
+	ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+	ble.gap().setAdvertisingInterval(ADVERTISING_INTERVAL);
+	ble.gap().setAdvertisingTimeout(0);
+
+	// Get current advertising power level...
+	// TODO: Remove this useless crap since it doesnt do anything...
+	/*
+	GapAdvertisingData payload = ble.gap().getAdvertisingPayload();
+
+	#ifdef DEBUG_MODE
+	SEGGER_RTT_printf(0, "Current advertising Tx power: %d dBm\n", payload.TX_POWER_LEVEL);
+	#endif
+
+	error = ble.gap().accumulateAdvertisingPayloadTxPower(TX_POWER_ADVERTISING);
+
+	if (error != BLE_ERROR_NONE) {
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_printf(0, "Failed to set advertising TX power to %d dBm!\n", TX_POWER_ADVERTISING);
+		#endif
+	} else {
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_printf(0, "Set advertising TX power to %d dBm!\n", TX_POWER_ADVERTISING);
+		#endif
+	}
+
+	// Get current advertising power level...
+	payload = ble.gap().getAdvertisingPayload();
+
+	#ifdef DEBUG_MODE
+	SEGGER_RTT_printf(0, "Current advertising Tx power: %d dBm\n", payload.TX_POWER_LEVEL);
+	#endif
+
+	error = ble.gap().setTxPower(TX_POWER_CONNECTION);
+
+	if (error != BLE_ERROR_NONE) {
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_printf(0, "Failed to set general TX power to %d dBm!\n", TX_POWER_CONNECTION);
+		#endif
+	} else {
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_printf(0, "Set general TX power to %d dBm!\n", TX_POWER_CONNECTION);
+		#endif
+	}
+	*/
+
+	ble.addService(customService);
+	ble.gap().startAdvertising();
+}
+
+void printPowerValues() {
+	const int8_t *power_values;
+	size_t count;
+
+	BLE &ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
+	ble.gap().getPermittedTxPowerValues(&power_values, &count);
+
+	for (int i = 0; i < count; ++i) {
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_printf(0, "Got power value: %d dBm\n", power_values[i]);
+		#endif
+	}
+}
+
+/*
+	Copies the measurements float array into the BLE readChar.
+	Make sure the destination buffer has sufficient space! Its size should be
+	at least 4 times that of measurements. This function assumes that
+	sizeof(float) = 4 bytes, aka 32 bits, aka 4 uint8_t values.
+*/
+void copyMeasurementsToBLEChar(float *measurements, uint8_t *destination, uint16_t numElements) {
+	for (uint16_t i = 0; i < numElements; i++) {
+		// Use a dirty union to slice the floats into 4 uint8_t's
+		union {
+			float f;
+			uint8_t uints[4];
+		} dirtyHack;
+
+		dirtyHack.f = measurements[i];
+		destination[i * 4]		= dirtyHack.uints[0];
+		destination[i * 4 + 1]	= dirtyHack.uints[1];
+		destination[i * 4 + 2]	= dirtyHack.uints[2];
+		destination[i * 4 + 3]	= dirtyHack.uints[3];
+	}
+}
+
+
+/*
+	Returns true/false if the sensor has new data. If not, no measurements are
+	done. If the sensor has new data, then the measurements array
+	(sized appropriately) will be filled according to the format specified at
+	the top of this file.
+*/
+bool measureSensor(MPU9250 *mpu9250, float *measurements) {
+
+	if (!mpu9250->hasNewData()) {
+		return false;
+	}
+
+	// Temporary variables to store results
+	float ax, ay, az, gx, gy, gz, mx, my, mz, temperature;
+
+	// Get the accleration value into actual g's
+	mpu9250->readAccelData(&ax, &ay, &az);
+
+	// Get the gyro values into actual degrees per second
+	mpu9250->readGyroData(&gx, &gy, &gz);
+
+	// Get actual magnetometer value, this depends on scale being set
+	mpu9250->readMagData(&gx, &gy, &gz);
+
+	// Temperature in degrees Celsius
+	temperature = mpu9250->getTemperature();
+
+	// Store it in the measurements array
+	measurements[0] = ax;
+	measurements[1] = ay;
+	measurements[2] = az;
+	measurements[3] = gx;
+	measurements[4] = gy;
+	measurements[5] = gz;
+	measurements[6] = mx;
+	measurements[7] = my;
+	measurements[8] = mz;
+	measurements[9] = temperature;
+    
+    detectTap();
+	return true;
+}
+
+
+/*
+	Sets up the given sensor
+*/
+void setupSensor(MPU9250  *mpu9250) {
+	#ifdef DEBUG_MODE
+	SEGGER_RTT_printf(0, "CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+	#endif
+
+	float SelfTest[6];
+
+	// Read the WHO_AM_I register, this is a good test of communication
+	// Read WHO_AM_I register for MPU-9250
+	uint8_t whoami = mpu9250->getWhoAmI();
+
+	// WHO_AM_I should always be 0x71
+	if (whoami == CORRECT_WHO_AM_I) {
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_WriteString(0, "MPU9250 is online...\n\n");
+		#endif
+
+		wait(1);
+
+		// Reset registers to default in preparation for device calibration
+		mpu9250->resetMPU9250();
+
+		// Start by performing self test and reporting values
+		mpu9250->MPU9250SelfTest(SelfTest);
+
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_printf(0, "x-axis self test: accel trim within : %d % of factory value\n",
+			toInt(SelfTest[0]));
+		SEGGER_RTT_printf(0, "y-axis self test: accel trim within : %d % of factory value\n",
+			toInt(SelfTest[1]));
+		SEGGER_RTT_printf(0, "z-axis self test: accel trim within : %d % of factory value\n",
+			toInt(SelfTest[2]));
+		SEGGER_RTT_printf(0, "x-axis self test: gyration trim within : %d % of factory value\n",
+			toInt(SelfTest[3]));
+		SEGGER_RTT_printf(0, "y-axis self test: gyration trim within : %d % of factory value\n",
+			toInt(SelfTest[4]));
+		SEGGER_RTT_printf(0, "z-axis self test: gyration trim within : %d % of factory value\n\n",
+			toInt(SelfTest[5]));
+		#endif
+
+		// Calibrate gyro and accelerometers, load biases in bias registers
+		mpu9250->calibrateMPU9250();
+
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_printf(0, "x gyro bias = %d\n", toInt(mpu9250->gyroBias[0]));
+		SEGGER_RTT_printf(0, "y gyro bias = %d\n", toInt(mpu9250->gyroBias[1]));
+		SEGGER_RTT_printf(0, "z gyro bias = %d\n", toInt(mpu9250->gyroBias[2]));
+		SEGGER_RTT_printf(0, "x accel bias = %d\n", toInt(mpu9250->accelBias[0]));
+		SEGGER_RTT_printf(0, "y accel bias = %d\n", toInt(mpu9250->accelBias[1]));
+		SEGGER_RTT_printf(0, "z accel bias = %d\n\n", toInt(mpu9250->accelBias[2]));
+		#endif
+
+		wait(2);
+
+		// Initialize device for active mode read of acclerometer, gyroscope, and temperature
+		mpu9250->initMPU9250();
+
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_WriteString(0, "MPU9250 initialized for active data mode....\n");
+		#endif
+
+		// Initialize device for active mode read of magnetometer
+		mpu9250->initAK8963();
+
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_WriteString(0, "AK8963 initialized for active data mode....\n");
+		SEGGER_RTT_printf(0, "Accelerometer full-scale range = %d g\n",
+			toInt(2.0f * (float) (1<<mpu9250->Ascale)));
+		SEGGER_RTT_printf(0, "Gyroscope full-scale range = %d deg/s\n",
+			toInt(250.0f * (float) (1<<mpu9250->Gscale)));
+
+		if (mpu9250->Mscale == 0) {
+			SEGGER_RTT_WriteString(0, "Magnetometer resolution = 14 bits\n");
+		}
+
+		if (mpu9250->Mscale == 1) {
+			SEGGER_RTT_WriteString(0, "Magnetometer resolution = 16 bits\n");
+		}
+
+		if (mpu9250->Mmode == 2) {
+			SEGGER_RTT_WriteString(0, "Magnetometer ODR = 8 Hz\n");
+		}
+
+		if (mpu9250->Mmode == 6) {
+			SEGGER_RTT_WriteString(0, "Magnetometer ODR = 100 Hz\n");
+		}
+
+		SEGGER_RTT_WriteString(0, "\n");
+
+		// Scale resolutions per LSB for the sensors
+		SEGGER_RTT_printf(0, "Accelerometer sensitivity is %d LSB/g \n",
+			toInt(1.0f / mpu9250->getAres()));
+		SEGGER_RTT_printf(0, "Gyroscope sensitivity is %d LSB/deg/s \n",
+			toInt(1.0f / mpu9250->getGres()));
+		SEGGER_RTT_printf(0, "Magnetometer sensitivity is %d LSB/G \n",
+			toInt(1.0f / mpu9250->getMres()));
+		#endif
+
+		wait(1);
+	} else {
+		// Loop forever if communication doesn't happen...
+		#ifdef DEBUG_MODE
+		SEGGER_RTT_printf(0, "Could not connect to MPU9250: 0x%x \n", whoami);
+		#endif
+		while(1);
+	}
+}
+
+
+
+/*
+	Main function
+*/
+int main() {
+	// Setup periodicCallback ticker
+	Ticker ticker;
+	ticker.attach(&periodicCallback, 1);
+
+	BLE &ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
+	ble.init(bleInitComplete);
+
+	// SpinWait for initialization to complete. This is necessary because the
+	// BLE object is used in the main loop below.
+	while (ble.hasInitialized() == false) { /* spin loop */ }
+
+	#ifdef DEBUG_MODE
+	SEGGER_RTT_WriteString(0, "Initialized BLE...\n");
+	#endif
+
+	printPowerValues();
+
+	// Setup temporary array of 10 bytes so we don't have to use the heap
+	uint8_t newReadValue[READ_CHAR_SIZE] = {0};
+
+	// Setup I2C and MPU9250 sensor
+	I2C i2cConnection(P0_8, P0_9);
+	MPU9250 mpu9250(&i2cConnection);
+	setupSensor(&mpu9250);
+
+	Timer t;
+	t.start();
+
+	// Variables to hold latest sensor data values
+	//float pitch, yaw, roll;
+	bool sensorHasNewData = false;
+
+	// Used to control display output rate
+	float shift = 0.0f;
+	int delt_t = 0;
+	int count = 0;
+	int cycle = 0;
+
+	// Used to calculate integration interval
+	int lastUpdate = 0, firstUpdate = 0, currentTime = 0;                                                         
+
+    resetTap();
+    
+	while (1) {
+		// If not enough measurements for the sensor have been done, or the
+		// trigger has been activated, do a measurement
+		if (triggerSensorPolling || !mpu9250.sufficientMeasurements) {
+			triggerSensorPolling = false;
+
+			// Check if sensor has new data
+			sensorHasNewData = measureSensor(&mpu9250, measurements);
+			currentTime = t.read_us();
+
+			// Set integration time by time elapsed since last filter update
+			mpu9250.deltat = (float) ((currentTime - lastUpdate) / 1000000.0f);
+			lastUpdate = currentTime;
+
+			if (lastUpdate - firstUpdate > 10000000.0f) {
+				// Decrease filter gain and increase bias drift gain after stabilized
+				mpu9250.beta = 0.04;
+				mpu9250.zeta = 0.015;
+				mpu9250.sufficientMeasurements = true;
+			}
+
+			// Pass gyro rate as rad/s
+			// mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx * PI/180.0f,
+			// gy * PI/180.0f, gz * PI/180.0f, my, mx, mz);
+			/*mpu9250.MahonyQuaternionUpdate(measurements[0], measurements[1],
+				measurements[2], DEG2RAD(measurements[3]), DEG2RAD(measurements[4]),
+				DEG2RAD(measurements[5]), measurements[6], measurements[7], measurements[8]);
+            */
+			// Serial print 1 s rate independent of data rates
+			delt_t = t.read_ms() - count;
+
+			// Update print once per second independent of read rate
+			#ifdef DEBUG_MODE
+			SEGGER_RTT_printf(0, "\n\nax = %d", toInt(1000 * measurements[0]));
+			SEGGER_RTT_printf(0, " ay = %d", toInt(1000 * measurements[1]));
+			SEGGER_RTT_printf(0, " az = %d mg\n", toInt(1000 * measurements[2]));
+
+			SEGGER_RTT_printf(0, "gx = %d", toInt(measurements[3]));
+			SEGGER_RTT_printf(0, " gy = %d", toInt(measurements[4]));
+			SEGGER_RTT_printf(0, " gz = %d deg/s\n", toInt(measurements[5]));
+
+			SEGGER_RTT_printf(0, "mx = %d", toInt(measurements[6]));
+			SEGGER_RTT_printf(0, " my = %d", toInt(measurements[7]));
+			SEGGER_RTT_printf(0, " mz = %d mG\n", toInt(measurements[8]));
+
+			SEGGER_RTT_printf(0, "Temperature = %d C\n\n", toInt(measurements[9]));
+			#endif
+
+			// Get yaw, pitch and roll
+			/*mpu9250.getYawPitchRoll(&yaw, &pitch, &roll, SENSOR_DECLINATION);
+
+			#ifdef DEBUG_MODE
+			SEGGER_RTT_printf(0, "Yaw: %d Pitch: %d Roll: %d\n\n",
+				toInt(yaw), toInt(pitch), toInt(roll));
+			#endif*/
+
+			count = t.read_ms();
+
+			#ifdef DEBUG_MODE
+			SEGGER_RTT_printf(0, "Time active: %d minutes\n----------------",
+				getTime(count, shift));
+			#endif
+
+			if (count > 1<<21) {
+				// Start the timer over again if ~30 minutes has passed
+				t.stop();
+				t.reset();
+				t.start();
+				count = 0;
+
+				#ifdef DEBUG_MODE
+				SEGGER_RTT_printf(0, "Resetting timer! t.read_ms() now gives: %d\n",
+					t.read_ms());
+				#endif
+
+				mpu9250.deltat = 0;
+				lastUpdate = t.read_us();
+				shift = (++cycle * 34.9525f);
+			}
+
+			if (ble.getGapState().connected) {
+				// Send measurements via bluetooth
+				copyMeasurementsToBLEChar(measurements, readValue, NUM_MEASUREMENTS);
+				ble.gattServer().write(readChar.getValueHandle(), readValue, READ_CHAR_SIZE);
+			}
+		} else {
+			// Save power by waiting for BLE events, if we already have enough
+			// measurements for sufficient sensor accuracy
+			if (mpu9250.sufficientMeasurements) {
+				ble.waitForEvent();
+			}
+		}
+	}
+}

diff -r 000000000000 -r 4dc21c013b2a mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Mar 02 10:44:47 2018 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/abea610beb85

diff -r 000000000000 -r 4dc21c013b2a mbed_app.json
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed_app.json	Fri Mar 02 10:44:47 2018 +0000
@@ -0,0 +1,13 @@
+{
+    "target_overrides": {
+        "K64F": {
+            "target.features_add": ["BLE"],
+            "target.extra_labels_add": ["ST_BLUENRG"],
+            "target.macros_add": ["IDB0XA1_D13_PATCH"]
+        },
+        "NUCLEO_F401RE": {
+            "target.features_add": ["BLE"],
+            "target.extra_labels_add": ["ST_BLUENRG"]
+        }
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 4dc21c013b2a nRF51822.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nRF51822.lib	Fri Mar 02 10:44:47 2018 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/teams/Nordic-Semiconductor/code/nRF51822/#c90ae1400bf2

diff -r 000000000000 -r 4dc21c013b2a shields/TARGET_ST_BLUENRG.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shields/TARGET_ST_BLUENRG.lib	Fri Mar 02 10:44:47 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/X_NUCLEO_IDB0XA1/#fa98703ece8e