Revision 21:f456b1a487e0, committed 2015-11-16

Comitter:

tatulea

Date:

Mon Nov 16 19:39:26 2015 +0000

Parent:

20:0404c7f31c69

Commit message:

Initial revision of freescale FRDM K64F demo code; Publish accelerometer & magnetometer data to devicehub; Publish state of left & right buttons; Control 2 relays connected to GPIO pins, control the state of the RGB led & control LED via PWM

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FXOS8700Q.lib	Mon Nov 16 19:39:26 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/Freescale/code/FXOS8700Q/#aee7dea904e2

--- a/Linux-example/LinuxIPStack.h	Sat Aug 01 20:50:27 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,119 +0,0 @@
-#if !defined(LINUX_IPSTACK_H)
-#define LINUX_IPSTACK_H
-
-class IPStack 
-{
-public:    
-    IPStack()
-    {
-
-    }
-    
-    int Socket_error(const char* aString)
-    {
-
-        if (errno != EINTR && errno != EAGAIN && errno != EINPROGRESS && errno != EWOULDBLOCK)
-        {
-            if (strcmp(aString, "shutdown") != 0 || (errno != ENOTCONN && errno != ECONNRESET))
-                printf("Socket error %s in %s for socket %d\n", strerror(errno), aString, mysock);
-        }
-        return errno;
-    }
-
-    int connect(const char* hostname, int port)
-    {
-        int type = SOCK_STREAM;
-        struct sockaddr_in address;
-        int rc = -1;
-        sa_family_t family = AF_INET;
-        struct addrinfo *result = NULL;
-        struct addrinfo hints = {0, AF_UNSPEC, SOCK_STREAM, IPPROTO_TCP, 0, NULL, NULL, NULL};
-
-        if ((rc = getaddrinfo(hostname, NULL, &hints, &result)) == 0)
-        {
-            struct addrinfo* res = result;
-
-            /* prefer ip4 addresses */
-            while (res)
-            {
-                if (res->ai_family == AF_INET)
-                {
-                    result = res;
-                    break;
-                }
-                res = res->ai_next;
-            }
-
-            if (result->ai_family == AF_INET)
-            {
-                address.sin_port = htons(port);
-                address.sin_family = family = AF_INET;
-                address.sin_addr = ((struct sockaddr_in*)(result->ai_addr))->sin_addr;
-            }
-            else
-                rc = -1;
-
-            freeaddrinfo(result);
-        }
-
-        if (rc == 0)
-        {
-            mysock = socket(family, type, 0);
-            if (mysock != -1)
-            {
-                int opt = 1;
-
-                //if (setsockopt(mysock, SOL_SOCKET, SO_NOSIGPIPE, (void*)&opt, sizeof(opt)) != 0)
-                //  printf("Could not set SO_NOSIGPIPE for socket %d", mysock);
-                
-                rc = ::connect(mysock, (struct sockaddr*)&address, sizeof(address));
-            }
-        }
-
-        return rc;
-    }
-
-    int read(char* buffer, int len, int timeout_ms)
-    {
-        struct timeval interval = {timeout_ms / 1000, (timeout_ms % 1000) * 1000};
-        if (interval.tv_sec < 0 || (interval.tv_sec == 0 && interval.tv_usec <= 0))
-        {
-            interval.tv_sec = 0;
-            interval.tv_usec = 100;
-        }
-
-        setsockopt(mysock, SOL_SOCKET, SO_RCVTIMEO, (char *)&interval, sizeof(struct timeval));
-
-        //printf("reading %d bytes\n", len);
-        int rc = ::recv(mysock, buffer, (size_t)len, 0);
-        if (rc == -1)
-            Socket_error("read");
-        //printf("read %d bytes\n", rc);
-        return rc;
-    }
-    
-    int write(char* buffer, int len, int timeout)
-    {
-        struct timeval tv;
-
-        tv.tv_sec = 0;  /* 30 Secs Timeout */
-        tv.tv_usec = timeout * 1000;  // Not init'ing this can cause strange errors
-
-        setsockopt(mysock, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv,sizeof(struct timeval));
-        int rc = ::write(mysock, buffer, len);
-        //printf("write rc %d\n", rc);
-        return rc;
-    }
-
-    int disconnect()
-    {
-        return ::close(mysock);
-    }
-    
-private:
-
-    int mysock; 
-    
-};
-
-#endif
\ No newline at end of file

