File content as of revision 47:4905acf20758:

/* 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.h"
#include "MPU9250.h"

// Serial comms
Serial pc(USBTX, USBRX);

// Sensor board library
MPU9250 mpu = MPU9250(p26, p27);


// Configuration
bool test_comms = true;
bool do_sensor_init = false;
bool do_sensor_self_test = true;
bool print_accel = true;
bool print_gyro = true;

#ifndef __BLE_BUTTON_SERVICE_H__
#define __BLE_BUTTON_SERVICE_H__

class ButtonService {
public:
    const static uint16_t BUTTON_SERVICE_UUID              = 0xA000;
    const static uint16_t BUTTON_STATE_CHARACTERISTIC_UUID = 0xA001;

    ButtonService(BLE &_ble, bool buttonPressedInitial) :
        ble(_ble), buttonState(BUTTON_STATE_CHARACTERISTIC_UUID, (uint8_t []){0,0}, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)
    {
        GattCharacteristic *charTable[] = {&buttonState};
        GattService         buttonService(ButtonService::BUTTON_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
        ble.gattServer().addService(buttonService);
    }
    


    void updateButtonState(bool newState) {
        
            float test_result[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
    int16_t accel[3] = {0,0,0};
    int16_t gyro[3] = {0,0,0};
    int16_t temp = 0;
//        uint8_t v[12] = {0};
//        if (newState) {
//            v[0] = 0;
//            v[1] = 1;
//            v[2] = 2;
//            v[3] = 3;
//            v[4] = 4;
//            v[5] = 5;
//            v[6] = 0;
//            v[7] = 1;
//            v[8] = 2;
//            v[9] = 3;
//            v[10] = 4;
//            v[11] = 5;
//            }
//        else {
//            v[0] = 6;
//            v[1] = 7;
//            v[2] = 8;
//            v[3] = 9;
//            v[4] = 10;
//            v[5] = 11;
//            v[6] = 6;
//            v[7] = 7;
//            v[8] = 8;
//            v[9] = 9;
//            v[10] = 10;
//            v[11] = 11;
//            }
        uint8_t v[6] = {0};
         if (print_accel) {
            mpu.readAccelData(accel);
            float ax = accel[0] * 2.0 / 32768.0; v[0] = ax;
            float ay = accel[1] * 2.0 / 32768.0; v[1] = ay;
            float az = accel[2] * 2.0 / 32768.0; v[2] = az;
            pc.printf("accel: (%f, %f, %f)\n", ax,ay,az);
        }
        
        if (print_gyro) {
            mpu.readGyroData(gyro);
            float gx = gyro[0] * 250.0 / 32768.0; v[3] = gx;
            float gy = gyro[1] * 250.0 / 32768.0; v[4] = gy;
            float gz = gyro[2] * 250.0 / 32768.0; v[5] = gz;
            pc.printf("gyro: (%f, %f, %f)\n", gx,gy,gz);
        }
        //ble.gattServer().write(buttonState.getValueHandle(), (uint8_t *)v, sizeof(v));
        ble.updateCharacteristicValue(buttonState.getValueHandle(), (uint8_t *)v ,sizeof(v));
    }

private:
    BLE                              &ble;
    ReadOnlyArrayGattCharacteristic<uint8_t, sizeof(uint8_t[6])>  buttonState;
};

#endif /* #ifndef __BLE_BUTTON_SERVICE_H__ */