Diff: main.cpp

Revision:

17:31ed13e8a394

Parent:

16:28d062c5522b

--- a/main.cpp	Mon Oct 06 11:42:25 2014 +0000
+++ b/main.cpp	Wed Aug 12 02:39:32 2015 +0000
@@ -25,109 +25,461 @@
  */
 
 
-#include "C12832.h"
-C12832 lcd(p5, p7, p6, p8, p11);
-
 #include "MQTTEthernet.h"
 #include "MQTTClient.h"
+#include "FXOS8700Q.h"
+#include "HTTPClient.h"
+#include "WISEAgent.h"
 
 int arrivedcount = 0;
 
+#define FXOS8700 1
+
+// FXOS8700
+//FXOS8700Q acc( A4, A5, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C address for Freescale Multi Axis shield
+//FXOS8700Q mag( A4, A5, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C address for Freescale Multi Axis shield
+FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
+FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
+
+
+#define SampelTime 1 /// 1 sec
+#define SampleCount 30
+#define MQTT_BROKER_IP "172.22.12.206"
+//#define MQTT_BROKER_IP "192.168.1.13"
+#define MQTT_CLIENT_ID "Eric"
+#define QUARK_THREAD 0.2
+#define SMALL_VAR 1
+
+float X_LOGs[SampleCount];
+float Y_LOGs[SampleCount];
+float Z_LOGs[SampleCount];
+
+float stdDevX = 0.0;
+float stdDevY = 0.0;
+float stdDevZ = 0.0;
+
+float g_coeff_X = 0.0;
+float g_coeff_Y = 0.0;
+float g_coeff_Z = 0.0;
+
+float g_G_Value = 0.0;
+
+float meanX = 0.0;
+float meanY = 0.0;
+float meanZ = 0.0;
+
+int   log_index = 0;
+
+float X_BASE = 0;
+float Y_BASE = 0;
+float Z_BASE = 0;
+
+
+#ifdef FXOS8700
+Serial pc(USBTX, USBRX);
+
+MotionSensorDataUnits mag_data;
+MotionSensorDataUnits acc_data;
+
+MotionSensorDataCounts mag_raw;
+MotionSensorDataCounts acc_raw;
+#endif
+
+float standard_deviation(float data[], int n, float *mean );
+void CalaulateXYZStatisticValue();
+
+void Get_G_SensorValue(float *praX, float *praY, float *praZ )
+{
+#ifdef FXOS8700    
+    acc.getAxis(acc_data);      
+    *praX = acc_data.x;
+    *praY = acc_data.y;
+    *praZ = acc_data.z;
+#endif                        
+}
+
 
 void messageArrived(MQTT::MessageData& md)
 {
     MQTT::Message &message = md.message;
-    lcd.cls();
-    lcd.locate(0,3);
     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;
-    lcd.puts((char*)message.payload);
+}
+
+
+void SaveLogRingBuf( float faX, float faY, float fzZ )
+{
+    X_LOGs[log_index] = faX;
+    Y_LOGs[log_index] = faY;
+    Z_LOGs[log_index] = fzZ;
+    log_index++;
+    if( log_index >= SampleCount )
+        log_index = 0;
+    
+}
+
+
+
+void CorrectGSensor()
+{
+    float faX=0,faXt=0, faY=0, faYt=0, faZ=0, faZt=0; 
+
+    int count = 0;          
+    
+    printf("Start Correct G-Sensor \n");
+    while(1) {   
+        count++;
+        
+        Get_G_SensorValue(&faX,&faY,&faZ);
+        SaveLogRingBuf(faX, faY, faZ);
+        
+        faXt += faX; 
+        faYt += faY; 
+        faZt += faZ; 
+        if(count >= SampleCount ) {
+            X_BASE = faXt / count;
+            Y_BASE = faYt / count;
+            Z_BASE = faZt / count;
+            printf("Stop to Correct G-Sensor Base X=%1.4f Y= %1.4f Z=%1.4f\n", X_BASE, Y_BASE, Z_BASE);
+            break;
+        }
+        //wait(SampelTime); 
+        wait(0.1); 
+    }          
+    CalaulateXYZStatisticValue();
+}
+
+void CalaulateXYZStatisticValue()
+{
+    stdDevX = standard_deviation(X_LOGs,SampleCount,&meanX);
+    g_coeff_X =   stdDevX / meanX;
+    
+    stdDevY = standard_deviation(Y_LOGs,SampleCount,&meanY);
+    g_coeff_Y =   stdDevY / meanY;
+    
+    stdDevZ = standard_deviation(Z_LOGs,SampleCount,&meanZ);
+    g_coeff_Z =   stdDevZ / meanZ;
+    
+    printf(" X St=%1.4f Cof=%1.4f\n Y St=%1.4f Cof=%1.4f\n Z St=%1.4f Cof=%1.4f\n",stdDevX,g_coeff_X,stdDevY,g_coeff_Y,stdDevZ,g_coeff_Z);
+}
+
+
+void CheckCalibration()
+{
+    int small_coeff = 0;
+    
+    if( ( g_coeff_X < 1.0 && g_coeff_X > -1.0) && ( g_coeff_Y < 1.0 && g_coeff_Y > -1.0) && (g_coeff_Z < 1.0 && g_coeff_Z > -1.0) )
+            small_coeff = 1;
+
+    
+    if( g_G_Value > 0.2 ) {
+        if( small_coeff == 1 ) { 
+            printf("Device's bais be changed in Correct Mode\n");
+            CorrectGSensor();
+        }
+    }
 }
 
 
