File content as of revision 55:381a4d2fdebb:

#include "simpleclient.h"
#include "mbed-trace/mbed_trace.h"
#include "mbedtls/entropy_poll.h"
#include "FXOS8700Q.h"
#include "MODSERIAL.h"

#define PI 3.14159265
 
//MODSERIAL gps
MODSERIAL gps(PTD3, PTD2); // (PTC17, PTC16) UART3 not functional on etherenet enabling
I2C i2c(PTE25, PTE24);     // Configured for the FRDM-K64F with onboard sensors
FXOS8700QAccelerometer accel(i2c,FXOS8700CQ_SLAVE_ADDR1);
char cDataBuffer[500];

struct gnss_params
{
     float   latitude;    //Latitude
     float   longitude;   //Longitude
     float   altitude;    //Altitude 
     float   baselineLen; //BaseLine Length
     float   heading;     // Heading
     int     date;
     int     time;
     int     fix_quality; // 0 INVALID, 1 GPS, 2 DIFF   
     int     numsat;     
};


#if MBED_CONF_APP_NETWORK_INTERFACE == WIFI
    #if TARGET_UBLOX_EVK_ODIN_W2
        #include "OdinWiFiInterface.h"
        OdinWiFiInterface wifi;
    #else
        #include "ESP8266Interface.h"
        ESP8266Interface wifi(MBED_CONF_APP_WIFI_TX, MBED_CONF_APP_WIFI_RX);
    #endif
#elif MBED_CONF_APP_NETWORK_INTERFACE == ETHERNET
    #include "EthernetInterface.h"
    EthernetInterface eth;
#elif MBED_CONF_APP_NETWORK_INTERFACE == MESH_LOWPAN_ND
    #define MESH
    #include "NanostackInterface.h"
    LoWPANNDInterface mesh;
#elif MBED_CONF_APP_NETWORK_INTERFACE == MESH_THREAD
    #define MESH
    #include "NanostackInterface.h"
    ThreadInterface mesh;
#endif

#if defined(MESH)
#if MBED_CONF_APP_MESH_RADIO_TYPE == ATMEL
#include "NanostackRfPhyAtmel.h"
NanostackRfPhyAtmel rf_phy(ATMEL_SPI_MOSI, ATMEL_SPI_MISO, ATMEL_SPI_SCLK, ATMEL_SPI_CS,
                           ATMEL_SPI_RST, ATMEL_SPI_SLP, ATMEL_SPI_IRQ, ATMEL_I2C_SDA, ATMEL_I2C_SCL);
#elif MBED_CONF_APP_MESH_RADIO_TYPE == MCR20
#include "NanostackRfPhyMcr20a.h"
NanostackRfPhyMcr20a rf_phy(MCR20A_SPI_MOSI, MCR20A_SPI_MISO, MCR20A_SPI_SCLK, MCR20A_SPI_CS, MCR20A_SPI_RST, MCR20A_SPI_IRQ);
#endif //MBED_CONF_APP_RADIO_TYPE
#endif //MESH

#ifdef MESH
    // Mesh does not have DNS, so must use direct IPV6 address
    #define MBED_SERVER_ADDRESS "coaps://[2607:f0d0:2601:52::20]:5684"
#else
    // This is address to mbed Device Connector, name based
    // assume all other stacks support DNS properly
    #define MBED_SERVER_ADDRESS "coap://leshan.eclipse.org:5683" //"coap://api.connector.mbed.com:5684"
#endif

RawSerial output(USBTX, USBRX);

// Status indication
DigitalOut red_led(LED1);
DigitalOut green_led(LED2);
DigitalOut blue_led(LED3);
//Ticker status_ticker;
void blinky() {
    red_led = !red_led;

}

// These are example resource values for the Device Object
struct MbedClientDevice device = {
    "SwarrosTechPvtLtd",         // Manufacturer
    "ANtennaAlignment",              // Type
    "0.01",                 // ModelNumber
    "007"                   // SerialNumber
};

// Instantiate the class which implements LWM2M Client API (from simpleclient.h)
MbedClient mbed_client(device);


