Marijn Jeurissen / Mbed 2 deprecated Sensor_tap_BLE

bla

Dependencies:   BLE_API MPU9250 SEGGER_RTT mbed nRF51822 X_NUCLEO_IDB0XA1

main.cpp@0:4dc21c013b2a, 2018-03-02 (annotated)

Committer:
MarijnJ
Date:
Fri Mar 02 10:44:47 2018 +0000
Revision:
0:4dc21c013b2a
bla

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MarijnJ 0:4dc21c013b2a 1 /*
MarijnJ 0:4dc21c013b2a 2
MarijnJ 0:4dc21c013b2a 3 MPU9250 Basic Example Code
MarijnJ 0:4dc21c013b2a 4 by: Kris Winer
MarijnJ 0:4dc21c013b2a 5 date: April 1, 2014
MarijnJ 0:4dc21c013b2a 6 license: Beerware - Use this code however you'd like. If you
MarijnJ 0:4dc21c013b2a 7 find it useful you can buy me a beer some time.
MarijnJ 0:4dc21c013b2a 8
MarijnJ 0:4dc21c013b2a 9 Demonstrate basic MPU-9250 functionality including parameterizing the
MarijnJ 0:4dc21c013b2a 10 register addresses, initializing the sensor, getting properly scaled
MarijnJ 0:4dc21c013b2a 11 accelerometer, gyroscope, and magnetometer data out. Added display functions
MarijnJ 0:4dc21c013b2a 12 to allow display to on breadboard monitor. Addition of 9 DoF sensor fusion
MarijnJ 0:4dc21c013b2a 13 using open source Madgwick and Mahony filter algorithms. Sketch runs on
MarijnJ 0:4dc21c013b2a 14 the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
MarijnJ 0:4dc21c013b2a 15
MarijnJ 0:4dc21c013b2a 16 -----------------------------------------------------------------------
MarijnJ 0:4dc21c013b2a 17
MarijnJ 0:4dc21c013b2a 18 Adapted by Marijn Jeurissen for the anyThing Connected Sensor Sticker based on Nordic nRF51822
MarijnJ 0:4dc21c013b2a 19 date: February 16, 2018
MarijnJ 0:4dc21c013b2a 20
MarijnJ 0:4dc21c013b2a 21
MarijnJ 0:4dc21c013b2a 22 */
MarijnJ 0:4dc21c013b2a 23
MarijnJ 0:4dc21c013b2a 24
MarijnJ 0:4dc21c013b2a 25 // If you define DEBUG_MODE, then RTT debug prints will be enabled
MarijnJ 0:4dc21c013b2a 26 #define DEBUG_MODE
MarijnJ 0:4dc21c013b2a 27
MarijnJ 0:4dc21c013b2a 28
MarijnJ 0:4dc21c013b2a 29 #include "mbed.h"
MarijnJ 0:4dc21c013b2a 30 #include "ble/BLE.h"
MarijnJ 0:4dc21c013b2a 31 #include "ble/services/DeviceInformationService.h"
MarijnJ 0:4dc21c013b2a 32 #include "MPU9250.h"
MarijnJ 0:4dc21c013b2a 33 #include "softdevice_handler.h"
MarijnJ 0:4dc21c013b2a 34
MarijnJ 0:4dc21c013b2a 35 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 36 #include "SEGGER_RTT.h"
MarijnJ 0:4dc21c013b2a 37 #include "SEGGER_RTT.c"
MarijnJ 0:4dc21c013b2a 38 #include "SEGGER_RTT_printf.c"
MarijnJ 0:4dc21c013b2a 39 #endif
MarijnJ 0:4dc21c013b2a 40
MarijnJ 0:4dc21c013b2a 41
MarijnJ 0:4dc21c013b2a 42 // Sensor config
MarijnJ 0:4dc21c013b2a 43 #define SENSOR_SDA P0_8
MarijnJ 0:4dc21c013b2a 44 #define SENSOR_SCL P0_9
MarijnJ 0:4dc21c013b2a 45
MarijnJ 0:4dc21c013b2a 46 // Declination at Delft is 1 degrees 3 minutes on 2018-02-16
MarijnJ 0:4dc21c013b2a 47 #define SENSOR_DECLINATION (-1.05f)
MarijnJ 0:4dc21c013b2a 48
MarijnJ 0:4dc21c013b2a 49
MarijnJ 0:4dc21c013b2a 50 // BLE TxPower values for advertising and connection, highest possible
MarijnJ 0:4dc21c013b2a 51 // TODO: Remove this shit as it DOESN'T FUCKING DO ANYTHING!
MarijnJ 0:4dc21c013b2a 52 static const int TX_POWER_ADVERTISING = 10;
MarijnJ 0:4dc21c013b2a 53 static const int TX_POWER_CONNECTION = 10;
MarijnJ 0:4dc21c013b2a 54
MarijnJ 0:4dc21c013b2a 55
MarijnJ 0:4dc21c013b2a 56 // Used for triggering a sensor polling, updating all sensor values
MarijnJ 0:4dc21c013b2a 57 // TODO: Use multiple values/bit masking to allow fine-grained control of what
MarijnJ 0:4dc21c013b2a 58 // sensor value is read/polled
MarijnJ 0:4dc21c013b2a 59 // Always read sensor value at startup
MarijnJ 0:4dc21c013b2a 60 static volatile bool triggerSensorPolling = true;
MarijnJ 0:4dc21c013b2a 61
MarijnJ 0:4dc21c013b2a 62 // Array used to keep track of sensor data, 10 floats in the format:
MarijnJ 0:4dc21c013b2a 63 // Acceleration x, y, z, Gyrometer x, y, z, Magnetometer x, y, z, Temperature
MarijnJ 0:4dc21c013b2a 64 static const uint16_t NUM_MEASUREMENTS = 10;
MarijnJ 0:4dc21c013b2a 65 static float measurements[NUM_MEASUREMENTS] = {0.0f};
MarijnJ 0:4dc21c013b2a 66
MarijnJ 0:4dc21c013b2a 67
MarijnJ 0:4dc21c013b2a 68 // Buffer for float printing
MarijnJ 0:4dc21c013b2a 69 static const uint16_t BUFFER_SIZE = 50;
MarijnJ 0:4dc21c013b2a 70 static char buffer[BUFFER_SIZE];
MarijnJ 0:4dc21c013b2a 71
MarijnJ 0:4dc21c013b2a 72
MarijnJ 0:4dc21c013b2a 73 // Define UUIDs for service and characteristics
MarijnJ 0:4dc21c013b2a 74 static const uint16_t customServiceUUID = 0xFFFF; // Custom UUID, FFFF is reserved for development
MarijnJ 0:4dc21c013b2a 75 static const uint16_t readCharUUID = 0xA001;
MarijnJ 0:4dc21c013b2a 76 static const uint16_t writeCharUUID = 0xA002;
MarijnJ 0:4dc21c013b2a 77
MarijnJ 0:4dc21c013b2a 78 // Define BLE configuration things
MarijnJ 0:4dc21c013b2a 79 static const char DEVICE_NAME[] = "AnyConSensor";
MarijnJ 0:4dc21c013b2a 80 static const uint16_t uuid16List[] = {customServiceUUID,
MarijnJ 0:4dc21c013b2a 81 GattService::UUID_DEVICE_INFORMATION_SERVICE};
MarijnJ 0:4dc21c013b2a 82 static const int ADVERTISING_INTERVAL = 1000; // in ms
MarijnJ 0:4dc21c013b2a 83
MarijnJ 0:4dc21c013b2a 84 // Set Up custom Characteristics
MarijnJ 0:4dc21c013b2a 85 static const uint16_t READ_CHAR_SIZE = NUM_MEASUREMENTS * sizeof(float);
MarijnJ 0:4dc21c013b2a 86 static const uint16_t WRITE_CHAR_SIZE = NUM_MEASUREMENTS * sizeof(float);
MarijnJ 0:4dc21c013b2a 87
MarijnJ 0:4dc21c013b2a 88 static uint8_t readValue[READ_CHAR_SIZE] = {0};
MarijnJ 0:4dc21c013b2a 89 ReadOnlyArrayGattCharacteristic<uint8_t, READ_CHAR_SIZE> readChar(readCharUUID, readValue);
MarijnJ 0:4dc21c013b2a 90
MarijnJ 0:4dc21c013b2a 91 static uint8_t writeValue[WRITE_CHAR_SIZE] = {0};
MarijnJ 0:4dc21c013b2a 92 WriteOnlyArrayGattCharacteristic<uint8_t, WRITE_CHAR_SIZE> writeChar(writeCharUUID, writeValue);
MarijnJ 0:4dc21c013b2a 93
MarijnJ 0:4dc21c013b2a 94 // Set up custom service
MarijnJ 0:4dc21c013b2a 95 GattCharacteristic *characteristics[] = {&readChar, &writeChar};
MarijnJ 0:4dc21c013b2a 96 GattService customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic*));
MarijnJ 0:4dc21c013b2a 97
MarijnJ 0:4dc21c013b2a 98 DeviceInformationService *deviceInfoService;
MarijnJ 0:4dc21c013b2a 99
MarijnJ 0:4dc21c013b2a 100 //Initialize tap variables
MarijnJ 0:4dc21c013b2a 101 bool zTap = 0, zTapStarted = 0;
MarijnJ 0:4dc21c013b2a 102 int zDiff = 0, zTapLimit = 0, zTapDelay = 0;
MarijnJ 0:4dc21c013b2a 103 int tapDiff = 0, tapCount = 0, lastTapFrame = 0, tapCycleStart = 0 , tapCycleDuration = 0, now = 0, timerCycle = 0;
MarijnJ 0:4dc21c013b2a 104 float zMin = 0, zMax = 0, zTapStartValue = 0;
MarijnJ 0:4dc21c013b2a 105
MarijnJ 0:4dc21c013b2a 106 // Tap Settings
MarijnJ 0:4dc21c013b2a 107 int tapZThresh = 3000 ; //x and y axis acceleration threshold to trigger tap in mg
MarijnJ 0:4dc21c013b2a 108 int tapPulseLimit = 50 ; //Not a tap if acceleration duration over this limit
MarijnJ 0:4dc21c013b2a 109 int tapDelayDuration = 500; //Minimum time between taps in ms
MarijnJ 0:4dc21c013b2a 110 int tapCycleLimit = 3000; //Maximum cycle duration to detect multitap from first tap in ms
MarijnJ 0:4dc21c013b2a 111 int tapAction = 3; //Number of taps needed to trigger multitap action
MarijnJ 0:4dc21c013b2a 112
MarijnJ 0:4dc21c013b2a 113 int toInt(float x) {
MarijnJ 0:4dc21c013b2a 114 return (int) ((x >= 0.0f) ? x + 0.5f : x - 0.5f);
MarijnJ 0:4dc21c013b2a 115 }
MarijnJ 0:4dc21c013b2a 116
MarijnJ 0:4dc21c013b2a 117 int getTime(int counter, int shift) {
MarijnJ 0:4dc21c013b2a 118 return (int)((counter / 60000.0f) + shift);
MarijnJ 0:4dc21c013b2a 119 }
MarijnJ 0:4dc21c013b2a 120
MarijnJ 0:4dc21c013b2a 121 void resetTap()
MarijnJ 0:4dc21c013b2a 122 {
MarijnJ 0:4dc21c013b2a 123 zTapLimit = now + tapPulseLimit;
MarijnJ 0:4dc21c013b2a 124 zMax = measurements[2];
MarijnJ 0:4dc21c013b2a 125 zMin = measurements[2];
MarijnJ 0:4dc21c013b2a 126 zDiff = 0;
MarijnJ 0:4dc21c013b2a 127 zTap = 0;
MarijnJ 0:4dc21c013b2a 128 zTapStarted = 0;
MarijnJ 0:4dc21c013b2a 129 zTapStartValue = 0;
MarijnJ 0:4dc21c013b2a 130 }
MarijnJ 0:4dc21c013b2a 131
MarijnJ 0:4dc21c013b2a 132 void detectTap()
MarijnJ 0:4dc21c013b2a 133 {
MarijnJ 0:4dc21c013b2a 134 now = t.read_ms();
MarijnJ 0:4dc21c013b2a 135 if (now < zTapLimit)
MarijnJ 0:4dc21c013b2a 136 {
MarijnJ 0:4dc21c013b2a 137 if (measurements[2] < zMin) zMin = measurements[2];
MarijnJ 0:4dc21c013b2a 138 if (measurements[2] > zMax) zMax = measurements[2];
MarijnJ 0:4dc21c013b2a 139 tapDiff = toInt(1000*(zMax - zMin));
MarijnJ 0:4dc21c013b2a 140 if (tapDiff > zDiff) zDiff = tapDiff;
MarijnJ 0:4dc21c013b2a 141 }
MarijnJ 0:4dc21c013b2a 142 else //Reset tap difference values every 50ms max tap detection duration time
MarijnJ 0:4dc21c013b2a 143 {
MarijnJ 0:4dc21c013b2a 144 resetTap();
MarijnJ 0:4dc21c013b2a 145 }
MarijnJ 0:4dc21c013b2a 146
MarijnJ 0:4dc21c013b2a 147 if ((zDiff > tapZThresh) //if acceleration is above threshold
MarijnJ 0:4dc21c013b2a 148 && (now > zTapDelay) //only register tap again after tap delay
MarijnJ 0:4dc21c013b2a 149 && (zTapStarted == 0)) //wait for acceleration to end
MarijnJ 0:4dc21c013b2a 150 {
MarijnJ 0:4dc21c013b2a 151 resetTap();
MarijnJ 0:4dc21c013b2a 152 zTapStarted = 1;
MarijnJ 0:4dc21c013b2a 153 zTapStartValue = measurements[2];
MarijnJ 0:4dc21c013b2a 154 }
MarijnJ 0:4dc21c013b2a 155 if (now < zTapLimit && zTapStarted == 1) //Check if acceleration stops within pulse window
MarijnJ 0:4dc21c013b2a 156 {
MarijnJ 0:4dc21c013b2a 157 if (measurements[2] < zTapStartValue) //if acceleration falls again within limit, tap detected
MarijnJ 0:4dc21c013b2a 158 {
MarijnJ 0:4dc21c013b2a 159 resetTap();
MarijnJ 0:4dc21c013b2a 160 zTap = 1;
MarijnJ 0:4dc21c013b2a 161 zTapDelay = now + tapDelayDuration;
MarijnJ 0:4dc21c013b2a 162 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 163 SEGGER_RTT_WriteString(0, "---------------------------------Tap detected\n");
MarijnJ 0:4dc21c013b2a 164 #endif
MarijnJ 0:4dc21c013b2a 165 }
MarijnJ 0:4dc21c013b2a 166 }
MarijnJ 0:4dc21c013b2a 167 else
MarijnJ 0:4dc21c013b2a 168 {
MarijnJ 0:4dc21c013b2a 169 resetTap();
MarijnJ 0:4dc21c013b2a 170 }
MarijnJ 0:4dc21c013b2a 171 // Detects taps on the x axis and resets some of the flags
MarijnJ 0:4dc21c013b2a 172 if (zTap) // check for tap
MarijnJ 0:4dc21c013b2a 173 {
MarijnJ 0:4dc21c013b2a 174 now = t.read_ms();
MarijnJ 0:4dc21c013b2a 175 if (tapCount == 0) tapCycleStart = now; // if first tap reset tap cycle
MarijnJ 0:4dc21c013b2a 176 tapCount++; // increment tap counter
MarijnJ 0:4dc21c013b2a 177 if ((now - tapCycleStart) > tapCycleLimit || tapCount >= tapAction) tapCount = 0; //Reset tap count after cycle ends
MarijnJ 0:4dc21c013b2a 178 if (tapCount == tapAction)
MarijnJ 0:4dc21c013b2a 179 {
MarijnJ 0:4dc21c013b2a 180 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 181 SEGGER_RTT_WriteString(0, "---------------------------------Triple tap\n"); // do we have 3 taps during cycle?
MarijnJ 0:4dc21c013b2a 182 #endif
MarijnJ 0:4dc21c013b2a 183 }
MarijnJ 0:4dc21c013b2a 184 }
MarijnJ 0:4dc21c013b2a 185 }
MarijnJ 0:4dc21c013b2a 186
MarijnJ 0:4dc21c013b2a 187 /*
MarijnJ 0:4dc21c013b2a 188 Restart advertising when client disconnects
MarijnJ 0:4dc21c013b2a 189 */
MarijnJ 0:4dc21c013b2a 190 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *) {
MarijnJ 0:4dc21c013b2a 191 BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
MarijnJ 0:4dc21c013b2a 192 }
MarijnJ 0:4dc21c013b2a 193
MarijnJ 0:4dc21c013b2a 194 /*
MarijnJ 0:4dc21c013b2a 195 Callback when a device disconnects
MarijnJ 0:4dc21c013b2a 196 */
MarijnJ 0:4dc21c013b2a 197 void timeoutCallback(Gap::TimeoutSource_t source) {
MarijnJ 0:4dc21c013b2a 198 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 199 SEGGER_RTT_printf(0, "Fuck, got a timeout on BLE connection with error %d...", source);
MarijnJ 0:4dc21c013b2a 200 #endif
MarijnJ 0:4dc21c013b2a 201 }
MarijnJ 0:4dc21c013b2a 202 /*
MarijnJ 0:4dc21c013b2a 203 Callback function to check if we need to update sensor values
MarijnJ 0:4dc21c013b2a 204 */
MarijnJ 0:4dc21c013b2a 205 void periodicCallback() {
MarijnJ 0:4dc21c013b2a 206 // Note that the periodicCallback() executes in interrupt context, so it is
MarijnJ 0:4dc21c013b2a 207 // safer to do heavy-weight sensor polling from the main thread
MarijnJ 0:4dc21c013b2a 208 triggerSensorPolling = true;
MarijnJ 0:4dc21c013b2a 209 }
MarijnJ 0:4dc21c013b2a 210
MarijnJ 0:4dc21c013b2a 211
MarijnJ 0:4dc21c013b2a 212 /*
MarijnJ 0:4dc21c013b2a 213 Handle writes to writeCharacteristic
MarijnJ 0:4dc21c013b2a 214 TODO: Remove this or find a use for it
MarijnJ 0:4dc21c013b2a 215 */
MarijnJ 0:4dc21c013b2a 216 void writeCharCallback(const GattWriteCallbackParams *params) {
MarijnJ 0:4dc21c013b2a 217 // Check to see what characteristic was written, by handle
MarijnJ 0:4dc21c013b2a 218 if (params->handle == writeChar.getValueHandle()) {
MarijnJ 0:4dc21c013b2a 219 // toggle LED if only 1 byte is written
MarijnJ 0:4dc21c013b2a 220 if (params->len == 1) {
MarijnJ 0:4dc21c013b2a 221 (params->data[0] == 0x00) ? printf("led on\n\r") : printf("led off\n\r");
MarijnJ 0:4dc21c013b2a 222 } else {
MarijnJ 0:4dc21c013b2a 223 // Print the data if more than 1 byte is written
MarijnJ 0:4dc21c013b2a 224 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 225 SEGGER_RTT_printf(0, "Data received: length = %d, data = 0x", params->len);
MarijnJ 0:4dc21c013b2a 226 #endif
MarijnJ 0:4dc21c013b2a 227
MarijnJ 0:4dc21c013b2a 228 for(int x=0; x < params->len; x++) {
MarijnJ 0:4dc21c013b2a 229 printf("%x", params->data[x]);
MarijnJ 0:4dc21c013b2a 230 }
MarijnJ 0:4dc21c013b2a 231
MarijnJ 0:4dc21c013b2a 232 printf("\n\r");
MarijnJ 0:4dc21c013b2a 233 }
MarijnJ 0:4dc21c013b2a 234 }
MarijnJ 0:4dc21c013b2a 235 }
MarijnJ 0:4dc21c013b2a 236
MarijnJ 0:4dc21c013b2a 237 /*
MarijnJ 0:4dc21c013b2a 238 Initialization callback
MarijnJ 0:4dc21c013b2a 239 */
MarijnJ 0:4dc21c013b2a 240 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params) {
MarijnJ 0:4dc21c013b2a 241 BLE &ble = params->ble;
MarijnJ 0:4dc21c013b2a 242 ble_error_t error = params->error;
MarijnJ 0:4dc21c013b2a 243
MarijnJ 0:4dc21c013b2a 244 if (error != BLE_ERROR_NONE) {
MarijnJ 0:4dc21c013b2a 245 return;
MarijnJ 0:4dc21c013b2a 246 }
MarijnJ 0:4dc21c013b2a 247
MarijnJ 0:4dc21c013b2a 248 ble.gap().onDisconnection(disconnectionCallback);
MarijnJ 0:4dc21c013b2a 249 // ble.gattServer().onDataWritten(writeCharCallback);
MarijnJ 0:4dc21c013b2a 250 ble.gap().onTimeout(timeoutCallback);
MarijnJ 0:4dc21c013b2a 251
MarijnJ 0:4dc21c013b2a 252 // Setup information service
MarijnJ 0:4dc21c013b2a 253 deviceInfoService = new DeviceInformationService(ble, "ARM", "Model1", "SN1",
MarijnJ 0:4dc21c013b2a 254 "hw-rev1", "fw-rev1", "soft-rev1");
MarijnJ 0:4dc21c013b2a 255
MarijnJ 0:4dc21c013b2a 256 // Setup advertising, BLE only, no classic BT
MarijnJ 0:4dc21c013b2a 257 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
MarijnJ 0:4dc21c013b2a 258 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
MarijnJ 0:4dc21c013b2a 259 (uint8_t *)uuid16List, sizeof(uuid16List));
MarijnJ 0:4dc21c013b2a 260 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME,
MarijnJ 0:4dc21c013b2a 261 (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
MarijnJ 0:4dc21c013b2a 262
MarijnJ 0:4dc21c013b2a 263 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
MarijnJ 0:4dc21c013b2a 264 ble.gap().setAdvertisingInterval(ADVERTISING_INTERVAL);
MarijnJ 0:4dc21c013b2a 265 ble.gap().setAdvertisingTimeout(0);
MarijnJ 0:4dc21c013b2a 266
MarijnJ 0:4dc21c013b2a 267 // Get current advertising power level...
MarijnJ 0:4dc21c013b2a 268 // TODO: Remove this useless crap since it doesnt do anything...
MarijnJ 0:4dc21c013b2a 269 /*
MarijnJ 0:4dc21c013b2a 270 GapAdvertisingData payload = ble.gap().getAdvertisingPayload();
MarijnJ 0:4dc21c013b2a 271
MarijnJ 0:4dc21c013b2a 272 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 273 SEGGER_RTT_printf(0, "Current advertising Tx power: %d dBm\n", payload.TX_POWER_LEVEL);
MarijnJ 0:4dc21c013b2a 274 #endif
MarijnJ 0:4dc21c013b2a 275
MarijnJ 0:4dc21c013b2a 276 error = ble.gap().accumulateAdvertisingPayloadTxPower(TX_POWER_ADVERTISING);
MarijnJ 0:4dc21c013b2a 277
MarijnJ 0:4dc21c013b2a 278 if (error != BLE_ERROR_NONE) {
MarijnJ 0:4dc21c013b2a 279 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 280 SEGGER_RTT_printf(0, "Failed to set advertising TX power to %d dBm!\n", TX_POWER_ADVERTISING);
MarijnJ 0:4dc21c013b2a 281 #endif
MarijnJ 0:4dc21c013b2a 282 } else {
MarijnJ 0:4dc21c013b2a 283 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 284 SEGGER_RTT_printf(0, "Set advertising TX power to %d dBm!\n", TX_POWER_ADVERTISING);
MarijnJ 0:4dc21c013b2a 285 #endif
MarijnJ 0:4dc21c013b2a 286 }
MarijnJ 0:4dc21c013b2a 287
MarijnJ 0:4dc21c013b2a 288 // Get current advertising power level...
MarijnJ 0:4dc21c013b2a 289 payload = ble.gap().getAdvertisingPayload();
MarijnJ 0:4dc21c013b2a 290
MarijnJ 0:4dc21c013b2a 291 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 292 SEGGER_RTT_printf(0, "Current advertising Tx power: %d dBm\n", payload.TX_POWER_LEVEL);
MarijnJ 0:4dc21c013b2a 293 #endif
MarijnJ 0:4dc21c013b2a 294
MarijnJ 0:4dc21c013b2a 295 error = ble.gap().setTxPower(TX_POWER_CONNECTION);
MarijnJ 0:4dc21c013b2a 296
MarijnJ 0:4dc21c013b2a 297 if (error != BLE_ERROR_NONE) {
MarijnJ 0:4dc21c013b2a 298 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 299 SEGGER_RTT_printf(0, "Failed to set general TX power to %d dBm!\n", TX_POWER_CONNECTION);
MarijnJ 0:4dc21c013b2a 300 #endif
MarijnJ 0:4dc21c013b2a 301 } else {
MarijnJ 0:4dc21c013b2a 302 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 303 SEGGER_RTT_printf(0, "Set general TX power to %d dBm!\n", TX_POWER_CONNECTION);
MarijnJ 0:4dc21c013b2a 304 #endif
MarijnJ 0:4dc21c013b2a 305 }
MarijnJ 0:4dc21c013b2a 306 */
MarijnJ 0:4dc21c013b2a 307
MarijnJ 0:4dc21c013b2a 308 ble.addService(customService);
MarijnJ 0:4dc21c013b2a 309 ble.gap().startAdvertising();
MarijnJ 0:4dc21c013b2a 310 }
MarijnJ 0:4dc21c013b2a 311
MarijnJ 0:4dc21c013b2a 312 void printPowerValues() {
MarijnJ 0:4dc21c013b2a 313 const int8_t *power_values;
MarijnJ 0:4dc21c013b2a 314 size_t count;
MarijnJ 0:4dc21c013b2a 315
MarijnJ 0:4dc21c013b2a 316 BLE &ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
MarijnJ 0:4dc21c013b2a 317 ble.gap().getPermittedTxPowerValues(&power_values, &count);
MarijnJ 0:4dc21c013b2a 318
MarijnJ 0:4dc21c013b2a 319 for (int i = 0; i < count; ++i) {
MarijnJ 0:4dc21c013b2a 320 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 321 SEGGER_RTT_printf(0, "Got power value: %d dBm\n", power_values[i]);
MarijnJ 0:4dc21c013b2a 322 #endif
MarijnJ 0:4dc21c013b2a 323 }
MarijnJ 0:4dc21c013b2a 324 }
MarijnJ 0:4dc21c013b2a 325
MarijnJ 0:4dc21c013b2a 326 /*
MarijnJ 0:4dc21c013b2a 327 Copies the measurements float array into the BLE readChar.
MarijnJ 0:4dc21c013b2a 328 Make sure the destination buffer has sufficient space! Its size should be
MarijnJ 0:4dc21c013b2a 329 at least 4 times that of measurements. This function assumes that
MarijnJ 0:4dc21c013b2a 330 sizeof(float) = 4 bytes, aka 32 bits, aka 4 uint8_t values.
MarijnJ 0:4dc21c013b2a 331 */
MarijnJ 0:4dc21c013b2a 332 void copyMeasurementsToBLEChar(float *measurements, uint8_t *destination, uint16_t numElements) {
MarijnJ 0:4dc21c013b2a 333 for (uint16_t i = 0; i < numElements; i++) {
MarijnJ 0:4dc21c013b2a 334 // Use a dirty union to slice the floats into 4 uint8_t's
MarijnJ 0:4dc21c013b2a 335 union {
MarijnJ 0:4dc21c013b2a 336 float f;
MarijnJ 0:4dc21c013b2a 337 uint8_t uints[4];
MarijnJ 0:4dc21c013b2a 338 } dirtyHack;
MarijnJ 0:4dc21c013b2a 339
MarijnJ 0:4dc21c013b2a 340 dirtyHack.f = measurements[i];
MarijnJ 0:4dc21c013b2a 341 destination[i * 4] = dirtyHack.uints[0];
MarijnJ 0:4dc21c013b2a 342 destination[i * 4 + 1] = dirtyHack.uints[1];
MarijnJ 0:4dc21c013b2a 343 destination[i * 4 + 2] = dirtyHack.uints[2];
MarijnJ 0:4dc21c013b2a 344 destination[i * 4 + 3] = dirtyHack.uints[3];
MarijnJ 0:4dc21c013b2a 345 }
MarijnJ 0:4dc21c013b2a 346 }
MarijnJ 0:4dc21c013b2a 347
MarijnJ 0:4dc21c013b2a 348
MarijnJ 0:4dc21c013b2a 349 /*
MarijnJ 0:4dc21c013b2a 350 Returns true/false if the sensor has new data. If not, no measurements are
MarijnJ 0:4dc21c013b2a 351 done. If the sensor has new data, then the measurements array
MarijnJ 0:4dc21c013b2a 352 (sized appropriately) will be filled according to the format specified at
MarijnJ 0:4dc21c013b2a 353 the top of this file.
MarijnJ 0:4dc21c013b2a 354 */
MarijnJ 0:4dc21c013b2a 355 bool measureSensor(MPU9250 *mpu9250, float *measurements) {
MarijnJ 0:4dc21c013b2a 356
MarijnJ 0:4dc21c013b2a 357 if (!mpu9250->hasNewData()) {
MarijnJ 0:4dc21c013b2a 358 return false;
MarijnJ 0:4dc21c013b2a 359 }
MarijnJ 0:4dc21c013b2a 360
MarijnJ 0:4dc21c013b2a 361 // Temporary variables to store results
MarijnJ 0:4dc21c013b2a 362 float ax, ay, az, gx, gy, gz, mx, my, mz, temperature;
MarijnJ 0:4dc21c013b2a 363
MarijnJ 0:4dc21c013b2a 364 // Get the accleration value into actual g's
MarijnJ 0:4dc21c013b2a 365 mpu9250->readAccelData(&ax, &ay, &az);
MarijnJ 0:4dc21c013b2a 366
MarijnJ 0:4dc21c013b2a 367 // Get the gyro values into actual degrees per second
MarijnJ 0:4dc21c013b2a 368 mpu9250->readGyroData(&gx, &gy, &gz);
MarijnJ 0:4dc21c013b2a 369
MarijnJ 0:4dc21c013b2a 370 // Get actual magnetometer value, this depends on scale being set
MarijnJ 0:4dc21c013b2a 371 mpu9250->readMagData(&gx, &gy, &gz);
MarijnJ 0:4dc21c013b2a 372
MarijnJ 0:4dc21c013b2a 373 // Temperature in degrees Celsius
MarijnJ 0:4dc21c013b2a 374 temperature = mpu9250->getTemperature();
MarijnJ 0:4dc21c013b2a 375
MarijnJ 0:4dc21c013b2a 376 // Store it in the measurements array
MarijnJ 0:4dc21c013b2a 377 measurements[0] = ax;
MarijnJ 0:4dc21c013b2a 378 measurements[1] = ay;
MarijnJ 0:4dc21c013b2a 379 measurements[2] = az;
MarijnJ 0:4dc21c013b2a 380 measurements[3] = gx;
MarijnJ 0:4dc21c013b2a 381 measurements[4] = gy;
MarijnJ 0:4dc21c013b2a 382 measurements[5] = gz;
MarijnJ 0:4dc21c013b2a 383 measurements[6] = mx;
MarijnJ 0:4dc21c013b2a 384 measurements[7] = my;
MarijnJ 0:4dc21c013b2a 385 measurements[8] = mz;
MarijnJ 0:4dc21c013b2a 386 measurements[9] = temperature;
MarijnJ 0:4dc21c013b2a 387
MarijnJ 0:4dc21c013b2a 388 detectTap();
MarijnJ 0:4dc21c013b2a 389 return true;
MarijnJ 0:4dc21c013b2a 390 }
MarijnJ 0:4dc21c013b2a 391
MarijnJ 0:4dc21c013b2a 392
MarijnJ 0:4dc21c013b2a 393 /*
MarijnJ 0:4dc21c013b2a 394 Sets up the given sensor
MarijnJ 0:4dc21c013b2a 395 */
MarijnJ 0:4dc21c013b2a 396 void setupSensor(MPU9250 *mpu9250) {
MarijnJ 0:4dc21c013b2a 397 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 398 SEGGER_RTT_printf(0, "CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
MarijnJ 0:4dc21c013b2a 399 #endif
MarijnJ 0:4dc21c013b2a 400
MarijnJ 0:4dc21c013b2a 401 float SelfTest[6];
MarijnJ 0:4dc21c013b2a 402
MarijnJ 0:4dc21c013b2a 403 // Read the WHO_AM_I register, this is a good test of communication
MarijnJ 0:4dc21c013b2a 404 // Read WHO_AM_I register for MPU-9250
MarijnJ 0:4dc21c013b2a 405 uint8_t whoami = mpu9250->getWhoAmI();
MarijnJ 0:4dc21c013b2a 406
MarijnJ 0:4dc21c013b2a 407 // WHO_AM_I should always be 0x71
MarijnJ 0:4dc21c013b2a 408 if (whoami == CORRECT_WHO_AM_I) {
MarijnJ 0:4dc21c013b2a 409 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 410 SEGGER_RTT_WriteString(0, "MPU9250 is online...\n\n");
MarijnJ 0:4dc21c013b2a 411 #endif
MarijnJ 0:4dc21c013b2a 412
MarijnJ 0:4dc21c013b2a 413 wait(1);
MarijnJ 0:4dc21c013b2a 414
MarijnJ 0:4dc21c013b2a 415 // Reset registers to default in preparation for device calibration
MarijnJ 0:4dc21c013b2a 416 mpu9250->resetMPU9250();
MarijnJ 0:4dc21c013b2a 417
MarijnJ 0:4dc21c013b2a 418 // Start by performing self test and reporting values
MarijnJ 0:4dc21c013b2a 419 mpu9250->MPU9250SelfTest(SelfTest);
MarijnJ 0:4dc21c013b2a 420
MarijnJ 0:4dc21c013b2a 421 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 422 SEGGER_RTT_printf(0, "x-axis self test: accel trim within : %d % of factory value\n",
MarijnJ 0:4dc21c013b2a 423 toInt(SelfTest[0]));
MarijnJ 0:4dc21c013b2a 424 SEGGER_RTT_printf(0, "y-axis self test: accel trim within : %d % of factory value\n",
MarijnJ 0:4dc21c013b2a 425 toInt(SelfTest[1]));
MarijnJ 0:4dc21c013b2a 426 SEGGER_RTT_printf(0, "z-axis self test: accel trim within : %d % of factory value\n",
MarijnJ 0:4dc21c013b2a 427 toInt(SelfTest[2]));
MarijnJ 0:4dc21c013b2a 428 SEGGER_RTT_printf(0, "x-axis self test: gyration trim within : %d % of factory value\n",
MarijnJ 0:4dc21c013b2a 429 toInt(SelfTest[3]));
MarijnJ 0:4dc21c013b2a 430 SEGGER_RTT_printf(0, "y-axis self test: gyration trim within : %d % of factory value\n",
MarijnJ 0:4dc21c013b2a 431 toInt(SelfTest[4]));
MarijnJ 0:4dc21c013b2a 432 SEGGER_RTT_printf(0, "z-axis self test: gyration trim within : %d % of factory value\n\n",
MarijnJ 0:4dc21c013b2a 433 toInt(SelfTest[5]));
MarijnJ 0:4dc21c013b2a 434 #endif
MarijnJ 0:4dc21c013b2a 435
MarijnJ 0:4dc21c013b2a 436 // Calibrate gyro and accelerometers, load biases in bias registers
MarijnJ 0:4dc21c013b2a 437 mpu9250->calibrateMPU9250();
MarijnJ 0:4dc21c013b2a 438
MarijnJ 0:4dc21c013b2a 439 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 440 SEGGER_RTT_printf(0, "x gyro bias = %d\n", toInt(mpu9250->gyroBias[0]));
MarijnJ 0:4dc21c013b2a 441 SEGGER_RTT_printf(0, "y gyro bias = %d\n", toInt(mpu9250->gyroBias[1]));
MarijnJ 0:4dc21c013b2a 442 SEGGER_RTT_printf(0, "z gyro bias = %d\n", toInt(mpu9250->gyroBias[2]));
MarijnJ 0:4dc21c013b2a 443 SEGGER_RTT_printf(0, "x accel bias = %d\n", toInt(mpu9250->accelBias[0]));
MarijnJ 0:4dc21c013b2a 444 SEGGER_RTT_printf(0, "y accel bias = %d\n", toInt(mpu9250->accelBias[1]));
MarijnJ 0:4dc21c013b2a 445 SEGGER_RTT_printf(0, "z accel bias = %d\n\n", toInt(mpu9250->accelBias[2]));
MarijnJ 0:4dc21c013b2a 446 #endif
MarijnJ 0:4dc21c013b2a 447
MarijnJ 0:4dc21c013b2a 448 wait(2);
MarijnJ 0:4dc21c013b2a 449
MarijnJ 0:4dc21c013b2a 450 // Initialize device for active mode read of acclerometer, gyroscope, and temperature
MarijnJ 0:4dc21c013b2a 451 mpu9250->initMPU9250();
MarijnJ 0:4dc21c013b2a 452
MarijnJ 0:4dc21c013b2a 453 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 454 SEGGER_RTT_WriteString(0, "MPU9250 initialized for active data mode....\n");
MarijnJ 0:4dc21c013b2a 455 #endif
MarijnJ 0:4dc21c013b2a 456
MarijnJ 0:4dc21c013b2a 457 // Initialize device for active mode read of magnetometer
MarijnJ 0:4dc21c013b2a 458 mpu9250->initAK8963();
MarijnJ 0:4dc21c013b2a 459
MarijnJ 0:4dc21c013b2a 460 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 461 SEGGER_RTT_WriteString(0, "AK8963 initialized for active data mode....\n");
MarijnJ 0:4dc21c013b2a 462 SEGGER_RTT_printf(0, "Accelerometer full-scale range = %d g\n",
MarijnJ 0:4dc21c013b2a 463 toInt(2.0f * (float) (1<<mpu9250->Ascale)));
MarijnJ 0:4dc21c013b2a 464 SEGGER_RTT_printf(0, "Gyroscope full-scale range = %d deg/s\n",
MarijnJ 0:4dc21c013b2a 465 toInt(250.0f * (float) (1<<mpu9250->Gscale)));
MarijnJ 0:4dc21c013b2a 466
MarijnJ 0:4dc21c013b2a 467 if (mpu9250->Mscale == 0) {
MarijnJ 0:4dc21c013b2a 468 SEGGER_RTT_WriteString(0, "Magnetometer resolution = 14 bits\n");
MarijnJ 0:4dc21c013b2a 469 }
MarijnJ 0:4dc21c013b2a 470
MarijnJ 0:4dc21c013b2a 471 if (mpu9250->Mscale == 1) {
MarijnJ 0:4dc21c013b2a 472 SEGGER_RTT_WriteString(0, "Magnetometer resolution = 16 bits\n");
MarijnJ 0:4dc21c013b2a 473 }
MarijnJ 0:4dc21c013b2a 474
MarijnJ 0:4dc21c013b2a 475 if (mpu9250->Mmode == 2) {
MarijnJ 0:4dc21c013b2a 476 SEGGER_RTT_WriteString(0, "Magnetometer ODR = 8 Hz\n");
MarijnJ 0:4dc21c013b2a 477 }
MarijnJ 0:4dc21c013b2a 478
MarijnJ 0:4dc21c013b2a 479 if (mpu9250->Mmode == 6) {
MarijnJ 0:4dc21c013b2a 480 SEGGER_RTT_WriteString(0, "Magnetometer ODR = 100 Hz\n");
MarijnJ 0:4dc21c013b2a 481 }
MarijnJ 0:4dc21c013b2a 482
MarijnJ 0:4dc21c013b2a 483 SEGGER_RTT_WriteString(0, "\n");
MarijnJ 0:4dc21c013b2a 484
MarijnJ 0:4dc21c013b2a 485 // Scale resolutions per LSB for the sensors
MarijnJ 0:4dc21c013b2a 486 SEGGER_RTT_printf(0, "Accelerometer sensitivity is %d LSB/g \n",
MarijnJ 0:4dc21c013b2a 487 toInt(1.0f / mpu9250->getAres()));
MarijnJ 0:4dc21c013b2a 488 SEGGER_RTT_printf(0, "Gyroscope sensitivity is %d LSB/deg/s \n",
MarijnJ 0:4dc21c013b2a 489 toInt(1.0f / mpu9250->getGres()));
MarijnJ 0:4dc21c013b2a 490 SEGGER_RTT_printf(0, "Magnetometer sensitivity is %d LSB/G \n",
MarijnJ 0:4dc21c013b2a 491 toInt(1.0f / mpu9250->getMres()));
MarijnJ 0:4dc21c013b2a 492 #endif
MarijnJ 0:4dc21c013b2a 493
MarijnJ 0:4dc21c013b2a 494 wait(1);
MarijnJ 0:4dc21c013b2a 495 } else {
MarijnJ 0:4dc21c013b2a 496 // Loop forever if communication doesn't happen...
MarijnJ 0:4dc21c013b2a 497 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 498 SEGGER_RTT_printf(0, "Could not connect to MPU9250: 0x%x \n", whoami);
MarijnJ 0:4dc21c013b2a 499 #endif
MarijnJ 0:4dc21c013b2a 500 while(1);
MarijnJ 0:4dc21c013b2a 501 }
MarijnJ 0:4dc21c013b2a 502 }
MarijnJ 0:4dc21c013b2a 503
MarijnJ 0:4dc21c013b2a 504
MarijnJ 0:4dc21c013b2a 505
MarijnJ 0:4dc21c013b2a 506 /*
MarijnJ 0:4dc21c013b2a 507 Main function
MarijnJ 0:4dc21c013b2a 508 */
MarijnJ 0:4dc21c013b2a 509 int main() {
MarijnJ 0:4dc21c013b2a 510 // Setup periodicCallback ticker
MarijnJ 0:4dc21c013b2a 511 Ticker ticker;
MarijnJ 0:4dc21c013b2a 512 ticker.attach(&periodicCallback, 1);
MarijnJ 0:4dc21c013b2a 513
MarijnJ 0:4dc21c013b2a 514 BLE &ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
MarijnJ 0:4dc21c013b2a 515 ble.init(bleInitComplete);
MarijnJ 0:4dc21c013b2a 516
MarijnJ 0:4dc21c013b2a 517 // SpinWait for initialization to complete. This is necessary because the
MarijnJ 0:4dc21c013b2a 518 // BLE object is used in the main loop below.
MarijnJ 0:4dc21c013b2a 519 while (ble.hasInitialized() == false) { /* spin loop */ }
MarijnJ 0:4dc21c013b2a 520
MarijnJ 0:4dc21c013b2a 521 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 522 SEGGER_RTT_WriteString(0, "Initialized BLE...\n");
MarijnJ 0:4dc21c013b2a 523 #endif
MarijnJ 0:4dc21c013b2a 524
MarijnJ 0:4dc21c013b2a 525 printPowerValues();
MarijnJ 0:4dc21c013b2a 526
MarijnJ 0:4dc21c013b2a 527 // Setup temporary array of 10 bytes so we don't have to use the heap
MarijnJ 0:4dc21c013b2a 528 uint8_t newReadValue[READ_CHAR_SIZE] = {0};
MarijnJ 0:4dc21c013b2a 529
MarijnJ 0:4dc21c013b2a 530 // Setup I2C and MPU9250 sensor
MarijnJ 0:4dc21c013b2a 531 I2C i2cConnection(P0_8, P0_9);
MarijnJ 0:4dc21c013b2a 532 MPU9250 mpu9250(&i2cConnection);
MarijnJ 0:4dc21c013b2a 533 setupSensor(&mpu9250);
MarijnJ 0:4dc21c013b2a 534
MarijnJ 0:4dc21c013b2a 535 Timer t;
MarijnJ 0:4dc21c013b2a 536 t.start();
MarijnJ 0:4dc21c013b2a 537
MarijnJ 0:4dc21c013b2a 538 // Variables to hold latest sensor data values
MarijnJ 0:4dc21c013b2a 539 //float pitch, yaw, roll;
MarijnJ 0:4dc21c013b2a 540 bool sensorHasNewData = false;
MarijnJ 0:4dc21c013b2a 541
MarijnJ 0:4dc21c013b2a 542 // Used to control display output rate
MarijnJ 0:4dc21c013b2a 543 float shift = 0.0f;
MarijnJ 0:4dc21c013b2a 544 int delt_t = 0;
MarijnJ 0:4dc21c013b2a 545 int count = 0;
MarijnJ 0:4dc21c013b2a 546 int cycle = 0;
MarijnJ 0:4dc21c013b2a 547
MarijnJ 0:4dc21c013b2a 548 // Used to calculate integration interval
MarijnJ 0:4dc21c013b2a 549 int lastUpdate = 0, firstUpdate = 0, currentTime = 0;
MarijnJ 0:4dc21c013b2a 550
MarijnJ 0:4dc21c013b2a 551 resetTap();
MarijnJ 0:4dc21c013b2a 552
MarijnJ 0:4dc21c013b2a 553 while (1) {
MarijnJ 0:4dc21c013b2a 554 // If not enough measurements for the sensor have been done, or the
MarijnJ 0:4dc21c013b2a 555 // trigger has been activated, do a measurement
MarijnJ 0:4dc21c013b2a 556 if (triggerSensorPolling || !mpu9250.sufficientMeasurements) {
MarijnJ 0:4dc21c013b2a 557 triggerSensorPolling = false;
MarijnJ 0:4dc21c013b2a 558
MarijnJ 0:4dc21c013b2a 559 // Check if sensor has new data
MarijnJ 0:4dc21c013b2a 560 sensorHasNewData = measureSensor(&mpu9250, measurements);
MarijnJ 0:4dc21c013b2a 561 currentTime = t.read_us();
MarijnJ 0:4dc21c013b2a 562
MarijnJ 0:4dc21c013b2a 563 // Set integration time by time elapsed since last filter update
MarijnJ 0:4dc21c013b2a 564 mpu9250.deltat = (float) ((currentTime - lastUpdate) / 1000000.0f);
MarijnJ 0:4dc21c013b2a 565 lastUpdate = currentTime;
MarijnJ 0:4dc21c013b2a 566
MarijnJ 0:4dc21c013b2a 567 if (lastUpdate - firstUpdate > 10000000.0f) {
MarijnJ 0:4dc21c013b2a 568 // Decrease filter gain and increase bias drift gain after stabilized
MarijnJ 0:4dc21c013b2a 569 mpu9250.beta = 0.04;
MarijnJ 0:4dc21c013b2a 570 mpu9250.zeta = 0.015;
MarijnJ 0:4dc21c013b2a 571 mpu9250.sufficientMeasurements = true;
MarijnJ 0:4dc21c013b2a 572 }
MarijnJ 0:4dc21c013b2a 573
MarijnJ 0:4dc21c013b2a 574 // Pass gyro rate as rad/s
MarijnJ 0:4dc21c013b2a 575 // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx * PI/180.0f,
MarijnJ 0:4dc21c013b2a 576 // gy * PI/180.0f, gz * PI/180.0f, my, mx, mz);
MarijnJ 0:4dc21c013b2a 577 /*mpu9250.MahonyQuaternionUpdate(measurements[0], measurements[1],
MarijnJ 0:4dc21c013b2a 578 measurements[2], DEG2RAD(measurements[3]), DEG2RAD(measurements[4]),
MarijnJ 0:4dc21c013b2a 579 DEG2RAD(measurements[5]), measurements[6], measurements[7], measurements[8]);
MarijnJ 0:4dc21c013b2a 580 */
MarijnJ 0:4dc21c013b2a 581 // Serial print 1 s rate independent of data rates
MarijnJ 0:4dc21c013b2a 582 delt_t = t.read_ms() - count;
MarijnJ 0:4dc21c013b2a 583
MarijnJ 0:4dc21c013b2a 584 // Update print once per second independent of read rate
MarijnJ 0:4dc21c013b2a 585 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 586 SEGGER_RTT_printf(0, "\n\nax = %d", toInt(1000 * measurements[0]));
MarijnJ 0:4dc21c013b2a 587 SEGGER_RTT_printf(0, " ay = %d", toInt(1000 * measurements[1]));
MarijnJ 0:4dc21c013b2a 588 SEGGER_RTT_printf(0, " az = %d mg\n", toInt(1000 * measurements[2]));
MarijnJ 0:4dc21c013b2a 589
MarijnJ 0:4dc21c013b2a 590 SEGGER_RTT_printf(0, "gx = %d", toInt(measurements[3]));
MarijnJ 0:4dc21c013b2a 591 SEGGER_RTT_printf(0, " gy = %d", toInt(measurements[4]));
MarijnJ 0:4dc21c013b2a 592 SEGGER_RTT_printf(0, " gz = %d deg/s\n", toInt(measurements[5]));
MarijnJ 0:4dc21c013b2a 593
MarijnJ 0:4dc21c013b2a 594 SEGGER_RTT_printf(0, "mx = %d", toInt(measurements[6]));
MarijnJ 0:4dc21c013b2a 595 SEGGER_RTT_printf(0, " my = %d", toInt(measurements[7]));
MarijnJ 0:4dc21c013b2a 596 SEGGER_RTT_printf(0, " mz = %d mG\n", toInt(measurements[8]));
MarijnJ 0:4dc21c013b2a 597
MarijnJ 0:4dc21c013b2a 598 SEGGER_RTT_printf(0, "Temperature = %d C\n\n", toInt(measurements[9]));
MarijnJ 0:4dc21c013b2a 599 #endif
MarijnJ 0:4dc21c013b2a 600
MarijnJ 0:4dc21c013b2a 601 // Get yaw, pitch and roll
MarijnJ 0:4dc21c013b2a 602 /*mpu9250.getYawPitchRoll(&yaw, &pitch, &roll, SENSOR_DECLINATION);
MarijnJ 0:4dc21c013b2a 603
MarijnJ 0:4dc21c013b2a 604 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 605 SEGGER_RTT_printf(0, "Yaw: %d Pitch: %d Roll: %d\n\n",
MarijnJ 0:4dc21c013b2a 606 toInt(yaw), toInt(pitch), toInt(roll));
MarijnJ 0:4dc21c013b2a 607 #endif*/
MarijnJ 0:4dc21c013b2a 608
MarijnJ 0:4dc21c013b2a 609 count = t.read_ms();
MarijnJ 0:4dc21c013b2a 610
MarijnJ 0:4dc21c013b2a 611 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 612 SEGGER_RTT_printf(0, "Time active: %d minutes\n----------------",
MarijnJ 0:4dc21c013b2a 613 getTime(count, shift));
MarijnJ 0:4dc21c013b2a 614 #endif
MarijnJ 0:4dc21c013b2a 615
MarijnJ 0:4dc21c013b2a 616 if (count > 1<<21) {
MarijnJ 0:4dc21c013b2a 617 // Start the timer over again if ~30 minutes has passed
MarijnJ 0:4dc21c013b2a 618 t.stop();
MarijnJ 0:4dc21c013b2a 619 t.reset();
MarijnJ 0:4dc21c013b2a 620 t.start();
MarijnJ 0:4dc21c013b2a 621 count = 0;
MarijnJ 0:4dc21c013b2a 622
MarijnJ 0:4dc21c013b2a 623 #ifdef DEBUG_MODE
MarijnJ 0:4dc21c013b2a 624 SEGGER_RTT_printf(0, "Resetting timer! t.read_ms() now gives: %d\n",
MarijnJ 0:4dc21c013b2a 625 t.read_ms());
MarijnJ 0:4dc21c013b2a 626 #endif
MarijnJ 0:4dc21c013b2a 627
MarijnJ 0:4dc21c013b2a 628 mpu9250.deltat = 0;
MarijnJ 0:4dc21c013b2a 629 lastUpdate = t.read_us();
MarijnJ 0:4dc21c013b2a 630 shift = (++cycle * 34.9525f);
MarijnJ 0:4dc21c013b2a 631 }
MarijnJ 0:4dc21c013b2a 632
MarijnJ 0:4dc21c013b2a 633 if (ble.getGapState().connected) {
MarijnJ 0:4dc21c013b2a 634 // Send measurements via bluetooth
MarijnJ 0:4dc21c013b2a 635 copyMeasurementsToBLEChar(measurements, readValue, NUM_MEASUREMENTS);
MarijnJ 0:4dc21c013b2a 636 ble.gattServer().write(readChar.getValueHandle(), readValue, READ_CHAR_SIZE);
MarijnJ 0:4dc21c013b2a 637 }
MarijnJ 0:4dc21c013b2a 638 } else {
MarijnJ 0:4dc21c013b2a 639 // Save power by waiting for BLE events, if we already have enough
MarijnJ 0:4dc21c013b2a 640 // measurements for sufficient sensor accuracy
MarijnJ 0:4dc21c013b2a 641 if (mpu9250.sufficientMeasurements) {
MarijnJ 0:4dc21c013b2a 642 ble.waitForEvent();
MarijnJ 0:4dc21c013b2a 643 }
MarijnJ 0:4dc21c013b2a 644 }
MarijnJ 0:4dc21c013b2a 645 }
MarijnJ 0:4dc21c013b2a 646 }