File content as of revision 5:ab49c12aab25:

// https://github.com/jrowberg/i2cdevlib/issues/15

#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 "DFUService.h"
#include "UARTService.h"

/* DESCOMENTAR PARA VER SALIDA DE DATOS POR PUERTO SERIE */
//#define SERIAL_DEBUG
/* DESCOMENTAR PARA VER LA TRAMA DE DATOS DE BLE POR PUERTO SERIE */
//#define BLE_DEBUG
/* DESCOMENTAR SI SE QUIERE EJECUTAR LA CALIBRACION INICIAL */
#define CALIBRATE

/* DESCOMENTAR SI SE QUIERE ENVIAR TAMBIEN EL ACELEROMETRO A LA FIFO */
//#define SEND_ACCEL

/* CUSTOM FULL-SCALE RANGE */
#define ACCEL_CUSTOM_FSR  (4)
#define GYRO_CUSTOM_FSR  (1000)
/* Starting sampling rate. */
#define DEFAULT_MPU_HZ  (80)

#define LOG(...)    { pc.printf(__VA_ARGS__); }

#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

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

InterruptIn button(BUTTON_PIN);
AnalogIn    battery(BATTERY_PIN);
Serial pc(UART_TX, UART_RX);

InterruptIn motion_probe(p14);      // Interrupcion recibida desde el MPU (Pueder ser por TAP o por FIFO overflow)

int read_none_count = 0;
bool cal_flag = 0;                  // Calibration done flag

BLEDevice  ble;
UARTService *uartServicePtr;

volatile bool bleIsConnected = false;
volatile uint8_t tick_event = 0;
volatile uint8_t motion_event = 0;
static signed char board_orientation[9] = {
    1, 0, 0,
    0, 1, 0,
    0, 0, 1
};

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


void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    LOG("Connected!\r\n");
    bleIsConnected = true;
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
{
    LOG("Disconnected!\r\n");
    LOG("Restarting the advertising process\r\n");
    ble.startAdvertising();
    bleIsConnected = false;
}

void tick(void)
{
    //static uint32_t count = 0;
    if (cal_flag == 0){
        red = !red;
    }
    if (cal_flag == 1){
        green = !green;
    }
    //LOG("%d, ", count++);         // Contador en pantalla
}

void detect(void)                   // Flag de detección de interrupcion por boton
{
    LOG("\r\nButton pressed\r\n");
    blue = !blue;
}

void motion_interrupt_handle(void)  // Flag de detección de interrupcion por movimiento
{
    motion_event = 1;
}

void tap_cb(unsigned char direction, unsigned char count)
{
    LOG("Tap motion detected\r\n");
}

void android_orient_cb(unsigned char orientation)
{
    LOG("Oriention changed\r\n");
}