+void ResetLogBuf()
+{
+    int i = 0;
+    
+    for(i=0; i<SampleCount; i++) {
+        X_LOGs[i]=0.0;
+        Y_LOGs[i]=0.0;
+        Z_LOGs[i]=0.0;
+    }    
+}
+
+void ShowCalibrationValue()
+{
+    float faX=0,faX2=0, faY=0, faY2=0, faZ=0, faZ2=0; 
+    float g1, g2;
+    int count = 0;
+    while(1) {     
+        Get_G_SensorValue(&faX,&faY,&faZ);
+
+        count++;
+        faZ2 = faZ - Z_BASE;
+        faX2 = faX - X_BASE;
+        faY2 = faY - Y_BASE;
+        g1 = faX2 * faX2 + faY2 * faY2 + faZ2 * faZ2;
+        g2 = sqrt(g1);
+        printf(" %1.4f %1.4f %1.4f  %1.4f\n", faX2, faY2, faZ2, g2 );    
+        wait(SampelTime);  
+        if( count >= SampleCount )
+         break;
+    }
+}
+
+
+float standard_deviation(float data[], int n, float *Mean )
+{
+    float mean=0.0;
+    float sum_deviation=0.0;
+    int i;
+    for(i=0; i<n;++i)
+    {
+        mean+=data[i];
+    }
+    mean=mean/n;
+    *Mean = mean;
+    for(i=0; i<n;++i)
+    sum_deviation+=(data[i]-mean)*(data[i]-mean);
+    return sqrt(sum_deviation/n);           
+}
+
+
+char *RegistJson = "{\"susiCommData\":{\"devID\":\"%s\",\"hostname\":\"mbed\",\"sn\":\"%s\",\"mac\":\"%s\",\"version\":\"3.1.0.440\",\"type\":\"SenHub\",\"product\":\"mbed\",\"manufacture\":\"Adv\",\"status\":%d,\"commCmd\":1,\"requestID\":21,\"agentID\":\"%s\",\"handlerName\":\"general\",\"sendTS\":%d}}\n";
+                                
+char *OSInfoJson = "{\"susiCommData\":{\"osInfo\":{\"cagentVersion\":\"3.1.0.440\",\"cagentType\":\"IoTGW\",\"osVersion\":\"\",\"biosVersion\":\"1\",\"platformName\":\"\",\"processorName\":\"\",\"osArch\":\"RTOS\",\"totalPhysMemKB\":101240,\"macs\":\"14:DA:E9:96:BE:05\",\"IP\":\"%s\"},\"commCmd\":116,\"requestID\":109,\"agentID\":\"%s\",\"handlerName\":\"general\",\"sendTS\":%d}}\n";
+
+#if 1
 int main(int argc, char* argv[])
 {   
+    float faX=0,faX2=0, faY=0, faY2=0, faZ=0, faZ2=0;
+    acc.enable();
+    float g1=0;
+    g_G_Value = 0.0;
+    char *mac = MQTT_CLIENT_ID;
+    char buf[340];
+    
+    
     MQTTEthernet ipstack = MQTTEthernet();
-    float version = 0.47;
-    char* topic = "mbed-sample";
     
-    lcd.printf("Version is %f\n", version);
-    printf("Version is %f\n", version);
-              
+   //char* topic = "sen/g-sensor";
+    char topic[128]={0};   
+          
+        
     MQTT::Client<MQTTEthernet, Countdown> client = MQTT::Client<MQTTEthernet, Countdown>(ipstack);
+    MQTT::Message message;
     
-    char* hostname = "m2m.eclipse.org";
+    char* hostname = MQTT_BROKER_IP;
     int port = 1883;
-    lcd.printf("Connecting to %s:%d\n", hostname, port);
+    printf("Connecting to %s:%d\n", hostname, port);
     int rc = ipstack.connect(hostname, port);
     if (rc != 0)
-        lcd.printf("rc from TCP connect is %d\n", rc);
+        printf("rc from TCP connect is %d\n", rc);
+    else
+        printf("TCP connect %s OK\n", hostname);
+ 
  
+    //snprintf(buf,sizeof(topic),RegistJson,DEV_UNID, DEV_UNID, DEV_UNID, 0, DEV_UNID, 1436160081000);
+        
+    printf("%s\n",buf);
+    
     MQTTPacket_connectData data = MQTTPacket_connectData_initializer;       
-    data.MQTTVersion = 3;
-    data.clientID.cstring = "mbed-sample";
-    data.username.cstring = "testuser";
-    data.password.cstring = "testpassword";
-    if ((rc = client.connect(data)) != 0)
-        lcd.printf("rc from MQTT connect is %d\n", rc);
+    data.MQTTVersion = 3;   
+    data.clientID.cstring= DEV_UNID;//mac;
+    //data.username.cstring = "ral";
+    //data.password.cstring = "123";
+    // willmessage
+    //data.willFlag = '1';
+    //data.will.topicName.cstring = DEF_WILLMSG_TOPIC;
+    //data.will.message.cstring = buf;
     
-    if ((rc = client.subscribe(topic, MQTT::QOS1, messageArrived)) != 0)
-        lcd.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\n", version);
+       
+    if ((rc = client.connect(data)) != 0) {
+       printf("rc from MQTT connect is %d\n", rc);
+       return rc;
+    }
+    
+    memset(topic, 0, sizeof(topic));
+    memset(buf, 0, sizeof(buf));     
+     
+    snprintf(topic,sizeof(topic),WA_PUB_CONNECT_TOPIC,DEV_UNID);
+    snprintf(buf,sizeof(buf),RegistJson,DEV_UNID, DEV_UNID, DEV_UNID, 1, DEV_UNID, 1436160081020);
+    
     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 < 1)
