Revision 7:bc915651d90e, committed 2018-11-23

Comitter:

suads

Date:

Fri Nov 23 13:44:27 2018 +0000

Parent:

6:75ed671f455e

Commit message:

seismo

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/MotorService.cpp	Fri Nov 23 13:44:27 2018 +0000
@@ -0,0 +1,11 @@
+#include "MotorService.h"
+
+const uint8_t MOTOR_SERVICE_LONG_UUID[UUID::LENGTH_OF_LONG_UUID]          = {0x92, 0xcb, 0x93, 0x26, 0x21, 0x55, 0x11, 0xe8, 0xb4, 0x67, 0x0e, 0xd5, 0xf8, 0x9f, 0x71, 0x8b};
+
+const uint8_t MOTOR_1_CHARACTERISTIC_LONG_UUID[UUID::LENGTH_OF_LONG_UUID] = {0x92, 0xcb, 0x96, 0x00, 0x21, 0x55, 0x11, 0xe8, 0xb4, 0x67, 0x0e, 0xd5, 0xf8, 0x9f, 0x71, 0x8b};
+
+const uint8_t MOTOR_2_CHARACTERISTIC_LONG_UUID[UUID::LENGTH_OF_LONG_UUID] = {0x92, 0xcb, 0x97, 0x68, 0x21, 0x55, 0x11, 0xe8, 0xb4, 0x67, 0x0e, 0xd5, 0xf8, 0x9f, 0x71, 0x8b};
+
+const uint8_t MOTOR_3_CHARACTERISTIC_LONG_UUID[UUID::LENGTH_OF_LONG_UUID] = {0x92, 0xcb, 0x98, 0x9e, 0x21, 0x55, 0x11, 0xe8, 0xb4, 0x67, 0x0e, 0xd5, 0xf8, 0x9f, 0x71, 0x8b};
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/MotorService.h	Fri Nov 23 13:44:27 2018 +0000
@@ -0,0 +1,52 @@
+#ifndef __BLE_MOTOR_SERVICE_H_
+#define __BLE_MOTOR_SERVICE_H_
+
+#include "ble/BLE.h"
+#include "UUID.h"
+
+extern const uint8_t MOTOR_SERVICE_LONG_UUID[UUID::LENGTH_OF_LONG_UUID];
+extern const uint8_t MOTOR_1_CHARACTERISTIC_LONG_UUID[UUID::LENGTH_OF_LONG_UUID];
+extern const uint8_t MOTOR_2_CHARACTERISTIC_LONG_UUID[UUID::LENGTH_OF_LONG_UUID];
+extern const uint8_t MOTOR_3_CHARACTERISTIC_LONG_UUID[UUID::LENGTH_OF_LONG_UUID];
+
+
+class MotorService {
+public:
+
+    const static uint16_t TEST_CHARACTERISTIC_UUID = 0xA011;
+
+
+    MotorService(BLEDevice &_ble, float initialValueMotor1, float initialValueMotor2, float initialValueMotor3) :
+        ble(_ble), 
+        motor1(MOTOR_1_CHARACTERISTIC_LONG_UUID, &initialValueMotor1), 
+        motor2(MOTOR_2_CHARACTERISTIC_LONG_UUID, &initialValueMotor2), 
+        motor3(MOTOR_3_CHARACTERISTIC_LONG_UUID, &initialValueMotor3)
+    {
+        GattCharacteristic *charTable[] = {&motor1, &motor2, &motor3};
+        GattService         motorService(MOTOR_SERVICE_LONG_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
+        ble.gattServer().addService(motorService);
+    }
+
+    GattAttribute::Handle_t getValueHandleMotor1() const {
+        return motor1.getValueHandle();
+    }
+
+    GattAttribute::Handle_t getValueHandleMotor2() const {
+        return motor2.getValueHandle();
+    }
+
+    GattAttribute::Handle_t getValueHandleMotor3() const {
+        return motor3.getValueHandle();
+    }
+
+    
+private:
+    BLEDevice                         &ble;
+    
+    ReadWriteGattCharacteristic<float>  motor1;
+    ReadWriteGattCharacteristic<float>  motor2;
+    ReadWriteGattCharacteristic<float>  motor3;
+};
+
+#endif /* #ifndef __BLE_MOTOR_SERVICE_H_ */
+

--- a/source/main.cpp	Thu Nov 22 17:50:31 2018 +0000
+++ b/source/main.cpp	Fri Nov 23 13:44:27 2018 +0000
@@ -3,77 +3,237 @@
 #include "MPU6050.h"
 #include <string>
 #include <stdio.h>
+#include <events/mbed_events.h>
+#include "ble/BLE.h"
+#include "ble/Gap.h"
+#include "MotorService.h"
 
+#define N_128_BIT_SERVICE 2
+
+static uint8_t UUID_128_LIST[N_128_BIT_SERVICE*UUID::LENGTH_OF_LONG_UUID];
+static uint8_t uuid128ListPos = 0;
+static uint8_t uuid128ListAddedServices = 0;
+
+static MotorService* motorServicePtr;
+
+DigitalOut indicatorLED(p25, 0); 
+MPU6050 mpu6050;
+Serial pc(USBTX,USBRX);
+
+
+const static char     DEVICE_NAME[] = "Spike";
+// static const uint16_t uuid16_list[] = {GattService::UUID_BATTERY_SERVICE};
+static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);
+
+
+void add128bitServiceToList(const uint8_t * service){
+    uuid128ListAddedServices++;
+    for(int i=UUID::LENGTH_OF_LONG_UUID-1; i>=0; i--){
+        UUID_128_LIST[uuid128ListPos] = service[i];
+        uuid128ListPos++;
+    }
+}
+ 
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params){
+    BLE::Instance().gap().startAdvertising();
+}
+
+void connectionStatusIndicator(void) {
+    BLE &ble = BLE::Instance();
+    if (ble.gap().getState().connected) {
+        indicatorLED = 1;
+        //eventQueue.call(updateSensorValue);
+    } else {
+        indicatorLED = 0;
+    }
+}
+
+void onBleInitError(BLE &ble, ble_error_t error) {
+    /* Initialization error handling should go here */
+}
+
+void onDataWrittenCallback(const GattWriteCallbackParams *params) {
+
+}
+
+void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
+{
+    BLE&        ble   = params->ble;
+    ble_error_t error = params->error;
+ 
+    if (error != BLE_ERROR_NONE) {
+        /* In case of error, forward the error handling to onBleInitError */
+        onBleInitError(ble, error);
+        return;
+    }
+    /* Ensure that it is the default instance of BLE */
+    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
+        return;
+    }
 
 
-DigitalOut led(p25, 1);
-MPU6050 mpu6050;
-Serial pc(USBTX,USBRX);
+    ble.gap().onDisconnection(disconnectionCallback);
+    ble.gattServer().onDataWritten(onDataWrittenCallback);
+
+    /* Setup GATT services */
+    motorServicePtr = new MotorService(ble, 0, 0, 0);
+    add128bitServiceToList(MOTOR_SERVICE_LONG_UUID);
+    /* Setup advertising */
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
+    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    ble.gap().setAdvertisingInterval(1000); /* 1000ms */
+
+    //ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
+                                     (const uint8_t *)UUID_128_LIST, uuid128ListAddedServices*UUID::LENGTH_OF_LONG_UUID);
+    
+    ble.gap().startAdvertising();
+
+    //printMacAddress();
+ }
+ 
+void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
+    BLE &ble = BLE::Instance();
+    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
+}
+
+int i = 0;
+
+void updateAccelValues(){
+ 
+    // mpu6050.readAccelData(accelData);
+    // mpu6050.getAres();
+    // float ax = accelData[0]*aRes  - accelBias[0];
+    // float ay = accelData[1]*aRes  - accelBias[1];
+    // float az = accelData[2]*aRes  - accelBias[2];
+    // float accelValues[3] = {ax,ay,az};
+    mpu6050.readGyroData(gyroData);
+    mpu6050.getGres();
+    float gx = gyroData[0]*gRes - gyroBias[0];
+    float gy = gyroData[1]*gRes - gyroBias[1];
+    float gz = gyroData[2]*gRes - gyroBias[2];
+    float gyroValues[3] = {gx,gy,gz};
+        
+    pc.printf("%f, ",gx);
+    pc.printf("%f, ",gy);
+    pc.printf("%f\r\n",gz);
+
+    BLE &ble = BLE::Instance();
+    if (ble.gap().getState().connected) {
+        // update char vals
+        ble.gattServer().write(
+            motorServicePtr->getValueHandleMotor1(),
+            (const uint8_t*)&gx,
+            4
+        );
+        ble.gattServer().write(
+            motorServicePtr->getValueHandleMotor2(),
+            (const uint8_t*)&gy,
+            4
+        );
+        ble.gattServer().write(
+            motorServicePtr->getValueHandleMotor3(),
+            (const uint8_t*)&gz,
+            4
+        );
+    } 
+}
+
+void blink(){
+    indicatorLED = 1;
+    wait(1);
+    indicatorLED = 0;
+}
 
 int main()
 {
     pc.baud(115200);
     pc.printf("START_1\n");
+
     mpu6050.calibrate(accelBias, gyroBias);
     mpu6050.initialize();
+    blink();
     pc.printf("START_2\n");
     
+
+    eventQueue.call_every(2000, connectionStatusIndicator);
+    eventQueue.call_every(200, updateAccelValues);
+ 
+    BLE &ble = BLE::Instance();
+    ble.onEventsToProcess(scheduleBleEventsProcessing);
+    ble.init(bleInitComplete);
+ 
+    blink();
+    eventQueue.dispatch_forever();
+    return 0;
+   
+}
+
+// int main()
+// {
+//     pc.baud(115200);
+//     pc.printf("START_1\n");
+//     mpu6050.calibrate(accelBias, gyroBias);
+//     mpu6050.initialize();
+//     pc.printf("START_2\n");
+    
    
  
- /////////////////////////////////////////////////////////SERIAL////////////////////////////////////////////
-    while(0){
-        led = 0;
-        wait(1);
-   if(pc.readable()){
-       if(pc.getc()=='.'){
-            pc.putc(pc.getc());
-            led=1;
-          wait(1);
-          }
-            }
+//  /////////////////////////////////////////////////////////SERIAL////////////////////////////////////////////
+//     while(0){
+//         indicatorLED = 0;
+//         wait(1);
+//    if(pc.readable()){
+//        if(pc.getc()=='.'){
+//             pc.putc(pc.getc());
+//             indicatorLED=1;
+//           wait(1);
+//           }
+//             }
   
-        }
+//         }
        
