Marijn Jeurissen
bla
Dependencies: BLE_API MPU9250 SEGGER_RTT mbed nRF51822 X_NUCLEO_IDB0XA1
Diff: main.cpp
- Revision:
- 0:4dc21c013b2a
--- /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();
+ }
+ }
+ }
+}