-        client.yield(100);
-        
-    // QoS 1
-    sprintf(buf, "Hello World!  QoS 1 message from app version %f\n", version);
-    message.qos = MQTT::QOS1;
+     
+
+    printf("len=%d\n",message.payloadlen);       
+    if( rc = client.publish(topic, message) != 0 ) {
+       printf("rc from MQTT publish topic=%s rc= %d\n", topic, rc);
+       return rc;        
+    }else {
+        printf("rc topic2 ok\n");
+    }
+    wait(SampelTime);
+    
+    memset(topic, 0, sizeof(topic));
+    memset(buf, 0, sizeof(buf)); 
+    
+    EthernetInterface eth = ipstack.getEth();
+    
+    snprintf(topic,sizeof(topic),WA_PUB_ACTION_TOPIC,DEV_UNID);
+    snprintf(buf,sizeof(buf),OSInfoJson,eth.getIPAddress(), DEV_UNID, 1436160081030);   
+    
     message.payloadlen = strlen(buf)+1;
-    rc = client.publish(topic, message);
-    while (arrivedcount < 2)
-        client.yield(100);
+    printf("len=%d\n",message.payloadlen);              
+    if( rc = client.publish(topic, message) != 0 ) {
+       printf("rc from MQTT publish topic=%s rc= %d\n", topic, rc);
+       return rc;        
+    }else {
+        printf("rc topic3 ok\n");
+    }     
+    //Init(ipstack);
+    
+    
+    //if( WISEAgentConnect( eth.getIPAddress(), DEV_UNID)!= 0 ) { //eth.getMACAddress());
+//        printf("Connect to WISECloud Fail\n");
+//    }else
+  //      printf("Connected to WISECloud =%s\n",MQTT_BROKER_IP);
         
-    // QoS 2
-    sprintf(buf, "Hello World!  QoS 2 message from app version %f\n", version);
-    message.qos = MQTT::QOS2;
-    message.payloadlen = strlen(buf)+1;
-    rc = client.publish(topic, message);
-    while (arrivedcount < 3)
-        client.yield(100);
+    while(1){
+         wait(SampelTime); 
+         printf("111\n");
+    }    
+#if 0     
+    MQTTPacket_connectData data = MQTTPacket_connectData_initializer;       
+    data.MQTTVersion = 3;   
+    //mac = eth.getMACAddress();
+    data.clientID.cstring= DEV_UNID;//mac;
+    printf("\nMAC =%s IP=%s\n", data.clientID.cstring, eth.getIPAddress() );
+    //data.username.cstring = "testuser";
+    //data.password.cstring = "testpassword";
+    if ((rc = client.connect(data)) != 0)
+       printf("rc from MQTT connect is %d\n", rc);
+    
+    //if ((rc = client.subscribe(topic, MQTT::QOS1, messageArrived)) != 0)
+      //  printf("rc from MQTT subscribe is %d\n", rc);
+
+    MQTT::Message message;
+    char buf[100];
+    while(1) {      
+        Get_G_SensorValue(&faX,&faY,&faZ);
         
-    // n * QoS 2
-    for (int i = 1; i <= 10; ++i)
-    {
-        sprintf(buf, "Hello World!  QoS 2 message number %d from app version %f\n", i, version);
-        message.qos = MQTT::QOS2;
+        SaveLogRingBuf(faX,faY,faZ);
+        CalaulateXYZStatisticValue();
+        
+        faZ2 = faZ - Z_BASE;
+        faX2 = faX - X_BASE;
+        faY2 = faY - Y_BASE;
+        g1 = faX2 * faX2 + faY2 * faY2 + faZ2 * faZ2;
+        g_G_Value = sqrt(g1);
+        //printf(" %1.4f %1.4f %1.4f  %1.4f\n", faX2, faY2, faZ2, g_G_Value );   
+    
+        sprintf(buf, "%s %1.4f %1.4f %1.4f %1.4f\n", mac, faX2, faY2, faZ2, g_G_Value );
+        
+        CheckCalibration();
+        
+        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 < i + 3)
-            client.yield(100);
+
+        wait(SampelTime); 
+                  
+        memset(buf,0,100);        
     }
