File content as of revision 4:19a0764d6b81:

#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"

#include "W25Q16BV.h"
// flash
DigitalOut vccFlash(p30, 1);
#define PIN_SPI_MOSI p3
#define PIN_SPI_MISO p5
#define PIN_SPI_SCLK p4
#define PIN_CS_FLASH p6
W25Q16BV flash(PIN_SPI_MOSI, PIN_SPI_MISO, PIN_SPI_SCLK, PIN_CS_FLASH); // mosi,miso,clk,cs
int offsetAddr = 0;
bool saveBufferFull = false;

#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

/* Starting sampling rate. */
#define DEFAULT_MPU_HZ  (200)

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);

int read_none_count = 0;

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
};


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!\n");
    bleIsConnected = true;
}

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

//void tick(void)
//{
//    static uint32_t count = 0;
//    
//    LOG("%d\r\n", count++);
//    green = !green;
//}

void detect(void)
{
    LOG("Button pressed\n");  
    blue = !blue;
}

void motion_interrupt_handle(void)
{
    motion_event = 1;
}

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

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


void saveMPUDataToFlash(int addr, unsigned long timestamp, short accel[3], short gyro[3], long quat[4])
{
    uint8_t mpuRawData[32] = {0,};
    mpuRawData[0] = timestamp; 
    mpuRawData[1] = timestamp>>8;
    mpuRawData[2] = timestamp>>16;
    mpuRawData[3] = timestamp>>24;
    mpuRawData[4] = accel[0]; 
    mpuRawData[5] = accel[0]>>8;
    mpuRawData[6] = accel[1];
    mpuRawData[7] = accel[1]>>8;
    mpuRawData[8] = accel[2]; 
    mpuRawData[9] = accel[2]>>8;
    mpuRawData[10] = gyro[0];
    mpuRawData[11] = gyro[0]>>8;
    mpuRawData[12] = gyro[1]; 
    mpuRawData[13] = gyro[1]>>8;
    mpuRawData[14] = gyro[2];
    mpuRawData[15] = gyro[2]>>8;
    mpuRawData[16] = quat[0]; 
    mpuRawData[17] = quat[0]>>8;
    mpuRawData[18] = quat[0]>>16;
    mpuRawData[19] = quat[0]>>24;
    mpuRawData[20] = quat[1]; 
    mpuRawData[21] = quat[1]>>8;
    mpuRawData[22] = quat[1]>>16;
    mpuRawData[23] = quat[1]>>24;
    mpuRawData[24] = quat[2]; 
    mpuRawData[25] = quat[2]>>8;
    mpuRawData[26] = quat[2]>>16;
    mpuRawData[27] = quat[2]>>24;
    mpuRawData[28] = quat[3]; 
    mpuRawData[29] = quat[3]>>8;
    mpuRawData[30] = quat[3]>>16;
    mpuRawData[31] = quat[3]>>24;
    flash.writeStream(addr, (char *)mpuRawData, 32);
    pc.printf("%02x %02x %02x %02x\r\n", mpuRawData[0], mpuRawData[1], mpuRawData[2], mpuRawData[3]);
}

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

    pc.baud(115200);
    
    pc.printf("SPI init done\n");
    flash.chipErase();
    pc.printf("Flash Erase done\n");
    uint8_t buff[128];
    pc.printf("write after erase\r\n");
    uint8_t datas[100] = {0,};
    for (uint8_t i=0; i<100;i++) {
        datas[i] = i+0x01;
    }
    long dfa = 32524;
    datas[0] = dfa;
    datas[1] = dfa >> 8;
    datas[2] = dfa >> 16;
    datas[3] = dfa >> 24;
    flash.writeStream(100, (char *)datas, 100); 
    
    pc.printf("read after write\r\n");
    flash.readStream(100, (char*)buff, 100);
    for (int i=0; i<100; i++) {
        pc.printf("%02x", buff[i]);
    }
    pc.printf("\r\n");
    
    pc.printf("%d\r\n", flash.readByte(0x34));
    int tmp = 66;
    pc.printf("%d\r\n", flash.readByte(tmp));
    // read stream from 0x168
    char str2[11] = {0};
    flash.readStream(tmp, str2, 11);
    pc.printf("%s\n",str2);
    
    for (int i=50; i< 150;i++){
        pc.printf("%02x", flash.readByte(i));
    }
    pc.printf("\r\n");
    
    char string[] = "ABCDEFGHIJK";
    flash.writeStream(tmp, string, 11);
    
    char str3[11] = {0};
    flash.readStream(tmp, str3, 11);
    pc.printf("%s\n",str3);
    
    wait(1);
    
    LOG("---- Seeed Tiny BLE ----\n");
    
    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");
    }
    
    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    mpu_set_sample_rate(DEFAULT_MPU_HZ);
    mpu_set_gyro_fsr(2000);
    mpu_set_accel_fsr(16);
    dmp_load_motion_driver_firmware();
    dmp_set_orientation(
        inv_orientation_matrix_to_scalar(board_orientation));
    dmp_register_tap_cb(tap_cb);
    dmp_register_android_orient_cb(android_orient_cb);
    uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
                       DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | 
                       DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
    dmp_enable_feature(dmp_features);
    dmp_set_fifo_rate(DEFAULT_MPU_HZ);
    mpu_set_dmp_state(1);
    dmp_set_tap_thresh(TAP_XYZ, 50);
    
    
    motion_probe.fall(motion_interrupt_handle);

    button.fall(detect);

    LOG("Initialising the nRF51822\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 *)"smurfs", sizeof("smurfs"));
    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();

    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
    ble.gap().startAdvertising();
    
    
    uint8_t tmp1[32] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32};
    Timer timer;
    timer.start();
    while (true) {
        if (motion_event) {
            
            unsigned long sensor_timestamp;
            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.
                 */
                dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
                              &more);
                
//                saveMPUDataToFlash(offsetAddr, sensor_timestamp, accel, gyro, quat);
//                char tmpe[32];
//                flash.readStream(offsetAddr, tmpe, 32);
//                pc.printf("%02x%02x%02x%02x\r\n", tmpe[0],tmpe[1],tmpe[2],tmpe[3]);
                
                /* 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.
                 */
                if (sensors & INV_XYZ_GYRO) {
                     LOG("time:%d, GYRO: %d, %d, %d\n", timer.read_ms(), gyro[0], gyro[1], gyro[2]);
                }
                if (sensors & INV_XYZ_ACCEL) {
                    //LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
                }
                
                /* Unlike gyro and accel, quaternions are written to the FIFO in
                 * the body frame, q30. The orientation is set by the scalar passed
                 * to dmp_set_orientation during initialization.
                 */
                if (sensors & INV_WXYZ_QUAT) {
                    // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
                }
                
                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);
                    }
                }
            }
            // test flash
            flash.writeStream(offsetAddr, (char *)tmp1, 32);
            char tmpe[32];
            flash.readStream(offsetAddr, tmpe, 32);
            for (int i=0; i<32; i++) {
                pc.printf("%02x", tmpe[i]);
                tmp1[i] += 1;
            }
            pc.printf("\r\n");
            offsetAddr += 32;
            if (offsetAddr >= 0xFFF) {
                printf("one turn\r\n");
                offsetAddr = 0;
            }
            motion_event = 0;
        } else {
            ble.waitForEvent();
        }
    }
}

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