--- a/Linux-example/LinuxMQTT.h	Sat Aug 01 20:50:27 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,64 +0,0 @@
-#if !defined(LINUXMQTT_H)
-#define LINUXMQTT_H
-
-class Countdown
-{
-public:
-    Countdown()
-    { 
-    
-    }
-
-    Countdown(int ms)
-    { 
-        countdown_ms(ms);
-    }
-    
-
-    bool expired()
-    {
-        struct timeval now, res;
-        gettimeofday(&now, NULL);
-        timersub(&end_time, &now, &res);        
-        //printf("left %d ms\n", (res.tv_sec < 0) ? 0 : res.tv_sec * 1000 + res.tv_usec / 1000);
-        //if (res.tv_sec > 0 || res.tv_usec > 0)
-        //  printf("expired %d %d\n", res.tv_sec, res.tv_usec);
-        return res.tv_sec < 0 || (res.tv_sec == 0 && res.tv_usec <= 0);
-    }
-    
-
-    void countdown_ms(int ms)  
-    {
-        struct timeval now;
-        gettimeofday(&now, NULL);
-        struct timeval interval = {ms / 1000, (ms % 1000) * 1000};
-        //printf("interval %d %d\n", interval.tv_sec, interval.tv_usec);
-        timeradd(&now, &interval, &end_time);
-    }
-
-    
-    void countdown(int seconds)
-    {
-        struct timeval now;
-        gettimeofday(&now, NULL);
-        struct timeval interval = {seconds, 0};
-        timeradd(&now, &interval, &end_time);
-    }
-
-    
-    int left_ms()
-    {
-        struct timeval now, res;
-        gettimeofday(&now, NULL);
-        timersub(&end_time, &now, &res);
-        //printf("left %d ms\n", (res.tv_sec < 0) ? 0 : res.tv_sec * 1000 + res.tv_usec / 1000);
-        return (res.tv_sec < 0) ? 0 : res.tv_sec * 1000 + res.tv_usec / 1000;
-    }
-    
-private:
-
-    struct timeval end_time;
-};
-
-
-#endif
\ No newline at end of file

