Revision 9:3e967b414bd5, committed 2017-04-30

Comitter:

sycorax

Date:

Sun Apr 30 12:30:24 2017 +0000

Parent:

8:1b030068f28c

Commit message:

Added a PosService for Broadcasting the measured distances

Changed in this revision

--- a/main.cpp	Sun Apr 30 08:41:06 2017 +0000
+++ b/main.cpp	Sun Apr 30 12:30:24 2017 +0000
@@ -1,44 +1,84 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2006-2015 ARM Limited
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
+#include <map>
+#include <string>
+#include "posService.h"
 #include "mbed.h"
 #include "ble/BLE.h"
 
 DigitalOut led1(LED1, 1);
 Ticker     ticker;
+typedef pair<long,int> map_val;
+map<string, map_val > dmap;
+
+         typedef uint8_t  dist_t[3];
+         dist_t poss;
+
+const static char DEVICE_NAME[] = "Locator Mobile";
+static const uint16_t serviceList[] = {
+    0x2601
+};
 
 void periodicCallback(void)
 {
     led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
 }
 
+double calculateAccuracy(int txPower, double rssi) {
+  if (rssi == 0) {
+    return -1.0; // if we cannot determine accuracy, return -1.
+  }
+
+  double ratio = rssi*1.0/txPower;
+  if (ratio < 1.0) {
+    return pow(ratio,10);
+  }
+  else {
+    double accuracy =  (0.89976) * pow(ratio,7.7095) + 0.111;    
+    return accuracy;
+  }
+}  
+
+
 void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) {
 
     if (params->type == 3) {
-        printf("Adv peerAddr: [%02x %02x %02x %02x %02x %02x] rssi %d, ScanResp: %u, AdvType: %u\r\n",
+        /*printf("Adv peerAddr: [%02x %02x %02x %02x %02x %02x] rssi %d, ScanResp: %u, AdvType: %u\r\n",
                params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
                params->rssi, params->isScanResponse, params->type);
-    
+        
         for (unsigned index = 0; index < params->advertisingDataLen; index++) {
             printf("%02x ", params->advertisingData[index]);
         }
+        */
+        char buf[20];
+        sprintf(buf, "%02x%02x%02x%02x%02x%02x",params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0]);
+        printf("ID=%s  ", buf);
+        if (dmap.count(buf) > 0) { //element exists
+            map_val val = dmap[buf];
+            val.first += params->rssi;
+            val.second++;
+            dmap[buf] = val;
+            if (val.second==8)//reset
+                dmap[buf] = map_val((int)val.first/val.second, 1);
+            }
+        else { //new element
+            dmap[buf] = map_val(0,0);
+        }
+ 
+ map_val val = dmap[buf];
+        printf("%ld/%d=%.2f  ", val.first, val.second, (float)val.first/val.second);
+        printf("dist=%.2lf  ", calculateAccuracy(-65,(double)val.first/val.second));
         printf("\r\n");
+        
+        int i = 0;
+        map<string, map_val >::iterator it,end;
+        for (it = dmap.begin(), end = dmap.end();
+                       i<3 && it != end ; ++it, ++i)
+            {
+                map_val val = it->second;
+                poss[i] = val.first;
+            }
     }
 }
-
 /**
  * This function is called when the ble initialization process has failed
  */
@@ -66,18 +106,25 @@
         return;
     }
  
-    ble.gap().setScanParams(500 /* scan interval */, 200 /* scan window */);
+    ble.gap().setScanParams(200 /* scan interval */, 200 /* scan window */);
     ble.gap().startScan(advertisementCallback);
 }
 
 int main(void)
-{
+{   
     ticker.attach(periodicCallback, 1);
 
     BLE &ble = BLE::Instance();
+    PosService posService(ble);
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)serviceList, sizeof(serviceList));
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
+    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED);
+    
     ble.init(bleInitComplete);
 
     while (true) {
         ble.waitForEvent();
+        posService.updatePos(poss);
     }
 }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/posService.h	Sun Apr 30 12:30:24 2017 +0000
@@ -0,0 +1,25 @@
+#include "ble/BLE.h"
+
+class PosService {
+public:
+    typedef uint8_t  dist_t[3];
+    ReadOnlyGattCharacteristic<dist_t> distanceCharacteristic;
+    
+        PosService(BLE& _ble) :
+            ble(_ble),
+            distanceCharacteristic(0x2A59, &pos) 
+        {            
+            GattCharacteristic *charTable[] = { &distanceCharacteristic };
+            GattService posService(0x2601, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
+            ble.gattServer().addService(posService);
+        }
+        
+    void updatePos(dist_t poss) {
+        
+        //ble.gattServer().write(distanceCharacteristic.getValueHandle(), &poss, 1);
+    }
+private:
+    BLE& ble;
+    
+    dist_t pos;
+};
\ No newline at end of file