- /////////////////////////////////////////////////////////SERIAL/////////////////////////////////////////////
+//  /////////////////////////////////////////////////////////SERIAL/////////////////////////////////////////////
  
- /////////////////////////////////////////////////////////MPU6050////////////////////////////////////////////
+//  /////////////////////////////////////////////////////////MPU6050////////////////////////////////////////////
  
-    while(0){
-         pc.printf("i am %d\n",mpu6050.getDeviceID());
-         wait(1);
-        }
+//     while(0){
+//          pc.printf("i am %d\n",mpu6050.getDeviceID());
+//          wait(1);
+//         }
        
- /////////////////////////////////////////////////////////MPU6050/////////////////////////////////////////////
+//  /////////////////////////////////////////////////////////MPU6050/////////////////////////////////////////////
  
  
  
- //////////////////////////////////////////////////////////////GET_DATA////////////////////////////////////////////
-    while(1){
+//  //////////////////////////////////////////////////////////////GET_DATA////////////////////////////////////////////
+//     while(1){
  
-    mpu6050.readAccelData(accelData);
-    mpu6050.getAres();
-    ax = accelData[0]*aRes  - accelBias[0];
-    ay = accelData[1]*aRes  - accelBias[1];
-    az = accelData[2]*aRes  - accelBias[2];
-    float accelValues[3] = {ax,ay,az};
-    mpu6050.readGyroData(gyroData);
-    mpu6050.getGres();
-    gx = gyroData[0]*gRes - gyroBias[0];
-    gy = gyroData[1]*gRes - gyroBias[1];
-    gz = gyroData[2]*gRes - gyroBias[2];
-    float gyroValues[3] = {gx,gy,gz};
+//     mpu6050.readAccelData(accelData);
+//     mpu6050.getAres();
+//     ax = accelData[0]*aRes  - accelBias[0];
+//     ay = accelData[1]*aRes  - accelBias[1];
+//     az = accelData[2]*aRes  - accelBias[2];
+//     float accelValues[3] = {ax,ay,az};
+//     mpu6050.readGyroData(gyroData);
+//     mpu6050.getGres();
+//     gx = gyroData[0]*gRes - gyroBias[0];
+//     gy = gyroData[1]*gRes - gyroBias[1];
+//     gz = gyroData[2]*gRes - gyroBias[2];
+//     float gyroValues[3] = {gx,gy,gz};
         
-    if(pc.readable())
-    {
-        if(pc.getc()=='.'){
+//     if(pc.readable())
+//     {
+//         if(pc.getc()=='.'){
             
-            pc.printf("%f, ",gx);
-            pc.printf("%f, ",gy);
-            pc.printf("%f",gz);
+//             pc.printf("%f, ",gx);
+//             pc.printf("%f, ",gy);
+//             pc.printf("%f",gz);
            
-        }
+//         }
 
-    }
+//     }
     
-    }
-}
+//     }
+// }
\ No newline at end of file