Amatuer Rocket Tilt Sensing

I use Invensense MPU sensors for this project hoping to take advantage of their motion fusion engine. The MPU 6050, 9150, 9250 can work for this project. The I2C library used for these experiments was the excellent work from Jeff Rowberg.

I designed some board adapters to convert low cost in the wind IMU modules to RFduino and Simblee Shields.
Adapt GY-521 GY-9150 IMU's to RFduino

Adapt GY-87 GY-87 IMU's to RFduino

My first sketch sent Invensense Teapot packets over serial/USB to a Processing sketch to display 3D movements. The Processing sketch was based on Jeff Rowberg example. I replaced his airplane with a rocket. I was able to send the Teapot packets over BLE as well, but I had no way to access the data for Android or iOS.

MPU6050_DMP6_MyTilt3.ino MPU6050_DMP6_MyTilt3.ino // MyTilt by Thomas Olson Rev 20140425.1 // 20140525.1 Removed my FIFO mods to test RFduino v1.9 fixes. // Using I2Cdev class by Jeff Rowberg <jeff@rowberg.net> #include "Wire.h" #include "I2Cdev.h" #include <RFduinoBLE.h> char buf[20]; // buffer to send to BLE radio. #include "MPU6050_6Axis_MotionApps20.h" MPU6050 mpu; //MPU6050 mpu(0x69); // <-- use for AD0 high /* ========================================================================= NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch depends on the MPU-6050's INT pin being connected to GPIO2. (RFduino) * ========================================================================= */ // IMPORTANT NOTE: The default RFduino library for I2C uses S0D1 for the pin mode // of the SCL and SDA lines. Typical BOBs with built-in pullups of 2.2K,4.7K and 10K // violate the nRF51822 maximum current of 0.5mA for the S0D1 mode. // Change the libraries/Wire/Wire.cpp to use H0D1 (5.0mA) if combined parallel resistance is // less than 6.8Kohm. NOTE: RFD v1.9 has switched to H0D1 by default. // My goal is to send the TEAPOT data over the BLE. I need to figure out to get Processing // to receive bluetooth packets. In mean time I can see the packets using Nordic tool // and on Linux using gatttool. // For testing send to Serial Port at 9600(RFduino speed limitation) baud which doesn't // seem to be fast enough to do serious IMU / DMP work. // Comment this out to not send to serial port #define TEAPOT_ON_SERIAL_PORT #define TILT_ANALYSIS #define LED_PIN 3 // just needed for testing that the loop is working. bool blinkState = false; #define DMP_IRQ_PIN 2 // the INT pin from the MPU for FIFO updates #ifdef TILT_ANALYSIS #define LED_TILT 4 #define R2D 57.295 float XYtilt; float TiltThreshold = 30.0; // Threshold of too much tilt #endif // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // I'm only interested in quaternions. Quaternion q; // [w, x, y, z] quaternion container // packet structure for InvenSense teapot demo uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; // ================================================================ // === INTERRUPT DETECTION ROUTINE === // ================================================================ volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high // To keep from getting hosed by conflict between DMP and Radio priorities disable the // DMP until FIFO data is dealt with later. NOTE: no longer needed for RFD v1.9 int dmpDataReady(uint32_t ulPin) { buf[17]++; mpuInterrupt = true; // mpuIntStatus = mpu.getIntStatus(); // mpu.setDMPEnabled(false); } // ================================================================ // === INITIAL SETUP === // ================================================================ void setup() { buf[17]=0; // just a loop counter I send to the BLE. override_uart_limit = true; Serial.begin(115200); Wire.begin(); NRF_TWI1->FREQUENCY = TWI_FREQUENCY_FREQUENCY_K250 << TWI_FREQUENCY_FREQUENCY_Pos; // initialize device Serial.println(F("Initializing I2C devices...")); mpu.initialize(); // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); #ifdef TILT_ANALYSIS Serial.print("LimitAngle: "); Serial.print(TiltThreshold); TiltThreshold = sin(TiltThreshold/R2D)/2; Serial.print(" - "); Serial.println(TiltThreshold); pinMode(LED_TILT, OUTPUT); digitalWrite(LED_TILT,false); #endif // configure LED for output pinMode(LED_PIN, OUTPUT); // load and configure the DMP Serial.println(F("Initializing DMP...")); devStatus = mpu.dmpInitialize(); // make sure it worked (returns 0 if so) if (devStatus == 0) { // supply your own gyro offsets here, scaled for min sensitivity mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset(1788); // set our DMP Ready flag so the main loop() function knows it's okay to use it Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady = true; // enable RFduino interrupt detection Serial.println(F("Enabling interrupt detection RFduino GPIO2)...")); pinMode(DMP_IRQ_PIN,INPUT); mpuIntStatus = mpu.getIntStatus(); RFduino_pinWakeCallback(DMP_IRQ_PIN,HIGH,dmpDataReady); // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) Serial.print(F("DMP Initialization failed (code ")); Serial.print(devStatus); Serial.println(F(")")); } RFduinoBLE.advertisementData = "myDMP"; RFduinoBLE.begin(); } // ================================================================ // === MAIN PROGRAM LOOP === // ================================================================ void loop() { RFduinoBLE.send(buf,20); // if programming failed, don't try to do anything if (!dmpReady) return; while (!mpuInterrupt) { // other program behavior stuff here } // Hmmm! Since Radio interrupt can hose our DMP activity throwing it out of wack, // send how many DMP interrupts occurred since last action. // If more than once print the number of times. if(buf[17] > 1){ //Serial.print("Missed IRQs "); //Serial.println((uint8_t)buf[17]); buf[16] = buf[17]; buf[17] = 0; } // reset interrupt flag mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus(); fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & 0x10) || fifoCount >= 1024) { mpu.resetFIFO(); // } else if (mpuIntStatus & 0x02) { //WTH: should be 0x01 } else if (mpuIntStatus & 0x01) { // wait for correct available data length, should be a VERY short wait while (fifoCount < packetSize) { fifoCount = mpu.getFIFOCount(); } // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize; //Actually, the way I am doing it now I don't care. #ifdef TILT_ANALYSIS // Calcultate tilt angle about the Z axis. Set TILT_FLAG if > limit mpu.dmpGetQuaternion(&q, fifoBuffer); XYtilt = sqrt(q.x * q.x + q.y * q.y); //Serial.print("XYtilt: "); //Serial.println(XYtilt); if(XYtilt >TiltThreshold){ digitalWrite(LED_TILT,true); teapotPacket[10] = 0x01; // I'm defining the unused byte as a status byte }else{ digitalWrite(LED_TILT,false); teapotPacket[10] = 0x00; } #endif // display quaternion values in InvenSense Teapot demo format: teapotPacket[2] = fifoBuffer[0]; teapotPacket[3] = fifoBuffer[1]; teapotPacket[4] = fifoBuffer[4]; teapotPacket[5] = fifoBuffer[5]; teapotPacket[6] = fifoBuffer[8]; teapotPacket[7] = fifoBuffer[9]; teapotPacket[8] = fifoBuffer[12]; teapotPacket[9] = fifoBuffer[13]; teapotPacket[11]++; // packetCount, loops at 0xFF on purpose #ifdef TEAPOT_ON_SERIAL_PORT Serial.write(teapotPacket, 14); #endif for(int i=0;i<14;i++){ buf[i] = teapotPacket[i]; //teapot data to BLE. } // The DMP FIFO gets lost when too much radio activity is going on. // So forget it. Just reset it // mpu.resetFIFO(); // Don't just see if Radio is not running. Instead wait for radio to // start and then wait for it to be done to ensure we don't get // interrupted. // while(!RFduinoBLE.radioActive); // while(RFduinoBLE.radioActive); // mpu.setDMPEnabled(true); // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } }//loop // Example Commanding over BLE. // Not really doing anything important here yet. void RFduinoBLE_onReceive(char *data, int len) { if (len >= 1) { switch(data[0]){ case 'R': //Got RESET CMD; RFduino_systemReset(); break; case 'C': buf[19]++; //Got the Count increment CMD"); break; default: break; } } } // For now just store RSSI in outgoing data buffer. void RFduinoBLE_onRSSI(int rssi){ buf[18] = (uint8_t)rssi; }

