File content as of revision 22:65f63e2d06bd:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2015 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mbed.h"
#include "MPU6050.h"
#include "ble/BLE.h"
#include "ble/services/HeartRateService.h"
#define ratio 1

int i= 0,j = 0;
float sum = 0;
uint32_t sumCount = 0;
volatile uint8_t hrmCounter;

float central1[3], central2[3];
float drum1_min[3],drum2_min[3],drum3_min[3],drum4_min[3],drum5_min[3],drum6_min[3],drum7_min[3],drum8_min[3],drum9_min[3],drum10_min[3];
float drum1_max[3],drum2_max[3],drum3_max[3],drum4_max[3],drum5_max[3],drum6_max[3],drum7_max[3],drum8_max[3],drum9_max[3],drum10_max[3]; 
int flag = 0;
int stt1 = 0, stt2 = 0;
int drum1_stt1 = 0,drum2_stt1 = 0,drum3_stt1 = 0,drum4_stt1 = 0,drum5_stt1 = 0;
int drum1_stt2 = 0,drum2_stt2 = 0,drum3_stt2 = 0,drum4_stt2 = 0,drum5_stt2 = 0;


   InterruptIn mybutton(USER_BUTTON);

   MPU6050 mpu6050;
   
   MPU6050 mpu6050_2;
   
   Timer t;

   Serial pc(USBTX, USBRX); // tx, rx

   //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
   //N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
   
void get();

DigitalOut led1(LED1, 1);

const static char     DEVICE_NAME[]        = "IDB";
static const uint16_t uuid16_list[]        = {GattService::UUID_HEART_RATE_SERVICE};

static volatile bool  triggerSensorPolling = false;

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    (void)params;
    BLE::Instance().gap().startAdvertising(); // restart advertising
}

void periodicCallback(void)
{
    led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
    /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
     * heavy-weight sensor polling from the main thread. */
    triggerSensorPolling = true;
}

void onBleInitError(BLE &ble, ble_error_t error)
{
    (void)ble;
    (void)error;
   /* Initialization error handling should go here */
}

