Diff: main.cpp

Revision:

3:7875f062a4ea

Parent:

2:bf1a19d489eb

Child:

4:aea4ff8e52ef

--- a/main.cpp	Fri Sep 14 20:06:28 2018 +0000
+++ b/main.cpp	Tue Sep 18 19:44:58 2018 +0000
@@ -1,10 +1,8 @@
 
 #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 "MPU6050.h"
 
 #include "BLE.h"
 #include "DFUService.h"
@@ -47,6 +45,7 @@
 InterruptIn button(BUTTON_PIN);
 AnalogIn    battery(BATTERY_PIN);
 Serial pc(UART_TX, UART_RX);
+MPU6050 mpu(MPU6050_SDA, MPU6050_SCL);
 
 InterruptIn motion_probe(p14);
 
@@ -56,17 +55,16 @@
 UARTService *uartServicePtr;
 
 // variables to monitor the battery voltage
-float                   batteryVoltage = 100.0f;
-bool                    batteryVoltageChanged = false;
+volatile float  batteryVoltage = 100.0f;
+volatile bool   batteryVoltageChanged = false;
+
+volatile bool   startMeasure = false;
 
 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
-};
+
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
 
 
 void check_i2c_bus(void);
@@ -89,10 +87,8 @@
 
 void tick(void)
 {
-    static uint32_t count = 0;
-    
-    LOG("%d\r\n", count++);
     green = !green;
+    startMeasure = true;    // notify the main-loop to start measuring the MPU6050
 }
 
 // timer callback function to measure the ADC battery level
@@ -112,11 +108,6 @@
     blue = !blue;
 }
 
-void motion_interrupt_handle(void)
-{
-    motion_event = 1;
-}
-
 void tap_cb(unsigned char direction, unsigned char count)
 {
     LOG("Tap motion detected\n");
@@ -140,51 +131,17 @@
     
     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");
+    LOG("MPU6050 testConnection \n");
+    bool mpu6050TestResult = mpu.testConnection();
+    if(mpu6050TestResult) {
+        LOG("MPU6050 test passed \n");
+    } else {
+        LOG("MPU6050 test failed \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);
-    mpu_set_sample_rate(DEFAULT_MPU_HZ);
-    
-    /* 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_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);
-
-
-    
     Ticker ticker;
-    ticker.attach(tick, 3);
+    ticker.attach(tick, 5);
     
     Ticker batteryMonitorTicker;
     batteryMonitorTicker.attach(batteryMonitorCallback, 60.0f);
@@ -203,8 +160,6 @@
     ble_error_t error = ble.gap().getAddress(&addr_type, address);
     if (error == BLE_ERROR_NONE) {
         for (int i = 5; i >= 0; i--){
-            //sprintf(name[4 + ((5-i)*2)], "%02x", (uint8_t)address[i]);
-            //sprintf(buffer, "%02x", address[i]);
             char buffer[3];
             sprintf(buffer, "%02x", address[i]);
             name[4 + ((5-i)*2)] = buffer[0];
@@ -231,73 +186,25 @@
     ble.gap().startAdvertising();
     
     while (true) {
-        if (motion_event) {
-            
-            unsigned long sensor_timestamp;
-            short gyro[3], accel[3], sensors;
-            long quat[4];
-            unsigned char more = 1;
+        ble.waitForEvent();
+    
+        // update battery level after the level is measured
+        if (batteryVoltageChanged == true) {
+            LOG("VBat: %4.3f, ADC: %4.3f, Vadc: %4.3f\n", batteryVoltage*2.0f, batteryVoltage, batteryVoltage*3.3f);
+            battery.updateBatteryLevel((uint8_t)(batteryVoltage*100.0f));    // input is 0-1.0 of 3.3V -> *100 = percentage of 3.3V
+            batteryVoltageChanged = false;
+        }
+        else if (startMeasure == true) {
+            LOG("Start measuring the acceleration\n");
             
-            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);
-                
-                
-                /* 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]);
-                }
-                
-                /* 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);
-                    }
-                }
-            }
+    float a[3];
+            mpu.getAccelero(a);
+            //writing current accelerometer and gyro position 
+            LOG("%.2f;%.2f;%.2f\n", a[0], a[1], a[2]);
+            startMeasure = false;
             
-            motion_event = 0;
-        } else {
-            ble.waitForEvent();
-        
-            // update battery level after the level is measured
-            if (batteryVoltageChanged == true) {
-                LOG("VBat: %4.3f, ADC: %4.3f, Vadc: %4.3f\n", batteryVoltage*2.0f, batteryVoltage, batteryVoltage*3.3f);
-                battery.updateBatteryLevel((uint8_t)(batteryVoltage*100.0f));    // input is 0-1.0 of 3.3V -> *100 = percentage of 3.3V
-                batteryVoltageChanged = false;
-            }
+            float temp = mpu.getTemp();
+            LOG("Temp = %f\n", temp);
         }
     }
 }