File content as of revision 3:b2087af18efe:

#include <events/mbed_events.h>
#include <mbed.h>
#include "ble/BLE.h"
#include "ble/Gap.h"
#include "ble/services/BatteryService.h"
#include "MPU6050.h"
#include "MPUService.h"
#include <string> 
#include <stdio.h>
#include "SoftSerial.h"
Serial pc(p6, p8);
//SoftSerial pc1(p9, p10);
MPU6050 mpu6050;
//InterruptIn pin(p10);
//DigitalOut led(LED1, 1);
//DigitalOut led2(LED2, 1);
uint16_t customServiceUUID  = 0xA000;
uint16_t readCharUUID       = 0xA001;
uint16_t interruptUUID      = 0xA002;
DigitalOut alivenessLED(LED1, 0);
const static char     DEVICE_NAME[] = "MPU";
static const uint16_t uuid16_list[] = {0x0A0A};
//static int counter;
/* Set Up custom Characteristics */
static float readValue[3] = {0};
static float interruptValue[3] = {0};
ReadOnlyArrayGattCharacteristic<float, sizeof(readValue)> readChar(readCharUUID, readValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
ReadOnlyArrayGattCharacteristic<float, sizeof(interruptValue)> interrupt(interruptUUID, interruptValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);

//static float writeValue[2] = {3,4};
//WriteOnlyArrayGattCharacteristic<float, sizeof(writeValue)> writeChar(writeCharUUID, writeValue);

/* Set up custom service */
GattCharacteristic *characteristics[] = {&readChar/*, &writeChar*/, &interrupt};
GattService        customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));



static EventQueue eventQueue(/* event count */ 10 * /* event size */ 32);
InterruptIn button(BUTTON1);




void updateReadValue() {
    alivenessLED = !alivenessLED; 
    mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
    
    //mpu6050.readAccelData(accelData);
    //mpu6050.getAres();
    //ax = accelData[0] * aRes - accelBias[0];
    //ay = accelData[1] * aRes - accelBias[1];
    //az = accelData[2] * aRes - accelBias[2];
    float newValue[3] = {ax,ay,az};
    //mpu6050.readGyroData(gyroData);
    //mpu6050.getGres();
    
    //gx = gyroData[0] * gRes; // - gyroBias[0];
    //gy = gyroData[1] * gRes; // - gyroBias[1];
    //gz = gyroData[2] * gRes; // - gyroBias[2];
    //float newValue[3] = {gx,gy,gz};*/
    //counter++;
    //pc.printf("| Before | ax=%.3f | ay=%.3f | az=%.3f \r\n",gx,gy,gz);
    pc.printf("| Acce | ax=%.3f | ay=%.3f | az=%.3f | Gyro | gx=%.3f | gy=%.3f | gz=%.3f\r\n",ax,ay,az,gx,gy,gz);
    //len_send = strlen((const char *)newValue);
    

    uint8_t motionByte = mpu6050.readThisByte(MPU6050_RA_MOT_DETECT_STATUS);
    pc.printf("Motion byte: %d\r\n",motionByte);
    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(interrupt.getValueHandle(), (uint8_t*)motionByte, 8);
    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), (uint8_t*)newValue, 12);
}
void periodicCallback(void)
{
    if (BLE::Instance().getGapState().connected) {
        eventQueue.call(updateReadValue);
    }
}

void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context)
{
    BLE& ble = BLE::Instance();
    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    (void) params;
    BLE::Instance().gap().startAdvertising();
}
void onBleInitError(BLE &ble, ble_error_t 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) {
        /* In case of error, forward the error handling to onBleInitError */
        onBleInitError(ble, error);
        return;
    }

    /* Ensure that it is the default instance of BLE */
    if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }
    ble.gap().onDisconnection(disconnectionCallback);

    /* 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::COMPLETE_LOCAL_NAME, (uint8_t*)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    
    ble.gap().startAdvertising();
    ble.addService(customService);
    ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
    

}
void flip() {
    //pc.printf("Desio se interupt");
}
bool XnegMD, XposMD, YnegMD, YposMD, ZnegMD, ZposMD;
int main()
{  
    //mpu6050.whoAmI();
    //mpu6050.calibrate(accelBias, gyroBias);
    //mpu6050.init();
    mpu6050.calibrate(accelBias, gyroBias);
    mpu6050.initialize();
    mpu6050.setSleepEnabled(0);
    pc.baud(115200);
    mpu6050.setDHPFMode(1);
    //setMotionDetectionThreshold
    //char myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x1F);
    //pc.printf("Prije postavljanja\n");
    //pc.printf("MotionDetectionThreshold iznosi: %x\n",myChar);
    //pc.printf("Poslije postavljanja\n");      
    //mpu6050.writeByte(MPU6050_ADDRESS, 0x1F,0x02);
    //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x1F);
    //pc.printf("MotionDetectionThreshold iznosi: %x\n",myChar);
    
    
    //setMotionDetectionDuration
    //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x20);
    //pc.printf("Prije postavljanja\n");
    //pc.printf("MotionDetectionDuration iznosi: %x\n",myChar);
    //pc.printf("Poslije postavljanja\n");   
    //mpu6050.writeByte(MPU6050_ADDRESS, 0x20,0x28);
    //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x20);
    //pc.printf("MotionDetectionDuration iznosi: %x\n",myChar);
    
//    XnegMD = mpu6050.getXNegMotionDetected();
    //mpu6050.whoAmI();
    //uint8_t *data;
    //char byte = mpu6050.readByte(MPU6050_ADDRESS, 0x61);
    //*data = byte & (1 << 7);
    //pc.printf("Byte iznosi: %x\n",byte);
    //int i = data[0];
    //pc.printf("7 bit iznosi: %d\n", i);
    
    //XposMD = mpu6050.getXPosMotionDetected();
   
    
    //pin.fall(&flip);
    eventQueue.call_every(500, periodicCallback);
    BLE& ble = BLE::Instance();
    ble.onEventsToProcess(scheduleBleEventsProcessing);
    ble.init(bleInitComplete);  
    eventQueue.dispatch_forever();
    while (ble.hasInitialized()  == false) { /* spin loop */ }
  
  
  
  
  
  
   // pc1.baud(9600);
    //while (true) {
        //ble.waitForEvent();
      //  pc1.printf("tare ");
        //wait_ms(1000);
        
    //}
    //return 0;
}