bla

Dependencies:   BLE_API MPU9250 SEGGER_RTT mbed nRF51822 X_NUCLEO_IDB0XA1

Files at this revision

API Documentation at this revision

Comitter:
MarijnJ
Date:
Fri Mar 02 10:44:47 2018 +0000
Commit message:
bla

Changed in this revision

BLE_API.lib Show annotated file Show diff for this revision Revisions of this file
MPU9250.lib Show annotated file Show diff for this revision Revisions of this file
SEGGER_RTT.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
mbed_app.json Show annotated file Show diff for this revision Revisions of this file
nRF51822.lib Show annotated file Show diff for this revision Revisions of this file
shields/TARGET_ST_BLUENRG.lib Show annotated file Show diff for this revision Revisions of this file
--- /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
--- /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
--- /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
--- /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();
+			}
+		}
+	}
+}
--- /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
--- /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
--- /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
--- /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