File content as of revision 4:147bbe6f9626:

#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"
#include "circular_buffer.h"
#include <Fsm.h>
#include <Events.h>
bool test=0;
InterruptIn mpuInterrupt(p5);
struct TurnOff turnoff;
struct TurnOn turnon;
struct Move move;

Fsm* fsm;

struct MotionLog {
    int motX;
    int motY;
    int motZ;
};
MotionLog motionLog;
circular_buffer<MotionLog> cb(5);

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;
uint16_t gyroUUID       = 0xA003;



uint16_t motionThresholdUUID      = 0xA004;
static uint8_t motionThresholdValue[10] = {0};
WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(motionThresholdValue)> motionThreshold(motionThresholdUUID, motionThresholdValue);


uint16_t motionDurationUUID      = 0xA005;
static uint8_t motionDurationValue[10] = {0};
WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(motionDurationValue)> motionDuration(motionDurationUUID, motionDurationValue);


uint16_t zeroMotionThresholdUUID      = 0xA006;
static uint8_t zeroMotionThresholdValue[10] = {0};
WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(zeroMotionThresholdValue)> zeroMotionThreshold(zeroMotionThresholdUUID, zeroMotionThresholdValue);


uint16_t zeroMotionDurationUUID      = 0xA007;
static uint8_t zeroMotionDurationValue[10] = {0};
WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(zeroMotionDurationValue)> zeroMotionDuration(zeroMotionDurationUUID, zeroMotionDurationValue);