The Processing sketch is...

MyRocket.pde MyRocket.pde // MyRocket MPURocket by Thomas Olson is based on Jeff Rowberg Libraries. // I've changed the model to be a Rocket instead of Teapot or Airplane. // I use the MPU Z-axis for rocket pointing up. This way the typical yaw // instability is in the rockets roll direction which is what I // am least concerned with. // I am more interested in the tilt of the rocket which can lead // to off course situations. // // I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output // 6/20/2012 by Jeff Rowberg <jeff@rowberg.net> // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: // 2014-03-15 - MyRocket Teapot /* ============================================ I2Cdev device library code is placed under the MIT license Copyright (c) 2012 Jeff Rowberg MyRocket portions Copyright (c) 2014 Thomas Olson Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. =============================================== */ import processing.serial.*; import processing.opengl.*; import toxi.geom.*; import toxi.processing.*; // NOTE: requires ToxicLibs to be installed in order to run properly. // 1. Download from http://toxiclibs.org/downloads // 2. Extract into [userdir]/Processing/libraries // (location may be different on Mac/Linux) // 3. Run and bask in awesomeness ToxiclibsSupport gfx; Serial port; // The serial port char[] teapotPacket = new char[14]; // InvenSense Teapot packet int serialCount = 0; // current packet byte position int aligned = 0; int interval = 0; float[] q = new float[4]; Quaternion quat = new Quaternion(1, 0, 0, 0); float[] gravity = new float[3]; float[] euler = new float[3]; float[] ypr = new float[3]; void setup() { // 300px square viewport using OpenGL rendering size(300, 300, OPENGL); gfx = new ToxiclibsSupport(this); // setup lights and antialiasing lights(); smooth(); // display serial port list for debugging/clarity println(Serial.list()); // get the first available port (use EITHER this OR the specific port code below) String portName = Serial.list()[0]; portName = "/dev/ttyUSB0"; // get a specific serial port (use EITHER this OR the first-available code above) //String portName = "COM4"; // open the serial port port = new Serial(this, portName, 9600); // send single character to trigger DMP init/start // (expected by MPU6050_DMP6 example Arduino sketch) println("SEND R"); port.write('r'); println("SENT R"); } void draw() { if (millis() - interval > 1000) { // resend single character to trigger DMP init/start // in case the MPU is halted/reset while applet is running println("SEND R AGAIN"); port.write('r'); interval = millis(); } // black background if(teapotPacket[10] == 0) { background(0); // Normally Black background }else{ background(255,255,0); // Yellow for Caution Too Much Tilt } // translate everything to the middle of the viewport pushMatrix(); translate(width / 2, height / 2); // 3-step rotation from yaw/pitch/roll angles (gimbal lock!) // ...and other weirdness I haven't figured out yet //rotateY(-ypr[0]); //rotateZ(-ypr[1]); //rotateX(-ypr[2]); // toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!) // (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of // different coordinate system orientation assumptions between Processing // and InvenSense DMP) float[] axis = quat.toAxisAngle(); rotate(axis[0], -axis[1], axis[3], axis[2]); // draw main body in red fill(255, 0, 0, 200); //box(10, 10, 200); drawCylinder(10, 10, -100, 8); // draw front-facing tip in blue fill(0, 0, 255, 200); pushMatrix(); // translate(0, 0, -120); translate(0, -130, 0); // rotateX(PI/2); drawCylinder(0, 12, 30, 8); popMatrix(); // draw fins in green beginShape(TRIANGLES); fill(0, 255, 0, 200); vertex(-30, 0, -2); vertex(30, 0, -2); vertex(0, -50, -2); // fin top layer vertex(-30, 0, 2); vertex(30, 0, 2); vertex(0, -50, 2); // fin bottom layer // draw orthogonal fins in purple fill(200, 0, 200, 200); vertex(-2, 0, -30); vertex(-2, 0, 30); vertex(-2, -50, 0); // fin left layer vertex( 2, 0, -30); vertex(2, 0, 30); vertex( 2, -50, 0); // fin right layer endShape(); beginShape(QUADS); fill(0, 255, 0, 200); vertex(-30, 0, -2); vertex(-30, 0, 2); vertex(30, 0, 2); vertex(30, 0, -2); vertex(30, 0, -2); vertex(30, 0, 2); vertex(0, -50, 2); vertex(0, -50, -2); vertex(-30, 0, -2); vertex(0, -50, -2); vertex(0, -50, 2); vertex(-30, 0, 2); fill(200, 0, 200, 200); vertex(-2, 0, -30); vertex(2, 0, -30); vertex(-2, 0, 30); vertex(-2, 0, 30); vertex(-2, 0, -30); vertex(2, 0, -30); vertex(2, -50, 0); vertex(-2, -50, 0); vertex(-2, 0, 30); vertex(2, 0, 30); vertex(2, -50, 0); vertex(-2, -50, 0); endShape(); popMatrix(); translate(0, 100, 0); } void serialEvent(Serial port) { interval = millis(); while (port.available() > 0) { int ch = port.read(); // print((char)ch); if (ch == '$') {serialCount = 0;} // this will help with alignment if (aligned < 4) { // make sure we are properly aligned on a 14-byte packet if (serialCount == 0) { if (ch == '$') aligned++; else aligned = 0; } else if (serialCount == 1) { if (ch == 2) aligned++; else aligned = 0; } else if (serialCount == 12) { if (ch == '\r') aligned++; else aligned = 0; } else if (serialCount == 13) { if (ch == '\n') aligned++; else aligned = 0; } //println(ch + " " + aligned + " " + serialCount); serialCount++; if (serialCount == 14) serialCount = 0; } else { if (serialCount > 0 || ch == '$') { teapotPacket[serialCount++] = (char)ch; if (serialCount == 14) { serialCount = 0; // restart packet byte position // get quaternion from data packet q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f; q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f; q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f; q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f; for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i]; // set our toxilibs quaternion to new data quat.set(q[0], q[1], q[2], q[3]); /* // below calculations unnecessary for orientation only using toxilibs // calculate gravity vector gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]); gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]); gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; // calculate Euler angles euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]); euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1); // calculate yaw/pitch/roll angles ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2])); ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2])); // output various components for debugging //println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f); //println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI); //println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI); */ } } } } } void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) { float angle = 0; float angleIncrement = TWO_PI / sides; beginShape(QUAD_STRIP); for (int i = 0; i < sides + 1; ++i) { vertex(topRadius*cos(angle), 0, topRadius*sin(angle)); vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle)); angle += angleIncrement; } endShape(); // If it is not a cone, draw the circular top cap if (topRadius != 0) { angle = 0; beginShape(TRIANGLE_FAN); // Center point vertex(0, 0, 0); for (int i = 0; i < sides + 1; i++) { vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); angle += angleIncrement; } endShape(); } // If it is not a cone, draw the circular bottom cap if (bottomRadius != 0) { angle = 0; beginShape(TRIANGLE_FAN); // Center point vertex(0, tall, 0); for (int i = 0; i < sides + 1; i++) { vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); angle += angleIncrement; } endShape(); } }

