Valentin Capaldi / Mbed 2 deprecated Accel_and_Gyro_BLE_eMD5

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@5:ab49c12aab25, 2018-09-20 (annotated)

Committer:
valecapaldi
Date:
Thu Sep 20 13:56:49 2018 +0000
Revision:
5:ab49c12aab25
Parent:
4:988f87cfa73c 
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.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
valecapaldi 4:988f87cfa73c 1 // https://github.com/jrowberg/i2cdevlib/issues/15
yihui 0:26da608265f8 2
yihui 0:26da608265f8 3 #include "mbed.h"
yihui 0:26da608265f8 4 #include "mbed_i2c.h"
yihui 0:26da608265f8 5 #include "inv_mpu.h"
yihui 0:26da608265f8 6 #include "inv_mpu_dmp_motion_driver.h"
yihui 2:b61ddbb8528e 7 #include "nrf51.h"
yihui 2:b61ddbb8528e 8 #include "nrf51_bitfields.h"
yihui 0:26da608265f8 9
yihui 3:24e365bd1b97 10 #include "BLE.h"
yihui 0:26da608265f8 11 #include "DFUService.h"
yihui 0:26da608265f8 12 #include "UARTService.h"
yihui 0:26da608265f8 13
valecapaldi 4:988f87cfa73c 14 /* DESCOMENTAR PARA VER SALIDA DE DATOS POR PUERTO SERIE */
valecapaldi 4:988f87cfa73c 15 //#define SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 16 /* DESCOMENTAR PARA VER LA TRAMA DE DATOS DE BLE POR PUERTO SERIE */
valecapaldi 4:988f87cfa73c 17 //#define BLE_DEBUG
valecapaldi 4:988f87cfa73c 18 /* DESCOMENTAR SI SE QUIERE EJECUTAR LA CALIBRACION INICIAL */
valecapaldi 4:988f87cfa73c 19 #define CALIBRATE
valecapaldi 4:988f87cfa73c 20
valecapaldi 4:988f87cfa73c 21 /* DESCOMENTAR SI SE QUIERE ENVIAR TAMBIEN EL ACELEROMETRO A LA FIFO */
valecapaldi 4:988f87cfa73c 22 //#define SEND_ACCEL
valecapaldi 4:988f87cfa73c 23
valecapaldi 4:988f87cfa73c 24 /* CUSTOM FULL-SCALE RANGE */
valecapaldi 4:988f87cfa73c 25 #define ACCEL_CUSTOM_FSR (4)
valecapaldi 4:988f87cfa73c 26 #define GYRO_CUSTOM_FSR (1000)
valecapaldi 4:988f87cfa73c 27 /* Starting sampling rate. */
valecapaldi 4:988f87cfa73c 28 #define DEFAULT_MPU_HZ (80)
yihui 0:26da608265f8 29
yihui 0:26da608265f8 30 #define LOG(...) { pc.printf(__VA_ARGS__); }
yihui 0:26da608265f8 31
yihui 0:26da608265f8 32 #define LED_GREEN p21
yihui 0:26da608265f8 33 #define LED_RED p22
yihui 0:26da608265f8 34 #define LED_BLUE p23
yihui 0:26da608265f8 35 #define BUTTON_PIN p17
yihui 0:26da608265f8 36 #define BATTERY_PIN p1
yihui 0:26da608265f8 37
yihui 0:26da608265f8 38 #define MPU6050_SDA p12
yihui 0:26da608265f8 39 #define MPU6050_SCL p13
yihui 0:26da608265f8 40
yihui 0:26da608265f8 41 #define UART_TX p9
yihui 0:26da608265f8 42 #define UART_RX p11
yihui 0:26da608265f8 43 #define UART_CTS p8
yihui 0:26da608265f8 44 #define UART_RTS p10
yihui 0:26da608265f8 45
yihui 0:26da608265f8 46 DigitalOut blue(LED_BLUE);
yihui 0:26da608265f8 47 DigitalOut green(LED_GREEN);
yihui 0:26da608265f8 48 DigitalOut red(LED_RED);
yihui 0:26da608265f8 49
yihui 0:26da608265f8 50 InterruptIn button(BUTTON_PIN);
yihui 0:26da608265f8 51 AnalogIn battery(BATTERY_PIN);
yihui 0:26da608265f8 52 Serial pc(UART_TX, UART_RX);
yihui 0:26da608265f8 53
valecapaldi 4:988f87cfa73c 54 InterruptIn motion_probe(p14); // Interrupcion recibida desde el MPU (Pueder ser por TAP o por FIFO overflow)
yihui 0:26da608265f8 55
yihui 2:b61ddbb8528e 56 int read_none_count = 0;
valecapaldi 4:988f87cfa73c 57 bool cal_flag = 0; // Calibration done flag
yihui 2:b61ddbb8528e 58
yihui 0:26da608265f8 59 BLEDevice ble;
yihui 0:26da608265f8 60 UARTService *uartServicePtr;
yihui 0:26da608265f8 61
yihui 0:26da608265f8 62 volatile bool bleIsConnected = false;
yihui 0:26da608265f8 63 volatile uint8_t tick_event = 0;
yihui 0:26da608265f8 64 volatile uint8_t motion_event = 0;
yihui 0:26da608265f8 65 static signed char board_orientation[9] = {
yihui 0:26da608265f8 66 1, 0, 0,
yihui 0:26da608265f8 67 0, 1, 0,
yihui 0:26da608265f8 68 0, 0, 1
yihui 0:26da608265f8 69 };
yihui 0:26da608265f8 70
valecapaldi 4:988f87cfa73c 71 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 72 // PROTOTIPOS
valecapaldi 4:988f87cfa73c 73 ////////////////////////////////////////////////////////////////////////////////
yihui 0:26da608265f8 74 void check_i2c_bus(void);
yihui 0:26da608265f8 75 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
yihui 0:26da608265f8 76
yihui 2:b61ddbb8528e 77
yihui 3:24e365bd1b97 78 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
yihui 0:26da608265f8 79 {
valecapaldi 4:988f87cfa73c 80 LOG("Connected!\r\n");
yihui 0:26da608265f8 81 bleIsConnected = true;
yihui 0:26da608265f8 82 }
yihui 0:26da608265f8 83
yihui 3:24e365bd1b97 84 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
yihui 0:26da608265f8 85 {
valecapaldi 4:988f87cfa73c 86 LOG("Disconnected!\r\n");
valecapaldi 4:988f87cfa73c 87 LOG("Restarting the advertising process\r\n");
yihui 0:26da608265f8 88 ble.startAdvertising();
yihui 0:26da608265f8 89 bleIsConnected = false;
yihui 0:26da608265f8 90 }
yihui 0:26da608265f8 91
yihui 0:26da608265f8 92 void tick(void)
yihui 0:26da608265f8 93 {
valecapaldi 4:988f87cfa73c 94 //static uint32_t count = 0;
valecapaldi 4:988f87cfa73c 95 if (cal_flag == 0){
valecapaldi 4:988f87cfa73c 96 red = !red;
valecapaldi 4:988f87cfa73c 97 }
valecapaldi 4:988f87cfa73c 98 if (cal_flag == 1){
valecapaldi 4:988f87cfa73c 99 green = !green;
valecapaldi 4:988f87cfa73c 100 }
valecapaldi 4:988f87cfa73c 101 //LOG("%d, ", count++); // Contador en pantalla
yihui 0:26da608265f8 102 }
yihui 0:26da608265f8 103
valecapaldi 4:988f87cfa73c 104 void detect(void) // Flag de detección de interrupcion por boton
yihui 0:26da608265f8 105 {
valecapaldi 4:988f87cfa73c 106 LOG("\r\nButton pressed\r\n");
yihui 0:26da608265f8 107 blue = !blue;
yihui 0:26da608265f8 108 }
yihui 0:26da608265f8 109
valecapaldi 4:988f87cfa73c 110 void motion_interrupt_handle(void) // Flag de detección de interrupcion por movimiento
yihui 0:26da608265f8 111 {
yihui 0:26da608265f8 112 motion_event = 1;
yihui 0:26da608265f8 113 }
yihui 0:26da608265f8 114
yihui 0:26da608265f8 115 void tap_cb(unsigned char direction, unsigned char count)
yihui 0:26da608265f8 116 {
valecapaldi 4:988f87cfa73c 117 LOG("Tap motion detected\r\n");
yihui 0:26da608265f8 118 }
yihui 0:26da608265f8 119
yihui 0:26da608265f8 120 void android_orient_cb(unsigned char orientation)
yihui 0:26da608265f8 121 {
valecapaldi 4:988f87cfa73c 122 LOG("Oriention changed\r\n");
yihui 0:26da608265f8 123 }
yihui 0:26da608265f8 124
valecapaldi 4:988f87cfa73c 125 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 126 // INICIALIZACIONES (MAIN)
valecapaldi 4:988f87cfa73c 127 ////////////////////////////////////////////////////////////////////////////////
yihui 0:26da608265f8 128 int main(void)
yihui 0:26da608265f8 129 {
yihui 0:26da608265f8 130 blue = 1;
yihui 0:26da608265f8 131 green = 1;
yihui 0:26da608265f8 132 red = 1;
yihui 0:26da608265f8 133
valecapaldi 4:988f87cfa73c 134 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 135 // INICIALIZACION SERIAL PORT
valecapaldi 4:988f87cfa73c 136 ////////////////////////////////////////////////////////////////////////////////
yihui 0:26da608265f8 137 pc.baud(115200);
yihui 2:b61ddbb8528e 138 wait(1);
valecapaldi 4:988f87cfa73c 139
valecapaldi 4:988f87cfa73c 140 LOG("---- Seeed Tiny BLE ----\r\n");
valecapaldi 4:988f87cfa73c 141 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 142 // INICIALIZACION MPU I2C
valecapaldi 4:988f87cfa73c 143 ////////////////////////////////////////////////////////////////////////////////
yihui 2:b61ddbb8528e 144 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
yihui 0:26da608265f8 145 mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
yihui 0:26da608265f8 146
yihui 0:26da608265f8 147 if (mpu_init(0)) {
yihui 2:b61ddbb8528e 148 LOG("failed to initialize mpu6050\r\n");
yihui 0:26da608265f8 149 }
valecapaldi 4:988f87cfa73c 150
yihui 0:26da608265f8 151 /* Get/set hardware configuration. Start gyro. */
valecapaldi 4:988f87cfa73c 152 //#ifdef SEND_ACCEL
valecapaldi 4:988f87cfa73c 153 /* Wake up all sensors. */
valecapaldi 4:988f87cfa73c 154 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
valecapaldi 4:988f87cfa73c 155 /* Push both gyro and accel data into the FIFO. */
valecapaldi 4:988f87cfa73c 156 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
valecapaldi 4:988f87cfa73c 157 /* Como el DMP esta activo, se utiliza el sample rate por defecto de 200Hz (ver pag.8/12 eMD 5.1.1 - Tutorial.pdf) */
valecapaldi 4:988f87cfa73c 158 //#else
valecapaldi 4:988f87cfa73c 159 /* Wake up only gyro */
valecapaldi 4:988f87cfa73c 160 // mpu_set_sensors(INV_XYZ_GYRO);
valecapaldi 4:988f87cfa73c 161 /* Push only gyro data into the FIFO. */
valecapaldi 4:988f87cfa73c 162 // mpu_configure_fifo(INV_XYZ_GYRO);
valecapaldi 4:988f87cfa73c 163 /* Como el DMP esta activo, se utiliza el sample rate por defecto de 200Hz (ver pag.8/12 eMD 5.1.1 - Tutorial.pdf) */
valecapaldi 4:988f87cfa73c 164 //#endif
valecapaldi 4:988f87cfa73c 165
yihui 0:26da608265f8 166 mpu_set_sample_rate(DEFAULT_MPU_HZ);
valecapaldi 4:988f87cfa73c 167
valecapaldi 4:988f87cfa73c 168 /* AFS_SEL | Full Scale Range | LSB Sensitivity
valecapaldi 4:988f87cfa73c 169 * --------+------------------+----------------
valecapaldi 4:988f87cfa73c 170 * 0 | +/- 2g | 8192 LSB/mg
valecapaldi 4:988f87cfa73c 171 * 1 | +/- 4g | 4096 LSB/mg
valecapaldi 4:988f87cfa73c 172 * 2 | +/- 8g | 2048 LSB/mg
valecapaldi 4:988f87cfa73c 173 * 3 | +/- 16g | 1024 LSB/mg */
valecapaldi 4:988f87cfa73c 174 mpu_set_accel_fsr(ACCEL_CUSTOM_FSR); // Seteo el custom full-scale range del acelerometro
valecapaldi 4:988f87cfa73c 175
valecapaldi 4:988f87cfa73c 176 /* FS_SEL | Full Scale Range | LSB Sensitivity
valecapaldi 4:988f87cfa73c 177 * -------+--------------------+----------------
valecapaldi 4:988f87cfa73c 178 * 0 | +/- 250 degrees/s | 131 LSB/deg/s
valecapaldi 4:988f87cfa73c 179 * 1 | +/- 500 degrees/s | 65.5 LSB/deg/s
valecapaldi 4:988f87cfa73c 180 * 2 | +/- 1000 degrees/s | 32.8 LSB/deg/s
valecapaldi 4:988f87cfa73c 181 * 3 | +/- 2000 degrees/s | 16.4 LSB/deg/s */
valecapaldi 4:988f87cfa73c 182 mpu_set_gyro_fsr(GYRO_CUSTOM_FSR); // Seteo el custom full-scale range del giroscopo
valecapaldi 4:988f87cfa73c 183
yihui 0:26da608265f8 184 /* Read back configuration in case it was set improperly. */
valecapaldi 4:988f87cfa73c 185 //#ifdef SEND_ACCEL
valecapaldi 4:988f87cfa73c 186 unsigned char accel_fsr;
valecapaldi 4:988f87cfa73c 187 mpu_get_accel_fsr(&accel_fsr); // Get the accel full-scale range
valecapaldi 4:988f87cfa73c 188 //#endif
yihui 0:26da608265f8 189 unsigned short gyro_rate, gyro_fsr;
valecapaldi 4:988f87cfa73c 190 mpu_get_sample_rate(&gyro_rate); // Current sampling rate (Hz)
valecapaldi 4:988f87cfa73c 191 mpu_get_gyro_fsr(&gyro_fsr); // Get the gyro full-scale range
valecapaldi 4:988f87cfa73c 192
valecapaldi 4:988f87cfa73c 193 wait(1);
valecapaldi 4:988f87cfa73c 194 //LOG("Gyro FSR: %u\r\n", gyro_fsr); // Print Gyro FSR
valecapaldi 4:988f87cfa73c 195 //LOG("Accel FSR: %u\r\n", accel_fsr); // Print Gyro FSR
valecapaldi 4:988f87cfa73c 196
valecapaldi 4:988f87cfa73c 197 dmp_load_motion_driver_firmware(); //Load the DMP with the fw image
yihui 0:26da608265f8 198 dmp_set_orientation(
valecapaldi 4:988f87cfa73c 199 inv_orientation_matrix_to_scalar(board_orientation)); // Push gyro and accel orientation to the DMP
valecapaldi 4:988f87cfa73c 200
valecapaldi 4:988f87cfa73c 201 // Configure which DMP features will be available
valecapaldi 4:988f87cfa73c 202 uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL; // ORIGINAL
valecapaldi 4:988f87cfa73c 203 // #ifdef SEND_ACCEL
valecapaldi 4:988f87cfa73c 204 // uint16_t dmp_features = DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
valecapaldi 4:988f87cfa73c 205 //#else
valecapaldi 4:988f87cfa73c 206 // uint16_t dmp_features = DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
valecapaldi 4:988f87cfa73c 207 //#endif
yihui 0:26da608265f8 208 dmp_enable_feature(dmp_features);
valecapaldi 4:988f87cfa73c 209
valecapaldi 4:988f87cfa73c 210 dmp_set_fifo_rate(DEFAULT_MPU_HZ); // Set DMP output rate in (Hz)
valecapaldi 4:988f87cfa73c 211 mpu_set_dmp_state(1); // Enable/disable DMP support (1=EN)
valecapaldi 4:988f87cfa73c 212
valecapaldi 4:988f87cfa73c 213 dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); // Specify when a DMP interrupt should occur. One FIFO period elapsed or gesture detected)
valecapaldi 4:988f87cfa73c 214 // DMP = EN => Default sample rate 200Hz
valecapaldi 4:988f87cfa73c 215
valecapaldi 4:988f87cfa73c 216 motion_probe.fall(motion_interrupt_handle); // Asigno interrupcion al pin14 proveniente del MPU
yihui 0:26da608265f8 217
valecapaldi 4:988f87cfa73c 218 Ticker ticker;
valecapaldi 4:988f87cfa73c 219 ticker.attach(tick, 1);
yihui 3:24e365bd1b97 220
valecapaldi 4:988f87cfa73c 221 button.fall(detect); // Asigno una interrupción al botón durante el fall
yihui 2:b61ddbb8528e 222
valecapaldi 4:988f87cfa73c 223 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 224 // INICIALIZACION BLE
valecapaldi 4:988f87cfa73c 225 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 226 LOG("Initialising the nRF51822\r\n");
yihui 2:b61ddbb8528e 227 ble.init();
yihui 3:24e365bd1b97 228 ble.gap().onDisconnection(disconnectionCallback);
yihui 3:24e365bd1b97 229 ble.gap().onConnection(connectionCallback);
yihui 2:b61ddbb8528e 230
yihui 2:b61ddbb8528e 231 /* setup advertising */
yihui 2:b61ddbb8528e 232 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
yihui 2:b61ddbb8528e 233 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
yihui 2:b61ddbb8528e 234 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
valecapaldi 5:ab49c12aab25 235 (const uint8_t *)"TinyBLE", sizeof("TinyBLE"));
yihui 2:b61ddbb8528e 236 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
yihui 2:b61ddbb8528e 237 (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
valecapaldi 4:988f87cfa73c 238 DFUService dfu(ble);
yihui 2:b61ddbb8528e 239 UARTService uartService(ble);
yihui 2:b61ddbb8528e 240 uartServicePtr = &uartService;
yihui 2:b61ddbb8528e 241 //uartService.retargetStdout();
yihui 2:b61ddbb8528e 242
valecapaldi 4:988f87cfa73c 243 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 244 // CALIBRACION
valecapaldi 4:988f87cfa73c 245 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 246 #ifdef CALIBRATE
valecapaldi 4:988f87cfa73c 247 unsigned long sensor_timestamp_cal = 0;
valecapaldi 4:988f87cfa73c 248 short gyro_cal[3], accel_cal[3], sensors_cal;
valecapaldi 4:988f87cfa73c 249 long quat_cal[4];
valecapaldi 4:988f87cfa73c 250 unsigned char more_cal = 1;
valecapaldi 4:988f87cfa73c 251 red = 0; // Prendo led rojo para avisar que esta calibrando
valecapaldi 4:988f87cfa73c 252 LOG("Waiting for auto-calibration, please do not move the sensor....\r\n");
valecapaldi 4:988f87cfa73c 253 while (sensor_timestamp_cal < 20000){
valecapaldi 4:988f87cfa73c 254 dmp_read_fifo(gyro_cal, accel_cal, quat_cal, &sensor_timestamp_cal, &sensors_cal, // Get one packet from the FIFO.
valecapaldi 4:988f87cfa73c 255 &more_cal);
valecapaldi 4:988f87cfa73c 256 }
valecapaldi 4:988f87cfa73c 257 cal_flag = 1; red = 1; // Apago led Rojo.
valecapaldi 4:988f87cfa73c 258 LOG("Calibration complete!");
valecapaldi 4:988f87cfa73c 259 #endif
valecapaldi 4:988f87cfa73c 260 ticker.attach(tick, 2); // Asigno 2 segundos a la frecuencia de on/off del led
valecapaldi 4:988f87cfa73c 261
valecapaldi 4:988f87cfa73c 262 // Inicio servicio Advertising BLE
valecapaldi 4:988f87cfa73c 263 ble.setAdvertisingInterval(8); /* 5ms; in multiples of 0.625ms. */
yihui 3:24e365bd1b97 264 ble.gap().startAdvertising();
valecapaldi 4:988f87cfa73c 265 char BLE_MPU_DATA[80]; // Vector para envio de datos del MPU via BTLE
valecapaldi 4:988f87cfa73c 266 char gyro_val[30], accel_val[20], timestamp_val[20];
valecapaldi 4:988f87cfa73c 267
valecapaldi 4:988f87cfa73c 268 char r[4];
valecapaldi 4:988f87cfa73c 269 r[1]='\r';
valecapaldi 4:988f87cfa73c 270 r[2]='\n';
valecapaldi 4:988f87cfa73c 271 r[3]='\0';
valecapaldi 4:988f87cfa73c 272
valecapaldi 4:988f87cfa73c 273 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 274 // MAIN LOOP
valecapaldi 4:988f87cfa73c 275 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 276 #ifdef SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 277 #ifdef SEND_ACCEL
valecapaldi 4:988f87cfa73c 278 LOG("\r\n\nTimeStamp(ms); AX (g); AY (g); AZ (g); GX (dps); GY (dps); GZ (dps)");
valecapaldi 4:988f87cfa73c 279 #else
valecapaldi 4:988f87cfa73c 280 LOG("\r\n\nTimeStamp(ms); GX (dps); GY (dps); GZ (dps)");
valecapaldi 4:988f87cfa73c 281 #endif
valecapaldi 4:988f87cfa73c 282 #endif
valecapaldi 4:988f87cfa73c 283
yihui 0:26da608265f8 284 while (true) {
valecapaldi 4:988f87cfa73c 285 if (motion_event) { // Si detecta fifo overflow, tengo una interrupcion
valecapaldi 4:988f87cfa73c 286 unsigned long sensor_timestamp = 0;
yihui 3:24e365bd1b97 287 short gyro[3], accel[3], sensors;
yihui 3:24e365bd1b97 288 long quat[4];
yihui 3:24e365bd1b97 289 unsigned char more = 1;
valecapaldi 4:988f87cfa73c 290
yihui 3:24e365bd1b97 291 while (more) {
yihui 3:24e365bd1b97 292 /* This function gets new data from the FIFO when the DMP is in
yihui 3:24e365bd1b97 293 * use. The FIFO can contain any combination of gyro, accel,
yihui 3:24e365bd1b97 294 * quaternion, and gesture data. The sensors parameter tells the
yihui 3:24e365bd1b97 295 * caller which data fields were actually populated with new data.
yihui 3:24e365bd1b97 296 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
yihui 3:24e365bd1b97 297 * the FIFO isn't being filled with accel data.
yihui 3:24e365bd1b97 298 * The driver parses the gesture data to determine if a gesture
yihui 3:24e365bd1b97 299 * event has occurred; on an event, the application will be notified
yihui 3:24e365bd1b97 300 * via a callback (assuming that a callback function was properly
yihui 3:24e365bd1b97 301 * registered). The more parameter is non-zero if there are
yihui 3:24e365bd1b97 302 * leftover packets in the FIFO.
valecapaldi 4:988f87cfa73c 303 * Sensor Timestamp is in milliseconds
yihui 3:24e365bd1b97 304 */
valecapaldi 4:988f87cfa73c 305 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, // Get one packet from the FIFO.
yihui 3:24e365bd1b97 306 &more);
yihui 3:24e365bd1b97 307
valecapaldi 4:988f87cfa73c 308 sprintf((char *)timestamp_val, "%lu;", sensor_timestamp);
valecapaldi 4:988f87cfa73c 309
valecapaldi 4:988f87cfa73c 310 #ifdef SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 311 LOG("\r\n%lu;", sensor_timestamp);
valecapaldi 4:988f87cfa73c 312 #endif
yihui 3:24e365bd1b97 313 /* Gyro and accel data are written to the FIFO by the DMP in chip
yihui 3:24e365bd1b97 314 * frame and hardware units. This behavior is convenient because it
yihui 3:24e365bd1b97 315 * keeps the gyro and accel outputs of dmp_read_fifo and
yihui 3:24e365bd1b97 316 * mpu_read_fifo consistent.
yihui 3:24e365bd1b97 317 */
valecapaldi 4:988f87cfa73c 318 #ifdef SEND_ACCEL
valecapaldi 4:988f87cfa73c 319 if (sensors & INV_XYZ_ACCEL) { // Accel data in hardware units.
valecapaldi 4:988f87cfa73c 320 //LOG("ACC: %d, %d, %d\r\n", accel[0], accel[1], accel[2]); // ORIGINAL
valecapaldi 4:988f87cfa73c 321 //LOG("%d; %d; %d; ", accel[0], accel[1], accel[2]); // NUEVO EN HARDWARE UNITS
valecapaldi 4:988f87cfa73c 322 if (accel_fsr == 2){
valecapaldi 4:988f87cfa73c 323 sprintf((char *)accel_val, "%0.2f;%0.2f;%0.2f;", (float)accel[0]/8192, (float)accel[1]/8192, (float)accel[2]/8192);
valecapaldi 4:988f87cfa73c 324 #ifdef SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 325 LOG(" %0.2f; %0.2f; %0.2f;", accel[0]/8192, accel[1]/8192, accel[2]/8192); // NUEVO EN G (escala +/- 2G)
valecapaldi 4:988f87cfa73c 326 #endif
valecapaldi 4:988f87cfa73c 327 } else if (accel_fsr == 4){
valecapaldi 4:988f87cfa73c 328 sprintf((char *)accel_val, "%0.2f;%0.2f;%0.2f;", (float)accel[0]/4096, (float)accel[1]/4096, (float)accel[2]/4096);
valecapaldi 4:988f87cfa73c 329 #ifdef SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 330 LOG(" %0.2f; %0.2f; %0.2f;", accel[0]/4096, accel[1]/4096, accel[2]/4096); // NUEVO EN G (escala +/- 4G)
valecapaldi 4:988f87cfa73c 331 #endif
valecapaldi 4:988f87cfa73c 332 } else if (accel_fsr == 8){
valecapaldi 4:988f87cfa73c 333 sprintf((char *)accel_val, "%0.2f;%0.2f;%0.2f;", (float)accel[0]/2048, (float)accel[1]/2048, (float)accel[2]/2048);
valecapaldi 4:988f87cfa73c 334 #ifdef SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 335 LOG(" %0.2f; %0.2f; %0.2f;", accel[0]/2048, accel[1]/2048, accel[2]/2048); // NUEVO EN G (escala +/- 8G)
valecapaldi 4:988f87cfa73c 336 #endif
valecapaldi 4:988f87cfa73c 337 } else if (accel_fsr == 16){
valecapaldi 4:988f87cfa73c 338 sprintf((char *)accel_val, "%0.2f;%0.2f;%0.2f;", (float)accel[0]/1024, (float)accel[1]/1024, (float)accel[2]/1024);
valecapaldi 4:988f87cfa73c 339 #ifdef SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 340 LOG(" %0.2f; %0.2f; %0.2f;", accel[0]/1024, accel[1]/1024, accel[2]/1024); // NUEVO EN G (escala +/- 16G)
valecapaldi 4:988f87cfa73c 341 #endif
valecapaldi 4:988f87cfa73c 342 }
valecapaldi 4:988f87cfa73c 343 }
valecapaldi 4:988f87cfa73c 344 #endif
valecapaldi 4:988f87cfa73c 345 if (sensors & INV_XYZ_GYRO) { // Gyro data in hardware units
valecapaldi 4:988f87cfa73c 346 //LOG("GYRO: %d, %d, %d\r\n", gyro[0], gyro[1], gyro[2]); // ORIGINAL
valecapaldi 4:988f87cfa73c 347 //LOG("%d; %d; %d\r\n", gyro[0], gyro[1], gyro[2]); // NUEVO EN HARDWARE UNITS
valecapaldi 4:988f87cfa73c 348 if (gyro_fsr == 250){
valecapaldi 4:988f87cfa73c 349 sprintf((char *)gyro_val, "%0.2f;%0.2f;%0.2f;", (float)gyro[0]/131, (float)gyro[1]/131, (float)gyro[2]/131);
valecapaldi 4:988f87cfa73c 350 #ifdef SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 351 LOG(" %0.2f; %0.2f; %0.2f", gyro[0]/131, gyro[1]/131, gyro[2]/131); // NUEVO EN DPS (escala +/- 250dps)
valecapaldi 4:988f87cfa73c 352 #endif
valecapaldi 4:988f87cfa73c 353 } else if (gyro_fsr == 500){
valecapaldi 4:988f87cfa73c 354 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);
valecapaldi 4:988f87cfa73c 355 #ifdef SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 356 LOG(" %0.2f; %0.2f; %0.2f", gyro[0]/65.5, gyro[1]/65.5, gyro[2]/65.5); // NUEVO EN DPS (escala +/- 500dps)
valecapaldi 4:988f87cfa73c 357 #endif
valecapaldi 4:988f87cfa73c 358 } else if (gyro_fsr == 1000){
valecapaldi 4:988f87cfa73c 359 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);
valecapaldi 4:988f87cfa73c 360 #ifdef SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 361 LOG(" %0.2f; %0.2f; %0.2f", gyro[0]/32.8, gyro[1]/32.8, gyro[2]/32.8); // NUEVO EN DPS (escala +/- 1000dps)
valecapaldi 4:988f87cfa73c 362 #endif
valecapaldi 4:988f87cfa73c 363 } else if (gyro_fsr == 2000){
valecapaldi 4:988f87cfa73c 364 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);
valecapaldi 4:988f87cfa73c 365 #ifdef SERIAL_DEBUG
valecapaldi 4:988f87cfa73c 366 LOG(" %0.2f; %0.2f; %0.2f", gyro[0]/16.4, gyro[1]/16.4, gyro[2]/16.4); // NUEVO EN DPS (escala +/- 2000dps)
valecapaldi 4:988f87cfa73c 367 #endif
valecapaldi 4:988f87cfa73c 368 }
yihui 3:24e365bd1b97 369 }
yihui 3:24e365bd1b97 370
valecapaldi 4:988f87cfa73c 371 #ifdef SEND_ACCEL
valecapaldi 4:988f87cfa73c 372 sprintf((char *)BLE_MPU_DATA, "%s%s%s\r\n\0", timestamp_val, accel_val, gyro_val);
valecapaldi 4:988f87cfa73c 373 #else
valecapaldi 4:988f87cfa73c 374 sprintf((char *)BLE_MPU_DATA, "%s%s\r\n\0", timestamp_val, gyro_val);
valecapaldi 4:988f87cfa73c 375 #endif
valecapaldi 4:988f87cfa73c 376
valecapaldi 4:988f87cfa73c 377 #ifdef BLE_DEBUG
valecapaldi 4:988f87cfa73c 378 LOG("%s",BLE_MPU_DATA);
valecapaldi 4:988f87cfa73c 379 #endif
valecapaldi 4:988f87cfa73c 380 uartService.writeString(BLE_MPU_DATA); // ENVIO DATOS VIA BLE
valecapaldi 4:988f87cfa73c 381
yihui 3:24e365bd1b97 382 if (sensors) {
yihui 3:24e365bd1b97 383 read_none_count = 0;
yihui 3:24e365bd1b97 384 } else {
yihui 3:24e365bd1b97 385 read_none_count++;
yihui 3:24e365bd1b97 386 if (read_none_count > 3) {
yihui 3:24e365bd1b97 387 read_none_count = 0;
valecapaldi 4:988f87cfa73c 388
yihui 3:24e365bd1b97 389 LOG("I2C may be stuck @ %d\r\n", sensor_timestamp);
yihui 3:24e365bd1b97 390 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
yihui 3:24e365bd1b97 391 }
yihui 3:24e365bd1b97 392 }
yihui 3:24e365bd1b97 393 }
valecapaldi 4:988f87cfa73c 394 motion_event = 0;
yihui 0:26da608265f8 395 } else {
valecapaldi 4:988f87cfa73c 396 // ESPERO UNA LETRA PARA CAMBIAR EL COLOR DEL LED
yihui 0:26da608265f8 397 ble.waitForEvent();
valecapaldi 4:988f87cfa73c 398 int c;
valecapaldi 4:988f87cfa73c 399 r[0]=c=uartService._getc();
valecapaldi 4:988f87cfa73c 400 if (c<=0) continue;
valecapaldi 4:988f87cfa73c 401 if (c=='R' || c=='r') { red=0; green=1; blue=1; }
valecapaldi 4:988f87cfa73c 402 else if (c=='G' || c=='g') { red=1; green=0; blue=1; }
valecapaldi 4:988f87cfa73c 403 else if (c=='B' || c=='b') { red=1; green=1; blue=0; }
valecapaldi 4:988f87cfa73c 404 else r[0]='?';
valecapaldi 4:988f87cfa73c 405 uartService.writeString(r); // Devuelve la misma letra leida por BLE
yihui 0:26da608265f8 406 }
yihui 0:26da608265f8 407 }
yihui 0:26da608265f8 408 }
yihui 0:26da608265f8 409
valecapaldi 4:988f87cfa73c 410 ////////////////////////////////////////////////////////////////////////////////
valecapaldi 4:988f87cfa73c 411 // FUNCIONES
valecapaldi 4:988f87cfa73c 412 ////////////////////////////////////////////////////////////////////////////////
yihui 0:26da608265f8 413 /* These next two functions converts the orientation matrix (see
yihui 0:26da608265f8 414 * gyro_orientation) to a scalar representation for use by the DMP.
yihui 0:26da608265f8 415 * NOTE: These functions are borrowed from Invensense's MPL.
yihui 0:26da608265f8 416 */
yihui 0:26da608265f8 417 static inline unsigned short inv_row_2_scale(const signed char *row)
yihui 0:26da608265f8 418 {
yihui 0:26da608265f8 419 unsigned short b;
yihui 0:26da608265f8 420
yihui 0:26da608265f8 421 if (row[0] > 0)
yihui 0:26da608265f8 422 b = 0;
yihui 0:26da608265f8 423 else if (row[0] < 0)
yihui 0:26da608265f8 424 b = 4;
yihui 0:26da608265f8 425 else if (row[1] > 0)
yihui 0:26da608265f8 426 b = 1;
yihui 0:26da608265f8 427 else if (row[1] < 0)
yihui 0:26da608265f8 428 b = 5;
yihui 0:26da608265f8 429 else if (row[2] > 0)
yihui 0:26da608265f8 430 b = 2;
yihui 0:26da608265f8 431 else if (row[2] < 0)
yihui 0:26da608265f8 432 b = 6;
yihui 0:26da608265f8 433 else
yihui 0:26da608265f8 434 b = 7; // error
yihui 0:26da608265f8 435 return b;
yihui 0:26da608265f8 436 }
yihui 0:26da608265f8 437
yihui 0:26da608265f8 438 unsigned short inv_orientation_matrix_to_scalar(
yihui 0:26da608265f8 439 const signed char *mtx)
yihui 0:26da608265f8 440 {
yihui 0:26da608265f8 441 unsigned short scalar;
yihui 0:26da608265f8 442
yihui 0:26da608265f8 443 /*
yihui 0:26da608265f8 444 XYZ 010_001_000 Identity Matrix
yihui 0:26da608265f8 445 XZY 001_010_000
yihui 0:26da608265f8 446 YXZ 010_000_001
yihui 0:26da608265f8 447 YZX 000_010_001
yihui 0:26da608265f8 448 ZXY 001_000_010
yihui 0:26da608265f8 449 ZYX 000_001_010
yihui 0:26da608265f8 450 */
yihui 0:26da608265f8 451
yihui 0:26da608265f8 452 scalar = inv_row_2_scale(mtx);
yihui 0:26da608265f8 453 scalar |= inv_row_2_scale(mtx + 3) << 3;
yihui 0:26da608265f8 454 scalar |= inv_row_2_scale(mtx + 6) << 6;
yihui 0:26da608265f8 455
yihui 0:26da608265f8 456
yihui 0:26da608265f8 457 return scalar;
valecapaldi 4:988f87cfa73c 458 }