// In case of K64F board , there is button resource available
// to change resource value and unregister
#ifdef TARGET_K64F
// Set up Hardware interrupt button.
InterruptIn obs_button(SW2);
InterruptIn unreg_button(SW3);
#endif

// set up a timer to simulate updating resource,
// there is no functionality to unregister.
Ticker timer;

/*
 * Arguments for running "blink" in it's own thread.
 */
class BlinkArgs {
public:
    BlinkArgs() {
        clear();
    }
    void clear() {
        position = 0;
        blink_pattern.clear();
    }
    uint16_t position;
    std::vector<uint32_t> blink_pattern;
};

/*
 * The Led contains one property (pattern) and a function (blink).
 * When the function blink is executed, the pattern is read, and the LED
 * will blink based on the pattern.
 */
class LedResource {
public:
    LedResource() {
        // create ObjectID with metadata tag of '3201', which is 'digital output'
        led_object = M2MInterfaceFactory::create_object("3201");
        M2MObjectInstance* led_inst = led_object->create_object_instance();

        // 5853 = Multi-state output
        M2MResource* pattern_res = led_inst->create_dynamic_resource("5853", "Pattern",
            M2MResourceInstance::STRING, false);
        // read and write
        pattern_res->set_operation(M2MBase::GET_PUT_ALLOWED);
        // set initial pattern (toggle every 200ms. 7 toggles in total)
        pattern_res->set_value((const uint8_t*)"500:500:500:500:500:500:500", 27);

        // there's not really an execute LWM2M ID that matches... hmm...
        M2MResource* led_res = led_inst->create_dynamic_resource("5850", "Blink",
            M2MResourceInstance::OPAQUE, false);
        // we allow executing a function here...
        led_res->set_operation(M2MBase::POST_ALLOWED);
        // when a POST comes in, we want to execute the led_execute_callback
        led_res->set_execute_function(execute_callback(this, &LedResource::blink));
        // Completion of execute function can take a time, that's why delayed response is used
        led_res->set_delayed_response(true);
        blink_args = new BlinkArgs();
    }

    ~LedResource() {
        delete blink_args;
    }

    M2MObject* get_object() {
        return led_object;
    }

    void blink(void *argument) {
        // read the value of 'Pattern'
        //status_ticker.detach();
        green_led = 1;

        M2MObjectInstance* inst = led_object->object_instance();
        M2MResource* res = inst->resource("5853");
        // Clear previous blink data
        blink_args->clear();

        // values in mbed Client are all buffers, and we need a vector of int's
        uint8_t* buffIn = NULL;
        uint32_t sizeIn;
        res->get_value(buffIn, sizeIn);

        // turn the buffer into a string, and initialize a vector<int> on the heap
        std::string s((char*)buffIn, sizeIn);
        free(buffIn);
        output.printf("led_execute_callback pattern=%s\r\n", s.c_str());

        // our pattern is something like 500:200:500, so parse that
        std::size_t found = s.find_first_of(":");
        while (found!=std::string::npos) {
            blink_args->blink_pattern.push_back(atoi((const char*)s.substr(0,found).c_str()));
            s = s.substr(found+1);
            found=s.find_first_of(":");
            if(found == std::string::npos) {
                blink_args->blink_pattern.push_back(atoi((const char*)s.c_str()));
            }
        }
        // check if POST contains payload
        if (argument) {
            M2MResource::M2MExecuteParameter* param = (M2MResource::M2MExecuteParameter*)argument;
            String object_name = param->get_argument_object_name();
            uint16_t object_instance_id = param->get_argument_object_instance_id();
            String resource_name = param->get_argument_resource_name();
            int payload_length = param->get_argument_value_length();
            uint8_t* payload = param->get_argument_value();
            output.printf("Resource: %s/%d/%s executed\r\n", object_name.c_str(), object_instance_id, resource_name.c_str());
            output.printf("Payload: %.*s\r\n", payload_length, payload);
        }
        // do_blink is called with the vector, and starting at -1
        blinky_thread.start(this, &LedResource::do_blink);
    }

private:
    M2MObject* led_object;
    Thread blinky_thread;
    BlinkArgs *blink_args;
    void do_blink() {
        for (;;) {
            // blink the LED
            green_led = !green_led;
            // up the position, if we reached the end of the vector
            if (blink_args->position >= blink_args->blink_pattern.size()) {
                // send delayed response after blink is done
                M2MObjectInstance* inst = led_object->object_instance();
                M2MResource* led_res = inst->resource("5850");
                led_res->send_delayed_post_response();
                red_led = 1;
                //status_ticker.attach_us(blinky, 250000);
                return;
            }
            // Wait requested time, then continue prosessing the blink pattern from next position.
            Thread::wait(blink_args->blink_pattern.at(blink_args->position));
            blink_args->position++;
        }
    }
};