--- a/Linux-example/linux-main.cpp	Sat Aug 01 20:50:27 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,149 +0,0 @@
-/*******************************************************************************
- * Copyright (c) 2014 IBM Corp.
- *
- * All rights reserved. This program and the accompanying materials
- * are made available under the terms of the Eclipse Public License v1.0
- * and Eclipse Distribution License v1.0 which accompany this distribution.
- *
- * The Eclipse Public License is available at
- *    http://www.eclipse.org/legal/epl-v10.html
- * and the Eclipse Distribution License is available at
- *   http://www.eclipse.org/org/documents/edl-v10.php.
- *
- * Contributors:
- *    Ian Craggs - initial API and implementation and/or initial documentation
- *******************************************************************************/
-  
- /**
-  This is a sample program to illustrate the use of the MQTT Client library
-  on Linux.  The Client class requires two classes which mediate
-  access to system interfaces for networking and timing.  As long as these two
-  classes provide the required public programming interfaces, it does not matter
-  what facilities they use underneath. In this program, they use the Linux
-  system libraries.
- 
- */
-
-#if defined(LINUX)
-
-#include "LinuxMQTT.h"
-#include "LinuxIPStack.h"
-#include "MQTTClient.h"
-
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <sys/param.h>
-#include <sys/time.h>
-#include <sys/select.h>
-#include <netinet/in.h>
-#include <netinet/tcp.h>
-#include <arpa/inet.h>
-#include <netdb.h>
-#include <stdio.h>
-#include <unistd.h>
-#include <errno.h>
-#include <fcntl.h>
-
-#include <stdlib.h>
-#include <string.h>
-#include <signal.h>
-
-#define DEFAULT_STACK_SIZE -1
-
-
-int arrivedcount = 0;
-
-void messageArrived(MQTT::Message* message)
-{
-	printf("Message %d arrived: qos %d, retained %d, dup %d, packetid %d\n", 
-		++arrivedcount, message->qos, message->retained, message->dup, message->id);
-    printf("Payload %.*s\n", message->payloadlen, (char*)message->payload);
-}
-
-
-int connect(MQTT::Client<IPStack, Countdown>::connectionLostInfo* info)
-{
-    const char* hostname = "localhost"; //"m2m.eclipse.org";
-    int port = 1883;
-    printf("Connecting to %s:%d\n", hostname, port);
-    int rc = info->network->connect(hostname, port);
-	if (rc != 0)
-	    printf("rc from TCP connect is %d\n", rc);
- 
-    MQTTPacket_connectData data = MQTTPacket_connectData_initializer;       
-    data.MQTTVersion = 3;
-    data.clientID.cstring = (char*)"mbed-icraggs";
-    rc = info->client->connect(&data);
-	if (rc != 0)
-	    printf("rc from MQTT connect is %d\n", rc);
-    
-    return rc;
-}
-
-
-int main(int argc, char* argv[])
-{   
-    IPStack ipstack = IPStack();
-    float version = 0.3;
-    const char* topic = "mbed-sample";
-    
-    printf("Version is %f\n", version);
-              
-    MQTT::Client<IPStack, Countdown> client = MQTT::Client<IPStack, Countdown>(ipstack);
-    
-    client.setConnectionLostHandler(connect);
-
-    MQTT::Client<IPStack, Countdown>::connectionLostInfo info = {&client, &ipstack};
-    int rc = connect(&info);
-    
-    rc = client.subscribe(topic, MQTT::QOS2, messageArrived);   
-    if (rc != 0)
-        printf("rc from MQTT subscribe is %d\n", rc);
-
-    MQTT::Message message;
-
-    // QoS 0
-    char buf[100];
-    sprintf(buf, "Hello World!  QoS 0 message from app version %f", version);
-    message.qos = MQTT::QOS0;
-    message.retained = false;
-    message.dup = false;
-    message.payload = (void*)buf;
-    message.payloadlen = strlen(buf)+1;
-    rc = client.publish(topic, &message);
-    while (arrivedcount == 0)
-        client.yield(100);
-        
-    // QoS 1
-	printf("Now QoS 1\n");
-    sprintf(buf, "Hello World!  QoS 1 message from app version %f", version);
-    message.qos = MQTT::QOS1;
-    message.payloadlen = strlen(buf)+1;
-    rc = client.publish(topic, &message);
-    while (arrivedcount == 1)
-        client.yield(100);
-        
-    // QoS 2
-    sprintf(buf, "Hello World!  QoS 2 message from app version %f", version);
-    message.qos = MQTT::QOS2;
-    message.payloadlen = strlen(buf)+1;
-    rc = client.publish(topic, &message);
-    while (arrivedcount == 2)
-        client.yield(100);
-    
-    rc = client.unsubscribe(topic);
-    if (rc != 0)
-        printf("rc from unsubscribe was %d\n", rc);
-    
-    rc = client.disconnect();
-    if (rc != 0)
-        printf("rc from disconnect was %d\n", rc);
-    
-    ipstack.disconnect();
-    
-    printf("Finishing with %d messages received\n", arrivedcount);
-    
-    return 0;
-}
-
-#endif

--- a/MQTTS.lib	Sat Aug 01 20:50:27 2015 +0000
+++ b/MQTTS.lib	Mon Nov 16 19:39:26 2015 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/wolfSSL/code/MQTTS/#d8968fcc21b8
+https://developer.mbed.org/users/tatulea/code/MQTTS/#8a72071f259c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MbedJSONValue.lib	Mon Nov 16 19:39:26 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/samux/code/MbedJSONValue/#10a99cdf7846

--- a/NTPClient.lib	Sat Aug 01 20:50:27 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/donatien/code/NTPClient/#881559865a93

--- a/SDFileSystem.lib	Sat Aug 01 20:50:27 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/mbed_official/code/SDFileSystem/#7b35d1709458