-    
-    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);
+       printf("rc from disconnect was %d\n", rc);
+    
+#endif
     
     ipstack.disconnect();
     
-    lcd.cls();
-    lcd.locate(0,3);
-    lcd.printf("Version %.2f: finish %d msgs\n", version, arrivedcount);
-    printf("Finishing with %d messages received\n", arrivedcount);
+    //printf("Version %.2f: finish %d msgs\n", version, arrivedcount);
+    //printf("Finishing with %d messages received\n", arrivedcount);
     
     return 0;
 }
+#else // HTTP
+
+EthernetInterface eth;
+HTTPClient http;
+char str[512];
+char Instr[512];
+
+void MCB_HTTPPOST(const char *url, const char *data )
+{
+    sprintf(str, "%s", data);
+    HTTPText outText(str);
+    HTTPText inText(Instr, 512);
+
+    int ret = http.post(url, outText, &inText);    
+    if (!ret)
+    {
+      printf("Executed PUT successfully - read %d characters\n", strlen(Instr));
+      printf("Result: %s\n", str);
+    }
+    else
+    {
+      printf("Error - ret = %d - HTTP return code = %d\n", ret, http.getHTTPResponseCode());
+    }    
+}
+
+int main(int argc, char* argv[])
+{   
+    float faX=0,faX2=0, faY=0, faY2=0, faZ=0, faZ2=0;
+    acc.enable();
+    float g1=0;
+    g_G_Value = 0.0;
+    char buf[100];    
+    
+    ResetLogBuf();
+    
+    CorrectGSensor();    
+    
+    
+    eth.init(); //Use DHCP
+    
+
+    eth.connect();
+              
+
+    while(1) {      
+        Get_G_SensorValue(&faX,&faY,&faZ);
+        
+        SaveLogRingBuf(faX,faY,faZ);
+        CalaulateXYZStatisticValue();
+        
+        faZ2 = faZ - Z_BASE;
+        faX2 = faX - X_BASE;
+        faY2 = faY - Y_BASE;
+        g1 = faX2 * faX2 + faY2 * faY2 + faZ2 * faZ2;
+        g_G_Value = sqrt(g1);
+        //printf(" %1.4f %1.4f %1.4f  %1.4f\n", faX2, faY2, faZ2, g_G_Value );   
+    
+        //sprintf(buf, "%s %1.4f %1.4f %1.4f %1.4f\n", mac, faX, faY, faZ, g_G_Value );
+        sprintf(buf,"x,,%1.4f",faX2);
+        MCB_HTTPPOST("http://api.mediatek.com/mcs/v2/devices/DtIA7o7q/datapoints.csv",buf);
+        memset(buf,0,100);
+        
+        
+        sprintf(buf,"y,,%1.4f",faY2);
+        MCB_HTTPPOST("http://api.mediatek.com/mcs/v2/devices/DtIA7o7q/datapoints.csv",buf);
+        
+        sprintf(buf,"z,,%1.4f",faZ2);
+        MCB_HTTPPOST("http://api.mediatek.com/mcs/v2/devices/DtIA7o7q/datapoints.csv",buf);                        
+        
+        sprintf(buf,"g,,%1.4f",g_G_Value);
+        MCB_HTTPPOST("http://api.mediatek.com/mcs/v2/devices/DtIA7o7q/datapoints.csv",buf);
+                
+        CheckCalibration();
+        
+        // HTTPClient Send
+
+        wait(SampelTime); 
+                  
+        memset(buf,0,100);        
+    }
+
+    eth.disconnect();  
+    
+    
+    return 0;
+}
+#endif
+
+