void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;

    if (error != BLE_ERROR_NONE) {
        onBleInitError(ble, error);
        return;
    }

    if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }

    ble.gap().onDisconnection(disconnectionCallback);

    /* Setup primary service. */
    uint8_t hrmCounter = 'A'; // init HRM to 60bps
    HeartRateService hrService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);

    /* Setup advertising. */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(1000); /* 1000ms */
    ble.gap().startAdvertising();
    
    pc.baud(9600);  

  //Set up I2C
  i2c.frequency(400000);  // use fast (400 kHz) I2C   
  i2c2.frequency(400000);
  t.start();        
  
  //lcd.init();
  //lcd.setBrightness(0.05);
  
    
  // Read the WHO_AM_I register, this is a good test of communication
  uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
  pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
  
  if (whoami == 0x68) // WHO_AM_I should always be 0x68
  {  
    pc.printf("MPU6050 is online...");
    wait(1);
    //lcd.clear();
    //lcd.printString("MPU6050 OK", 0, 0);

    
    mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
    pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
    pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
    pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
    pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
    pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
    pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
    wait(1);

    if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) 
    {
    mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
    mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
    mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature

    /*lcd.clear();
    lcd.printString("MPU6050", 0, 0);
    lcd.printString("pass self test", 0, 1);
    lcd.printString("initializing", 0, 2);  */
    wait(2);
       }
    else
    {
    pc.printf("Device did not the pass self-test!\n\r");
 
       /*lcd.clear();
       lcd.printString("MPU6050", 0, 0);
       lcd.printString("no pass", 0, 1);
       lcd.printString("self test", 0, 2);*/      
      }
    }
    else
    {
    pc.printf("Could not connect to MPU6050: \n\r");
    pc.printf("%#x \n",  whoami);
 
    /*lcd.clear();
    lcd.printString("MPU6050", 0, 0);
    lcd.printString("no connection", 0, 1);
    lcd.printString("0x", 0, 2);  lcd.setXYAddress(20, 2); lcd.printChar(whoami);*/
 
    while(1) ; // Loop forever if communication doesn't happen
  }
    
    // infinite loop
    while (1) {
        // check for trigger from periodicCallback()
        if (triggerSensorPolling && ble.getGapState().connected) {
            triggerSensorPolling = false;

            // If data ready bit set, all data registers have new data
  if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
    mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
    mpu6050.getAres();
    
    // Now we'll calculate the accleration value into actual g's
    ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
    ay = (float)accelCount[1]*aRes - accelBias[1];   
    az = (float)accelCount[2]*aRes - accelBias[2];  
    
    ax2 = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
    ay2 = (float)accelCount[1]*aRes - accelBias[1];   
    az2 = (float)accelCount[2]*aRes - accelBias[2];
   
    mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
    mpu6050.getGres();
 
    // Calculate the gyro value into actual degrees per second
    gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
    gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
    gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
    
    gx2 = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
    gy2 = (float)gyroCount[1]*gRes; // - gyroBias[1];  
    gz2 = (float)gyroCount[2]*gRes; // - gyroBias[2];   

    tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
    temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
   }  
   
    Now = t.read_us();
    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
    lastUpdate = Now;
    
    sum += deltat;
    sumCount++;
    
    if(lastUpdate - firstUpdate > 10000000.0f) {
     beta = 0.04;  // decrease filter gain after stabilized
     zeta = 0.015; // increasey bias drift gain after stabilized
    }
    
   // Pass gyro rate as rad/s
    mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
    mpu6050.MadgwickQuaternionUpdate(ax2, ay2, az2, gx2*PI/180.0f, gy2*PI/180.0f, gz2*PI/180.0f);

    // Serial print and/or display at 0.5 s rate independent of data rates
    delt_t = t.read_ms() - count_mpu;
    //if (delt_t > 500) { // update LCD once per half-second independent of read rate

    //pc.printf("ax = %f", 1000*ax); 
    //pc.printf(" ay = %f", 1000*ay); 
    //pc.printf(" az = %f  mg\n\r", 1000*az); 

    //pc.printf("gx = %f", gx); 
    //pc.printf(" gy = %f", gy); 
    //pc.printf(" gz = %f  deg/s\n\r", gz); 
    
    //pc.printf(" temperature = %f  C\n\r", temperature); 
    
    //pc.printf("q0 = %f\n\r", q[0]);
    //pc.printf("q1 = %f\n\r", q[1]);
    //pc.printf("q2 = %f\n\r", q[2]);
    //pc.printf("q3 = %f\n\r", q[3]);      
    
    //lcd.clear();
    //lcd.printString("MPU6050", 0, 0);
    //lcd.printString("x   y   z", 0, 1);
    //lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax));
    //lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay));
    //lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2);
    
    
  // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
  // In this coordinate system, the positive z-axis is down toward Earth. 
  // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
  // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
  // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
  // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
  // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
  // applied in the correct order which for this configuration is yaw, pitch, and then roll.
  // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
    pitch *= 180.0f / PI;
    yaw   *= 180.0f / PI; 
    roll  *= 180.0f / PI;
    
    yaw2   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
    pitch2 = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
    roll2  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
    pitch2 *= 180.0f / PI;
    yaw2   *= 180.0f / PI; 
    roll2  *= 180.0f / PI;

//    pc.printf("Yaw, Pitch, Roll: \n\r");
//    pc.printf("%f", yaw);
//    pc.printf(", ");
//    pc.printf("%f", pitch);
//    pc.printf(", ");
//    pc.printf("%f\n\r", roll);
//    pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");

     //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
     //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
 
    myled= !myled;
    count_mpu = t.read_ms(); 
    sum = 0;
    sumCount = 0;
    if ((yaw < 1) && (yaw > 0) && (flag == 0))
    {

        central1[0] = yaw;
       central1[1] = pitch;
       central1[2] = roll;
         central2[0] = yaw2;
         central2[1] = pitch2;
         central2[2] = roll2;


    pc.printf("central x y z : %f %f %f \r\n", central1[0],central1[1],central1[2]);
    flag = 1;
    }
    //if (i == 2000) i = 0;
    if (flag == 1)
    {
        pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
        //hrService.updateHeartRate((uint8_t)yaw);
        //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", drum1_min[0], drum1_max[0], drum1_min[2]);
        switch (stt1)
    {
        case 0:
        pc.printf("%d",stt1);
            if (/*((yaw > drum1_min[0]) ||*/ (yaw < drum1_max[0]) /*&& (pitch > drum1_min[1]) && (pitch < drum1_max[1]) && (roll > drum1_max[2]) && (roll < drum1_max[2])*/) stt1 = 1;
            else if (/*(yaw > drum2_min[0]) ||*/ (yaw < drum2_max[0])/* && (pitch > drum2_min[1]) && (pitch < drum2_max[1]) && (roll > drum2_max[2]) && (roll < drum2_max[2])*/) stt1 = 2;
            else if (/*(yaw > drum3_min[0]) ||*/ (yaw < drum3_max[0])/* && (pitch > drum3_min[1]) && (pitch < drum3_max[1]) && (roll > drum3_max[2]) && (roll < drum3_max[2])*/) stt1 = 3;
            hrService.updateHeartRate((uint8_t)96);
             break;
        case 1:
        pc.printf("%d",stt1);
            if (drum1_stt1 == 0)
            {
                pc.printf("drum 1_1\r\n");
                drum1_stt1 = 1;
                hrService.updateHeartRate((uint8_t)69);
            } 
            else if ((yaw > drum1_max[0]) || (pitch > drum1_max[1]) /*&& (roll > drum1_max[2])*/) {
                 stt1 = 0 ;
                 drum1_stt1 = 0;
                 pc.printf("up\r\n");
                }
            break;
        case 2:
        pc.printf("%d",stt1);
            if (drum2_stt1 == 0)
            {
                pc.printf("drum 2_2\r\n");
                drum1_stt1 = 1;
                hrService.updateHeartRate((uint8_t)88);
            } 
            else if ((yaw > drum2_max[0]) && (pitch > drum2_max[1]) /*&& (roll > drum2_max[2])*/) {
                 stt1 = 0 ;
                 drum1_stt1 = 0;
                 pc.printf("up\r\n");
                }
            break;
        case 3:
        pc.printf("%d",stt1);
            if (drum3_stt1 == 0)
            {
                pc.printf("drum 3_3\r\n");
                drum3_stt1 = 1;
                hrService.updateHeartRate((uint8_t)3);
            } 
            else if ((yaw > drum3_max[0]) && (pitch > drum3_max[1]) /*&& (roll > drum2_max[2])*/) {
                 stt1 = 0 ;
                 drum3_stt1 = 0;
                 pc.printf("up\r\n");
                }
            break;
        case 4:
            pc.printf("%d",stt1);
            if (drum4_stt1 == 0)
            {
                pc.printf("drum 4_4\r\n");
                drum4_stt1 = 1;
                hrService.updateHeartRate((uint8_t)4);
            } 
            else if ((yaw > drum4_max[0]) && (pitch > drum4_max[1]) /*&& (roll > drum2_max[2])*/) {
                 stt1 = 0 ;
                 drum4_stt1 = 0;
                 pc.printf("up\r\n");
                }
            break;
        case 5:
        pc.printf("%d",stt1);
            if (drum5_stt1 == 0)
            {
                pc.printf("drum 4_4\r\n");
                drum5_stt1 = 1;
                hrService.updateHeartRate((uint8_t)5);
            } 
            else if ((yaw > drum5_max[0]) && (pitch > drum5_max[1]) /*&& (roll > drum2_max[2])*/) {
                 stt1 = 0 ;
                 drum5_stt1 = 0;
                 pc.printf("up\r\n");
                }
            break;
    };
}
    }