--- a/getline.cpp	Sat Aug 01 20:50:27 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,43 +0,0 @@
-
-#include <mbed.h>
-
-int getline(char *prompt, char *buff, int size)
-{
-    int sz ;
-    
-    printf("%s", prompt) ;
-    for(sz = 0 ;  (sz < size) && ((*buff = getchar()) != '\r'); sz++, buff++) {
-        putchar(*buff) ;
-        if(*buff == '\\') {
-            if(++sz >= size)break ;
-            *buff = getchar() ;
-            putchar(*buff) ;
-            switch(*buff) {
-                case 'n' :
-                    *buff = '\n' ;
-                    break ;
-                case 'r' :
-                    *buff = '\r' ;
-                    break ;
-                case 't' :
-                    *buff = '\t' ;
-                    break ;
-                case '\\':
-                    *buff = '\\' ;
-                    break ;
-                default:
-                    buff[1] = buff[0] ;
-                    buff[0] = '\\' ;
-                    buff++ ;
-            }
-        } else if(*buff == '\b') {
-            if(sz >= 2) {
-                buff-=2 ;
-                sz-=2;
-            }
-        }
-    } ;
-    putchar('\n') ;
-    *buff = '\0' ;
-    return sz ;
-}
\ No newline at end of file

--- a/getline.h	Sat Aug 01 20:50:27 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-int getline(char *prompt, char *buff, int size) ;
\ No newline at end of file

--- a/main.cpp	Sat Aug 01 20:50:27 2015 +0000
+++ b/main.cpp	Mon Nov 16 19:39:26 2015 +0000
@@ -1,227 +1,312 @@
-/*******************************************************************************
- * Copyright (c) 2014 IBM Corp.
- *
- * All rights reserved. This program and the accompanying materials
- * are made available under the terms of the Eclipse Public License v1.0
- * and Eclipse Distribution License v1.0 which accompany this distribution.
- *
- * The Eclipse Public License is available at
- *    http://www.eclipse.org/legal/epl-v10.html
- * and the Eclipse Distribution License is available at
- *   http://www.eclipse.org/org/documents/edl-v10.php.
- *
- * Contributors:
- *    Ian Craggs - initial API and implementation and/or initial documentation
- *******************************************************************************/
-
-/**
- This is a sample program to illustrate the use of the MQTT Client library
- on the mbed platform.  The Client class requires two classes which mediate
- access to system interfaces for networking and timing.  As long as these two
- classes provide the required public programming interfaces, it does not matter
- what facilities they use underneath. In this program, they use the mbed
- system libraries.
-
-*/
-#define MQTTCLIENT_QOS2 1
+#include "mbed.h"
 #include "MQTTEthernet.h"
 #include "MQTTClient.h"
+#include "MbedJSONValue.h"
+#include <string>
+#include "rtos.h"
+#include "FXOS8700Q.h"
 
-#include "NTPClient.h"
-#include "getline.h"
-#include "SDFileSystem.h"
-SDFileSystem sdCard(PTE3, PTE1, PTE2, PTE4, "sd"); /* pins for FRDM-K64F */
-const char* certFile = "/sd/mqtts-cert.pem";
+I2C i2c(PTE25, PTE24);
+
+FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1);
+FXOS8700QMagnetometer mag(i2c, FXOS8700CQ_SLAVE_ADDR1);
+motion_data_units_t acc_data, mag_data;
+
+DigitalOut red(LED_RED);
+DigitalOut green(LED_GREEN);
+DigitalOut blue(LED_BLUE);
+DigitalOut r1(PTB2);
+DigitalOut r2(PTB3);
+PwmOut led(PTD1);
+InterruptIn sw_left(SW3);
+InterruptIn sw_right(SW2);
+
+#define API_KEY         "7553e4b7-0930-4b72-b392-890ef25bde0a"
+#define PROJECT_ID      "5882"
+#define DEVICE_UUID     "ef51c7bf-105a-408d-ac63-c403d15f7668"              
+
+// Publishing topics
+char* pubTopic_acc_x      = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/sensor/Accelerometer_X/data";
+char* pubTopic_acc_y      = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/sensor/Accelerometer_Y/data";
+char* pubTopic_acc_z      = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/sensor/Accelerometer_Z/data";
+
+char* pubTopic_mag_x      = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/sensor/Magnetometer_X/data";
+char* pubTopic_mag_y      = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/sensor/Magnetometer_Y/data";
+char* pubTopic_mag_z      = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/sensor/Magnetometer_Z/data";
+
+char* pubTopic_sw_left      = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/sensor/Button_left/data";
+char* pubTopic_sw_right      = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/sensor/Button_right/data";
 