////////////////////////////////////////////////////////////////////////////////
//  INICIALIZACIONES (MAIN)
////////////////////////////////////////////////////////////////////////////////
int main(void)
{
    blue  = 1;
    green = 1;
    red   = 1;

    ////////////////////////////////////////////////////////////////////////////////
    //  INICIALIZACION SERIAL PORT
    ////////////////////////////////////////////////////////////////////////////////
    pc.baud(115200);
    wait(1);

    LOG("---- Seeed Tiny BLE ----\r\n");
    ////////////////////////////////////////////////////////////////////////////////
    //  INICIALIZACION MPU I2C
    ////////////////////////////////////////////////////////////////////////////////
    mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
    mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);

    if (mpu_init(0)) {
        LOG("failed to initialize mpu6050\r\n");
    }

    /* Get/set hardware configuration. Start gyro. */
    //#ifdef SEND_ACCEL
        /* 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);
        /* Como el DMP esta activo, se utiliza el sample rate por defecto de 200Hz (ver pag.8/12 eMD 5.1.1 - Tutorial.pdf) */
    //#else
        /* Wake up only gyro */
    //    mpu_set_sensors(INV_XYZ_GYRO);
        /* Push only gyro data into the FIFO. */
    //    mpu_configure_fifo(INV_XYZ_GYRO);
        /* Como el DMP esta activo, se utiliza el sample rate por defecto de 200Hz (ver pag.8/12 eMD 5.1.1 - Tutorial.pdf) */
    //#endif

    mpu_set_sample_rate(DEFAULT_MPU_HZ);

    /* AFS_SEL | Full Scale Range | LSB Sensitivity
     * --------+------------------+----------------
     * 0       | +/- 2g           | 8192 LSB/mg
     * 1       | +/- 4g           | 4096 LSB/mg
     * 2       | +/- 8g           | 2048 LSB/mg
     * 3       | +/- 16g          | 1024 LSB/mg */
    mpu_set_accel_fsr(ACCEL_CUSTOM_FSR);      // Seteo el custom full-scale range del acelerometro

    /* FS_SEL | Full Scale Range   | LSB Sensitivity
     * -------+--------------------+----------------
     * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
     * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
     * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
     * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s */
    mpu_set_gyro_fsr(GYRO_CUSTOM_FSR);      // Seteo el custom full-scale range del giroscopo

    /* Read back configuration in case it was set improperly. */
    //#ifdef SEND_ACCEL
        unsigned char accel_fsr;
        mpu_get_accel_fsr(&accel_fsr);      // Get the accel full-scale range
    //#endif
    unsigned short gyro_rate, gyro_fsr;
    mpu_get_sample_rate(&gyro_rate);    // Current sampling rate (Hz)
    mpu_get_gyro_fsr(&gyro_fsr);        // Get the gyro full-scale range

    wait(1);
    //LOG("Gyro FSR: %u\r\n", gyro_fsr); // Print Gyro FSR
    //LOG("Accel FSR: %u\r\n", accel_fsr); // Print Gyro FSR

    dmp_load_motion_driver_firmware();  //Load the DMP with the fw image
    dmp_set_orientation(
        inv_orientation_matrix_to_scalar(board_orientation));   // Push gyro and accel orientation to the DMP

    // Configure which DMP features will be available
    uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;   // ORIGINAL
   // #ifdef SEND_ACCEL
    //    uint16_t dmp_features = DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
    //#else
    //    uint16_t dmp_features = DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
    //#endif
    dmp_enable_feature(dmp_features);

    dmp_set_fifo_rate(DEFAULT_MPU_HZ);  // Set DMP output rate in (Hz)
    mpu_set_dmp_state(1);               // Enable/disable DMP support (1=EN)

    dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);   // Specify when a DMP interrupt should occur. One FIFO period elapsed or gesture detected)
                                                  // DMP = EN => Default sample rate 200Hz

    motion_probe.fall(motion_interrupt_handle);   // Asigno interrupcion al pin14 proveniente del MPU

    Ticker ticker;
    ticker.attach(tick, 1);

    button.fall(detect);                          // Asigno una interrupción al botón durante el fall

    ////////////////////////////////////////////////////////////////////////////////
    //  INICIALIZACION BLE
    ////////////////////////////////////////////////////////////////////////////////
    LOG("Initialising the nRF51822\r\n");
    ble.init();
    ble.gap().onDisconnection(disconnectionCallback);
    ble.gap().onConnection(connectionCallback);

    /* setup advertising */
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
                                     (const uint8_t *)"TinyBLE", sizeof("TinyBLE"));
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
                                     (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
    DFUService dfu(ble);
    UARTService uartService(ble);
    uartServicePtr = &uartService;
    //uartService.retargetStdout();

    ////////////////////////////////////////////////////////////////////////////////
    //  CALIBRACION
    ////////////////////////////////////////////////////////////////////////////////
    #ifdef CALIBRATE
        unsigned long sensor_timestamp_cal = 0;
        short gyro_cal[3], accel_cal[3], sensors_cal;
        long quat_cal[4];
        unsigned char more_cal = 1;
        red = 0;        // Prendo led rojo para avisar que esta calibrando
        LOG("Waiting for auto-calibration, please do not move the sensor....\r\n");
        while (sensor_timestamp_cal < 20000){
            dmp_read_fifo(gyro_cal, accel_cal, quat_cal, &sensor_timestamp_cal, &sensors_cal,   // Get one packet from the FIFO.
                  &more_cal);
        }
        cal_flag = 1; red = 1;        // Apago led Rojo.
        LOG("Calibration complete!");
    #endif
    ticker.attach(tick, 2);     // Asigno 2 segundos a la frecuencia de on/off del led
    
    // Inicio servicio Advertising BLE
    ble.setAdvertisingInterval(8); /* 5ms; in multiples of 0.625ms. */
    ble.gap().startAdvertising();
    char BLE_MPU_DATA[80];   // Vector para envio de datos del MPU via BTLE
    char gyro_val[30], accel_val[20], timestamp_val[20];

    char r[4];
    r[1]='\r';
    r[2]='\n';
    r[3]='\0';

    ////////////////////////////////////////////////////////////////////////////////
    //  MAIN LOOP
    ////////////////////////////////////////////////////////////////////////////////
    #ifdef SERIAL_DEBUG
        #ifdef SEND_ACCEL
            LOG("\r\n\nTimeStamp(ms); AX (g); AY (g); AZ (g); GX (dps); GY (dps); GZ (dps)");
        #else
            LOG("\r\n\nTimeStamp(ms); GX (dps); GY (dps); GZ (dps)");
        #endif
    #endif

    while (true) {
        if (motion_event) {       // Si detecta fifo overflow, tengo una interrupcion
            unsigned long sensor_timestamp = 0;
            short gyro[3], accel[3], sensors;
            long quat[4];
            unsigned char more = 1;

            while (more) {
                /* This function gets new data from the FIFO when the DMP is in
                 * use. The FIFO can contain any combination of gyro, accel,
                 * quaternion, and gesture data. The sensors parameter tells the
                 * caller which data fields were actually populated with new data.
                 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
                 * the FIFO isn't being filled with accel data.
                 * The driver parses the gesture data to determine if a gesture
                 * event has occurred; on an event, the application will be notified
                 * via a callback (assuming that a callback function was properly
                 * registered). The more parameter is non-zero if there are
                 * leftover packets in the FIFO.
                 * Sensor Timestamp is in milliseconds
                 */
                dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,   // Get one packet from the FIFO.
                              &more);
                
                sprintf((char *)timestamp_val, "%lu;", sensor_timestamp);

                #ifdef SERIAL_DEBUG
                    LOG("\r\n%lu;", sensor_timestamp);
                #endif
                /* Gyro and accel data are written to the FIFO by the DMP in chip
                 * frame and hardware units. This behavior is convenient because it
                 * keeps the gyro and accel outputs of dmp_read_fifo and
                 * mpu_read_fifo consistent.
                 */
                #ifdef SEND_ACCEL
                    if (sensors & INV_XYZ_ACCEL) {  // Accel data in hardware units.
                        //LOG("ACC: %d, %d, %d\r\n", accel[0], accel[1], accel[2]);     // ORIGINAL
                        //LOG("%d; %d; %d; ", accel[0], accel[1], accel[2]);            // NUEVO EN HARDWARE UNITS
                        if (accel_fsr == 2){
                            sprintf((char *)accel_val, "%0.2f;%0.2f;%0.2f;", (float)accel[0]/8192, (float)accel[1]/8192, (float)accel[2]/8192);
                            #ifdef SERIAL_DEBUG
                                LOG(" %0.2f; %0.2f; %0.2f;", accel[0]/8192, accel[1]/8192, accel[2]/8192); // NUEVO EN G (escala +/- 2G)
                            #endif
                        } else if (accel_fsr == 4){
                            sprintf((char *)accel_val, "%0.2f;%0.2f;%0.2f;", (float)accel[0]/4096, (float)accel[1]/4096, (float)accel[2]/4096);
                            #ifdef SERIAL_DEBUG
                                LOG(" %0.2f; %0.2f; %0.2f;", accel[0]/4096, accel[1]/4096, accel[2]/4096); // NUEVO EN G (escala +/- 4G)
                            #endif
                        } else if (accel_fsr == 8){
                            sprintf((char *)accel_val, "%0.2f;%0.2f;%0.2f;", (float)accel[0]/2048, (float)accel[1]/2048, (float)accel[2]/2048);
                            #ifdef SERIAL_DEBUG
                                LOG(" %0.2f; %0.2f; %0.2f;", accel[0]/2048, accel[1]/2048, accel[2]/2048); // NUEVO EN G (escala +/- 8G)
                            #endif
                        } else if (accel_fsr == 16){
                            sprintf((char *)accel_val, "%0.2f;%0.2f;%0.2f;", (float)accel[0]/1024, (float)accel[1]/1024, (float)accel[2]/1024);
                            #ifdef SERIAL_DEBUG
                                LOG(" %0.2f; %0.2f; %0.2f;", accel[0]/1024, accel[1]/1024, accel[2]/1024); // NUEVO EN G (escala +/- 16G)
                            #endif
                        }
                    }
                #endif
                if (sensors & INV_XYZ_GYRO) {   // Gyro data in hardware units
                    //LOG("GYRO: %d, %d, %d\r\n", gyro[0], gyro[1], gyro[2]);       // ORIGINAL
                    //LOG("%d; %d; %d\r\n", gyro[0], gyro[1], gyro[2]);               // NUEVO EN HARDWARE UNITS
                    if (gyro_fsr == 250){
                        sprintf((char *)gyro_val, "%0.2f;%0.2f;%0.2f;", (float)gyro[0]/131, (float)gyro[1]/131, (float)gyro[2]/131);
                        #ifdef SERIAL_DEBUG
                            LOG(" %0.2f; %0.2f; %0.2f", gyro[0]/131, gyro[1]/131, gyro[2]/131);    // NUEVO EN DPS (escala +/- 250dps)
                        #endif
                    } else if (gyro_fsr == 500){
                        sprintf((char *)gyro_val, "%0.2f;%0.2f;%0.2f;", (float)gyro[0]/65.5, (float)gyro[1]/65.5, (float)gyro[2]/65.5);
                        #ifdef SERIAL_DEBUG
                            LOG(" %0.2f; %0.2f; %0.2f", gyro[0]/65.5, gyro[1]/65.5, gyro[2]/65.5);    // NUEVO EN DPS (escala +/- 500dps)
                        #endif
                    } else if (gyro_fsr == 1000){
                        sprintf((char *)gyro_val, "%0.2f;%0.2f;%0.2f;", (float)gyro[0]/32.8, (float)gyro[1]/32.8, (float)gyro[2]/32.8);
                        #ifdef SERIAL_DEBUG
                            LOG(" %0.2f; %0.2f; %0.2f", gyro[0]/32.8, gyro[1]/32.8, gyro[2]/32.8);    // NUEVO EN DPS (escala +/- 1000dps)
                        #endif
                    } else if (gyro_fsr == 2000){
                        sprintf((char *)gyro_val, "%0.2f;%0.2f;%0.2f;", (float)gyro[0]/16.4, (float)gyro[1]/16.4, (float)gyro[2]/16.4);
                        #ifdef SERIAL_DEBUG
                            LOG(" %0.2f; %0.2f; %0.2f", gyro[0]/16.4, gyro[1]/16.4, gyro[2]/16.4);    // NUEVO EN DPS (escala +/- 2000dps)
                        #endif
                    }
                }
                
                #ifdef SEND_ACCEL
                    sprintf((char *)BLE_MPU_DATA, "%s%s%s\r\n\0", timestamp_val, accel_val, gyro_val);
                #else
                    sprintf((char *)BLE_MPU_DATA, "%s%s\r\n\0", timestamp_val, gyro_val);
                #endif

                #ifdef BLE_DEBUG
                    LOG("%s",BLE_MPU_DATA);
                #endif
                uartService.writeString(BLE_MPU_DATA);       // ENVIO DATOS VIA BLE

                if (sensors) {
                    read_none_count = 0;
                } else {
                    read_none_count++;
                    if (read_none_count > 3) {
                        read_none_count = 0;

                        LOG("I2C may be stuck @ %d\r\n", sensor_timestamp);
                        mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
                    }
                }
            }
           motion_event = 0;
        } else {
            // ESPERO UNA LETRA PARA CAMBIAR EL COLOR DEL LED
            ble.waitForEvent();
            int c;
            r[0]=c=uartService._getc();
            if (c<=0) continue;
            if (c=='R' || c=='r') {  red=0; green=1; blue=1; }
            else if (c=='G' || c=='g') {  red=1; green=0; blue=1; }
            else if (c=='B' || c=='b') {  red=1; green=1; blue=0; }
            else  r[0]='?';
            uartService.writeString(r);     // Devuelve la misma letra leida por BLE
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
//      FUNCIONES
////////////////////////////////////////////////////////////////////////////////
/* 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)
{
    unsigned short scalar;

    /*
       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
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}