Diff: main.cpp

Revision:

2:9d566ea4f4fa

Parent:

1:6bd72ba7fa56

Child:

5:973da26ee266

--- a/main.cpp	Wed Feb 18 08:15:33 2015 +0000
+++ b/main.cpp	Fri Mar 13 09:32:26 2015 +0000
@@ -4,7 +4,7 @@
 
 #define MMA8451_I2C_ADDRESS (0x1C<<1)
 
-#define NEED_CONSOLE_OUTPUT 0 /* Set this if you need debug messages on the console;
+#define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
                                * it will have an impact on code-size and power consumption. */
 
 #if NEED_CONSOLE_OUTPUT
@@ -14,8 +14,6 @@
 #define DEBUG(...) /* nothing */
 #endif /* #if NEED_CONSOLE_OUTPUT */
 
-
-
 PinName const SDA = p22;
 PinName const SCL = p20;
 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
@@ -41,7 +39,6 @@
 
 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)    // Mod
 {
-//    advertisingStateLed = 1;
     DEBUG("Disconnected handle %u, reason %u\n", handle, reason);
     DEBUG("Restarting the advertising process\n\r");
     ble.startAdvertising();
@@ -49,7 +46,6 @@
 
 void onConnectionCallback(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)   //Mod
 {
-//    advertisingStateLed = 0;
     DEBUG("connected. Got handle %u\r\n", handle);
     connectionParams.slaveLatency = 1;
     if (ble.updateConnectionParams(handle, &connectionParams) != BLE_ERROR_NONE) {
@@ -59,17 +55,13 @@
 void GetAccelterData( void );
 void periodicCallback(void)
 {
-//  DEBUG("periodicCallback\n\r");
-//    oneSecondLed = !oneSecondLed; /* Do blinky on LED1 while we're waiting for BLE events */
     GetAccelterData();
     triggerSensorPolling = true;
-
 }
 
 
 void initBLE( void )
 {
-
     ticker.attach_us(periodicCallback, 10000); //10msecの周期タイマー
 
     ble.init();
@@ -82,76 +74,54 @@
     /* setup advertising */
     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t*)uuid16_list, sizeof(uuid16_list));
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_THERMOMETER);
+
     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */     //Advertisingの周期時間 0.625msec単位
     ble.startAdvertising();
-//    advertisingStateLed = 1;
+
     DEBUG("Start Advertising\n");
 
     ble.addService(htmService);
 
-
 }
 //-------------------------------------
 
-float ox=0;
-float oy=0;
-float oz=0;
-float dx=0;
-float dy=0;
-float dz=0;
+float oxyz = 0;
+float dxyz = 0;
+
 bool f=1;
 float velocity = 0;
 float maxvelocity = 0;
 void GetAccelterData( void )
 {
-//        DEBUG("aaaaa\r\n");
-    float x = acc.getAccX() * 9.8;
-    float y = acc.getAccY() * 9.8;
-    float z = acc.getAccZ() * 9.8;
+    float x = acc.getAccX();
+    float y = acc.getAccY();
+    float z = acc.getAccZ();
+
+    float xyz = sqrt(x*x+y*y+z*z);
 
     if(f==0){
-        dx+=(x-ox);
-        dy+=(y-oy);
-        dz+=(z-oz);
+        dxyz += xyz - oxyz;
     }
     f=0;
-//    float v = sqrt(dx*dx+dy*dy+dz*dz) * 3600/1000/100;
-    float v = abs(dx+dy+dz) * 3600/1000/10/3;
+    velocity = dxyz*9.8*3600/1000/10;
+    oxyz = xyz;
 
-//    pc.printf("X: %7.2f, Y: %7.2f, Z: %7.2f   v:%7.2f\r\n", x-ox, y-oy, z-oz, v);
-//    pc.printf("X: %7.2f, Y: %7.2f, Z: %7.2f   v:%7.2f\r\n", dx, dy, dz, v);
-//    pc.printf("X: %7.2f, Y: %7.2f, Z: %7.2f   v:%7.2f\r\n", x, y, z, v);
+    pc.printf("X: %7.2f, Y: %7.2f, Z: %7.2f   v:%7.2f\r\n", x, y, z, velocity);
 
-    velocity = v;
     if (maxvelocity < velocity){
         maxvelocity = velocity;
     }
-
-//    if(v>1.0f){
-        DEBUG("%7.2f km/h  Max:%7.2f km/h (?)\r\n", v ,maxvelocity);
-//    }
-
-
-    ox=x;
-    oy=y;
-    oz=z;
 }
 void InitAccelterData( void )
 {
     DEBUG("MMA8451 ID: %d\r\n", acc.getWhoAmI());
-//    Ticker ticker;
-//    ticker2.attach_us(GetAccelterData, 10000); //10000usec = 10msecの周期タイマー
-//    ticker2.attach(GetAccelterData, 1); //10000usec = 10msecの周期タイマー
-}
+ }
 void onButton( void )
 {
     DEBUG("#####\r\n");
-    dx=0;
-    dy=0;
-    dz=0;
+    dxyz=0;
     maxvelocity = 0;
 }
 
@@ -161,7 +131,6 @@
 #if NEED_CONSOLE_OUTPUT
     pc.baud(115200);
 #endif
-    DEBUG("tttttt\r\n");
  
     button.rise(&onButton);
  
@@ -172,11 +141,9 @@
         if (triggerSensorPolling) {
             triggerSensorPolling = false;
             updateServiceValues();
-//            DEBUG("----------\r\n");
         } else {
             ble.waitForEvent();
         }
-
     }
 }
 
@@ -190,7 +157,6 @@
 
 void updateServiceValues(void)
 {
-   
       uint32_t temp_ieee11073 = quick_ieee11073_from_float(maxvelocity);
       memcpy(thermTempPayload+1, &temp_ieee11073, 4);
       ble.updateCharacteristicValue(tempChar.getValueAttribute().getHandle(), thermTempPayload, sizeof(thermTempPayload));  //Mod