-int arrivedcount = 0;
+// Subscription topics
+char* subTopic_RLED    = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/actuator/Red/state";
+char* subTopic_GLED    = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/actuator/Green/state";
+char* subTopic_BLED    = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/actuator/Blue/state";
+
+char* subTopic_Relay1 = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/actuator/Relay_1/state";
+char* subTopic_Relay2 = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/actuator/Relay_2/state";
+
+char* subTopic_LED     = "/a/"API_KEY"/p/"PROJECT_ID"/d/"DEVICE_UUID"/actuator/LED/state";
+
+int send = 0;
+int pressed_left = 0;
+int pressed_right = 0;
+int state_left = 0;
+int state_right = 0;
 
-void messageArrived(MQTT::MessageData& md)
+MbedJSONValue JSON;
+std::string value;
+
+void readSensors_thread(void const *args) {
+    while(true) {
+        Thread::wait(5000);
+        
+        // unit based results
+        acc.getAxis(acc_data);
+        mag.getAxis(mag_data);
+        //printf("ACC: X=%1.4ff Y=%1.4ff Z=%1.4ff \t MAG: X=%4.1ff Y=%4.1ff Z=%4.1ff\r\n", acc_data.x, acc_data.y, acc_data.z, mag_data.x, mag_data.y, mag_data.z);
+        
+        send = 1;
+    }
+}
+void messageArrived_RLED(MQTT::MessageData& md)
 {
-    MQTT::Message &message = md.message;
-    printf("Message arrived: qos %d, retained %d, dup %d, packetid %d\n", message.qos, message.retained, message.dup, message.id);
-    printf("Payload %.*s\n", message.payloadlen, (char*)message.payload);
-    ++arrivedcount;
-    puts((char*)message.payload);
-}
-
-static char * removeNL(char *str)
-{
-    for(int i=strlen(str)-1; (str[i]=='\n')||(str[i]=='\r'); i--)str[i]='\0' ;
-    return str ;
+    char *message = (char *)malloc(md.message.payloadlen);
+    int state = 0;
+    MbedJSONValue dhMessage;
+    memcpy(message, md.message.payload, md.message.payloadlen);
+    // printf("message: %s\n", message);
+    
+    parse(dhMessage, message);
+    state = atoi(dhMessage["state"].get<std::string>().c_str());
+    // printf("state: %d\n", state);
+    
+    red = !state;
+    
+    free(message);
 }
 
-static bool getConnectInfo(char **hn, char **un, char **pwd, char **cID,
-                           char **tc, int *pt, char **ms, char **cert)
+void messageArrived_GLED(MQTT::MessageData& md)
 {
-    static char hostname[100] ;
-    static char username[20] ;
-    static char password[20] ;
-    static char clientID[20] ;
-    static char topic[50] ;
-    static char port_s[10] ;
-    static int  port ;
-    static char msg[100] ;
-    static char certName[30] ;
-    static char fullName[sizeof(certName)+10] ;
-
-    *hn = hostname ;
-    *un = username ;
-    *pwd= password ;
-    *cID= clientID ;
-    *tc = topic ;
-    *ms = msg ;
-    *cert=fullName ;
+    char *message = (char *)malloc(md.message.payloadlen);
+    int state = 0;
+    MbedJSONValue dhMessage;
+    memcpy(message, md.message.payload, md.message.payloadlen);
+    // printf("message: %s\n", message);
+    
+    parse(dhMessage, message);
+    state = atoi(dhMessage["state"].get<std::string>().c_str());
+    // printf("state: %d\n", state);
+    
+    green = !state;
+    
+    free(message);
+}
 