When EVOthings/Cordova came available with BLE support I was able to graphically display 3D movements on both Android and iOS smart devices. EVOthings makes life easier by using HTML5, CSS3, and Javascript. No need for Android or Apple SDKs. The app required for this project for the smart device is the "EVOthings Client", not the "Evothings Viewer". The Client allows one to use a remote URL address for the app script. The modified version for the EVOthings test is...

MPU6050_DMP6_MyTilt3_EVOtest.ino MPU_DMP6_MyTilt3_EVOtest.ino // MyTilt by Thomas Olson Rev 20140425.1 // 20140525.1 Removed my FIFO mods to test RFduino v1.9 fixes. // Using I2Cdev class by Jeff Rowberg <jeff@rowberg.net> #include "Wire.h" #include "I2Cdev.h" #include <RFduinoBLE.h> char buf[20]; // buffer to send to BLE radio. #include "MPU6050_6Axis_MotionApps20.h" MPU6050 mpu; //MPU6050 mpu(0x69); // <-- use for AD0 high /* ========================================================================= NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch depends on the MPU-6050's INT pin being connected to GPIO2. (RFduino) * ========================================================================= */ // IMPORTANT NOTE: The default RFduino library for I2C uses S0D1 for the pin mode // of the SCL and SDA lines. Typical BOBs with built-in pullups of 2.2K,4.7K and 10K // violate the nRF51822 maximum current of 0.5mA for the S0D1 mode. // Change the libraries/Wire/Wire.cpp to use H0D1 (5.0mA) if combined parallel resistance is // less than 6.8Kohm. NOTE: RFD v1.9 has switched to H0D1 by default. // My goal is to send the TEAPOT data over the BLE. I need to figure out to get Processing // to receive bluetooth packets. In mean time I can see the packets using Nordic tool // and on Linux using gatttool. // For testing send to Serial Port at 9600(RFduino speed limitation) baud which doesn't // seem to be fast enough to do serious IMU / DMP work. // Comment this out to not send to serial port //#define TEAPOT_ON_SERIAL_PORT // OR send just the QUAT Array (Floats) to the serial port. //#define QUAT_ON_SERIAL_PORT #define TILT_ANALYSIS #define LED_PIN 3 // just needed for testing that the loop is working. bool blinkState = false; #define DMP_IRQ_PIN 2 // the INT pin from the MPU for FIFO updates #ifdef TILT_ANALYSIS #define LED_TILT 4 #define R2D 57.295 float XYtilt; float TiltThreshold = 30.0; // Threshold of too much tilt #endif // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // I'm only interested in quaternions. Quaternion q; // [w, x, y, z] quaternion container // packet structure for InvenSense teapot demo uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; // ================================================================ // === INTERRUPT DETECTION ROUTINE === // ================================================================ volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high // To keep from getting hosed by conflict between DMP and Radio priorities disable the // DMP until FIFO data is dealt with later. NOTE: no longer needed for RFD v1.9 int dmpDataReady(uint32_t ulPin) { buf[17]++; mpuInterrupt = true; // mpuIntStatus = mpu.getIntStatus(); // mpu.setDMPEnabled(false); } // ================================================================ // === INITIAL SETUP === // ================================================================ void setup() { buf[17]=0; // just a loop counter I send to the BLE. override_uart_limit = true; Serial.begin(115200); Wire.begin(); NRF_TWI1->FREQUENCY = TWI_FREQUENCY_FREQUENCY_K250 << TWI_FREQUENCY_FREQUENCY_Pos; // initialize device Serial.println(F("Initializing I2C devices...")); mpu.initialize(); // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); #ifdef TILT_ANALYSIS Serial.print("LimitAngle: "); Serial.print(TiltThreshold); TiltThreshold = sin(TiltThreshold/R2D)/2; Serial.print(" - "); Serial.println(TiltThreshold); pinMode(LED_TILT, OUTPUT); digitalWrite(LED_TILT,false); #endif // configure LED for output pinMode(LED_PIN, OUTPUT); // load and configure the DMP Serial.println(F("Initializing DMP...")); devStatus = mpu.dmpInitialize(); // make sure it worked (returns 0 if so) if (devStatus == 0) { // supply your own gyro offsets here, scaled for min sensitivity mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset(1788); // set our DMP Ready flag so the main loop() function knows it's okay to use it Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady = true; // enable RFduino interrupt detection Serial.println(F("Enabling interrupt detection RFduino GPIO2)...")); pinMode(DMP_IRQ_PIN,INPUT); mpuIntStatus = mpu.getIntStatus(); RFduino_pinWakeCallback(DMP_IRQ_PIN,HIGH,dmpDataReady); // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) Serial.print(F("DMP Initialization failed (code ")); Serial.print(devStatus); Serial.println(F(")")); } RFduinoBLE.advertisementData = "myDMP"; RFduinoBLE.begin(); } // ================================================================ // === MAIN PROGRAM LOOP === // ================================================================ void loop() { RFduinoBLE.send(buf,20); // if programming failed, don't try to do anything if (!dmpReady) return; while (!mpuInterrupt) { // other program behavior stuff here } // Hmmm! Since Radio interrupt can hose our DMP activity throwing it out of wack, // send how many DMP interrupts occurred since last action. // If more than once print the number of times. if(buf[17] > 1){ //Serial.print("Missed IRQs "); //Serial.println((uint8_t)buf[17]); buf[16] = buf[17]; buf[17] = 0; } // reset interrupt flag mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus(); fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & 0x10) || fifoCount >= 1024) { mpu.resetFIFO(); // } else if (mpuIntStatus & 0x02) { //WTH: should be 0x01 } else if (mpuIntStatus & 0x01) { // wait for correct available data length, should be a VERY short wait while (fifoCount < packetSize) { fifoCount = mpu.getFIFOCount(); } // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize; //Actually, the way I am doing it now I don't care. #ifdef TILT_ANALYSIS // Calcultate tilt angle about the Z axis. Set TILT_FLAG if > limit mpu.dmpGetQuaternion(&q, fifoBuffer); XYtilt = sqrt(q.x * q.x + q.y * q.y); //Serial.print("XYtilt: "); //Serial.println(XYtilt); if(XYtilt >TiltThreshold){ digitalWrite(LED_TILT,true); teapotPacket[10] = 0x01; // I'm defining the unused byte as a status byte }else{ digitalWrite(LED_TILT,false); teapotPacket[10] = 0x00; } #endif // display quaternion values in InvenSense Teapot demo format: teapotPacket[2] = fifoBuffer[0]; teapotPacket[3] = fifoBuffer[1]; teapotPacket[4] = fifoBuffer[4]; teapotPacket[5] = fifoBuffer[5]; teapotPacket[6] = fifoBuffer[8]; teapotPacket[7] = fifoBuffer[9]; teapotPacket[8] = fifoBuffer[12]; teapotPacket[9] = fifoBuffer[13]; teapotPacket[11]++; // packetCount, loops at 0xFF on purpose #ifdef TEAPOT_ON_SERIAL_PORT Serial.write(teapotPacket, 14); #endif #ifdef QUAT_ON_SERIAL_PORT float q[4]; // get quaternion from data packet q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f; q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f; q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f; q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f; for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i]; Serial.print("Q: "); Serial.print(q[0]); Serial.print(" "); Serial.print(q[1]); Serial.print(" "); Serial.print(q[2]); Serial.print(" "); Serial.println(q[3]); #endif for(int i=0;i<14;i++){ buf[i] = teapotPacket[i]; //teapot data to BLE. } // The DMP FIFO gets lost when too much radio activity is going on. // So forget it. Just reset it // mpu.resetFIFO(); // Don't just see if Radio is not running. Instead wait for radio to // start and then wait for it to be done to ensure we don't get // interrupted. // while(!RFduinoBLE.radioActive); // while(RFduinoBLE.radioActive); // mpu.setDMPEnabled(true); // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } }//loop // Example Commanding over BLE. // Not really doing anything important here yet. void RFduinoBLE_onReceive(char *data, int len) { if (len >= 1) { switch(data[0]){ case 'R': //Got RESET CMD; RFduino_systemReset(); break; case 'C': buf[19]++; //Got the Count increment CMD"); break; default: break; } } } // For now just store RSSI in outgoing data buffer. void RFduinoBLE_onRSSI(int rssi){ buf[18] = (uint8_t)rssi; }

Install the EVOthings Client from the appropriate App Store for your smart device. The icon looks like .
Then enter this URL into the "Connect with address" box.
http://ThomasOlson.com/ble/evo/evoTilt3/

Now waiting for "Simblee for Mobile" library support for more than basic widgets.
Really would be nice to have HTML5 and Javascript support to do custom canvas graphics.