Diff: main.cpp

Revision:

10:f226b7907cf2

Parent:

9:aca7ff8a1945

Child:

11:5a3dcafaffbb

--- a/main.cpp	Mon Mar 02 13:28:18 2015 +0000
+++ b/main.cpp	Sat Mar 14 15:22:51 2015 +0000
@@ -24,8 +24,9 @@
 
 BLEDevice   ble;
 
+//MPU6050 mpu(p4, p5);
 MPU6050 mpu(I2C_SDA0, I2C_SCL0);
-  
+
 static const char DEVICENAME[] = "BLE-Nano";
 //static volatile bool  triggerSensorPolling = false;
 
@@ -80,7 +81,6 @@
 
 
 void updateValue(void){
-
     float   acData[3];
     float   gyData[3];
     float   tempData = 0.0f;
@@ -120,8 +120,8 @@
                         *(float*)&gyroPayload[sizeof(float)*2]);
         
     pc.printf("Temp: %.3lf\r\n", *(float*)&tempPayload[sizeof(float)*0]);
-    pc.printf("Battery: %d\r\n", batt);
     */
+
     timer.stop();
     t = timer.read_ms();
     memcpy(accelPayload+sizeof(float)*7, &t, sizeof(t));
@@ -166,8 +166,11 @@
     //}
 }
 
-void onDataWritten(const GattCharacteristicWriteCBParams *params){
+void onDataWrittenCallback(const GattCharacteristicWriteCBParams *params){
 
+    char acceleroRange = 0xFF;
+    char gyroRange = 0xFF;
+    
     if (params->charHandle == writeChar.getValueAttribute().getHandle()) {
         uint16_t len = params->len;
         
@@ -175,24 +178,23 @@
             uint8_t controller[3] = {0};
 
             ble.readCharacteristicValue(writeChar.getValueAttribute().getHandle(), writePayload, &len);
+            memcpy(controller, writePayload, sizeof(controller));
             
-            memcpy(controller+sizeof(uint8_t)*0, &writePayload[0], sizeof(writePayload[0]));
-            memcpy(controller+sizeof(uint8_t)*1, &writePayload[1], sizeof(writePayload[1]));
-            memcpy(controller+sizeof(uint8_t)*2, &writePayload[2], sizeof(writePayload[2]));
-            
-            if(controller[0] == 1){
-                mpu.initialize();
-            }
+            //pc.printf("write: %u, %u, %u\r\n", controller[0],controller[1],controller[2]);   
             
             switch(controller[1]){
                 case 0:
-                    mpu.setAcceleroRange(0);
+                    acceleroRange = MPU6050_ACCELERO_RANGE_2G;
                     break;
                 case 1:
-                    mpu.setAcceleroRange(1);
+                    acceleroRange = MPU6050_ACCELERO_RANGE_4G;
                     break;
                 case 2:
-                    mpu.setAcceleroRange(2);
+                    acceleroRange = MPU6050_ACCELERO_RANGE_8G;
+                    break;
+                case 3:
+                    acceleroRange = MPU6050_ACCELERO_RANGE_16G;
+                    
                     break;
                 default:
                     break;  
@@ -200,25 +202,22 @@
             
             switch(controller[2]){
                 case 0:
-                    mpu.setGyroRange(0);
+                    gyroRange = MPU6050_GYRO_RANGE_250;
                     break;
                 case 1:
-                    mpu.setGyroRange(1);
+                    gyroRange = MPU6050_GYRO_RANGE_250; 
                     break;
                 case 2:
-                    mpu.setGyroRange(2);
+                    gyroRange = MPU6050_GYRO_RANGE_250;
                     break;
                 case 3:
-                    mpu.setGyroRange(3);
+                    gyroRange = MPU6050_GYRO_RANGE_250;
                     break;
                 default:
                     break;
             }
-            if( mpu.testConnection() ){
-                //pc.printf("mpu test:OK\n\r");
-            }else{
-                //pc.printf("mpu test:NG\n\r");
-            }
+            mpu.setAcceleroRange(acceleroRange);
+            mpu.setGyroRange(gyroRange);
         }
     }
 }
@@ -265,7 +264,7 @@
     */
 
     mpu.initialize();
-    mpu.setAcceleroRange(defaultWriteValue); //
+    mpu.setAcceleroRange(defaultWriteValue);
     mpu.setGyroRange(defaultWriteValue);
     
     if( mpu.testConnection() ){
@@ -273,14 +272,14 @@
     }else{
         //pc.printf("mpu test:NG\n\r");
     }
-    
+
     Ticker ticker;
     ticker.attach(periodicCallback, 0.25f); //1sec
     
     ble.init();
     ble.onDisconnection(disconnectionCallback);
     ble.onConnection(onConnectionCallback);
-    ble.onDataWritten(onDataWritten);
+    ble.onDataWritten(onDataWrittenCallback);
     ble.onTimeout(timeoutCallback);
 
     /* setup device name */
@@ -298,7 +297,7 @@
     ble.startAdvertising();
 
     ble.addService(MPU6050Service);
-    
+
     while(true) {
         //if (triggerSensorPolling && ble.getGapState().connected) {
             //triggerSensorPolling = false;