File content as of revision 18:67520755e27e:

/*******************************************************************************
 * 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.
 
 */


#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;
    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;
}


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();
    
   //char* topic = "sen/g-sensor";
    char topic[128]={0};   
          
        
    MQTT::Client<MQTTEthernet, Countdown> client = MQTT::Client<MQTTEthernet, Countdown>(ipstack);
    MQTT::Message message;
    
    char* hostname = MQTT_BROKER_IP;
    int port = 1883;
    printf("Connecting to %s:%d\n", hostname, port);
    int rc = ipstack.connect(hostname, port);
    if (rc != 0)
        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= 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.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;
     

    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;
    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);
        
    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);
        
        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);

        wait(SampelTime); 
                  
        memset(buf,0,100);        
    }

    
    if ((rc = client.disconnect()) != 0)
       printf("rc from disconnect was %d\n", rc);
    
#endif
    
    ipstack.disconnect();
    
    //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