3-axis accelerometer and gyroscope with BLE

File content as of revision 1:95ebaf1441e7:

#include "mbed.h"
#include "ble/BLE.h"

/* UART printf */
Serial pc(p5, p4);

/* LED blink */
Ticker ledBlinkTicker;
DigitalOut ledR(p16, 1);
DigitalOut ledG(p15, 1);
DigitalOut ledB(p6 , 1);

/* I2C */
#define I2C_BMA250_ADDR (0x30)
#define I2C_BMG160_ADDR (0xd0)
I2C i2c(p14, p13);
char wdata[8];
char rdata[8];

/* Sensor */
Ticker updateSensorTicker;
volatile bool  updateSensorTrigger = false;
uint8_t acceRange = 0x0C;   // 2G(0x03), 4G(0x05), 8G(0x08), 16G(0x0C) 
uint8_t gyroRange = 0x00;   // 2000deg/s(0x00), 1000deg/s(0x01), 500deg/s(0x02), 250deg/s(0x03), 125deg/s(0x04)
int16_t acceXYZ[3];
int16_t gyroXYZ[3];
uint8_t accePayload[7];
uint8_t gyroPayload[7];

/* UUID, Device name */
uint16_t sensServUUID = /*0xA000*/0x1811;
uint16_t acceCharUUID = /*0xA001*/0x2A56;
uint16_t gyroCharUUID = /*0xA002*/0x2A57;
static const char     DEVICE_NAME[] = "mbed Motion";
static const uint16_t uuid16_list[] = { /*0xA000*/0x1811 };

/* Setup custom characteristics */
GattCharacteristic  acceChar(   acceCharUUID, accePayload,
                                sizeof(accePayload), sizeof(accePayload),
                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY | 
                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);

GattCharacteristic  gyroChar(   gyroCharUUID, gyroPayload,
                                sizeof(gyroPayload), sizeof(gyroPayload),
                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY | 
                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);

/* Setup custom service */
GattCharacteristic *characteristics[] = {&acceChar, &gyroChar};
GattService sensServ(sensServUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));



void ledBlinkCallback(void)
{
    ledR = !ledR;
    ledG = !ledG;
    ledB = !ledB;
}

void updateSensorCallback(void)
{
    updateSensorTrigger = true;
}

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

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

    if (error != BLE_ERROR_NONE) {
        return;
    }

    ble.gap().onDisconnection(disconnectionCallback);

    /* Setup primary service. */
    ble.addService(sensServ);

    /* 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_TAG);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(500); /* 500ms */
    ble.gap().startAdvertising();
}

void i2cConfigBMA250()
{
    int status;

    wdata[0] = 0x14;    // SOFTRESET_REG
    wdata[1] = 0xB6;    // Reset value
    status = i2c.write(I2C_BMA250_ADDR, wdata, 2, 0);
    if (status != 0) goto FAIL;
    
    wdata[0] = 0x0F;    // RANGE_REG
    wdata[1] = 0x0C;    // (+/-)16G
    status = i2c.write(I2C_BMA250_ADDR, wdata, 2, 0);
    if (status != 0) goto FAIL;
    
    wdata[0] = 0x10;    // BANDWIDTH_REG
    wdata[1] = 0x0D;    // 250Hz
    status = i2c.write(I2C_BMA250_ADDR, wdata, 2, 0);
    if (status != 0) goto FAIL;
    
    return;
FAIL:
    printf("Error !!\n");
    while(1);
}

void i2cConfigBMG160()
{
    int status;

    wdata[0] = 0x10;    // BANDWIDTH_REG
    wdata[1] = 0x00;    // Unfiltered
    status = i2c.write(I2C_BMG160_ADDR, wdata, 2, 0);
    if (status != 0) goto FAIL;
    
    wdata[0] = 0x0F;    // RANGE_REG
    wdata[1] = 0x00;    // (+/-)2000deg/s
    status = i2c.write(I2C_BMG160_ADDR, wdata, 2, 0);
    if (status != 0) goto FAIL;
    
    wdata[0] = 0x1A;    // 
    wdata[1] = 0x20;    // slow_offset_unfilt = 1
    status = i2c.write(I2C_BMG160_ADDR, wdata, 2, 0);
    if (status != 0) goto FAIL;
    
    wdata[0] = 0x31;    // SOC_REG
    wdata[1] = 0x07;    // slow_offset_en_x/y/z = 1
    status = i2c.write(I2C_BMG160_ADDR, wdata, 2, 0);
    if (status != 0) goto FAIL;
    
    return;
FAIL:
    printf("Error !!\n");
    while(1);
}

