better logging, response packet

lib/Udp/V6MiLightUdpServer.cpp

@@ -19,6 +19,10 @@ uint8_t V6MiLightUdpServer::COMMAND_HEADER[] = {
   0x80, 0x00, 0x00, 0x00
 };
 
+uint8_t V6MiLightUdpServer::COMMAND_RESPONSE[] = {
+  0x88, 0x00, 0x00, 0x00, 0x03, 0x00, 0xFF, 0x00
+};
+
 template<typename T, size_t sz>
 size_t size(T(&)[sz]) {
     return sz;
@@ -80,13 +84,12 @@ void V6MiLightUdpServer::sendResponse(uint16_t sessionId, uint8_t* responseBuffe
   if (session == NULL) {
     Serial.print("Tried to send response to untracked session id: ");
     Serial.println(sessionId);
+    return;
   }
   
-  Serial.println("Sending a response!");
-  Serial.print("IP: ");
-  Serial.println(session->ipAddr.toString());
-  Serial.print("Port: ");
-  Serial.println(session->port);
+#ifdef MILIGHT_UDP_DEBUG
+  printf("Sending response to %s:%d\n", session->ipAddr.toString().c_str(), session->port);
+#endif
   
   socket.beginPacket(session->ipAddr, session->port);
   socket.write(responseBuffer, responseSize);
@@ -155,22 +158,17 @@ void V6MiLightUdpServer::handleCommand(
   bool handled = false;
   
   if ((cmdHeader & 0x0800) == 0x0800) {
-    printf("Yup.\n");
     handled = handleV2BulbCommand(group, cmdHeader, cmdArg);
   } else if ((cmdHeader & 0x0700) == 0x0700) {
     handled = handleV1BulbCommand(group, cmdHeader, cmdArg);
   }
   
   if (handled) {
-    uint8_t* responsePacket = this->responseBuffer;
-    uint8_t* packetStart = responseBuffer;
-    
-    responsePacket = writeInt<uint32_t>(0x88000000, responsePacket);
-    responsePacket = writeInt<uint16_t>(0x0300, responsePacket);
-    responsePacket = writeInt<uint8_t>(sequenceNum, responsePacket);
-    responsePacket = writeInt<uint8_t>(0, responsePacket);
+    size_t len = size(COMMAND_RESPONSE);
+    memcpy(responseBuffer, COMMAND_RESPONSE, len);
+    responseBuffer[6] = sequenceNum;
     
-    sendResponse(sessionId, packetStart, responsePacket - packetStart);
+    sendResponse(sessionId, responseBuffer, len);
     
     return;
   }
@@ -190,7 +188,7 @@ void V6MiLightUdpServer::handlePacket(uint8_t* packet, size_t packetSize) {
   if (packetSize == size(START_SESSION_COMMAND) && memcmp(START_SESSION_COMMAND, packet, packetSize) == 0) {
     handleStartSession();
   } else if (packetSize == 22 && memcmp(COMMAND_HEADER, packet, size(COMMAND_HEADER)) == 0) {
-    uint16_t sessionId = (packet[5] << 8) | packet[6];
+    uint16_t sessionId = readInt<uint16_t>(packet+5);
     uint8_t sequenceNum = packet[8];
     uint8_t* cmd = packet+10;
     uint8_t group = packet[19];

lib/Udp/V6MiLightUdpServer.h

@@ -54,6 +54,7 @@ protected:
   static uint8_t START_SESSION_COMMAND[];
   static uint8_t START_SESSION_RESPONSE[];
   static uint8_t COMMAND_HEADER[];
+  static uint8_t COMMAND_RESPONSE[];
   
   Vector<V6Session> sessions;
   uint16_t sessionId;