YUFEI JIANG / Mbed 2 deprecated CSSE4011_BLE_IMU

CSSE4011_BLE_IMU IMU Seeed Tiny Ble

Dependencies:   BLE_API_Tiny_BLE MPU6050-DMP-Seeed-Tiny-BLE mbed

main.cpp@2:44bc61abdf33, 2015-06-10 (annotated)

Committer:
flywind
Date:
Wed Jun 10 12:56:48 2015 +0000
Revision:
2:44bc61abdf33
Parent:
1:3723e08bf4fd 
milestone working one , yaw pitch roll update in BLE sucessfull

Who changed what in which revision?

UserRevisionLine numberNew contents of line
flywind 0:f90c3452d779 1
flywind 0:f90c3452d779 2 #include "mbed.h"
flywind 0:f90c3452d779 3
flywind 0:f90c3452d779 4
flywind 0:f90c3452d779 5 #include "MPU6050_6Axis_MotionApps20.h"
flywind 0:f90c3452d779 6
flywind 0:f90c3452d779 7
flywind 0:f90c3452d779 8
flywind 0:f90c3452d779 9 #include "tiny_ble.h"
flywind 0:f90c3452d779 10 //#include "kalman.h"
flywind 0:f90c3452d779 11
flywind 0:f90c3452d779 12
flywind 0:f90c3452d779 13 #include "BLEDevice.h"
flywind 0:f90c3452d779 14 #include "MPUService.h"
flywind 0:f90c3452d779 15 //#include "UARTService.h"
flywind 0:f90c3452d779 16 #include "HeartRateService.h"
flywind 0:f90c3452d779 17 #include "DeviceInformationService.h"
flywind 0:f90c3452d779 18
flywind 0:f90c3452d779 19
flywind 0:f90c3452d779 20 /* Starting sampling rate. */
flywind 0:f90c3452d779 21
flywind 0:f90c3452d779 22 #define LOG(...) { pc.printf(__VA_ARGS__); }
flywind 0:f90c3452d779 23
flywind 0:f90c3452d779 24
flywind 0:f90c3452d779 25 /* ============================================
flywind 0:f90c3452d779 26 I2Cdev device library code is placed under the MIT license
flywind 0:f90c3452d779 27 Copyright (c) 2012 Jeff Rowberg
flywind 0:f90c3452d779 28
flywind 0:f90c3452d779 29 Permission is hereby granted, free of charge, to any person obtaining a copy
flywind 0:f90c3452d779 30 of this software and associated documentation files (the "Software"), to deal
flywind 0:f90c3452d779 31 in the Software without restriction, including without limitation the rights
flywind 0:f90c3452d779 32 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
flywind 0:f90c3452d779 33 copies of the Software, and to permit persons to whom the Software is
flywind 0:f90c3452d779 34 furnished to do so, subject to the following conditions:
flywind 0:f90c3452d779 35
flywind 0:f90c3452d779 36 The above copyright notice and this permission notice shall be included in
flywind 0:f90c3452d779 37 all copies or substantial portions of the Software.
flywind 0:f90c3452d779 38
flywind 0:f90c3452d779 39 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
flywind 0:f90c3452d779 40 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
flywind 0:f90c3452d779 41 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
flywind 0:f90c3452d779 42 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
flywind 0:f90c3452d779 43 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
flywind 0:f90c3452d779 44 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
flywind 0:f90c3452d779 45 THE SOFTWARE.
flywind 0:f90c3452d779 46 ===============================================
flywind 0:f90c3452d779 47 */
flywind 0:f90c3452d779 48
flywind 0:f90c3452d779 49 // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
flywind 0:f90c3452d779 50 // is used in I2Cdev.h
flywind 0:f90c3452d779 51 //#include "Wire.h"
flywind 0:f90c3452d779 52
flywind 0:f90c3452d779 53 // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
flywind 0:f90c3452d779 54 // for both classes must be in the include path of your project
flywind 0:f90c3452d779 55 #include "MPU6050_6Axis_MotionApps20.h"
flywind 0:f90c3452d779 56 #include "mbed_i2c.h"
flywind 0:f90c3452d779 57
flywind 0:f90c3452d779 58 //#include "MPU6050.h" // not necessary if using MotionApps include file
flywind 0:f90c3452d779 59
flywind 0:f90c3452d779 60 // class default I2C address is 0x68
flywind 0:f90c3452d779 61 // specific I2C addresses may be passed as a parameter here
flywind 0:f90c3452d779 62 // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
flywind 0:f90c3452d779 63 // AD0 high = 0x69
flywind 0:f90c3452d779 64
flywind 0:f90c3452d779 65
flywind 0:f90c3452d779 66 /* =========================================================================
flywind 0:f90c3452d779 67 NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
flywind 0:f90c3452d779 68 depends on the MPU-6050's INT pin being connected to the Arduino's
flywind 0:f90c3452d779 69 external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
flywind 0:f90c3452d779 70 digital I/O pin 2.
flywind 0:f90c3452d779 71 * ========================================================================= */
flywind 0:f90c3452d779 72
flywind 0:f90c3452d779 73 /* =========================================================================
flywind 0:f90c3452d779 74 NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
flywind 0:f90c3452d779 75 when using Serial.write(buf, len). The Teapot output uses this method.
flywind 0:f90c3452d779 76 The solution requires a modification to the Arduino USBAPI.h file, which
flywind 0:f90c3452d779 77 is fortunately simple, but annoying. This will be fixed in the next IDE
flywind 0:f90c3452d779 78 release. For more info, see these links:
flywind 0:f90c3452d779 79
flywind 0:f90c3452d779 80 http://arduino.cc/forum/index.php/topic,109987.0.html
flywind 0:f90c3452d779 81 http://code.google.com/p/arduino/issues/detail?id=958
flywind 0:f90c3452d779 82 * ========================================================================= */
flywind 0:f90c3452d779 83
flywind 0:f90c3452d779 84
flywind 0:f90c3452d779 85
flywind 0:f90c3452d779 86 // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
flywind 0:f90c3452d779 87 // quaternion components in a [w, x, y, z] format (not best for parsing
flywind 0:f90c3452d779 88 // on a remote host such as Processing or something though)
flywind 1:3723e08bf4fd 89 //#define OUTPUT_READABLE_QUATERNION
flywind 0:f90c3452d779 90
flywind 0:f90c3452d779 91 // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
flywind 0:f90c3452d779 92 // (in degrees) calculated from the quaternions coming from the FIFO.
flywind 0:f90c3452d779 93 // Note that Euler angles suffer from gimbal lock (for more info, see
flywind 0:f90c3452d779 94 // http://en.wikipedia.org/wiki/Gimbal_lock)
flywind 2:44bc61abdf33 95 //#define OUTPUT_READABLE_EULER
flywind 0:f90c3452d779 96
flywind 0:f90c3452d779 97 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
flywind 0:f90c3452d779 98 // pitch/roll angles (in degrees) calculated from the quaternions coming
flywind 0:f90c3452d779 99 // from the FIFO. Note this also requires gravity vector calculations.
flywind 0:f90c3452d779 100 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
flywind 0:f90c3452d779 101 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
flywind 0:f90c3452d779 102 #define OUTPUT_READABLE_YAWPITCHROLL
flywind 0:f90c3452d779 103
flywind 0:f90c3452d779 104 // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
flywind 0:f90c3452d779 105 // components with gravity removed. This acceleration reference frame is
flywind 0:f90c3452d779 106 // not compensated for orientation, so +X is always +X according to the
flywind 0:f90c3452d779 107 // sensor, just without the effects of gravity. If you want acceleration
flywind 0:f90c3452d779 108 // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
flywind 1:3723e08bf4fd 109 //#define OUTPUT_READABLE_REALACCEL
flywind 0:f90c3452d779 110
flywind 0:f90c3452d779 111 // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
flywind 0:f90c3452d779 112 // components with gravity removed and adjusted for the world frame of
flywind 0:f90c3452d779 113 // reference (yaw is relative to initial orientation, since no magnetometer
flywind 0:f90c3452d779 114 // is present in this case). Could be quite handy in some cases.
flywind 1:3723e08bf4fd 115 //#define OUTPUT_READABLE_WORLDACCEL
flywind 0:f90c3452d779 116
flywind 0:f90c3452d779 117 // uncomment "OUTPUT_TEAPOT" if you want output that matches the
flywind 0:f90c3452d779 118 // format used for the InvenSense teapot demo
flywind 0:f90c3452d779 119 //#define OUTPUT_TEAPOT
flywind 0:f90c3452d779 120
flywind 0:f90c3452d779 121
flywind 0:f90c3452d779 122 // uncomment "OUTPUT_READABLE_ACCEL" if you want output that matches the
flywind 0:f90c3452d779 123 // format used for the InvenSense teapot demo
flywind 0:f90c3452d779 124 //#define OUTPUT_READABLE_ACCEL
flywind 0:f90c3452d779 125
flywind 0:f90c3452d779 126 // uncomment "OUTPUT_READABLE_ACCEL" if you want output that matches the
flywind 0:f90c3452d779 127 // format used for the InvenSense teapot demo
flywind 0:f90c3452d779 128 //#define OUTPUT_READABLE_ACCEL
flywind 0:f90c3452d779 129
flywind 0:f90c3452d779 130 /*
flywind 0:f90c3452d779 131 * ====================================================================================================================================
flywind 0:f90c3452d779 132
flywind 0:f90c3452d779 133
flywind 0:f90c3452d779 134 * =====================================================================================================================================
flywind 0:f90c3452d779 135 */
flywind 0:f90c3452d779 136
flywind 0:f90c3452d779 137 DigitalOut blue(LED_BLUE);
flywind 0:f90c3452d779 138 DigitalOut green(LED_GREEN);
flywind 0:f90c3452d779 139 DigitalOut red(LED_RED);
flywind 0:f90c3452d779 140
flywind 0:f90c3452d779 141 InterruptIn button(BUTTON_PIN);
flywind 0:f90c3452d779 142 AnalogIn battery(BATTERY_PIN);
flywind 0:f90c3452d779 143 Serial pc(USBTX, USBRX);
flywind 0:f90c3452d779 144 //Serial pc(UART_TX, UART_RX);
flywind 0:f90c3452d779 145
flywind 0:f90c3452d779 146 /*
flywind 0:f90c3452d779 147 * ====================================================================================================================================
flywind 0:f90c3452d779 148
flywind 0:f90c3452d779 149
flywind 0:f90c3452d779 150 * =====================================================================================================================================
flywind 0:f90c3452d779 151 */
flywind 0:f90c3452d779 152 const static char DEVICE_NAME[] = "CSSE BLE";
flywind 0:f90c3452d779 153 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE,
flywind 0:f90c3452d779 154 GattService::UUID_DEVICE_INFORMATION_SERVICE
flywind 0:f90c3452d779 155 };
flywind 0:f90c3452d779 156 volatile bool bleIsConnected = false;
flywind 0:f90c3452d779 157 BLEDevice ble;
flywind 0:f90c3452d779 158 static volatile bool triggerSensorPolling = false;
flywind 0:f90c3452d779 159 /*
flywind 0:f90c3452d779 160 * ====================================================================================================================================
flywind 0:f90c3452d779 161
flywind 0:f90c3452d779 162
flywind 0:f90c3452d779 163 * =====================================================================================================================================
flywind 0:f90c3452d779 164 */
flywind 0:f90c3452d779 165
flywind 0:f90c3452d779 166
flywind 0:f90c3452d779 167 MPU6050 mpu(MPU6050_SDA, MPU6050_SCL);
flywind 0:f90c3452d779 168 void check_i2c_bus(void);
flywind 0:f90c3452d779 169 void mpuInitialSetup();
flywind 0:f90c3452d779 170 void mpuData();
flywind 0:f90c3452d779 171 // MPU control/status vars
flywind 0:f90c3452d779 172 bool dmpReady = false; // set true if DMP init was successful
flywind 0:f90c3452d779 173 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
flywind 0:f90c3452d779 174 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
flywind 0:f90c3452d779 175 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
flywind 0:f90c3452d779 176 uint16_t fifoCount; // count of all bytes currently in FIFO
flywind 0:f90c3452d779 177 uint8_t fifoBuffer[64]; // FIFO storage buffer
flywind 0:f90c3452d779 178
flywind 0:f90c3452d779 179 // orientation/motion vars
flywind 0:f90c3452d779 180 Quaternion q; // [w, x, y, z] quaternion container
flywind 0:f90c3452d779 181 VectorInt16 aa; // [x, y, z] accel sensor measurements
flywind 0:f90c3452d779 182 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
flywind 0:f90c3452d779 183 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
flywind 0:f90c3452d779 184 VectorFloat gravity; // [x, y, z] gravity vector
flywind 2:44bc61abdf33 185 float euler[3]= {0}; // [psi, theta, phi] Euler angle container
flywind 2:44bc61abdf33 186 float ypr[3]= {0}; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
flywind 0:f90c3452d779 187
flywind 0:f90c3452d779 188 // packet structure for InvenSense teapot demo
flywind 0:f90c3452d779 189 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
flywind 0:f90c3452d779 190
flywind 0:f90c3452d779 191 InterruptIn checkpin(p14);
flywind 0:f90c3452d779 192
flywind 0:f90c3452d779 193 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
flywind 0:f90c3452d779 194 void dmpDataReady()
flywind 0:f90c3452d779 195 {
flywind 0:f90c3452d779 196 mpuInterrupt = true;
flywind 0:f90c3452d779 197 }
flywind 0:f90c3452d779 198
flywind 0:f90c3452d779 199 //kalman_data pitch_data;
flywind 0:f90c3452d779 200 //kalman_data roll_data;
flywind 0:f90c3452d779 201
flywind 0:f90c3452d779 202 /*
flywind 0:f90c3452d779 203 * ====================================================================================================================================
flywind 0:f90c3452d779 204
flywind 0:f90c3452d779 205
flywind 0:f90c3452d779 206 * =====================================================================================================================================
flywind 0:f90c3452d779 207 */
flywind 0:f90c3452d779 208 void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params)
flywind 0:f90c3452d779 209 {
flywind 0:f90c3452d779 210 LOG("Connected!\n");
flywind 0:f90c3452d779 211 bleIsConnected = true;
flywind 0:f90c3452d779 212 }
flywind 0:f90c3452d779 213
flywind 0:f90c3452d779 214 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
flywind 0:f90c3452d779 215 {
flywind 0:f90c3452d779 216 LOG("Disconnected!\n");
flywind 0:f90c3452d779 217 LOG("Restarting the advertising process\n");
flywind 0:f90c3452d779 218 ble.startAdvertising();
flywind 0:f90c3452d779 219 bleIsConnected = false;
flywind 0:f90c3452d779 220 }
flywind 0:f90c3452d779 221
flywind 0:f90c3452d779 222 void tick(void)
flywind 0:f90c3452d779 223 {
flywind 2:44bc61abdf33 224 if(dmpReady) {
flywind 2:44bc61abdf33 225 green = !green;
flywind 2:44bc61abdf33 226 } else {
flywind 2:44bc61abdf33 227 red = !red;
flywind 0:f90c3452d779 228 }
flywind 0:f90c3452d779 229 triggerSensorPolling = true;
flywind 0:f90c3452d779 230 }
flywind 0:f90c3452d779 231
flywind 0:f90c3452d779 232 void detect(void)
flywind 0:f90c3452d779 233 {
flywind 0:f90c3452d779 234 LOG("Button pressed\n");
flywind 0:f90c3452d779 235 blue = !blue;
flywind 0:f90c3452d779 236 }
flywind 0:f90c3452d779 237
flywind 0:f90c3452d779 238 void tap_cb(unsigned char direction, unsigned char count)
flywind 0:f90c3452d779 239 {
flywind 0:f90c3452d779 240 LOG("Tap motion detected\n");
flywind 0:f90c3452d779 241 }
flywind 0:f90c3452d779 242
flywind 0:f90c3452d779 243 void android_orient_cb(unsigned char orientation)
flywind 0:f90c3452d779 244 {
flywind 0:f90c3452d779 245 LOG("Oriention changed\n");
flywind 0:f90c3452d779 246 }
flywind 0:f90c3452d779 247
flywind 0:f90c3452d779 248
flywind 0:f90c3452d779 249 int main(void)
flywind 0:f90c3452d779 250 {
flywind 0:f90c3452d779 251 blue = 1;
flywind 0:f90c3452d779 252 green = 1;
flywind 0:f90c3452d779 253 red = 1;
flywind 0:f90c3452d779 254
flywind 0:f90c3452d779 255 pc.baud(115200);
flywind 0:f90c3452d779 256
flywind 0:f90c3452d779 257 LOG("---- Seeed Tiny BLE ----\n");
flywind 0:f90c3452d779 258
flywind 0:f90c3452d779 259 Ticker ticker;
flywind 0:f90c3452d779 260 ticker.attach(tick, 0.5);
flywind 0:f90c3452d779 261
flywind 0:f90c3452d779 262 button.fall(detect);
flywind 0:f90c3452d779 263
flywind 0:f90c3452d779 264 LOG("Initialising the nRF51822\n");
flywind 0:f90c3452d779 265
flywind 0:f90c3452d779 266
flywind 0:f90c3452d779 267 /*
flywind 0:f90c3452d779 268 * ====================================================================================================================================
flywind 0:f90c3452d779 269
flywind 0:f90c3452d779 270
flywind 0:f90c3452d779 271 * =====================================================================================================================================
flywind 0:f90c3452d779 272 */
flywind 0:f90c3452d779 273 ble.init();
flywind 0:f90c3452d779 274 ble.onDisconnection(disconnectionCallback);
flywind 0:f90c3452d779 275 ble.onConnection(connectionCallback);
flywind 0:f90c3452d779 276
flywind 0:f90c3452d779 277
flywind 0:f90c3452d779 278
flywind 0:f90c3452d779 279 /* setup advertising */
flywind 0:f90c3452d779 280 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED| GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
flywind 0:f90c3452d779 281
flywind 0:f90c3452d779 282 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
flywind 0:f90c3452d779 283
flywind 0:f90c3452d779 284 ble.accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_REMOTE_CONTROL);
flywind 0:f90c3452d779 285
flywind 0:f90c3452d779 286 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
flywind 0:f90c3452d779 287 (uint8_t *)uuid16_list, sizeof(uuid16_list));
flywind 0:f90c3452d779 288
flywind 0:f90c3452d779 289 //ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,(const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
flywind 0:f90c3452d779 290 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
flywind 0:f90c3452d779 291 (const uint8_t *)MPUServiceUUID, sizeof(MPUServiceUUID));
flywind 0:f90c3452d779 292
flywind 0:f90c3452d779 293 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
flywind 0:f90c3452d779 294
flywind 0:f90c3452d779 295 // UARTService uartService(ble);
flywind 0:f90c3452d779 296 // uartServicePtr = &uartService;
flywind 0:f90c3452d779 297
flywind 0:f90c3452d779 298 MPUService mpuServices(ble,ypr);
flywind 0:f90c3452d779 299
flywind 0:f90c3452d779 300 /* Setup primary service. */
flywind 0:f90c3452d779 301 uint8_t hrmCounter = 50; // init HRM to 100bps
flywind 0:f90c3452d779 302 HeartRateService hrService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
flywind 0:f90c3452d779 303
flywind 0:f90c3452d779 304 /* Setup auxiliary service. */
flywind 0:f90c3452d779 305 DeviceInformationService deviceInfo(ble, "Seeed", "Tniy BLE", "SN1", "hw-rev1", "fw-rev1", "soft-rev1");
flywind 0:f90c3452d779 306
flywind 0:f90c3452d779 307
flywind 0:f90c3452d779 308 ble.setAdvertisingInterval(50); /* 100ms; in multiples of 0.625ms. */
flywind 0:f90c3452d779 309 ble.startAdvertising();
flywind 0:f90c3452d779 310
flywind 0:f90c3452d779 311
flywind 0:f90c3452d779 312
flywind 0:f90c3452d779 313 /*
flywind 0:f90c3452d779 314 * ====================================================================================================================================
flywind 0:f90c3452d779 315
flywind 0:f90c3452d779 316
flywind 0:f90c3452d779 317 * =====================================================================================================================================
flywind 0:f90c3452d779 318 */
flywind 0:f90c3452d779 319 check_i2c_bus();
flywind 0:f90c3452d779 320 mpuInitialSetup();
flywind 0:f90c3452d779 321
flywind 0:f90c3452d779 322
flywind 0:f90c3452d779 323 while (true) {
flywind 0:f90c3452d779 324 if(dmpReady && (mpuInterrupt || fifoCount >= packetSize)) {
flywind 0:f90c3452d779 325 mpuData();
flywind 2:44bc61abdf33 326 pc.printf("ypr\t");
flywind 2:44bc61abdf33 327 pc.printf("%i\t", (int16_t)ypr[0]*100) ;
flywind 2:44bc61abdf33 328 pc.printf("%i\t", (int16_t)ypr[1]*100);
flywind 2:44bc61abdf33 329 pc.printf("%i\t\r\n", (int16_t)ypr[2]*100);
flywind 0:f90c3452d779 330 mpuServices.updateYawPitchRoll(ypr);
flywind 0:f90c3452d779 331
flywind 0:f90c3452d779 332 }
flywind 0:f90c3452d779 333 if (triggerSensorPolling) {
flywind 0:f90c3452d779 334 triggerSensorPolling = false;
flywind 0:f90c3452d779 335
flywind 0:f90c3452d779 336 // Do blocking calls or whatever is necessary for sensor polling.
flywind 0:f90c3452d779 337 // In our case, we simply update the HRM measurement.
flywind 0:f90c3452d779 338 hrmCounter++;
flywind 0:f90c3452d779 339
flywind 0:f90c3452d779 340 // 100 <= HRM bps <=175
flywind 0:f90c3452d779 341 if (hrmCounter == 175) {
flywind 0:f90c3452d779 342 hrmCounter = 50;
flywind 0:f90c3452d779 343
flywind 0:f90c3452d779 344 }
flywind 0:f90c3452d779 345
flywind 0:f90c3452d779 346 // update bps
flywind 0:f90c3452d779 347 hrService.updateHeartRate(hrmCounter);
flywind 0:f90c3452d779 348
flywind 0:f90c3452d779 349
flywind 0:f90c3452d779 350 } else {
flywind 0:f90c3452d779 351 ble.waitForEvent();
flywind 0:f90c3452d779 352 }
flywind 0:f90c3452d779 353 }
flywind 0:f90c3452d779 354 }
flywind 0:f90c3452d779 355
flywind 0:f90c3452d779 356 // ================================================================
flywind 0:f90c3452d779 357 // === INITIAL SETUP ===
flywind 0:f90c3452d779 358 // ================================================================
flywind 0:f90c3452d779 359
flywind 0:f90c3452d779 360 void mpuInitialSetup()
flywind 0:f90c3452d779 361 {
flywind 0:f90c3452d779 362 // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
flywind 0:f90c3452d779 363 // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
flywind 0:f90c3452d779 364 // the baud timing being too misaligned with processor ticks. You must use
flywind 0:f90c3452d779 365 // 38400 or slower in these cases, or use some kind of external separate
flywind 0:f90c3452d779 366 // crystal solution for the UART timer.
flywind 0:f90c3452d779 367
flywind 0:f90c3452d779 368 // initialize device
flywind 0:f90c3452d779 369 pc.printf("Initializing I2C devices...\r\n");
flywind 0:f90c3452d779 370 mpu.initialize();
flywind 0:f90c3452d779 371
flywind 0:f90c3452d779 372 // verify connection
flywind 0:f90c3452d779 373 pc.printf("Testing device connections...\r\n");
flywind 0:f90c3452d779 374 if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
flywind 0:f90c3452d779 375 else pc.printf("MPU6050 connection failed\r\n");
flywind 0:f90c3452d779 376
flywind 0:f90c3452d779 377 // wait for ready
flywind 0:f90c3452d779 378 //Serial.println(F("\nSend any character to begin DMP programming and demo: "));
flywind 0:f90c3452d779 379 //while (Serial.available() && Serial.read()); // empty buffer
flywind 0:f90c3452d779 380 //while (!Serial.available()); // wait for data
flywind 0:f90c3452d779 381 //while (Serial.available() && Serial.read()); // empty buffer again
flywind 0:f90c3452d779 382
flywind 0:f90c3452d779 383 // load and configure the DMP
flywind 0:f90c3452d779 384 pc.printf("Initializing DMP...\r\n");
flywind 0:f90c3452d779 385 devStatus = mpu.dmpInitialize();
flywind 0:f90c3452d779 386
flywind 0:f90c3452d779 387 // make sure it worked (returns 0 if so)
flywind 0:f90c3452d779 388 if (devStatus == 0) {
flywind 0:f90c3452d779 389 // turn on the DMP, now that it's ready
flywind 0:f90c3452d779 390 pc.printf("Enabling DMP...\r\n");
flywind 0:f90c3452d779 391 mpu.setDMPEnabled(true);
flywind 0:f90c3452d779 392
flywind 0:f90c3452d779 393 // enable Arduino interrupt detection
flywind 0:f90c3452d779 394 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
flywind 0:f90c3452d779 395 checkpin.rise(&dmpDataReady);
flywind 0:f90c3452d779 396
flywind 0:f90c3452d779 397 mpuIntStatus = mpu.getIntStatus();
flywind 0:f90c3452d779 398
flywind 0:f90c3452d779 399 // set our DMP Ready flag so the main loop() function knows it's okay to use it
flywind 0:f90c3452d779 400 pc.printf("DMP ready! Waiting for first interrupt...\r\n");
flywind 0:f90c3452d779 401 dmpReady = true;
flywind 0:f90c3452d779 402
flywind 0:f90c3452d779 403 // get expected DMP packet size for later comparison
flywind 0:f90c3452d779 404 packetSize = mpu.dmpGetFIFOPacketSize();
flywind 0:f90c3452d779 405 red = 1;
flywind 0:f90c3452d779 406 } else {
flywind 0:f90c3452d779 407 // ERROR!
flywind 0:f90c3452d779 408 // 1 = initial memory load failed
flywind 0:f90c3452d779 409 // 2 = DMP configuration updates failed
flywind 0:f90c3452d779 410 // (if it's going to break, usually the code will be 1)
flywind 0:f90c3452d779 411
flywind 2:44bc61abdf33 412 pc.printf("DMP Initialization failed (code ");
flywind 0:f90c3452d779 413 pc.printf("%d", devStatus);
flywind 0:f90c3452d779 414 pc.printf(")\r\n");
flywind 0:f90c3452d779 415 }
flywind 0:f90c3452d779 416
flywind 0:f90c3452d779 417 }
flywind 0:f90c3452d779 418
flywind 0:f90c3452d779 419 // ================================================================
flywind 0:f90c3452d779 420 // === MPU6050 DATA ===
flywind 0:f90c3452d779 421 // ================================================================
flywind 0:f90c3452d779 422
flywind 0:f90c3452d779 423 void mpuData()
flywind 0:f90c3452d779 424 {
flywind 0:f90c3452d779 425 // reset interrupt flag and get INT_STATUS byte
flywind 0:f90c3452d779 426 mpuInterrupt = false;
flywind 0:f90c3452d779 427 mpuIntStatus = mpu.getIntStatus();
flywind 0:f90c3452d779 428
flywind 0:f90c3452d779 429 // get current FIFO count
flywind 0:f90c3452d779 430 fifoCount = mpu.getFIFOCount();
flywind 0:f90c3452d779 431
flywind 0:f90c3452d779 432 // check for overflow (this should never happen unless our code is too inefficient)
flywind 0:f90c3452d779 433 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
flywind 0:f90c3452d779 434 // reset so we can continue cleanly
flywind 0:f90c3452d779 435 mpu.resetFIFO();
flywind 0:f90c3452d779 436 //Serial.println(F("FIFO overflow!"));
flywind 0:f90c3452d779 437
flywind 0:f90c3452d779 438 // otherwise, check for DMP data ready interrupt (this should happen frequently)
flywind 0:f90c3452d779 439 } else if (mpuIntStatus & 0x02) {
flywind 0:f90c3452d779 440 // wait for correct available data length, should be a VERY short wait
flywind 0:f90c3452d779 441 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
flywind 0:f90c3452d779 442
flywind 0:f90c3452d779 443 // read a packet from FIFO
flywind 0:f90c3452d779 444 mpu.getFIFOBytes(fifoBuffer, packetSize);
flywind 0:f90c3452d779 445
flywind 0:f90c3452d779 446 // track FIFO count here in case there is > 1 packet available
flywind 0:f90c3452d779 447 // (this lets us immediately read more without waiting for an interrupt)
flywind 0:f90c3452d779 448 fifoCount -= packetSize;
flywind 0:f90c3452d779 449
flywind 0:f90c3452d779 450 #ifdef OUTPUT_READABLE_QUATERNION
flywind 0:f90c3452d779 451 // display quaternion values in easy matrix form: w x y z
flywind 0:f90c3452d779 452 mpu.dmpGetQuaternion(&q, fifoBuffer);
flywind 0:f90c3452d779 453 pc.printf("quat\t");
flywind 0:f90c3452d779 454 pc.printf("%f\t", q.w);
flywind 0:f90c3452d779 455 pc.printf("%f\t", q.x);
flywind 0:f90c3452d779 456 pc.printf("%f\t", q.y);
flywind 0:f90c3452d779 457 pc.printf("%f\t\r\n", q.z);
flywind 0:f90c3452d779 458 #endif
flywind 0:f90c3452d779 459
flywind 0:f90c3452d779 460 #ifdef OUTPUT_READABLE_EULER
flywind 0:f90c3452d779 461 // display Euler angles in degrees
flywind 0:f90c3452d779 462 mpu.dmpGetQuaternion(&q, fifoBuffer);
flywind 0:f90c3452d779 463 mpu.dmpGetEuler(euler, &q);
flywind 0:f90c3452d779 464 pc.printf("euler\t");
flywind 0:f90c3452d779 465 pc.printf("%f\t", euler[0] * 180/M_PI);
flywind 0:f90c3452d779 466 pc.printf("%f\t", euler[1] * 180/M_PI);
flywind 0:f90c3452d779 467 pc.printf("%f\t\r\n", euler[2] * 180/M_PI);
flywind 0:f90c3452d779 468 #endif
flywind 0:f90c3452d779 469
flywind 0:f90c3452d779 470 #ifdef OUTPUT_READABLE_YAWPITCHROLL
flywind 0:f90c3452d779 471 // display Euler angles in degrees
flywind 0:f90c3452d779 472 mpu.dmpGetQuaternion(&q, fifoBuffer);
flywind 0:f90c3452d779 473 mpu.dmpGetGravity(&gravity, &q);
flywind 0:f90c3452d779 474 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
flywind 2:44bc61abdf33 475 ypr[0] =ypr[0] * 180/M_PI;
flywind 2:44bc61abdf33 476 ypr[1] =ypr[1] * 180/M_PI;
flywind 2:44bc61abdf33 477 ypr[2] =ypr[2] * 180/M_PI;
flywind 2:44bc61abdf33 478 /*
flywind 0:f90c3452d779 479 pc.printf("ypr\t");
flywind 2:44bc61abdf33 480 pc.printf("%f\t", ypr[0] ;
flywind 2:44bc61abdf33 481 pc.printf("%f\t", ypr[1]);
flywind 2:44bc61abdf33 482 pc.printf("%f\t\r\n", ypr[2]);
flywind 2:44bc61abdf33 483 */
flywind 0:f90c3452d779 484 #endif
flywind 0:f90c3452d779 485
flywind 0:f90c3452d779 486 #ifdef OUTPUT_READABLE_REALACCEL
flywind 0:f90c3452d779 487 // display real acceleration, adjusted to remove gravity
flywind 0:f90c3452d779 488 mpu.dmpGetQuaternion(&q, fifoBuffer);
flywind 0:f90c3452d779 489 mpu.dmpGetAccel(&aa, fifoBuffer);
flywind 0:f90c3452d779 490 mpu.dmpGetGravity(&gravity, &q);
flywind 0:f90c3452d779 491 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
flywind 0:f90c3452d779 492 pc.printf("areal\t");
flywind 0:f90c3452d779 493 pc.printf("%f\t", aaReal.x);
flywind 0:f90c3452d779 494 pc.printf("%f\t", aaReal.y);
flywind 0:f90c3452d779 495 pc.printf("%f\t\r\n", aaReal.z);
flywind 0:f90c3452d779 496 #endif
flywind 0:f90c3452d779 497
flywind 0:f90c3452d779 498 #ifdef OUTPUT_READABLE_WORLDACCEL
flywind 0:f90c3452d779 499 // display initial world-frame acceleration, adjusted to remove gravity
flywind 0:f90c3452d779 500 // and rotated based on known orientation from quaternion
flywind 0:f90c3452d779 501 mpu.dmpGetQuaternion(&q, fifoBuffer);
flywind 0:f90c3452d779 502 mpu.dmpGetAccel(&aa, fifoBuffer);
flywind 0:f90c3452d779 503 mpu.dmpGetGravity(&gravity, &q);
flywind 0:f90c3452d779 504 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
flywind 0:f90c3452d779 505 pc.printf("aworld\t");
flywind 0:f90c3452d779 506 pc.printf("%f\t", aaWorld.x);
flywind 0:f90c3452d779 507 pc.printf("%f\t", aaWorld.y);
flywind 0:f90c3452d779 508 pc.printf("%f\t\r\n", aaWorld.z);
flywind 0:f90c3452d779 509 #endif
flywind 0:f90c3452d779 510
flywind 0:f90c3452d779 511 #ifdef OUTPUT_TEAPOT
flywind 0:f90c3452d779 512 // display quaternion values in InvenSense Teapot demo format:
flywind 0:f90c3452d779 513 teapotPacket[2] = fifoBuffer[0];
flywind 0:f90c3452d779 514 teapotPacket[3] = fifoBuffer[1];
flywind 0:f90c3452d779 515 teapotPacket[4] = fifoBuffer[4];
flywind 0:f90c3452d779 516 teapotPacket[5] = fifoBuffer[5];
flywind 0:f90c3452d779 517 teapotPacket[6] = fifoBuffer[8];
flywind 0:f90c3452d779 518 teapotPacket[7] = fifoBuffer[9];
flywind 0:f90c3452d779 519 teapotPacket[8] = fifoBuffer[12];
flywind 0:f90c3452d779 520 teapotPacket[9] = fifoBuffer[13];
flywind 0:f90c3452d779 521 for (int i = 0; i < 14; ++i) {
flywind 0:f90c3452d779 522 pc.putc(teapotPacket[i]);
flywind 0:f90c3452d779 523 }
flywind 0:f90c3452d779 524 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
flywind 0:f90c3452d779 525 #endif
flywind 0:f90c3452d779 526
flywind 0:f90c3452d779 527 }
flywind 0:f90c3452d779 528 }
flywind 0:f90c3452d779 529
flywind 0:f90c3452d779 530 void check_i2c_bus(void)
flywind 0:f90c3452d779 531 {
flywind 0:f90c3452d779 532
flywind 0:f90c3452d779 533 DigitalInOut scl(MPU6050_SCL);
flywind 0:f90c3452d779 534 DigitalInOut sda(MPU6050_SDA);
flywind 0:f90c3452d779 535
flywind 0:f90c3452d779 536 scl.input();
flywind 0:f90c3452d779 537 sda.input();
flywind 0:f90c3452d779 538 int scl_level = scl;
flywind 0:f90c3452d779 539 int sda_level = sda;
flywind 0:f90c3452d779 540 if (scl_level == 0 || sda_level == 0) {
flywind 0:f90c3452d779 541 printf("scl: %d, sda: %d, i2c bus is not released\r\n", scl_level, sda_level);
flywind 0:f90c3452d779 542
flywind 0:f90c3452d779 543 scl.output();
flywind 0:f90c3452d779 544 for (int i = 0; i < 8; i++) {
flywind 0:f90c3452d779 545 scl = 0;
flywind 0:f90c3452d779 546 wait_us(10);
flywind 0:f90c3452d779 547 scl = 1;
flywind 0:f90c3452d779 548 wait_us(10);
flywind 0:f90c3452d779 549 }
flywind 0:f90c3452d779 550 } else {
flywind 0:f90c3452d779 551 printf("scl: %d, sda: %d, i2c bus is released\r\n", scl_level, sda_level);
flywind 0:f90c3452d779 552 }
flywind 0:f90c3452d779 553 scl.input();
flywind 0:f90c3452d779 554 scl_level = scl;
flywind 0:f90c3452d779 555 sda_level = sda;
flywind 0:f90c3452d779 556 if (scl_level == 0 || sda_level == 0) {
flywind 0:f90c3452d779 557 printf("scl: %d, sda: %d, i2c bus is still not released\r\n", scl_level, sda_level);
flywind 0:f90c3452d779 558 } else {
flywind 0:f90c3452d779 559 printf("scl: %d, sda: %d, i2c bus is released\r\n", scl_level, sda_level);
flywind 0:f90c3452d779 560 }
flywind 0:f90c3452d779 561
flywind 0:f90c3452d779 562 }