//}
         else {
            ble.waitForEvent(); // low power wait for event
        }
    }
}

int main(void)
{
    Ticker ticker;
    ticker.attach(periodicCallback, 0.01); // blink LED every second
     mybutton.fall(get);
   

    BLE::Instance().init(bleInitComplete);
}

void get()
{
    j++;
    if (j == 1){
    drum1_min[0] = yaw - ratio;
    drum1_min[1] = pitch - ratio;
    drum1_min[2] = roll - ratio;
    
    drum1_max[0] = yaw + ratio;
    drum1_max[1] = pitch + ratio;
    drum1_max[2] = roll + ratio;
    }
    else if (j == 2){
    drum2_min[0] = yaw - ratio;
    drum2_min[1] = pitch - ratio;
    drum2_min[2] = roll - ratio;
    
    drum2_max[0] = yaw + ratio;
    drum2_max[1] = pitch + ratio;
    drum2_max[2] = roll + ratio;
    }
    else if (j == 3){
    drum3_min[0] = yaw - ratio;
    drum3_min[1] = pitch - ratio;
    drum3_min[2] = roll - ratio;
    
    drum3_max[0] = yaw + ratio;
    drum3_max[1] = pitch + ratio;
    drum3_max[2] = roll + ratio;
    }
    else if (j == 4){
    drum4_min[0] = yaw - ratio;
    drum4_min[1] = pitch - ratio;
    drum4_min[2] = roll - ratio;
    
    drum4_max[0] = yaw + ratio;
    drum4_max[1] = pitch + ratio;
    drum4_max[2] = roll + ratio;
    }
    else if (j == 5){
    drum5_min[0] = yaw - ratio;
    drum5_min[1] = pitch - ratio;
    drum5_min[2] = roll - ratio;
    
    drum5_max[0] = yaw + ratio;
    drum5_max[1] = pitch + ratio;
    drum5_max[2] = roll + ratio;
    }
    else if (j == 6){
    drum6_min[0] = yaw - ratio;
    drum6_min[1] = pitch - ratio;
    drum6_min[2] = roll - ratio;
    
    drum6_max[0] = yaw + ratio;
    drum6_max[1] = pitch + ratio;
    drum6_max[2] = roll + ratio;
    }
    else if (j == 7){
    drum7_min[0] = yaw - ratio;
    drum7_min[1] = pitch - ratio;
    drum7_min[2] = roll - ratio;
    
    drum7_max[0] = yaw + ratio;
    drum7_max[1] = pitch + ratio;
    drum7_max[2] = roll + ratio;
    }
    else if (j == 8){
    drum8_min[0] = yaw - ratio;
    drum8_min[1] = pitch - ratio;
    drum8_min[2] = roll - ratio;
    
    drum8_max[0] = yaw + ratio;
    drum8_max[1] = pitch + ratio;
    drum8_max[2] = roll + ratio;
    }
    else if (j == 9){
    drum9_min[0] = yaw - ratio;
    drum9_min[1] = pitch - ratio;
    drum9_min[2] = roll - ratio;
    
    drum9_max[0] = yaw + ratio;
    drum9_max[1] = pitch + ratio;
    drum9_max[2] = roll + ratio;
    }
    else if (j == 10){
    drum10_min[0] = yaw - ratio;
    drum10_min[1] = pitch - ratio;
    drum10_min[2] = roll - ratio;
    
    drum10_max[0] = yaw + ratio;
    drum10_max[1] = pitch + ratio;
    drum10_max[2] = roll + ratio;
    }
    if (j == 10) j = 0;
    pc.printf("x,y,z: %f %f %f \r\n",yaw,pitch,roll);
}