/*
 * The button contains one property (click count).
 * When `handle_button_click` is executed, the counter updates.
 */
class ButtonResource {
public:
    ButtonResource(): counter(0) {
        // create ObjectID with metadata tag of '3200', which is 'digital input'
        btn_object = M2MInterfaceFactory::create_object("3200");
        M2MObjectInstance* btn_inst = btn_object->create_object_instance();
        // create resource with ID '5501', which is digital input counter
        M2MResource* btn_res = btn_inst->create_dynamic_resource("5501", "Button",
            M2MResourceInstance::INTEGER, true /* observable */);
        // we can read this value
        btn_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        btn_res->set_value((uint8_t*)"0", 1);
    }

    ~ButtonResource() {
    }

    M2MObject* get_object() {
        return btn_object;
    }

    /*
     * When you press the button, we read the current value of the click counter
     * from mbed Device Connector, then up the value with one.
     */
    void handle_button_click() {
        M2MObjectInstance* inst = btn_object->object_instance();
        M2MResource* res = inst->resource("5501");

        // up counter
        counter++;
#ifdef TARGET_K64F
        //printf("handle_button_click, new value of counter is %d\r\n", counter);
#endif

        // serialize the value of counter as a string, and tell connector
        char buffer[20];
        int size = sprintf(buffer,"%d",counter);
        res->set_value((uint8_t*)buffer, size);
    }

private:
    M2MObject* btn_object;
    uint16_t counter;
};

/*
 * The GNSS contains 1 property (Azimuth).
 * When `handle_azimuth_update` is executed, the azimuth updates.
 */
class GnssResource {
public:
    GnssResource(): azimuth(0) {
        // create ObjectID with metadata tag of '3336', which is 'GPS location'
        gnss_object = M2MInterfaceFactory::create_object("3336");
        
        M2MObjectInstance* gnss_inst = gnss_object->create_object_instance();
        // create resource with ID '5513', which is digital input Latitude
        M2MResource* lat_res = gnss_inst->create_dynamic_resource("5513", "Latitude",
            M2MResourceInstance::FLOAT, true /* observable */);
        // we can read this value
        lat_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        lat_res->set_value(0.0);
        
        M2MResource* long_res = gnss_inst->create_dynamic_resource("5514", "Longitude",
            M2MResourceInstance::FLOAT, true /* observable */);
        // we can read this value
        long_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        long_res->set_value(0.0);
        
        M2MResource* alt_res = gnss_inst->create_dynamic_resource("5515", "Altitude",
        M2MResourceInstance::FLOAT, true /* observable */);
        // we can read this value
        alt_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        alt_res->set_value(0.0);        

        M2MResource* azi_res = gnss_inst->create_dynamic_resource("5705", "Heading",
            M2MResourceInstance::FLOAT, true /* observable */);
        // we can read this value
        azi_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        azi_res->set_value(0.0);
        
        M2MResource* app_res = gnss_inst->create_dynamic_resource("5750", "AppType",
            M2MResourceInstance::STRING, true /* observable */);
        // we can read this value
        app_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        size = sprintf(buffer,"%s","AntennaAlignment");
        app_res->set_value((const uint8_t*)buffer, size);       
        
#if 0       
        M2MResource* time_res = gnss_inst->create_dynamic_resource("5707", "Time",
        M2MResourceInstance::INTEGER, true /* observable */);
        // we can read this value
        time_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        time_res->set_value((uint8_t*)"0", 1);
        
        M2MResource* numsat_res = gnss_inst->create_dynamic_resource("5708", "NumSat",
        M2MResourceInstance::INTEGER, true /* observable */);
        // we can read this value
        numsat_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        numsat_res->set_value((uint8_t*)"0", 1);
        
        M2MResource* blen_res = gnss_inst->create_dynamic_resource("5709", "BaseLen",
        M2MResourceInstance::FLOAT, true /* observable */);
        // we can read this value
        blen_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        blen_res->set_value(0.0);

        M2MResource* fq_res = gnss_inst->create_dynamic_resource("5710", "FixQuality",
        M2MResourceInstance::INTEGER, true /* observable */);
        // we can read this value
        fq_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        fq_res->set_value((uint8_t*)"0", 1);
        
        M2MResource* date_res = gnss_inst->create_dynamic_resource("5706", "Date",
        M2MResourceInstance::INTEGER, true /* observable */);
        // we can read this value
        date_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        date_res->set_value((uint8_t*)"0", 1);
#endif
        
    }