-    FILE *fp = fopen("/sd/connectInfo.txt", "r");
-    if (fp == NULL) {
-        printf("Cannot open \"connectInfo.txt\"\n") ;
-        return false ;
-    }
-    fgets(hostname, sizeof(hostname), fp) ;
-    fgets(username , sizeof(username), fp);
-    fgets(password, sizeof(password), fp) ;
-    fgets(clientID, sizeof(clientID), fp) ;
-    fgets(topic, sizeof(topic), fp) ;
-    getline("Port(1883/8883): ", port_s, sizeof(port_s)) ;
-    getline("Message: ", msg, sizeof(msg)) ;
-    removeNL(hostname) ;
-    removeNL(username) ;    
-    removeNL(password) ;
-    removeNL(clientID) ;
-    removeNL(topic) ;
-    port = atoi(removeNL(port_s)) ;
-    *pt = port ;
-    printf("Connecting to %s:%d, %s\n", hostname, port, port>8000? "MQTT-TLS":"MQTT" );
-    if(port>8000) {
-        getline("Cert File name: ", certName, sizeof(certName)) ;
-        removeNL(certName) ;
-        if(*certName != '\0') {
-            sprintf(fullName, "/sd/%s", certName) ;
-            FILE *fp = fopen(fullName, "r");
-            if (fp != NULL) {
-                NTPClient ntp; /* set Realtime clock for cert verification */
-                if(ntp.setTime("ntp.jst.mfeed.ad.jp") != 0) {
-                    printf("NTP Error\n") ;
-                    return false ;
-                }
-                printf("Verify Server: %s\n", certName) ;
-            } else {
-                printf("ERROR: file not found\n") ;
-                return false ;
-            }
-        } else {
-            fullName[0] = '\0' ;
-            printf("No verify Server\n") ;
-        }
-    }
-    return true ;
+void messageArrived_BLED(MQTT::MessageData& md)
+{
+    char *message = (char *)malloc(md.message.payloadlen);
+    int state = 0;
+    MbedJSONValue dhMessage;
+    memcpy(message, md.message.payload, md.message.payloadlen);
+    // printf("message: %s\n", message);
+    
+    parse(dhMessage, message);
+    state = atoi(dhMessage["state"].get<std::string>().c_str());
+    // printf("state: %d\n", state);
+    
+    blue = !state;
+    
+    free(message);
+}
+
+void messageArrived_LED(MQTT::MessageData& md)
+{
+    char *message = (char *)malloc(md.message.payloadlen);
+    int state = 0;
+    MbedJSONValue dhMessage;
+    memcpy(message, md.message.payload, md.message.payloadlen);
+    // printf("message: %s\n", message);
+    
+    parse(dhMessage, message);
+    state = atoi(dhMessage["state"].get<std::string>().c_str());
+    // printf("state: %d\n", state);
+    
+    led = state/100.0f;
+    
+    free(message);
 }
 
