Diff: source/main.cpp

Revision:

1:b8bbe971b26d

Parent:

0:e71874215e23

--- a/source/main.cpp	Mon Apr 17 01:56:57 2017 +0000
+++ b/source/main.cpp	Fri Apr 27 09:56:41 2018 +0000
@@ -19,24 +19,49 @@
 #include "ble/BLE.h"
 #include "ble/Gap.h"
 #include "BMA250E.h"
+#include "BMG160.h"
+
+
 /* UART printf */
+#ifdef NRF52
+Serial pc(p20, p24);
+#else
 Serial pc(p5, p4);
+#endif
+
+
+/* LED blink */
+Ticker ledBlinkTicker;
+DigitalOut ledR(p16, 1);
+DigitalOut ledG(p15, 1);
+DigitalOut ledB(p6 , 1);
 
 /* Sensor */
-BMA250E acclerameter(p14, p13, NC, NC);
+uint8_t acceRange = 0x0C;   // 2G(0x03), 4G(0x05), 8G(0x08), 16G(0x0C) 
+uint8_t gyroRange = 0x00;   // 2000deg/s(0x00), 1000deg/s(0x01), 500deg/s(0x02), 250deg/s(0x03), 125deg/s(0x04)
+int16_t acceXYZ[3];
+int16_t gyroXYZ[3];
+uint8_t accePayload[7];
+uint8_t gyroPayload[7];
 
-DigitalOut ledRed(p16, 1);
+/* UART printf */
+#ifdef NRF52
+BMA250E acclerameter(p14, p28, NC, NC, acceRange, 0x0D);
+BMG160 gyro(p14, p28, NC, NC, gyroRange, 0x00);
+#else
+BMA250E acclerameter(p14, p13, NC, NC, acceRange, 0x0D);
+BMG160 gyro(p14, p13, NC, NC, gyroRange, 0x00);
+#endif
+
+static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);
 
 /* UUID, Device name */
 uint16_t sensServUUID = /*0xA000*/0x1811;
 uint16_t acceCharUUID = /*0xA001*/0x2A56;
 uint16_t gyroCharUUID = /*0xA002*/0x2A57;
-static const char     DEVICE_NAME[] = "mbed Motion";
+static const char     DEVICE_NAME[] = "Mt5MtSense03";
 static const uint16_t uuid16_list[] = { /*0xA000*/0x1811 };
 
-uint8_t accePayload[7];
-uint8_t gyroPayload[7];
-
 /* Setup custom characteristics */
 GattCharacteristic  acceChar(   acceCharUUID, accePayload,
                                 sizeof(accePayload), sizeof(accePayload),
@@ -52,21 +77,41 @@
 GattCharacteristic *characteristics[] = {&acceChar, &gyroChar};
 GattService sensServ(sensServUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));
 
-static EventQueue eventQueue(
-    /* event count */ 16 * /* event size */ 32
-);
+
 
-void blinkCallback(void)
+void ledBlinkCallback(void)
 {
-    ledRed = !ledRed; /* Do blinky on LED1 while we're waiting for BLE events */
+    ledR = !ledR;
+    ledG = !ledG;
+    ledB = !ledB;
 }
 
-/**
- * This function is called when the ble initialization process has failled
- */
-void onBleInitError(BLE &ble, ble_error_t error)
+void updateSensorCallback(void)
 {
-    /* Initialization error handling should go here */
+    /* Get sensor data */
+    acclerameter.ReadXYZ(acceXYZ);
+    gyro.ReadXYZ(gyroXYZ);
+//    pc.printf("aXYZ(%6d,%6d,%6d), gXYZ(%6d,%6d,%6d)\r\n", acceXYZ[0], acceXYZ[1], acceXYZ[2], gyroXYZ[0], gyroXYZ[1], gyroXYZ[2]);
+    BLE &ble = BLE::Instance();
+    if (ble.getGapState().connected) {
+        /* Write data to client */
+        accePayload[0] = acceRange;
+        accePayload[1] = (uint8_t)(acceXYZ[0] >> 8);
+        accePayload[2] = (uint8_t)(acceXYZ[0] >> 0);
+        accePayload[3] = (uint8_t)(acceXYZ[1] >> 8);
+        accePayload[4] = (uint8_t)(acceXYZ[1] >> 0);
+        accePayload[5] = (uint8_t)(acceXYZ[2] >> 8);
+        accePayload[6] = (uint8_t)(acceXYZ[2] >> 0);
+        ble.gattServer().write(acceChar.getValueHandle(), accePayload, sizeof(accePayload));
+        gyroPayload[0] = gyroRange;
+        gyroPayload[1] = (uint8_t)(gyroXYZ[0] >> 8);
+        gyroPayload[2] = (uint8_t)(gyroXYZ[0] >> 0);
+        gyroPayload[3] = (uint8_t)(gyroXYZ[1] >> 8);
+        gyroPayload[4] = (uint8_t)(gyroXYZ[1] >> 0);
+        gyroPayload[5] = (uint8_t)(gyroXYZ[2] >> 8);
+        gyroPayload[6] = (uint8_t)(gyroXYZ[2] >> 0);
+        ble.gattServer().write(gyroChar.getValueHandle(), gyroPayload, sizeof(gyroPayload));
+    }
 }
 
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
@@ -74,30 +119,15 @@
     BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising(); // restart advertising
 }
 
-void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
-    BLE &ble = BLE::Instance();
-    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
-}
-
-/**
- * Callback triggered when the ble initialization process has finished
- */
 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
 {
-    BLE&        ble   = params->ble;
+    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;
-    }
-    
     ble.gap().onDisconnection(disconnectionCallback);
 
     /* Setup primary service. */
@@ -111,25 +141,24 @@
     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
     ble.gap().setAdvertisingInterval(500); /* 500ms */
     ble.gap().startAdvertising();
-    
+}
+void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
+    BLE &ble = BLE::Instance();
+    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
 }
-
-void BMA250Ecallback(void) {
-    int16_t xyz[3];
-    
-    acclerameter.ReadXYZ(xyz);
-    pc.printf("%d %d %d \r\n", xyz[0], xyz[1], xyz[2]);
-}
+int main(void)
+{
+    pc.set_flow_control(SerialBase::Disabled);
+    pc.baud(115200);
+    pc.printf("~ Hell World ~\n");
 
-
+    /* LED blink timer */
+    eventQueue.call_every(1000, ledBlinkCallback);
 
-int main()
-{
-    pc.printf("Hi \r\n");
-    
-    eventQueue.call_every(200, blinkCallback);
-    eventQueue.call_every(1000, BMA250Ecallback);
+    /* Update Sensor timer */
+    eventQueue.call_every(200, updateSensorCallback);
 
+    /* Init BLE */
     BLE &ble = BLE::Instance();
     ble.onEventsToProcess(scheduleBleEventsProcessing);
     ble.init(bleInitComplete);