Yihui Xiong
Use MPU9250 with nRF51822
Fork of
Seeed_Tiny_BLE_Flash
by 
Darren Huang
Diff: main.cpp
- Revision:
- 5:9b240c1d5251
- Parent:
- 4:19a0764d6b81
--- a/main.cpp Fri Nov 13 08:22:27 2015 +0000
+++ b/main.cpp Thu Dec 10 08:00:18 2015 +0000
@@ -1,62 +1,27 @@
+
#include "mbed.h"
-#include "mbed_i2c.h"
+#include "mbed_spi.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
+#define MPU9250_MISO p16
+#define MPU9250_MOSI p12
+#define MPU9250_SCLK p13
+#define MPU9250_CS p15
+#define MPU9250_INT p14
/* Starting sampling rate. */
-#define DEFAULT_MPU_HZ (200)
+#define DEFAULT_MPU_HZ (100)
-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;
+Serial pc(p9, p11);
+Ticker ticker;
+InterruptIn motion_probe(MPU9250_INT);
-BLEDevice ble;
-UARTService *uartServicePtr;
-
-volatile bool bleIsConnected = false;
-volatile uint8_t tick_event = 0;
+volatile uint8_t compass_event = 0;
volatile uint8_t motion_event = 0;
static signed char board_orientation[9] = {
1, 0, 0,
@@ -64,38 +29,14 @@
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)
+void compass_tick_handle(void)
{
- 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;
+ compass_event = 1;
}
-//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)
{
@@ -113,164 +54,65 @@
}
-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);
+ wait(1);
+ LOG("---- eMPL MPU library @ Seeed ----\n");
- 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);
-
+ mbed_spi_init(MPU9250_MOSI, MPU9250_MISO, MPU9250_SCLK, MPU9250_CS);
+
if (mpu_init(0)) {
- LOG("failed to initialize mpu6050\r\n");
+ LOG("failed to initialize mpu9250\r\n");
}
-
- mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
+
+ /* Get/set hardware configuration. Start gyro. */
+ /* Wake up all sensors. */
+ mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); //
+ /* Push both gyro and accel data into the FIFO. */
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);
+
+ /* 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_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_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_interrupt_mode(DMP_INT_CONTINUOUS);
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);
-
+ ticker.attach(compass_tick_handle, 0.1);
- /* 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();
+ int try_to_sleep = 1;
while (true) {
if (motion_event) {
-
+ try_to_sleep = 0;
+
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,
@@ -286,61 +128,51 @@
*/
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]);
+ LOG("gyro: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
}
if (sensors & INV_XYZ_ACCEL) {
- //LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
+ 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]);
+ 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;
+
+ motion_event = 0;
+ }
+
+ if (compass_event) {
+ try_to_sleep = 0;
+
+ unsigned long compass_timestamp = 0;
+ short compass[3];
+
+
+ int retval = mpu_get_compass_reg(compass, &compass_timestamp);
+ if (retval) {
+ LOG("read compass error: %d\n", retval);
+ } else {
+ LOG("compass: %d, %d, %d\n", compass[0], compass[1], compass[2]);
}
- pc.printf("\r\n");
- offsetAddr += 32;
- if (offsetAddr >= 0xFFF) {
- printf("one turn\r\n");
- offsetAddr = 0;
- }
- motion_event = 0;
- } else {
- ble.waitForEvent();
+
+ compass_event = 0;
+ }
+
+ if (try_to_sleep) {
+ sleep();
+ try_to_sleep = 1;
}
}
}