First test with Seeed Tiny BLE streaming realtime data over BLE and USB Serial. This program is based in the "Tiny BLE Getting started" using the eMotion Driver 5.12 pulling the sensor values from the DMP.
Dependencies: BLE_API eMPL_MPU6050 mbed nRF51822
main.cpp
- Committer:
- valecapaldi
- Date:
- 2018-09-20
- Revision:
- 5:ab49c12aab25
- Parent:
- 4:988f87cfa73c
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; }