Make advertisingData creation more generic

This commit prepares for sending manufacturerData and ServiceUuid in the same advertisement (not sure if possible, anyway)

Author: facchinm

src/utility/GAP.cpp

@@ -97,17 +97,17 @@ int GAPClass::advertise()
     BLEUuid uuid(_advertisedServiceUuid);
     int uuidLen = uuid.length();
 
-    advertisingData[3] = 1 + uuidLen;
-    advertisingData[4] = (uuidLen > 2) ? 0x06 : 0x02;
-    memcpy(&advertisingData[5], uuid.data(), uuidLen);
+    advertisingData[advertisingDataLen++] = 1 + uuidLen;
+    advertisingData[advertisingDataLen++] = (uuidLen > 2) ? 0x06 : 0x02;
+    memcpy(&advertisingData[advertisingDataLen], uuid.data(), uuidLen);
 
-    advertisingDataLen += (2 + uuidLen);
+    advertisingDataLen += uuidLen;
   } else if (_manufacturerData && _manufacturerDataLength) {
-    advertisingData[3] = 1 + _manufacturerDataLength;
-    advertisingData[4] = 0xff;
-    memcpy(&advertisingData[5], _manufacturerData, _manufacturerDataLength);
+    advertisingData[advertisingDataLen++] = 1 + _manufacturerDataLength;
+    advertisingData[advertisingDataLen++] = 0xff;
+    memcpy(&advertisingData[advertisingDataLen], _manufacturerData, _manufacturerDataLength);
 
-    advertisingDataLen += (2 + _manufacturerDataLength);
+    advertisingDataLen += _manufacturerDataLength;
   }
 
   if (_serviceData && _serviceDataLength > 0 && advertisingDataLen >= (_serviceDataLength + 4)) {