    ~GnssResource() {
    }

    M2MObject* get_object() {
        return gnss_object;
    }

    /*
     * When you press the button, we read the current value of the click azimuth
     * from mbed Device Connector, then up the value with one.
     */
    void gps_scan(void) 
    {
        char c;
        PSTI32 = false; GPGGA = false;
        Timer timeout;      
        timeout.start();
        
        while((!PSTI32 || !GPGGA) && (timeout.read() < 10))
        {           
            if(gps.readable())
            { 
                if(gps.getc() == '$');           // wait for a $
                {
                    for(int i=0; i<sizeof(cDataBuffer); i++)
                    {
                        c = gps.getc();
                        if( c == '\r' )
                        {
                            //pc.printf("%s\n", cDataBuffer);
                            parse(cDataBuffer, i);
                            i = sizeof(cDataBuffer);
                        }
                        else
                        {
                            cDataBuffer[i] = c;
                        }                 
                    }
                }
             }
             else break;     
        }       
        timeout.stop();
    }

    void parse(char *cmd, int n)
    {
        char ns, ew, tf, status, mode;
        int fq, nst, fix, date, pstino;   // fix quality, Number of satellites being tracked, 3D fix
        float latitude, longitude, speed, timefix, altitude, eastprj, northprj, upprj, blength, bcourse;
        
        // Global Positioning System Fix Data
        if(strncmp(cmd,"$GPGGA", 6) == 0)
        {
            sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
            output.printf("GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
            gnss_scan.latitude  = latitude;
            gnss_scan.longitude = longitude;
            //gnss_scan.date = date; 
            // gnss_scan.time = timefix;
            gnss_scan.altitude = altitude;
            gnss_scan.fix_quality = fq;
            gnss_scan.numsat = nst;
            GPGGA = true;
        }
        
        // Baseline length, Azimuth
        if(strncmp(cmd,"$PSTI", 5) == 0)
        {
            sscanf(cmd, "$PSTI,%d,%f,%d,%c,%c,%f,%f,%f,%f,%f", &pstino, &timefix, &date, &status, &mode, &eastprj, &northprj, &upprj, &blength, &bcourse);
            if(32 == pstino)
            {
                output.printf("PSTI32 Fix taken at: %d, Date: %d, Status: %c, Mode: %c, Baseline length: %f m, Azimuth: %f degrees\n", timefix, date, status, mode, blength, bcourse);
                gnss_scan.heading = bcourse; gnss_scan.date = date; //gnss_scan.time = timefix; 
                gnss_scan.baselineLen = blength; //BaseLine Length
                PSTI32 = true;
            }
        }
#if 0
        // Satellite status
        if(strncmp(cmd,"$GPGSA", 6) == 0) 
        {
            sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
            pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
        }
        
        // Geographic position, Latitude and Longitude
        if(strncmp(cmd,"$GPGLL", 6) == 0) 
        {
            sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
            pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
        }
        
        // Geographic position, Latitude and Longitude
        if(strncmp(cmd,"$GPRMC", 6) == 0) 
        {
            sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
            pc.printf("GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\n", timefix, status, latitude, ns, longitude, ew, speed, date);
        }
#endif
    }
    void handle_gnss_update() {
        M2MObjectInstance* inst = gnss_object->object_instance();
        M2MResource* latituderes = inst->resource("5513");
        M2MResource* longituderes = inst->resource("5514");
        M2MResource* altres = inst->resource("5515");
        M2MResource* azimuthres = inst->resource("5705");
        M2MResource* appres = inst->resource("5750");
        
        gps_scan();
        
        size = sprintf(buffer,"%f",gnss_scan.latitude);
        latituderes->set_value((const uint8_t*)buffer, size);

        size = sprintf(buffer,"%f",gnss_scan.longitude);
        longituderes->set_value((const uint8_t*)buffer, size);
        
        size = sprintf(buffer,"%f",gnss_scan.altitude);
        altres->set_value((const uint8_t*)buffer, size);
        
        size = sprintf(buffer,"%f",gnss_scan.heading);
        azimuthres->set_value((const uint8_t*)buffer, size);

        size = sprintf(buffer,"%s","AntennaAlignment");
        appres->set_value((const uint8_t*)buffer, size);            

#if 0       

        M2MResource* dateres = inst->resource("5706");
        M2MResource* timeres = inst->resource("5707");
        M2MResource* numsatres = inst->resource("5708");
        M2MResource* blenres = inst->resource("5709");
        M2MResource* fqres = inst->resource("5710");

        size = sprintf(buffer,"%d",gnss_scan.date);
        dateres->set_value((const uint8_t*)buffer, size);

        size = sprintf(buffer,"%d",gnss_scan.time);
        timeres->set_value((const uint8_t*)buffer, size);

        size = sprintf(buffer,"%d",gnss_scan.numsat);
        numsatres->set_value((const uint8_t*)buffer, size);

        size = sprintf(buffer,"%f",gnss_scan.baselineLen);
        blenres->set_value((const uint8_t*)buffer, size);

        size = sprintf(buffer,"%d",gnss_scan.fix_quality);
        fqres->set_value((const uint8_t*)buffer, size); 
#endif
        
        output.printf("GNSS data updated\n");   
    }

private:
    M2MObject* gnss_object;
    uint16_t azimuth;
    bool PSTI32;
    bool GPGGA;
    gnss_params gnss_scan;
    char buffer[20];
    int size;   
};

const uint8_t STATIC_VALUE[] = "Cellular Antenna Alignment";
/*
 * The GNSS custom object
 */
/*
 * The GNSS custom object
 */
class GnssCustomResource {
public:
    GnssCustomResource() {
        // create ObjectID with metadata custom tag
        gnss_object = M2MInterfaceFactory::create_object("CustomGNSScompass");    
        M2MObjectInstance* gnss_inst = gnss_object->create_object_instance();
        
        
        M2MResource* lat_res = gnss_inst->create_dynamic_resource("E",
                                                         "CustomLatitude",
                                                         M2MResourceInstance::FLOAT,
                                                         true);
        // we can read this value
        lat_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        lat_res->set_value(0.0);
        
        M2MResource* app_res = gnss_inst->create_static_resource("N",
                                                 "CustomAppType",
                                                 M2MResourceInstance::STRING,
                                                 STATIC_VALUE,
                                                 sizeof(STATIC_VALUE)-1);
        // we can read this value
        app_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        size = sprintf(buffer,"%s","AntennaAlignment");
        app_res->set_value((const uint8_t*)buffer, size);       
   
       
    }

