File content as of revision 4:dedfd6797c10:

#include "mbed.h"
#include "mbed_i2c.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "nrf51.h"
#include "nrf51_bitfields.h"

#include "BLE.h"
#include "UARTService.h"

//#define CALIBRATE
#define SERIAL_DEBUG
#ifdef SERIAL_DEBUG
#define LOG(...)    { pc.printf(__VA_ARGS__); pc.printf("\r\n"); }
#endif

#define LED_GREEN   p21
#define LED_RED     p22
#define LED_BLUE    p23
#define BUTTON_PIN  p17
#define BATTERY_PIN p1

#define MPU6050_SDA p12
#define MPU6050_SCL p13

#define UART_TX     p9
#define UART_RX     p11
#define UART_CTS    p8
#define UART_RTS    p10

/* Starting sampling rate. */
#define DEFAULT_MPU_HZ     (100)
#define DEFAULT_GYRO_FST   (2000) // 250, 500, 1000, 2000
#define DEFAULT_ACCEL_FST  (8)    // 2, 4, 8, 16


DigitalOut blue(LED_BLUE);
DigitalOut green(LED_GREEN);
DigitalOut red(LED_RED);

InterruptIn button(BUTTON_PIN);

#ifdef SERIAL_DEBUG
Serial pc(UART_TX, UART_RX);
#endif

InterruptIn motion_probe(p14);

int read_none_count = 0;
static const char *devName = "SKQP-B01";
UARTService *uartService = 0;
volatile bool bleIsConnected = false;
volatile bool updatesEnabled = false;
volatile uint8_t motion_event = 0;
static signed char board_orientation[9] = {
    1, 0, 0,
    0, 1, 0,
    0, 0, 1
};

void check_i2c_bus(void);
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);

void LedSwitchON(DigitalOut & led)
{
    if( led != 0 )
        led = 0;
}

void LedSwitchOFF(DigitalOut & led)
{
    if( led != 1 )
        led = 1;
}


void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
#ifdef SERIAL_DEBUG
    LOG("Connected!");
#endif
    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
    Gap::ConnectionParams_t connectionParams;
    ble.gap().getPreferredConnectionParams(&connectionParams);
    connectionParams.minConnectionInterval =  6;
    connectionParams.maxConnectionInterval = 10;
    if (ble.gap().updateConnectionParams(params->handle, &connectionParams) == BLE_ERROR_NONE) {
#ifdef SERIAL_DEBUG
        LOG("ConnectionParams Update Accepted!");
#endif
    }
    bleIsConnected = true;
    blue  = 0;
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
{
#ifdef SERIAL_DEBUG
    LOG("Disconnected: %d!",cbParams->reason);
    LOG("Restarting the advertising process");
#endif
    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
    ble.gap().startAdvertising();
    bleIsConnected = false;
    updatesEnabled = false;
    blue  = 1;
}

void  onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
{
    if ((uartService != NULL) && (handle == uartService->getRXCharacteristicHandle())) {
        updatesEnabled = true;
//        LOG("onUpdatesEnabledCallback");
    }
}

void  onUpdatesDisabledCallback(GattAttribute::Handle_t handle)
{
    if ((uartService != NULL) && (handle == uartService->getRXCharacteristicHandle())) {
        updatesEnabled = false;
//        LOG("onUpdatesDisabledCallback");
    }
}

void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE &ble          = params->ble;
    ble_error_t error = params->error;
    if (error != BLE_ERROR_NONE) {
        return;
    }
    uint8_t pass[6] = {'1', '2', '3', '4', '5', '6'};
    ble.securityManager().init(true,true,SecurityManager::IO_CAPS_DISPLAY_ONLY,pass);
    ble.gap().onDisconnection(disconnectionCallback);
    ble.gap().onConnection(connectionCallback);
    ble.gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
    ble.gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
    //
    uartService = new UARTService(ble);
    /* setup advertising */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setDeviceName((const uint8_t*)devName);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,(const uint8_t *)devName, sizeof(devName));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME,(const uint8_t *)devName, sizeof(devName));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,(const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
    ble.gap().setAdvertisingInterval(8); /* 5ms; in multiples of 0.625ms. */
    ble.gap().startAdvertising();
#ifdef SERIAL_DEBUG
//    LOG("bleInitComplete %s!",ble.getVersion());
#endif
}