void i2cMeasureBMA250(int16_t &ax, int16_t &ay, int16_t &az)
{
    int status;

    wdata[0] = 0x02;
    status = i2c.write(I2C_BMA250_ADDR, wdata, 1, 0);
    if (status != 0) goto FAIL;
    status = i2c.read(I2C_BMA250_ADDR, rdata, 6, 0);
    if (status != 0) goto FAIL;
//  printf("aXYZ_raw = 0x%02X%02X,0x%02X%02X,0x%02X%02X\n", 
//      rdata[1], rdata[0], rdata[3], rdata[2], rdata[5], rdata[4]);

    ax = ((int16_t)rdata[1] << 8) | (rdata[0] & 0xC0);
    ay = ((int16_t)rdata[3] << 8) | (rdata[2] & 0xC0);
    az = ((int16_t)rdata[5] << 8) | (rdata[4] & 0xC0);

    /* Align right */
    ax >>= 6;
    ay >>= 6;
    az >>= 6;

    return;
FAIL:
    printf("Error !!\n");
    while(1);
}

void i2cMeasureBMG160(int16_t &gx, int16_t &gy, int16_t &gz)
{
    int status;

    wdata[0] = 0x02;
    status = i2c.write(I2C_BMG160_ADDR, wdata, 1, 0);
    if (status != 0) goto FAIL;
    status = i2c.read(I2C_BMG160_ADDR, rdata, 6, 0);
    if (status != 0) goto FAIL;
//  printf("gXYZ_raw = 0x%02X%02X,0x%02X%02X,0x%02X%02X\n", 
//      rdata[1], rdata[0], rdata[3], rdata[2], rdata[5], rdata[4]);

    gx = ((int16_t)rdata[1] << 8) | rdata[0];
    gy = ((int16_t)rdata[3] << 8) | rdata[2];
    gz = ((int16_t)rdata[5] << 8) | rdata[4];

    return;
FAIL:
    printf("Error !!\n");
    while(1);
}

int main(void)
{
    printf("~ Hell World ~\n");

    /* LED blink ticker */
    ledBlinkTicker.attach(&ledBlinkCallback, 1);

    /* Update Sensor ticker */
    updateSensorTicker.attach(&updateSensorCallback, 0.1);

    /* Init BLE */
    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
    ble.init(bleInitComplete);
    while (ble.hasInitialized()  == false) { /* spin loop */ }

    /* Config sensor */
    i2cConfigBMA250();
    i2cConfigBMG160();

    /* Main loop */
    while (1) {
        if (updateSensorTrigger && ble.getGapState().connected) {
            updateSensorTrigger = false;
            
            /* Get sensor data */
            i2cMeasureBMA250(acceXYZ[0], acceXYZ[1], acceXYZ[2]);
            i2cMeasureBMG160(gyroXYZ[0], gyroXYZ[1], gyroXYZ[2]);
//          printf("aXYZ(%6d,%6d,%6d), gXYZ(%6d,%6d,%6d)\n", acceXYZ[0], acceXYZ[1], acceXYZ[2], gyroXYZ[0], gyroXYZ[1], gyroXYZ[2]);

            /* Write data to client */
            accePayload[0] = acceRange;
            accePayload[1] = (uint8_t)(acceXYZ[0] >> 8);
            accePayload[2] = (uint8_t)(acceXYZ[0] >> 0);
            accePayload[3] = (uint8_t)(acceXYZ[1] >> 8);
            accePayload[4] = (uint8_t)(acceXYZ[1] >> 0);
            accePayload[5] = (uint8_t)(acceXYZ[2] >> 8);
            accePayload[6] = (uint8_t)(acceXYZ[2] >> 0);
            ble.gattServer().write(acceChar.getValueHandle(), accePayload, sizeof(accePayload));
            gyroPayload[0] = gyroRange;
            gyroPayload[1] = (uint8_t)(gyroXYZ[0] >> 8);
            gyroPayload[2] = (uint8_t)(gyroXYZ[0] >> 0);
            gyroPayload[3] = (uint8_t)(gyroXYZ[1] >> 8);
            gyroPayload[4] = (uint8_t)(gyroXYZ[1] >> 0);
            gyroPayload[5] = (uint8_t)(gyroXYZ[2] >> 8);
            gyroPayload[6] = (uint8_t)(gyroXYZ[2] >> 0);
            ble.gattServer().write(gyroChar.getValueHandle(), gyroPayload, sizeof(gyroPayload));
        } else {
            /* low power wait for event */
            ble.waitForEvent();
        }
    }
}