Revision 2:44bc61abdf33, committed 2015-06-10

Comitter:

flywind

Date:

Wed Jun 10 12:56:48 2015 +0000

Parent:

1:3723e08bf4fd

Commit message:

milestone working one , yaw pitch roll update in BLE sucessfull

Changed in this revision

--- a/MPU6050-DMP-Seeed-Tiny-BLE.lib	Wed Jun 10 10:00:59 2015 +0000
+++ b/MPU6050-DMP-Seeed-Tiny-BLE.lib	Wed Jun 10 12:56:48 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/flywind/code/MPU6050-DMP-Seeed-Tiny-BLE/#305906acd732
+http://developer.mbed.org/users/flywind/code/MPU6050-DMP-Seeed-Tiny-BLE/#ae51ac8b7cf1

--- a/MPUService.h	Wed Jun 10 10:00:59 2015 +0000
+++ b/MPUService.h	Wed Jun 10 12:56:48 2015 +0000
@@ -63,8 +63,8 @@
         ble(_ble),
         receiveBuffer(),
         yprValueBytes(initialYPRData),
-        numBytesReceived(0),
-        receiveBufferIndex(0),
+        numBytesReceived(),
+        receiveBufferIndex(),
         
         YPRCharacteristic(MPUServiceYPRCharacteristicShortUUID, (YawPitchRollValueBytes *)yprValueBytes.getPointer(),GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
         settingCharacteristic(MPUServiceSettingCharacteristicShortUUID, receiveBuffer, 1, BLE_MPU_SERVICE_MAX_DATA_LEN,GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE){
@@ -138,26 +138,29 @@
     /* Private internal representation for the bytes used to work with the vaulue of the heart-rate characteristic. */
     struct YawPitchRollValueBytes {
         static const unsigned OFFSET_OF_FLAGS    = 0;
-        static const unsigned OFFSET_OF_VALUE    = OFFSET_OF_FLAGS + sizeof(uint8_t);
-        static const unsigned SIZEOF_VALUE_BYTES = sizeof(uint8_t) + 3*sizeof(float);
+        //static const unsigned OFFSET_OF_VALUE    = OFFSET_OF_FLAGS + sizeof(uint8_t);
+        static const unsigned SIZEOF_VALUE_BYTES = 3*sizeof(uint16_t);
 
 
         YawPitchRollValueBytes(float *yprData) : bytes() {
             /* assumption: temperature values are expressed in Celsius */
-            bytes[OFFSET_OF_FLAGS] =  1;
             updateYawPitchRoll(yprData);
         }
 
 
         void updateYawPitchRoll(float *yprData) {
             
-            
-            uint32_t yaw_ieee11073 = quick_ieee11073_from_float(yprData[0]);
-            uint32_t pitch_ieee11073 = quick_ieee11073_from_float(yprData[1]);
-            uint32_t roll_ieee11073 = quick_ieee11073_from_float(yprData[2]);
-            memcpy(&bytes[OFFSET_OF_VALUE], &yaw_ieee11073, sizeof(float));
-            memcpy(&bytes[OFFSET_OF_VALUE+sizeof(float)], &pitch_ieee11073, sizeof(float));
-            memcpy(&bytes[OFFSET_OF_VALUE+2*sizeof(float)], &roll_ieee11073, sizeof(float));
+            /*
+            int32_t yaw_ieee11073 = quick_ieee11073_from_float(yprData[0]);
+            int32_t pitch_ieee11073 = quick_ieee11073_from_float(yprData[1]);
+            int32_t roll_ieee11073 = quick_ieee11073_from_float(yprData[2]);
+            */
+            uint16_t yaw_ieee11073   = (uint16_t)(100*(yprData[0]+180));
+            uint16_t pitch_ieee11073 = (uint16_t)(100*(yprData[1]+180));
+            uint16_t roll_ieee11073  = (uint16_t)(100*(yprData[2]+180));
+            memcpy(&bytes[0], &yaw_ieee11073, sizeof(uint16_t));
+            memcpy(&bytes[OFFSET_OF_FLAGS+sizeof(uint16_t)], &pitch_ieee11073, sizeof(uint16_t));
+            memcpy(&bytes[OFFSET_OF_FLAGS+2*sizeof(uint16_t)], &roll_ieee11073, sizeof(uint16_t));
         }
 
         uint8_t       *getPointer(void) {
@@ -174,10 +177,10 @@
          * @param temperature The temperature as a float.
          * @return The temperature in 11073-20601 FLOAT-Type format.
          */
-        uint32_t quick_ieee11073_from_float(float yprData) {
-            uint8_t  exponent = 0xFE; //exponent is -2
-            uint32_t mantissa = (uint32_t)(yprData * 100);
-            return (((uint32_t)exponent) << 24) | mantissa;
+        int32_t quick_ieee11073_from_float(float yprData) {
+            int8_t  exponent = 0xFE; //exponent is -2
+            int32_t mantissa = (int32_t)(yprData * 100);
+            return (((int32_t)exponent) << 24) | mantissa;
         }
 
     private:
@@ -199,7 +202,7 @@
     uint8_t             YPRBuffer[BLE_MPU_SERVICE_MAX_DATA_LEN];
 
 
-    uint8_t             numBytesReceived;
+    uint8_t             numBytesReceived ;
     uint8_t             receiveBufferIndex;
 
     YawPitchRollValueBytes  yprValueBytes;

--- a/main.cpp	Wed Jun 10 10:00:59 2015 +0000
+++ b/main.cpp	Wed Jun 10 12:56:48 2015 +0000
@@ -92,7 +92,7 @@
 // (in degrees) calculated from the quaternions coming from the FIFO.
 // Note that Euler angles suffer from gimbal lock (for more info, see
 // http://en.wikipedia.org/wiki/Gimbal_lock)
-#define OUTPUT_READABLE_EULER
+//#define OUTPUT_READABLE_EULER
 
 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
 // pitch/roll angles (in degrees) calculated from the quaternions coming
@@ -155,8 +155,6 @@
                                              };
 volatile bool bleIsConnected = false;
 BLEDevice  ble;
-//UARTService *uartServicePtr;
-MPUService *mpuServicePtr;
 static volatile bool  triggerSensorPolling = false;
 /*
 * ====================================================================================================================================
@@ -184,8 +182,8 @@
 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
 VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
 VectorFloat gravity;    // [x, y, z]            gravity vector
-float euler[3]={0};         // [psi, theta, phi]    Euler angle container
-float ypr[3]={0};           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+float euler[3]= {0};        // [psi, theta, phi]    Euler angle container
+float ypr[3]= {0};          // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
 
 // packet structure for InvenSense teapot demo
 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
@@ -223,12 +221,10 @@
 
 void tick(void)
 {
-    if(dmpReady)
-    {
-    green = !green;
-    }
-    else{
-    red  = !red;
+    if(dmpReady) {
+        green = !green;
+    } else {
+        red  = !red;
     }
     triggerSensorPolling = true;
 }
@@ -327,6 +323,10 @@
     while (true) {
         if(dmpReady && (mpuInterrupt || fifoCount >= packetSize)) {
             mpuData();
+            pc.printf("ypr\t");
+            pc.printf("%i\t", (int16_t)ypr[0]*100) ;
+            pc.printf("%i\t", (int16_t)ypr[1]*100);
+            pc.printf("%i\t\r\n", (int16_t)ypr[2]*100);
             mpuServices.updateYawPitchRoll(ypr);
 
         }
@@ -409,7 +409,7 @@
         // 2 = DMP configuration updates failed
         // (if it's going to break, usually the code will be 1)
 
-        pc.printf("DDMP Initialization failed (code ");
+        pc.printf("DMP Initialization failed (code ");
         pc.printf("%d", devStatus);
         pc.printf(")\r\n");
     }
@@ -472,10 +472,15 @@
         mpu.dmpGetQuaternion(&q, fifoBuffer);
         mpu.dmpGetGravity(&gravity, &q);
         mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        ypr[0] =ypr[0] * 180/M_PI;
+        ypr[1] =ypr[1] * 180/M_PI;
+        ypr[2] =ypr[2] * 180/M_PI;
+        /*
         pc.printf("ypr\t");
-        pc.printf("%f\t", ypr[0] * 180/M_PI);
-        pc.printf("%f\t", ypr[1] * 180/M_PI);
-        pc.printf("%f\t\r\n", ypr[2] * 180/M_PI);
+        pc.printf("%f\t", ypr[0] ;
+        pc.printf("%f\t", ypr[1]);
+        pc.printf("%f\t\r\n", ypr[2]);
+        */
 #endif
 
 #ifdef OUTPUT_READABLE_REALACCEL

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed_i2c.c	Wed Jun 10 12:56:48 2015 +0000
@@ -0,0 +1,37 @@
+
+#include "i2c_api.h"
+
+static i2c_t mbed_i2c_object;
+
+void mbed_i2c_init(PinName sda, PinName scl)
+{
+    i2c_init(&mbed_i2c_object, sda, scl);
+    i2c_frequency(&mbed_i2c_object, 100000);
+}
+
+int mbed_i2c_write(unsigned char slave_addr,
+                     unsigned char reg_addr,
+                     unsigned char length,
+                     unsigned char const *data)
+{
+    int i;
+    slave_addr = slave_addr << 1;
+    i2c_start(&mbed_i2c_object);
+    i2c_byte_write(&mbed_i2c_object, slave_addr);
+    i2c_byte_write(&mbed_i2c_object, reg_addr);
+    for (i = 0; i < length; i++) {
+        i2c_byte_write(&mbed_i2c_object, data[i]);
+    }
+    i2c_stop(&mbed_i2c_object);
+    return 0;
+}
+
+int mbed_i2c_read(unsigned char slave_addr,
+                    unsigned char reg_addr,
+                    unsigned char length,
+                    unsigned char *data)
+{
+    slave_addr = slave_addr << 1;
+    i2c_write(&mbed_i2c_object, slave_addr, &reg_addr, 1, 0);
+    return (i2c_read(&mbed_i2c_object, slave_addr, data, length, 1) == length) ? 0 : 1;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed_i2c.h	Wed Jun 10 12:56:48 2015 +0000
@@ -0,0 +1,78 @@
+
+/******************************************************************************
+ * $Id: mbed_i2c.h $
+ *****************************************************************************/
+/**
+ *  @defgroup MBED_System_Layer MBED System Layer
+ *  @brief  MBED System Layer APIs.
+ *          To interface with any platform, eMPL needs access to various
+ *          system layer functions.
+ *
+ *  @{
+ *      @file       mbed_i2c.h
+ *      @brief      Serial communication functions needed by eMPL to
+ *                  communicate to the MPU devices.
+ *      @details    This driver assumes that eMPL is with a sub-master clock set
+ *                  to 20MHz. The following MBEDs are supported:
+ */
+#ifndef _MBED_I2C_H_
+#define _MBED_I2C_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "i2c_api.h"
+
+void mbed_i2c_init(PinName sda, PinName scl);
+
+/**
+ *  @brief	Set up the I2C port and configure the MBED as the master.
+ *  @return	0 if successful.
+ */
+int mbed_i2c_enable(void);
+/**
+ *  @brief  Disable I2C communication.
+ *  This function will disable the I2C hardware and should be called prior to
+ *  entering low-power mode.
+ *  @return 0 if successful.
+ */
+int mbed_i2c_disable(void);
+/**
+ *  @brief      Write to a device register.
+ *
+ *  @param[in]  slave_addr  Slave address of device.
+ *  @param[in]  reg_addr	Slave register to be written to.
+ *  @param[in]  length      Number of bytes to write.
+ *  @param[out] data        Data to be written to register.
+ *
+ *  @return     0 if successful.
+ */
+int mbed_i2c_write(unsigned char slave_addr,
+                     unsigned char reg_addr,
+                     unsigned char length,
+                     unsigned char const *data);
+/**
+ *  @brief      Read from a device.
+ *
+ *  @param[in]  slave_addr  Slave address of device.
+ *  @param[in]  reg_addr	Slave register to be read from.
+ *  @param[in]  length      Number of bytes to read.
+ *  @param[out] data        Data from register.
+ *
+ *  @return     0 if successful.
+ */
+int mbed_i2c_read(unsigned char slave_addr,
+                    unsigned char reg_addr,
+                    unsigned char length,
+                    unsigned char *data);
+                    
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* _MBED_I2C_H_ */
+
+/**
+ * @}
+ */