uint16_t fsmStateUUID       = 0xA008;
static float fsmStateValue[3] = {0};
ReadOnlyArrayGattCharacteristic<float, sizeof(fsmStateValue)> fsmState(fsmStateUUID, fsmStateValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);

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};
static float gyroValue[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);
ReadOnlyArrayGattCharacteristic<float, sizeof(gyroValue)> gyro(gyroUUID, gyroValue, 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, &gyro, &motionThreshold, &motionDuration, &zeroMotionThreshold, &zeroMotionDuration, &fsmState};
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.getAcceleration(&ax, &ay, &az);

    mpu6050.readAccelData(accelData);
    mpu6050.getAres();
    ax = accelData[0]*aRes  - accelBias[0];
    ay = accelData[1]*aRes  - accelBias[1];
    az = accelData[2]*aRes  - accelBias[2];
    float accelValues[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 gyroValues[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);
    uint8_t b;
    bool zero = false;
    float fx, fy,fz=0;
    //char x,y,z;
    int mx=0;
    int my=0;
    int mz=0;
    //x=y=z='0';
    for(int i = 0; i<=7; i++) {
        b = motionByte & (1<<i);
        if(i==0 && b==1) {
            zero = true;
            //pc.printf("    %c    %c    %c     |    ",x,y,z);
            //pc.printf(" | %.3f | %.3f | %.3f | %.3f | %.3f | %.3f  ZERO DETECTED\r\n",ax,ay,az, gx, gy, gz);
            //pc.printf(" | %d | %d | %d | %d | %d | %d  ZERO DETECTED\r\n",ax,ay,az, gx, gy, gz);
        }
        if(i==1 && b==2) {
            //pc.printf("This should not happen :).\r\n");
        }
        if(i==6 && b==64) {
            //x='+';
            fx=1;
            mx=1;
            //pc.printf("X POSITIVE motion detected.\r\n");
        }
        if(i==7 && b==128) {
            //x='-';
            fx=2;
            mx=1;
            //pc.printf("X NEGATIVE motion detected.\r\n");
        }
        if(i==4 && b==16) {
            //y='+';
            fy=1;
            my=1;
            //pc.printf("Y POSITIVE motion detected.\r\n");
        }
        if(i==5 && b==32) {
            //y='-';
            fy=2;
            my=1;
            //pc.printf("Y NEGATIVE motion detected.\r\n");
        }
        if(i==2 && b==4) {
            //z='+';
            fz=1;
            mz=1;
            //pc.printf("Z POSITIVE motion detected.\r\n");
        }
        if(i==3 && b==8) {
            //z='-';
            fz=2;
            mz=1;
            //pc.printf("Z NEGATIVE motion detected.\r\n");
        }
    }
   // if(test) {
        motionLog.motX = mx;
        motionLog.motY = my;
        motionLog.motZ = mz;

        cb.push_back(motionLog);
        //pc.printf("Desio se motion\r\n");
      //  test=0;
    //}

    int numX, numY, numZ=0;
    //pc.printf("\r\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
    numX=numY=numZ=0;
    for(int i=0; i<=4; i++) {
        //pc.printf("%d %d %d\r\n",cb[i].motX, cb[i].motY, cb[i].motZ);
        if(cb[i].motX!=0) {
            numX++;
        }
        if(cb[i].motY!=0) {
            numY++;
        }
        if(cb[i].motZ!=0) {
            numZ++;
        }
    }
    if(numX<=2) {
        numX=0;
    } else
        numX=1;

    if(numY<=2) {
        numY=0;
    } else
        numY=1;

    if(numZ<=2) {
        numZ=0;
    } else
        numZ=1;
    
    if(numX!=0 || numY!=0 || numZ!=0){
        numX=1;
        numY=1;
        numZ=1;
        }


    //pc.printf("***************************\r\n");
    //pc.printf("%d %d %d\r\n",numX,numY,numZ);
    float fsmStateValues[3] = {numX,numY,numZ};
    if(!zero) {
        //pc.printf("    %c    %c    %c     |    ",x,y,z);
        //pc.printf(" | %.3f | %.3f | %.3f | %.3f | %.3f | %.3f\r\n",ax,ay,az, gx, gy, gz);
        //pc.printf(" | %d | %d | %d | %d | %d | %d\r\n",ax,ay,az, gx, gy, gz);
    }

    float motionValues[3] = {fx,fy,fz};

    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(interrupt.getValueHandle(), (uint8_t*)motionValues, 12);
    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), (uint8_t*)accelValues, 12);
    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(gyro.getValueHandle(), (uint8_t*)gyroValues, 12);
    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(fsmState.getValueHandle(), (uint8_t*)fsmStateValues, 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 writeCharCallback(const GattWriteCallbackParams *params)
{
    printf("Nesto ima na ulazu\r\n");
    printf("Handle je: %d\r\n", params->handle);
    if(params->handle == motionThreshold.getValueHandle()) {
        uint8_t value = params->data[0];
        printf("Value is: %d\r\n", value);
        mpu6050.setMotionDetectionThreshold(value);
        uint8_t motionDetectionThreshold = mpu6050.getMotionDetectionThreshold();
        pc.printf("MPU6050 motion detection threshold is: %d \r\n", motionDetectionThreshold);

        //printf("Data received: length = %d, data = 0x",params->len);


        // for(int x=0; x < params->len; x++) {
        //     printf("%x", params->data[x]);
        //  }
        // printf("\n\r");
    }

    if(params->handle == motionDuration.getValueHandle()) {
        uint8_t value = params->data[0];
        printf("Value is: %d\r\n", value);
        mpu6050.setMotionDetectionDuration(value);
        uint8_t motionDetectionDuration = mpu6050.getMotionDetectionDuration();
        pc.printf("MPU6050 motion detection duration is: %d \r\n", motionDetectionDuration);
        //printf("Data received: length = %d, data = 0x",params->len);


        // for(int x=0; x < params->len; x++) {
        //     printf("%x", params->data[x]);
        //  }
        // printf("\n\r");
    }
    if(params->handle == zeroMotionThreshold.getValueHandle()) {
        uint8_t value = params->data[0];
        printf("Value is: %d\r\n", value);
        mpu6050.setZeroMotionDetectionThreshold(value);
        uint8_t zeroMotionDetectionThreshold = mpu6050.getZeroMotionDetectionThreshold();
        pc.printf("MPU6050 zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);

    }
    if(params->handle == zeroMotionDuration.getValueHandle()) {
        uint8_t value = params->data[0];
        printf("Value is: %d\r\n", value);
        mpu6050.setZeroMotionDetectionDuration(value);
        uint8_t zeroMotionDetectionDuration = mpu6050.getZeroMotionDetectionDuration();
        pc.printf("MPU6050 zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);

    }
}

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);
    ble.gattServer().onDataWritten(writeCharCallback);
    /* 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");
}
void mpuInterruptCallback()
{
    //test=1;
    
    if (BLE::Instance().getGapState().connected) {
        eventQueue.call(updateReadValue);
    }

}
bool XnegMD, XposMD, YnegMD, YposMD, ZnegMD, ZposMD;
int main()
{
    //mpu6050.whoAmI();
    //mpu6050.calibrate(accelBias, gyroBias);
    //mpu6050.init();

    motionLog.motX=0;
    motionLog.motY=0;
    motionLog.motZ=0;
    for(int i=0; i<=4; i++) {
        cb.push_back(motionLog);
    }


    mpu6050.calibrate(accelBias, gyroBias);
    mpu6050.initialize();
    mpu6050.setSleepEnabled(0);
    pc.baud(115200);
    mpuInterrupt.fall(mpuInterruptCallback);
    
    //mpu6050.setIntEnabled(1);
    mpu6050.setIntZeroMotionEnabled(1);
    mpu6050.setIntMotionEnabled(1);
    mpu6050.setDHPFMode(1);

    pc.printf("\n\n\n");
    uint8_t motionDetectionThreshold = mpu6050.getMotionDetectionThreshold();
    //pc.printf("MPU6050 motion detection threshold is: %d \r\n", motionDetectionThreshold);
    //pc.printf("MPU6050 set motion detection threshold to 2\r\n");
    mpu6050.setMotionDetectionThreshold(1);
    motionDetectionThreshold = mpu6050.getMotionDetectionThreshold();
    //pc.printf("MPU6050 motion detection threshold now is: %d \r\n", motionDetectionThreshold);
    pc.printf("Motion detection threshold is: %d \r\n", motionDetectionThreshold);
    uint8_t motionDetectionDuration = mpu6050.getMotionDetectionDuration();
    //pc.printf("MPU6050 motion detection duration is: %d \r\n", motionDetectionDuration);
    //pc.printf("MPU6050 set motion detection duration to 40\r\n");
    mpu6050.setMotionDetectionDuration(20);
    motionDetectionDuration = mpu6050.getMotionDetectionDuration();
    //pc.printf("MPU6050 motion detection duration now is: %d \r\n", motionDetectionDuration);
    pc.printf("Motion detection duration is: %d \r\n", motionDetectionDuration);

    //*******************************************************************************************************************************

    //pc.printf("\n\n\n");
    uint8_t zeroMotionDetectionThreshold = mpu6050.getZeroMotionDetectionThreshold();
    //pc.printf("MPU6050 zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
    //pc.printf("MPU6050 set zero motion detection threshold to 2\r\n");
    mpu6050.setZeroMotionDetectionThreshold(2);
    zeroMotionDetectionThreshold = mpu6050.getZeroMotionDetectionThreshold();
    //pc.printf("MPU6050 zero motion detection threshold now is: %d \r\n", zeroMotionDetectionThreshold);
    pc.printf("Zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
    //pc.printf("\n\n\n");
    int8_t zeroMotionDetectionDuration = mpu6050.getZeroMotionDetectionDuration();
    //pc.printf("MPU6050 zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);
    //pc.printf("MPU6050 set zero motion detection duration to 1\r\n");
    mpu6050.setZeroMotionDetectionDuration(1);
    zeroMotionDetectionDuration = mpu6050.getZeroMotionDetectionDuration();
    //pc.printf("MPU6050 zero motion detection duration now is: %d \r\n", zeroMotionDetectionDuration);
    pc.printf("Zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);

    //*******************************************************************************************************************************
    //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();
    fsm = new Fsm(ble);
    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;
}