fix nrf51822 i2c & spi conflict
Dependencies: BLE_API eMPL_MPU6050 nRF51822
Fork of Seeed_Tiny_BLE_Flash by
main.cpp
- Committer:
- yihui
- Date:
- 2015-11-17
- Revision:
- 5:b8c02645e6af
- Parent:
- 4:19a0764d6b81
File content as of revision 5:b8c02645e6af:
#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; }