bug fixes

gateway_protocol/gateway_protocol.c

@@ -12,13 +12,16 @@ void gateway_protocol_packet_encode (
     *packet_length = 0;
 
     packet[*packet_length] = dev_id;
-    packet_length++;
+    (*packet_length)++;
 
     packet[*packet_length] = packet_type;
     (*packet_length)++;
 
+    packet[*packet_length] = payload_length;
+    (*packet_length)++;
+
     memcpy(&packet[*packet_length], payload, payload_length);
-    *packet_length += payload_length;
+    (*packet_length) += payload_length;
 }
 
 uint8_t gateway_protocol_packet_decode (
@@ -37,6 +40,7 @@ uint8_t gateway_protocol_packet_decode (
     *packet_type = (gateway_protocol_packet_type_t) packet[p_len];
     p_len++;
 
+    p_len++;
     *payload_length = packet_length - p_len;
 
     memcpy(payload, &packet[p_len], *payload_length);