main.cpp

00001 
00002 #include "mbed.h"
00003 
00004 
00005 #include "MPU6050_6Axis_MotionApps20.h"
00006 
00007 
00008 
00009 #include "tiny_ble.h"
00010 //#include "kalman.h"
00011 
00012 
00013 #include "BLEDevice.h"
00014 #include "MPUService.h"
00015 //#include "UARTService.h"
00016 #include "HeartRateService.h"
00017 #include "DeviceInformationService.h"
00018 
00019 
00020 /* Starting sampling rate. */
00021 
00022 #define LOG(...)    { pc.printf(__VA_ARGS__); }
00023 
00024 
00025 /* ============================================
00026 I2Cdev device library code is placed under the MIT license
00027 Copyright (c) 2012 Jeff Rowberg
00028 
00029 Permission is hereby granted, free of charge, to any person obtaining a copy
00030 of this software and associated documentation files (the "Software"), to deal
00031 in the Software without restriction, including without limitation the rights
00032 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00033 copies of the Software, and to permit persons to whom the Software is
00034 furnished to do so, subject to the following conditions:
00035 
00036 The above copyright notice and this permission notice shall be included in
00037 all copies or substantial portions of the Software.
00038 
00039 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00040 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00041 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00042 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00043 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00044 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00045 THE SOFTWARE.
00046 ===============================================
00047 */
00048 
00049 // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
00050 // is used in I2Cdev.h
00051 //#include "Wire.h"
00052 
00053 // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
00054 // for both classes must be in the include path of your project
00055 #include "MPU6050_6Axis_MotionApps20.h"
00056 #include "mbed_i2c.h"
00057 
00058 //#include "MPU6050.h" // not necessary if using MotionApps include file
00059 
00060 // class default I2C address is 0x68
00061 // specific I2C addresses may be passed as a parameter here
00062 // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
00063 // AD0 high = 0x69
00064 
00065 
00066 /* =========================================================================
00067    NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
00068    depends on the MPU-6050's INT pin being connected to the Arduino's
00069    external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
00070    digital I/O pin 2.
00071  * ========================================================================= */
00072 
00073 /* =========================================================================
00074    NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
00075    when using Serial.write(buf, len). The Teapot output uses this method.
00076    The solution requires a modification to the Arduino USBAPI.h file, which
00077    is fortunately simple, but annoying. This will be fixed in the next IDE
00078    release. For more info, see these links:
00079 
00080    http://arduino.cc/forum/index.php/topic,109987.0.html
00081    http://code.google.com/p/arduino/issues/detail?id=958
00082  * ========================================================================= */
00083 
00084 
00085 
00086 // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
00087 // quaternion components in a [w, x, y, z] format (not best for parsing
00088 // on a remote host such as Processing or something though)
00089 //#define OUTPUT_READABLE_QUATERNION
00090 
00091 // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
00092 // (in degrees) calculated from the quaternions coming from the FIFO.
00093 // Note that Euler angles suffer from gimbal lock (for more info, see
00094 // http://en.wikipedia.org/wiki/Gimbal_lock)
00095 //#define OUTPUT_READABLE_EULER
00096 
00097 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
00098 // pitch/roll angles (in degrees) calculated from the quaternions coming
00099 // from the FIFO. Note this also requires gravity vector calculations.
00100 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
00101 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
00102 #define OUTPUT_READABLE_YAWPITCHROLL
00103 
00104 // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
00105 // components with gravity removed. This acceleration reference frame is
00106 // not compensated for orientation, so +X is always +X according to the
00107 // sensor, just without the effects of gravity. If you want acceleration
00108 // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
00109 //#define OUTPUT_READABLE_REALACCEL
00110 
00111 // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
00112 // components with gravity removed and adjusted for the world frame of
00113 // reference (yaw is relative to initial orientation, since no magnetometer
00114 // is present in this case). Could be quite handy in some cases.
00115 //#define OUTPUT_READABLE_WORLDACCEL
00116 
00117 // uncomment "OUTPUT_TEAPOT" if you want output that matches the
00118 // format used for the InvenSense teapot demo
00119 //#define OUTPUT_TEAPOT
00120 
00121 
00122 // uncomment "OUTPUT_READABLE_ACCEL" if you want output that matches the
00123 // format used for the InvenSense teapot demo
00124 //#define OUTPUT_READABLE_ACCEL
00125 
00126 // uncomment "OUTPUT_READABLE_ACCEL" if you want output that matches the
00127 // format used for the InvenSense teapot demo
00128 //#define OUTPUT_READABLE_ACCEL
00129 
00130 /*
00131 * ====================================================================================================================================
00132 
00133 
00134 * =====================================================================================================================================
00135 */
00136 
00137 DigitalOut blue(LED_BLUE);
00138 DigitalOut green(LED_GREEN);
00139 DigitalOut red(LED_RED);
00140 
00141 InterruptIn button(BUTTON_PIN);
00142 AnalogIn    battery(BATTERY_PIN);
00143 Serial pc(USBTX, USBRX);
00144 //Serial pc(UART_TX, UART_RX);
00145 
00146 /*
00147 * ====================================================================================================================================
00148 
00149 
00150 * =====================================================================================================================================
00151 */
00152 const static char     DEVICE_NAME[]        = "CSSE BLE";
00153 static const uint16_t uuid16_list[]        = {GattService::UUID_HEART_RATE_SERVICE,
00154         GattService::UUID_DEVICE_INFORMATION_SERVICE
00155                                              };
00156 volatile bool bleIsConnected = false;
00157 BLEDevice  ble;
00158 static volatile bool  triggerSensorPolling = false;
00159 /*
00160 * ====================================================================================================================================
00161 
00162 
00163 * =====================================================================================================================================
00164 */
00165 
00166 
00167 MPU6050 mpu(MPU6050_SDA, MPU6050_SCL);
00168 void check_i2c_bus(void);
00169 void mpuInitialSetup();
00170 void mpuData();
00171 // MPU control/status vars
00172 bool dmpReady = false;  // set true if DMP init was successful
00173 uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
00174 uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
00175 uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
00176 uint16_t fifoCount;     // count of all bytes currently in FIFO
00177 uint8_t fifoBuffer[64]; // FIFO storage buffer
00178 
00179 // orientation/motion vars
00180 Quaternion q;           // [w, x, y, z]         quaternion container
00181 VectorInt16 aa;         // [x, y, z]            accel sensor measurements
00182 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
00183 VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
00184 VectorFloat gravity;    // [x, y, z]            gravity vector
00185 float euler[3]= {0};        // [psi, theta, phi]    Euler angle container
00186 float ypr[3]= {0};          // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
00187 
00188 // packet structure for InvenSense teapot demo
00189 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
00190 
00191 InterruptIn checkpin(p14);
00192 
00193 volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
00194 void dmpDataReady()
00195 {
00196     mpuInterrupt = true;
00197 }
00198 
00199 //kalman_data pitch_data;
00200 //kalman_data roll_data;
00201 
00202 /*
00203 * ====================================================================================================================================
00204 
00205 
00206 * =====================================================================================================================================
00207 */
00208 void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params)
00209 {
00210     LOG("Connected!\n");
00211     bleIsConnected = true;
00212 }
00213 
00214 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
00215 {
00216     LOG("Disconnected!\n");
00217     LOG("Restarting the advertising process\n");
00218     ble.startAdvertising();
00219     bleIsConnected = false;
00220 }
00221 
00222 void tick(void)
00223 {
00224     if(dmpReady) {
00225         green = !green;
00226     } else {
00227         red  = !red;
00228     }
00229     triggerSensorPolling = true;
00230 }
00231 
00232 void detect(void)
00233 {
00234     LOG("Button pressed\n");
00235     blue = !blue;
00236 }
00237 
00238 void tap_cb(unsigned char direction, unsigned char count)
00239 {
00240     LOG("Tap motion detected\n");
00241 }
00242 
00243 void android_orient_cb(unsigned char orientation)
00244 {
00245     LOG("Oriention changed\n");
00246 }
00247 
00248 
00249 int main(void)
00250 {
00251     blue  = 1;
00252     green = 1;
00253     red   = 1;
00254 
00255     pc.baud(115200);
00256 
00257     LOG("---- Seeed Tiny BLE ----\n");
00258 
00259     Ticker ticker;
00260     ticker.attach(tick, 0.5);
00261 
00262     button.fall(detect);
00263 
00264     LOG("Initialising the nRF51822\n");
00265 
00266 
00267     /*
00268     * ====================================================================================================================================
00269 
00270 
00271     * =====================================================================================================================================
00272     */
00273     ble.init();
00274     ble.onDisconnection(disconnectionCallback);
00275     ble.onConnection(connectionCallback);
00276 
00277 
00278 
00279     /* setup advertising */
00280     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED| GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
00281 
00282     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
00283 
00284     ble.accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_REMOTE_CONTROL);
00285 
00286     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
00287                                      (uint8_t *)uuid16_list, sizeof(uuid16_list));
00288 
00289     //ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,(const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
00290     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
00291                                      (const uint8_t *)MPUServiceUUID, sizeof(MPUServiceUUID));
00292 
00293     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00294 
00295     // UARTService uartService(ble);
00296     // uartServicePtr = &uartService;
00297 
00298     MPUService mpuServices(ble,ypr);
00299 
00300     /* Setup primary service. */
00301     uint8_t hrmCounter = 50; // init HRM to 100bps
00302     HeartRateService hrService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
00303 
00304     /* Setup auxiliary service. */
00305     DeviceInformationService deviceInfo(ble, "Seeed", "Tniy BLE", "SN1", "hw-rev1", "fw-rev1", "soft-rev1");
00306 
00307 
00308     ble.setAdvertisingInterval(50); /* 100ms; in multiples of 0.625ms. */
00309     ble.startAdvertising();
00310 
00311 
00312 
00313     /*
00314     * ====================================================================================================================================
00315 
00316 
00317     * =====================================================================================================================================
00318     */
00319     check_i2c_bus();
00320     mpuInitialSetup();
00321 
00322 
00323     while (true) {
00324         if(dmpReady && (mpuInterrupt || fifoCount >= packetSize)) {
00325             mpuData();
00326             pc.printf("ypr\t");
00327             pc.printf("%i\t", (int16_t)ypr[0]*100) ;
00328             pc.printf("%i\t", (int16_t)ypr[1]*100);
00329             pc.printf("%i\t\r\n", (int16_t)ypr[2]*100);
00330             mpuServices.updateYawPitchRoll(ypr);
00331 
00332         }
00333         if (triggerSensorPolling) {
00334             triggerSensorPolling = false;
00335 
00336             // Do blocking calls or whatever is necessary for sensor polling.
00337             // In our case, we simply update the HRM measurement.
00338             hrmCounter++;
00339 
00340             //  100 <= HRM bps <=175
00341             if (hrmCounter == 175) {
00342                 hrmCounter = 50;
00343 
00344             }
00345 
00346             // update bps
00347             hrService.updateHeartRate(hrmCounter);
00348 
00349 
00350         } else {
00351             ble.waitForEvent();
00352         }
00353     }
00354 }
00355 
00356 // ================================================================
00357 // ===                      INITIAL SETUP                       ===
00358 // ================================================================
00359 
00360 void mpuInitialSetup()
00361 {
00362     // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
00363     // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
00364     // the baud timing being too misaligned with processor ticks. You must use
00365     // 38400 or slower in these cases, or use some kind of external separate
00366     // crystal solution for the UART timer.
00367 
00368     // initialize device
00369     pc.printf("Initializing I2C devices...\r\n");
00370     mpu.initialize();
00371 
00372     // verify connection
00373     pc.printf("Testing device connections...\r\n");
00374     if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
00375     else pc.printf("MPU6050 connection failed\r\n");
00376 
00377     // wait for ready
00378     //Serial.println(F("\nSend any character to begin DMP programming and demo: "));
00379     //while (Serial.available() && Serial.read()); // empty buffer
00380     //while (!Serial.available());                 // wait for data
00381     //while (Serial.available() && Serial.read()); // empty buffer again
00382 
00383     // load and configure the DMP
00384     pc.printf("Initializing DMP...\r\n");
00385     devStatus = mpu.dmpInitialize();
00386 
00387     // make sure it worked (returns 0 if so)
00388     if (devStatus == 0) {
00389         // turn on the DMP, now that it's ready
00390         pc.printf("Enabling DMP...\r\n");
00391         mpu.setDMPEnabled(true);
00392 
00393         // enable Arduino interrupt detection
00394         pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
00395         checkpin.rise(&dmpDataReady);
00396 
00397         mpuIntStatus = mpu.getIntStatus();
00398 
00399         // set our DMP Ready flag so the main loop() function knows it's okay to use it
00400         pc.printf("DMP ready! Waiting for first interrupt...\r\n");
00401         dmpReady = true;
00402 
00403         // get expected DMP packet size for later comparison
00404         packetSize = mpu.dmpGetFIFOPacketSize();
00405         red = 1;
00406     } else {
00407         // ERROR!
00408         // 1 = initial memory load failed
00409         // 2 = DMP configuration updates failed
00410         // (if it's going to break, usually the code will be 1)
00411 
00412         pc.printf("DMP Initialization failed (code ");
00413         pc.printf("%d", devStatus);
00414         pc.printf(")\r\n");
00415     }
00416 
00417 }
00418 
00419 // ================================================================
00420 // ===                      MPU6050 DATA                        ===
00421 // ================================================================
00422 
00423 void mpuData()
00424 {
00425     // reset interrupt flag and get INT_STATUS byte
00426     mpuInterrupt = false;
00427     mpuIntStatus = mpu.getIntStatus();
00428 
00429     // get current FIFO count
00430     fifoCount = mpu.getFIFOCount();
00431 
00432     // check for overflow (this should never happen unless our code is too inefficient)
00433     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
00434         // reset so we can continue cleanly
00435         mpu.resetFIFO();
00436         //Serial.println(F("FIFO overflow!"));
00437 
00438         // otherwise, check for DMP data ready interrupt (this should happen frequently)
00439     } else if (mpuIntStatus & 0x02) {
00440         // wait for correct available data length, should be a VERY short wait
00441         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00442 
00443         // read a packet from FIFO
00444         mpu.getFIFOBytes(fifoBuffer, packetSize);
00445 
00446         // track FIFO count here in case there is > 1 packet available
00447         // (this lets us immediately read more without waiting for an interrupt)
00448         fifoCount -= packetSize;
00449 
00450 #ifdef OUTPUT_READABLE_QUATERNION
00451         // display quaternion values in easy matrix form: w x y z
00452         mpu.dmpGetQuaternion(&q, fifoBuffer);
00453         pc.printf("quat\t");
00454         pc.printf("%f\t", q.w);
00455         pc.printf("%f\t", q.x);
00456         pc.printf("%f\t", q.y);
00457         pc.printf("%f\t\r\n", q.z);
00458 #endif
00459 
00460 #ifdef OUTPUT_READABLE_EULER
00461         // display Euler angles in degrees
00462         mpu.dmpGetQuaternion(&q, fifoBuffer);
00463         mpu.dmpGetEuler(euler, &q);
00464         pc.printf("euler\t");
00465         pc.printf("%f\t", euler[0] * 180/M_PI);
00466         pc.printf("%f\t", euler[1] * 180/M_PI);
00467         pc.printf("%f\t\r\n", euler[2] * 180/M_PI);
00468 #endif
00469 
00470 #ifdef OUTPUT_READABLE_YAWPITCHROLL
00471         // display Euler angles in degrees
00472         mpu.dmpGetQuaternion(&q, fifoBuffer);
00473         mpu.dmpGetGravity(&gravity, &q);
00474         mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
00475         ypr[0] =ypr[0] * 180/M_PI;
00476         ypr[1] =ypr[1] * 180/M_PI;
00477         ypr[2] =ypr[2] * 180/M_PI;
00478         /*
00479         pc.printf("ypr\t");
00480         pc.printf("%f\t", ypr[0] ;
00481         pc.printf("%f\t", ypr[1]);
00482         pc.printf("%f\t\r\n", ypr[2]);
00483         */
00484 #endif
00485 
00486 #ifdef OUTPUT_READABLE_REALACCEL
00487         // display real acceleration, adjusted to remove gravity
00488         mpu.dmpGetQuaternion(&q, fifoBuffer);
00489         mpu.dmpGetAccel(&aa, fifoBuffer);
00490         mpu.dmpGetGravity(&gravity, &q);
00491         mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
00492         pc.printf("areal\t");
00493         pc.printf("%f\t", aaReal.x);
00494         pc.printf("%f\t", aaReal.y);
00495         pc.printf("%f\t\r\n", aaReal.z);
00496 #endif
00497 
00498 #ifdef OUTPUT_READABLE_WORLDACCEL
00499         // display initial world-frame acceleration, adjusted to remove gravity
00500         // and rotated based on known orientation from quaternion
00501         mpu.dmpGetQuaternion(&q, fifoBuffer);
00502         mpu.dmpGetAccel(&aa, fifoBuffer);
00503         mpu.dmpGetGravity(&gravity, &q);
00504         mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
00505         pc.printf("aworld\t");
00506         pc.printf("%f\t", aaWorld.x);
00507         pc.printf("%f\t", aaWorld.y);
00508         pc.printf("%f\t\r\n", aaWorld.z);
00509 #endif
00510 
00511 #ifdef OUTPUT_TEAPOT
00512         // display quaternion values in InvenSense Teapot demo format:
00513         teapotPacket[2] = fifoBuffer[0];
00514         teapotPacket[3] = fifoBuffer[1];
00515         teapotPacket[4] = fifoBuffer[4];
00516         teapotPacket[5] = fifoBuffer[5];
00517         teapotPacket[6] = fifoBuffer[8];
00518         teapotPacket[7] = fifoBuffer[9];
00519         teapotPacket[8] = fifoBuffer[12];
00520         teapotPacket[9] = fifoBuffer[13];
00521         for (int i = 0; i < 14; ++i) {
00522             pc.putc(teapotPacket[i]);
00523         }
00524         teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
00525 #endif
00526 
00527     }
00528 }
00529 
00530 void check_i2c_bus(void)
00531 {
00532 
00533     DigitalInOut scl(MPU6050_SCL);
00534     DigitalInOut sda(MPU6050_SDA);
00535 
00536     scl.input();
00537     sda.input();
00538     int scl_level = scl;
00539     int sda_level = sda;
00540     if (scl_level == 0 || sda_level == 0) {
00541         printf("scl: %d, sda: %d, i2c bus is not released\r\n", scl_level, sda_level);
00542 
00543         scl.output();
00544         for (int i = 0; i < 8; i++) {
00545             scl = 0;
00546             wait_us(10);
00547             scl = 1;
00548             wait_us(10);
00549         }
00550     } else {
00551         printf("scl: %d, sda: %d, i2c bus is released\r\n", scl_level, sda_level);
00552     }
00553     scl.input();
00554     scl_level = scl;
00555     sda_level = sda;
00556     if (scl_level == 0 || sda_level == 0) {
00557         printf("scl: %d, sda: %d, i2c bus is still not released\r\n", scl_level, sda_level);
00558     } else {
00559         printf("scl: %d, sda: %d, i2c bus is released\r\n", scl_level, sda_level);
00560     }
00561 
00562 }