    ~GnssCustomResource() {
    }

    M2MObject* get_object() {
        return gnss_object;
    }

    /*
     * When you press the button, we read the current value of the click azimuth
     * from mbed Device Connector, then up the value with one.
     */
    void gps_scan(void) 
    {
        char c;
        PSTI32 = false; GPGGA = false;
        Timer timeout;      
        timeout.start();
        
        while((!PSTI32 || !GPGGA) && (timeout.read() < 10))
        {           
            if(gps.readable())
            { 
                if(gps.getc() == '$');           // wait for a $
                {
                    for(int i=0; i<sizeof(cDataBuffer); i++)
                    {
                        c = gps.getc();
                        if( c == '\r' )
                        {
                            //pc.printf("%s\n", cDataBuffer);
                            parse(cDataBuffer, i);
                            i = sizeof(cDataBuffer);
                        }
                        else
                        {
                            cDataBuffer[i] = c;
                        }                 
                    }
                }
             }
             else break;     
        }       
        timeout.stop();
    }

    void parse(char *cmd, int n)
    {
        char ns, ew, tf, status, mode;
        int fq, nst, fix, date, timefix, pstino;   // fix quality, Number of satellites being tracked, 3D fix
        float latitude, longitude, speed, altitude, eastprj, northprj, upprj, blength, bcourse;
        
        // Global Positioning System Fix Data
        if(strncmp(cmd,"$GPGGA", 6) == 0) 
        {
            sscanf(cmd, "$GPGGA,%d,%f,%c,%f,%c,%d,%d,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
            //output.printf("GPGGA Fix taken at: %d, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
            gnss_scan.latitude  = latitude;
            gnss_scan.longitude = longitude;
            //gnss_scan.date = date; 
            gnss_scan.time = timefix;
            gnss_scan.altitude = altitude;
            gnss_scan.fix_quality = fq;
            gnss_scan.numsat = nst;
            GPGGA = true;
        }
        
        // Baseline length, Azimuth
        if(strncmp(cmd,"$PSTI", 5) == 0)
        {
            sscanf(cmd, "$PSTI,%d,%d,%d,%c,%c,%f,%f,%f,%f,%f", &pstino, &timefix, &date, &status, &mode, &eastprj, &northprj, &upprj, &blength, &bcourse);
            if(32 == pstino)
            {
                //output.printf("PSTI32 Fix taken at: %d, Date: %d, Status: %c, Mode: %c, Baseline length: %f m, Azimuth: %f degrees\n", timefix, date, status, mode, blength, bcourse);
                gnss_scan.heading = bcourse; gnss_scan.date = date; gnss_scan.time = timefix; 
                gnss_scan.baselineLen = blength; //BaseLine Length
                PSTI32 = true;
            }
        }
#if 0
        // Satellite status
        if(strncmp(cmd,"$GPGSA", 6) == 0) 
        {
            sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
            pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
        }
        
        // Geographic position, Latitude and Longitude
        if(strncmp(cmd,"$GPGLL", 6) == 0) 
        {
            sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
            pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
        }
        
        // Geographic position, Latitude and Longitude
        if(strncmp(cmd,"$GPRMC", 6) == 0) 
        {
            sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
            pc.printf("GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\n", timefix, status, latitude, ns, longitude, ew, speed, date);
        }
#endif
    }
    void handle_gnss_update() {
        M2MObjectInstance* inst = gnss_object->object_instance();
        M2MResource* latituderes = inst->resource("E");
        M2MResource* appres = inst->resource("N");
        
        gps_scan();
        
        size = sprintf(buffer,"%f",gnss_scan.latitude);
        latituderes->set_value((const uint8_t*)buffer, size);

        
        output.printf("GNSS data updated\n");   
    }

private:
    M2MObject* gnss_object;
    uint16_t azimuth;
    bool PSTI32;
    bool GPGGA;
    gnss_params gnss_scan;
    char buffer[20];
    int size;   
};

class AccelResource {
public:
    AccelResource(){
        
        // create ObjectID with metadata tag of '3313', which is 'IPSO Accelerometer'
        accel_object = M2MInterfaceFactory::create_object("3313");
        M2MObjectInstance* accel_inst = accel_object->create_object_instance();
        
        // create resource with ID '5701', which is accelerometer units
        M2MResource* units_res = accel_inst->create_dynamic_resource("5701", "Units",
            M2MResourceInstance::STRING, false /* non-observable */);
        // we can read this value
        units_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        size = sprintf(buffer,"%s","degrees");
        units_res->set_value((const uint8_t*)buffer, size);     
        
        
        // create resource with ID '5702', which is accelerometer X value
        M2MResource* x_res = accel_inst->create_dynamic_resource("5702", "Tilt",
            M2MResourceInstance::FLOAT, true /* observable */);
        // we can read this value
        x_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        size = sprintf(buffer,"%f",0.0);
        x_res->set_value((const uint8_t*)buffer, size);
        
        // create resource with ID '5703', which is accelerometer Y value
        M2MResource* y_res = accel_inst->create_dynamic_resource("5703", "Roll",
            M2MResourceInstance::FLOAT, true /* observable */);
        // we can read this value
        y_res->set_operation(M2MBase::GET_ALLOWED);
        // set initial value (all values in mbed Client are buffers)
        // to be able to read this data easily in the Connector console, we'll use a string
        size = sprintf(buffer,"%f",0.0);
        y_res->set_value((const uint8_t*)buffer, size);
    
    }

