shitty example for the mbed mpu6050 library

gistfile1.txt

/* mbed Microcontroller Library
 * Copyright (c) 2006-2013 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-drivers/mbed.h"
#include "ble/BLE.h"
#include "mpu6050/MPU6050.h"
#include <math.h>
#include "ble/services/DFUService.h"

DFUService *dfuPtr;

#define DirectionUUID 0xAA00
const char DEVICE_NAME[] = "MPU6050";
const uint16_t uuid16_list[] = {DirectionUUID};

extern uint8_t direction_data[1];
 
enum Direction {
    UP,
    DOWN,
    LEFT,
    RIGHT,
    FRONT,
    BACK,
    UNDEFINED
};

const static int16_t ACCELERATION_EXITATION_THRESHOLD = 15000;

MPU6050 mpu(MPU6050_ADDRESS_AD0_HIGH, I2C_SDA1, I2C_SCL1);

int16_t ax, ay, az;
int16_t gx, gy, gz;
 
Direction direction = UNDEFINED;

int16_t direction_if_exited(int16_t acceleration) {
    if (acceleration > ACCELERATION_EXITATION_THRESHOLD) {
        return 1;
    }
    if (acceleration < -ACCELERATION_EXITATION_THRESHOLD) {
        return -1;
    }
    return 0;
}

void updatePayload(void)
{
    // Update the count in the SERVICE_DATA field of the advertising payload
    uint8_t service_data[3];
    service_data[0] = DirectionUUID & 0xff;
    service_data[1] = DirectionUUID >> 8;
    service_data[2] = direction; // Put the button click count in the third byte
    BLE::Instance().gap().updateAdvertisingPayload(GapAdvertisingData::SERVICE_DATA, (uint8_t *)service_data, sizeof(service_data));
}

void reading()
{
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
 
    int16_t x = direction_if_exited(ax);
    int16_t y = direction_if_exited(ay);
    int16_t z = direction_if_exited(az);
 
    int16_t sum = abs(x) + abs(y) + abs(z);
    if (sum != 1) {
        return;
    }
 
    Direction new_direction;
    if (z == 1) {
        new_direction = UP;
    } else if (z == -1) {
        new_direction = DOWN;
    } else if (y == 1) {
        new_direction = LEFT;
    } else if (y == -1) {
        new_direction = RIGHT;
    } else if (x == 1) {
        new_direction = BACK;
    } else if (x == -1) {
        new_direction = FRONT;
    }
 
    if (direction == new_direction) {
        return;
    }
 
    direction = new_direction;
    minar::Scheduler::postCallback(updatePayload);
}

void update(void)
{
    minar::Scheduler::postCallback(reading);
}

/**
 * This function is called when the ble initialization process has failled
 */
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;
    }

    dfuPtr = new DFUService(ble);

    /* setup advertising */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));

    // The Service Data data type consists of a service UUID with the data associated with that service.
    // We will encode the number of button clicks in the Service Data field
    // First two bytes of SERVICE_DATA field should contain the UUID of the service
    uint8_t service_data[3];
    service_data[0] = DirectionUUID & 0xff;
    service_data[1] = DirectionUUID >> 8;
    service_data[2] = direction; // Put the button click count in the third byte
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SERVICE_DATA, (uint8_t *)service_data, sizeof(service_data));

    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(200);
    ble.gap().startAdvertising();
}

void app_start(int, char **)
{
    mpu.initialize();

    minar::Scheduler::postCallback(update).period(minar::milliseconds(1000));

    BLE &ble = BLE::Instance();
    ble.init(bleInitComplete);
}