void tick(void)
{
    if( bleIsConnected ) {
        LedSwitchOFF(green);
        if( updatesEnabled ) {
            LedSwitchOFF(red);
            blue = !blue;
            return;

        }
        LedSwitchOFF(blue);
        red = !red;
        return;
    }
    LedSwitchOFF(blue);
    LedSwitchOFF(red);
    green = !green;
}

void ButtonReset(void)
{
    NVIC_SystemReset();
}

void motion_interrupt_handle(void)
{
    motion_event = 1;
}

int main(void)
{
    blue  = 1;
    green = 1;
    red   = 1;
#ifdef SERIAL_DEBUG
    pc.baud(115200);
    wait(1);
    LOG("---- Seeed Tiny BLE ----");
#endif
    mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
    mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
    if (mpu_init(0)) {
#ifdef SERIAL_DEBUG
        LOG("failed to initialize mpu6050");
#endif
    }
    /* Get/set hardware configuration. Start gyro. */
    /* Wake up all sensors. */
    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    /* Push both gyro and accel data into the FIFO. */
    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    mpu_set_sample_rate(4*DEFAULT_MPU_HZ);
    mpu_set_gyro_fsr(DEFAULT_GYRO_FST);
    mpu_set_accel_fsr(DEFAULT_ACCEL_FST);
    /* Read back configuration in case it was set improperly. */
    /* unsigned char accel_fsr;
    unsigned short gyro_rate, gyro_fsr;
    mpu_get_sample_rate(&gyro_rate);
    mpu_get_gyro_fsr(&gyro_fsr);
    mpu_get_accel_fsr(&accel_fsr); */
    dmp_load_motion_driver_firmware();
    dmp_set_orientation(inv_orientation_matrix_to_scalar(board_orientation));
    dmp_enable_feature(DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_LP_QUAT);
    dmp_set_fifo_rate(DEFAULT_MPU_HZ);
    mpu_set_dmp_state(1);
    dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);
    motion_probe.fall(motion_interrupt_handle);
    button.fall(ButtonReset);
    //
    unsigned long sensor_timestamp = 0;
    short gyro[3], accel[3], sensors;
    long quat[4];
#ifdef CALIBRATE
    LedSwitchON(red);
    while (sensor_timestamp < 20000) {
        unsigned char more = 1;
        dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
    }
    LedSwitchOFF(red);
#endif
    Ticker ticker;
    ticker.attach(tick, 1);
//
#ifdef SERIAL_DEBUG
    LOG("Initialising the nRF51822");
#endif
    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
    ble.init(bleInitComplete);
    while(ble.hasInitialized() == false);     // wait for init
    //
    unsigned char dataBuffer[20];
    memset(dataBuffer,0,sizeof(dataBuffer));
    dataBuffer[16] = 'E';
    dataBuffer[17] = 'N';
    dataBuffer[18] = 'D';
    dataBuffer[19] =  0;
    while (true) {
        ble.waitForEvent();
        if ( !motion_event || !updatesEnabled )
            continue;
        unsigned char more = 1;
        while (more) {
            ble.waitForEvent();
            if( dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more) == 0 ) {
                if ((sensors & INV_XYZ_GYRO) && (sensors & INV_XYZ_ACCEL)) {
                    memcpy(dataBuffer,&sensor_timestamp,4);
                    memcpy(dataBuffer+4,accel,6);
                    memcpy(dataBuffer+10,gyro,6);
                    uartService->write(dataBuffer,20);
#ifdef SERIAL_DEBUG
//                    LOG("%d: %d, %d, %d, %d, %d, %d", sensor_timestamp, accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
#endif
                }
            }
            if (sensors) {
                read_none_count = 0;
                continue;
            }
            read_none_count++;
            if (read_none_count > 2) {
                read_none_count = 0;
#ifdef SERIAL_DEBUG
                LOG("I2C may be stuck @ %d", sensor_timestamp);
#endif
                ble.waitForEvent();
                mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
                ble.waitForEvent();
                break;
            }
        }
        motion_event = 0;
    }
}

/* These next two functions converts the orientation matrix (see
 * gyro_orientation) to a scalar representation for use by the DMP.
 * NOTE: These functions are borrowed from Invensense's MPL.
 */
static inline unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}

unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */
    unsigned short scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;
    return scalar;
}