    ~AccelResource() {
    }

    M2MObject* get_object() {
        return accel_object;
    }

    /*
     * Timer based update triggers planned
     */
     
    void handle_accel_update() 
    {
        double angleX, denomX_T, denomX_A, denomX_B, denomX_C;           //intializing variable to hold the angle calculation 
        double angleY, denomY_T, denomY_A, denomY_B, denomY_C; 
        float faX, faY, faZ;
        
        M2MObjectInstance* inst = accel_object->object_instance();
        M2MResource* xres = inst->resource("5702");
        M2MResource* yres = inst->resource("5703");
        
        // Read accelerometer data
        //accel.getAxis(acc_data);
        accel.getX(faX);
        accel.getY(faY);
        accel.getZ(faZ);
        
        // Canculate angles in degrees
        //X-AXIS
        denomX_A = pow(faY, 2);
        denomX_B = pow(faZ, 2);
        denomX_C = denomX_A + denomX_B;
        denomX_T = pow(denomX_C, .5);                   //pow returns base raised to the power exponent 
        
        angleX = atan(faX/denomX_T) * 180/PI;           //should calculate the angle on the X axis in degrees based on raw data         

        //Y-AXIS
        denomY_A = pow(faX, 2);
        denomY_B = pow(faZ, 2);
        denomY_C = denomY_A + denomY_B;
        denomY_T = pow(denomY_C, .5);                   //pow returns base raised to the power exponent 
        
        angleY = atan(faY/denomY_T) * 180/PI;           //should calculate the angle on the Y axis in degrees based on raw data         

        // serialize the value of x & y accel ops as a string, and tell connector
        size = sprintf(buffer,"%f",angleX);
        xres->set_value((const uint8_t*)buffer, size);
        
        size = sprintf(buffer,"%f",angleY);
        yres->set_value((const uint8_t*)buffer, size);      
        printf("Accelerometer update, x:%f degrees, y:%f degrees\r\n", angleX, angleY );
    
    }

private:
    M2MObject* accel_object;
    //motion_data_units_t acc_data;
    char buffer[20];
    int size;   
};

class BigPayloadResource {
public:
    BigPayloadResource() {
        big_payload = M2MInterfaceFactory::create_object("1000");
        M2MObjectInstance* payload_inst = big_payload->create_object_instance();
        M2MResource* payload_res = payload_inst->create_dynamic_resource("1", "BigData",
            M2MResourceInstance::STRING, true /* observable */);
        payload_res->set_operation(M2MBase::GET_PUT_ALLOWED);
        payload_res->set_value((uint8_t*)"0", 1);
        payload_res->set_incoming_block_message_callback(
                    incoming_block_message_callback(this, &BigPayloadResource::block_message_received));
        payload_res->set_outgoing_block_message_callback(
                    outgoing_block_message_callback(this, &BigPayloadResource::block_message_requested));
    }

    M2MObject* get_object() {
        return big_payload;
    }

    void block_message_received(M2MBlockMessage *argument) {
        if (argument) {
            if (M2MBlockMessage::ErrorNone == argument->error_code()) {
                if (argument->is_last_block()) {
                    output.printf("Last block received\r\n");
                }
                output.printf("Block number: %d\r\n", argument->block_number());
                // First block received
                if (argument->block_number() == 0) {
                    // Store block
                // More blocks coming
                } else {
                    // Store blocks
                }
            } else {
                output.printf("Error when receiving block message!  - EntityTooLarge\r\n");
            }
            output.printf("Total message size: %d\r\n", argument->total_message_size());
        }
    }

    void block_message_requested(const String& resource, uint8_t *&/*data*/, uint32_t &/*len*/) {
        output.printf("GET request received for resource: %s\r\n", resource.c_str());
        // Copy data and length to coap response
    }

private:
    M2MObject*  big_payload;
};