Diff: main.cpp

Revision:

4:988f87cfa73c

Parent:

3:24e365bd1b97

Child:

5:ab49c12aab25

--- a/main.cpp	Thu Nov 05 06:58:30 2015 +0000
+++ b/main.cpp	Thu Sep 20 13:39:57 2018 +0000
@@ -1,3 +1,4 @@
+// https://github.com/jrowberg/i2cdevlib/issues/15
 
 #include "mbed.h"
 #include "mbed_i2c.h"
@@ -10,6 +11,21 @@
 #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__); }
 
@@ -27,9 +43,6 @@
 #define UART_CTS    p8
 #define UART_RTS    p10
 
-/* Starting sampling rate. */
-#define DEFAULT_MPU_HZ  (100)
-
 DigitalOut blue(LED_BLUE);
 DigitalOut green(LED_GREEN);
 DigitalOut red(LED_RED);
@@ -38,9 +51,10 @@
 AnalogIn    battery(BATTERY_PIN);
 Serial pc(UART_TX, UART_RX);
 
-InterruptIn motion_probe(p14);
+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;
@@ -54,144 +68,226 @@
     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!\n");
+    LOG("Connected!\r\n");
     bleIsConnected = true;
 }
 
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
 {
-    LOG("Disconnected!\n");
-    LOG("Restarting the advertising process\n");
+    LOG("Disconnected!\r\n");
+    LOG("Restarting the advertising process\r\n");
     ble.startAdvertising();
     bleIsConnected = false;
 }
 
 void tick(void)
 {
-    static uint32_t count = 0;
-    
-    LOG("%d\r\n", count++);
-    green = !green;
+    //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)
+void detect(void)                   // Flag de detección de interrupcion por boton
 {
-    LOG("Button pressed\n");  
+    LOG("\r\nButton pressed\r\n");
     blue = !blue;
 }
 
-void motion_interrupt_handle(void)
+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\n");
+    LOG("Tap motion detected\r\n");
 }
 
 void android_orient_cb(unsigned char orientation)
 {
-    LOG("Oriention changed\n");
+    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 ----\n");
-    
+
+    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. */
-    /* 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);
+    //#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. */
-    unsigned char accel_fsr;
+    //#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);
-    mpu_get_gyro_fsr(&gyro_fsr);
-    mpu_get_accel_fsr(&accel_fsr);
-    
-    dmp_load_motion_driver_firmware();
+    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));
-    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;
+        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);
-    mpu_set_dmp_state(1);
-    
-    dmp_set_interrupt_mode(DMP_INT_GESTURE);
-    dmp_set_tap_thresh(TAP_XYZ, 50);
-    
-    
-    motion_probe.fall(motion_interrupt_handle);
+
+    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);
 
-    
-    Ticker ticker;
-    ticker.attach(tick, 3);
+    button.fall(detect);                          // Asigno una interrupción al botón durante el fall
 
-    button.fall(detect);
-
-    LOG("Initialising the nRF51822\n");
+    ////////////////////////////////////////////////////////////////////////////////
+    //  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 *)"smurfs", sizeof("smurfs"));
+                                     (const uint8_t *)"CastingAnalyzer", sizeof("CastingAnalyzer"));
     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
                                      (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
-    DFUService dfu(ble);                                 
+    DFUService dfu(ble);
     UARTService uartService(ble);
     uartServicePtr = &uartService;
     //uartService.retargetStdout();
 
-    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
+    ////////////////////////////////////////////////////////////////////////////////
+    //  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) {
-            
-            unsigned long sensor_timestamp;
+        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,
@@ -204,51 +300,116 @@
                  * 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,
+                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.
                  */
-                if (sensors & INV_XYZ_GYRO) {
-                    // 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]);
+                #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
+                    }
                 }
                 
-                /* 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]);
-                }
-                
+                #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;
+           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.
@@ -294,5 +455,4 @@
 
 
     return scalar;
-}
-
+}
\ No newline at end of file