-void mqtt_main(void const *av)
+void messageArrived_Relay1(MQTT::MessageData& md)
+{
+    char *message = (char *)malloc(md.message.payloadlen);
+    int state = 0;
+    MbedJSONValue dhMessage;
+    memcpy(message, md.message.payload, md.message.payloadlen);
+    // printf("message: %s\n", message);
+    
+    parse(dhMessage, message);
+    state = atoi(dhMessage["state"].get<std::string>().c_str());
+    // printf("state: %d\n", state);
+    
+    r1 = !state;
+    
+    free(message);
+}
+
+void messageArrived_Relay2(MQTT::MessageData& md)
 {
+    char *message = (char *)malloc(md.message.payloadlen);
+    int state = 0;
+    MbedJSONValue dhMessage;
+    memcpy(message, md.message.payload, md.message.payloadlen);
+     printf("message: %s\n", message);
+    
+    parse(dhMessage, message);
+    state = atoi(dhMessage["state"].get<std::string>().c_str());
+     printf("state: %d\n", state);
+    
+    r2 = !state;
+    
+    free(message);
+}
+
+void sw_left_release(void)
+{
+    state_left = !state_left;
+    pressed_left = 1;
+}
+
+void sw_right_release(void)
+{
+     state_right = !state_right;
+     pressed_right = 1;   
+}
+
+int main()
+{
+    red = 1;
+    green = 1;
+    blue = 1;
+    r1 = 1;
+    r2 = 1;
+    led = 0;
+    
+    printf("Hello Freedom :)\n");
     MQTTEthernet ipstack = MQTTEthernet();
-    float version = 0.47;
-
-    printf("Version is %f\n", version);
-    printf("Version is %f\n", version);
 
     MQTT::Client<MQTTEthernet, Countdown> client = MQTT::Client<MQTTEthernet, Countdown>(ipstack);
     MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
     data.MQTTVersion = 3;
 
-    /* get connect info */
-    char *hostname ;
-    char *username ;
-    char *password ;
-    char *clientID ;
-    char *topic ;
-    int  port ;
-    char *msg ;
-    char *fullName ;
+    /* set connect info */
+    char *hostname  = "mqtt.devicehub.net";
+    char *username = NULL;
+    char *password = NULL;
+    int r = rand() % 100;
+    char buf_client[10] = {0};
+    sprintf(buf_client, "K64F-%d", r);
+    char *clientID = buf_client;
+    int  port = 1883;
 
-    if(!getConnectInfo(&hostname, &username, &password, &clientID, &topic, &port, &msg, &fullName))
-        return ;
     data.username.cstring = username ;
     data.password.cstring = password ;
     data.clientID.cstring = clientID ;
-        
-    int rc = ipstack.connect(hostname, port, port>8000 ? fullName : NULL);
-    if (rc != 0)
-        printf("rc from TCP connect is %d\n", rc);
-
-    if ((rc = client.connect(data)) != 0)
-        printf("rc from MQTT connect is %d\n", rc);
-    printf("MQTT connected\n") ;
-    if ((rc = client.subscribe(topic, MQTT::QOS0, messageArrived)) != 0)
-        printf("rc from MQTT subscribe is %d\n", rc);
-    printf("Subscribed\n") ;
+    
+    // Set publish info
     MQTT::Message message;
-
-// QoS 0
-    char buf[sizeof(message)+50] ;
-    sprintf(buf, "\"%s\", QoS 0 message from app version %f\n", msg, version);
     message.qos = MQTT::QOS0;
-    message.retained = false;
+    message.retained = true;
     message.dup = false;
-    message.payload = (void*)buf;
-    message.payloadlen = strlen(buf)+1;
-    rc = client.publish(topic, message);
-
-    printf("QoS0: Published to %s\n\t%s\n", topic, message.payload) ;
-    while (arrivedcount < 1)
-        client.yield(100);
-
-// QoS 1
-    sprintf(buf, "\"%s\", QoS 1 message from app version %f\n", msg, version);
-    message.qos = MQTT::QOS1;
-
-    message.payloadlen = strlen(buf)+1;
-    rc = client.publish(topic, message);
-    printf("QoS1: Published to %s\n\t%s\n", topic, message.payload) ;
-    while (arrivedcount < 2)
-        client.yield(100);
+    
+    int rc = ipstack.connect(hostname, port, NULL);
+    if (rc != 0){
+        printf("rc from TCP connect is %d\r\n", rc);
+        NVIC_SystemReset();
+    }
 
-#if 1 /* QoS 2 not tested */
-// QoS 2
-    sprintf(buf, "\"%s\",  QoS 2 message from app version %f\n", msg, version);
-    message.qos = MQTT::QOS2;
-    message.payloadlen = strlen(buf)+1;
-    rc = client.publish(topic, message);
-    printf("QoS2: Published to %s\n\t%s\n", topic, message.payload) ;
-    while (arrivedcount < 3)
-        client.yield(100);
-
-// n * QoS 2
-    for (int i = 1; i <= 10; ++i) {
-        sprintf(buf, "\"%s\", QoS 2 message number %d from app version %f\n",msg,  i, version);
-        message.qos = MQTT::QOS2;
-        message.payloadlen = strlen(buf)+1;
-        rc = client.publish(topic, message);
-        printf("QoS2[%d]: Published to %s\n\t%s\n", i, topic, message.payload) ;
-        while (arrivedcount < i + 3)
-            client.yield(100);
+    if ((rc = client.connect(data)) != 0){
+        printf("rc from MQTT connect is %d\r\n", rc);
+        NVIC_SystemReset();
     }
-#endif
-
-    printf("unsubscribing\n") ;
-    if ((rc = client.unsubscribe(topic)) != 0)
-        printf("rc from unsubscribe was %d\n", rc);
-
-    if ((rc = client.disconnect()) != 0)
-        printf("rc from disconnect was %d\n", rc);
-
-    ipstack.disconnect();
-    printf("Version %.2f: finish %d msgs\n", version, arrivedcount);
-    printf("Finishing with %d messages received\n", arrivedcount);
-
-    return ;
+    printf("MQTT connected\r\n");
+    
+    client.subscribe(subTopic_RLED, MQTT::QOS0, messageArrived_RLED);
+    client.subscribe(subTopic_GLED, MQTT::QOS0, messageArrived_GLED);
+    client.subscribe(subTopic_BLED, MQTT::QOS0, messageArrived_BLED);
+    client.subscribe(subTopic_LED, MQTT::QOS0, messageArrived_LED);
+    client.subscribe(subTopic_Relay1, MQTT::QOS0, messageArrived_Relay1);
+    client.subscribe(subTopic_Relay2, MQTT::QOS0, messageArrived_Relay2);
+    
+    printf("Subscribed to MQTT topics\r\n");
+    
+    acc.enable();
+    mag.enable();
+    
+    sw_left.rise(&sw_left_release);
+    sw_right.rise(&sw_right_release);
+    Thread thread(readSensors_thread);
+    
+    while(true) {
+        client.yield(100);
+        
+        if(send){
+            JSON["value"] = acc_data.x;
+            value = JSON.serialize();
+            message.payload = (void*)value.c_str();
+            message.payloadlen = strlen(value.c_str());
+            client.publish(pubTopic_acc_x, message);
+            
+            JSON["value"] = acc_data.y;
+            value = JSON.serialize();
+            message.payload = (void*)value.c_str();
+            message.payloadlen = strlen(value.c_str());
+            client.publish(pubTopic_acc_y, message);
+            
+            JSON["value"] = acc_data.z;
+            value = JSON.serialize();
+            message.payload = (void*)value.c_str();
+            message.payloadlen = strlen(value.c_str());
+            client.publish(pubTopic_acc_z, message);
+            
+            JSON["value"] = mag_data.x;
+            value = JSON.serialize();
+            message.payload = (void*)value.c_str();
+            message.payloadlen = strlen(value.c_str());
+            client.publish(pubTopic_mag_x, message);
+            
+            JSON["value"] = mag_data.y;
+            value = JSON.serialize();
+            message.payload = (void*)value.c_str();
+            message.payloadlen = strlen(value.c_str());
+            client.publish(pubTopic_mag_y, message);
+            
+            JSON["value"] = mag_data.z;
+            value = JSON.serialize();
+            message.payload = (void*)value.c_str();
+            message.payloadlen = strlen(value.c_str());
+            client.publish(pubTopic_mag_z, message);
+            
+            send = 0;  
+        }
+        
+        if(pressed_left){
+            JSON["value"] = state_left;
+            value = JSON.serialize();
+            message.payload = (void*)value.c_str();
+            message.payloadlen = strlen(value.c_str());
+            client.publish(pubTopic_sw_left, message);
+            
+            pressed_left = 0;
+        }
+        
+        if(pressed_right){
+            JSON["value"] = state_right;
+            value = JSON.serialize();
+            message.payload = (void*)value.c_str();
+            message.payloadlen = strlen(value.c_str());
+            client.publish(pubTopic_sw_right, message);
+            
+            pressed_right = 0;
+        }
+    };
 }
-
-main()
-{
-#define STACK_SIZE 24000
-    Thread t(mqtt_main, NULL, osPriorityNormal, STACK_SIZE);
-    while (true) {
-        Thread::wait(1000);
-    }
-}