Diff: main.cpp

Revision:

10:658720d51610

Parent:

7:3ccfc47b8e0b

Child:

12:ad92bbb6312f

--- a/main.cpp	Tue Aug 23 16:51:04 2016 +0000
+++ b/main.cpp	Tue Aug 23 18:03:01 2016 -0400
@@ -30,7 +30,15 @@
  *
  */
 
+// #define MTDOT_EVB
+
+//#define MTDOT_UDK
+
 #include "mbed.h"
+#include "senet_packet.h"
+
+// EVB Sensors
+#ifdef MTDOT_EVB
 #include "MMA845x.h"
 #include "MPL3115A2.h"
 #include "ISL29011.h"
@@ -38,6 +46,10 @@
 #include "DOGS102.h"
 #include "font_6x8.h"
 #include "MultiTech_Logo.h"
+#elif defined(MTDOT_UDK)
+#include "x_nucleo_iks01a1.h"
+#endif
+
 #include "mDot.h"
 #include "rtos.h"
 #include <string>
@@ -66,12 +78,33 @@
     uint8_t t_u[2];		// convert to 8 bit unsigned array
 };
 
+struct BoardSensorData
+{
+    float temperature;
+    float pressure;
+    int32_t accel_x;
+    int32_t accel_y;
+    int32_t accel_z;
+
+    inline void init()
+    {
+        temperature= 0;
+        pressure = 0;
+        accel_x = 0;
+        accel_y = 0;
+        accel_z = 0;
+    }
+
+    BoardSensorData() { init(); }
+};
+
 //DigitalIn mDot02(PA_2);          				//  GPIO/UART_TX
 //DigitalOut mDot03(PA_3);         				//  GPIO/UART_RX
 //DigitalIn mDot04(PA_6);          				//  GPIO/SPI_MISO
 //DigitalIn mDot06(PA_8);          				//  GPIO/I2C_SCL
 //DigitalIn mDot07(PC_9);         				//  GPIO/I2C_SDA
 
+#ifdef MDOT_EVB
 InterruptIn mDot08(PA_12);           			//  GPIO/USB       PB S1 on EVB
 InterruptIn mDot09(PA_11);           			//  GPIO/USB       PB S2 on EVB
 
@@ -95,6 +128,11 @@
 I2C mDoti2c(PC_9,PA_8);							// mDot External I2C mDot6 and mDot7
 
 SPI mDotspi(PA_7,PA_6,PA_5);					// mDot external SPI mDot11, mDot4, and mDot18
+#elif defined(MTDOT_UDK)
+
+Serial debugUART(USBTX, USBRX);				// mDot debug UART
+
+#endif
 
 /* **** replace these values with the proper public or private network settings ****
  * config_network_nameand config_network_pass are for private networks.
@@ -127,14 +165,20 @@
 
 bool exit_program = false;
 
+#ifdef  MTDOT_EVB
 MMA845x_DATA accel_data;
 MPL3115A2_DATA baro_data;
 uint16_t  lux_data;
-MMA845x* evbAccel;
-MPL3115A2* evbBaro;
-ISL29011* evbAmbLight;
-NCP5623B* evbBackLight;
-DOGS102* evbLCD;
+MMA845x* evbAccel      = NULL;
+MPL3115A2* evbBaro     = NULL;
+ISL29011* evbAmbLight  = NULL;
+NCP5623B* evbBackLight = NULL;
+DOGS102* evbLCD        = NULL;
+
+#elif defined(MTDOT_UDK)
+static X_NUCLEO_IKS01A1 *mems_shield; 
+#endif
+
 mDot* mdot_radio;
 
 convert32 convertl;
@@ -154,19 +198,48 @@
 
 void config_pkt_xmit (void const *args);
 
-int main()
+void SendFrame(std::vector<uint8_t> frame)
 {
+    if ((mdot_ret = mdot_radio->send(frame)) != mDot::MDOT_OK) {
+        log_error(mdot_radio, "failed to send", mdot_ret);
+    } 
+    else {
+        printf("successfully sent data to gateway\r\n");
+        frame.clear();
+        if ((mdot_ret = mdot_radio->recv(frame)) != mDot::MDOT_OK) {
+            log_error(mdot_radio,"failed to recv:", mdot_ret);
+        } else {
+            printf("recv data: ");
+            for(uint32_t i = 0;i < frame.size();i++)
+                printf("%02X",frame[i]);
+            printf("\r\n");
+            reflected_value = frame[0];
+            if(reflected_value == position_value)
+            {
+#if MTDOT_EVB
+                evbBackLight->setLEDCurrent(16);
+#endif
+                position_changed = false;
+            }
+#if MTDOT_EVB
+            else 
+            {
+                evbBackLight->setLEDCurrent(0);
+            }
+#endif
+        }
+    }
+}
 
-    std::vector<uint8_t> mdot_data;
-    std::vector<uint8_t> mdot_EUI;
-    uint16_t i = 0;
+#ifdef MDOT_EVB
+void BoardInit()
+{
+    static Thread thread_1(pb1_debounce);	 // threads for de-bouncing pushbutton switches
+    static Thread thread_2(pb2_debounce);
 
     debugUART.baud(115200);
 //  mDotUART.baud(9600);    // mdot UART unused but available on external connector
 
-    Thread thread_1(pb1_debounce);						// threads for de-bouncing pushbutton switches
-    Thread thread_2(pb2_debounce);
-
     thread_3 = new Thread(config_pkt_xmit);				// start thread that sends LoRa packet when SW2 pressed
 
     evbAccel = new MMA845x(mDoti2c,MMA845x::SA0_VSS); 	// setup Accelerometer
@@ -227,117 +300,11 @@
 
     evbLCD->endUpdate();
 
-   printf("\n\r setup mdot\n\r");
-
-    // get a mDot handle
-    mdot_radio = mDot::getInstance();
-
-    if (mdot_radio) {
-        // reset to default config so we know what state we're in
-        mdot_radio->resetConfig();
-        mdot_radio->setLogLevel(6);
-        mdot_radio->setAntennaGain(-3);
-
-        // Setting up LED1 as activity LED
-        mdot_radio->setActivityLedPin(PB_0);
-        mdot_radio->setActivityLedEnable(true);
-
-        // Read node ID
-        mdot_EUI = mdot_radio->getDeviceId();
-        printf("mDot EUI = ");
-
-        for (i=0; i<mdot_EUI.size(); i++) {
-            printf("%02x ", mdot_EUI[i]);
-        }
-        printf("\n\r");
-
-
-// Setting up the mDot with network information.
-
-/*
- * This call sets up private or public mode on the MTDOT. Set the function to true if
- * connecting to a public network
- */
-       printf("setting Public Network Mode\r\n");
-       if ((mdot_ret = mdot_radio->setPublicNetwork(true)) != mDot::MDOT_OK) {
-           log_error(mdot_radio, "failed to set Public Network Mode", mdot_ret);
-       }
-       mdot_radio->setTxDataRate(mDot::DR0);
-       mdot_radio->setTxPower(14);
-       mdot_radio->setJoinRetries(1);
-       mdot_radio->setJoinMode(mDot::OTA);
+    pckt_time = 10;
+}
 
-/*
- * Frequency sub-band is valid for NAM only and for Private networks should be set to a value
- * between 1-8 that matches the the LoRa gateway setting. Public networks use sub-band 0 only.
- * This function can be commented out for EU networks
- */
-        printf("setting frequency sub band\r\n");
-        if ((mdot_ret = mdot_radio->setFrequencySubBand(config_frequency_sub_band)) != mDot::MDOT_OK) {
-            log_error(mdot_radio, "failed to set frequency sub band", mdot_ret);
-        }
-        
-        printf("setting ADR\r\n");
-        if ((mdot_ret = mdot_radio->setAdr(config_adr_on)) != mDot::MDOT_OK) {
-            log_error(mdot_radio, "failed to set ADR", mdot_ret);
-        }
-/*
- * setNetworkName is used for private networks.
- * Use setNetworkID(AppID) for public networks
- */
-
-// 		config_app_id.assign(app_id,app_id+7);
-
-        printf("setting network name\r\n");
-//        if ((mdot_ret = mdot_radio->setNetworkName(config_network_name)) != mDot::MDOT_OK) {
-      if ((mdot_ret = mdot_radio->setNetworkId(config_app_id)) != mDot::MDOT_OK) {
-            log_error(mdot_radio, "failed to set network name", mdot_ret);
-        }
-
-/*
- * setNetworkPassphrase is used for private networks
- * Use setNetworkKey for public networks
- */
-
-// 		config_app_key.assign(app_key,app_key+15);
-
-        printf("setting network password\r\n");
-//        if ((mdot_ret = mdot_radio->setNetworkPassphrase(config_network_pass)) != mDot::MDOT_OK) {
-      if ((mdot_ret = mdot_radio->setNetworkKey(config_app_key)) != mDot::MDOT_OK) {
-            log_error(mdot_radio, "failed to set network password", mdot_ret);
-        }
-
-        // attempt to join the network
-        printf("joining network\r\n");
-        while (((mdot_ret = mdot_radio->joinNetwork()) != mDot::MDOT_OK) && (!exit_program)) {
-            log_error(mdot_radio,"failed to join network:", mdot_ret);
-            if (mdot_radio->getFrequencyBand() == mDot::FB_868){
-            	mdot_ret = mdot_radio->getNextTxMs();
-            }
-        	else {
-        		mdot_ret = 0;
-         	}
-        		
-            printf("delay = %lu\n\r",mdot_ret);
-            osDelay(mdot_ret + 10000);
-        }
-
-        /*
-         * Check for PB1 press during network join attempt
-         */
-        if (exit_program) {
-            printf("Exiting program\n\r");
-            evbLCD->clearBuffer();
-            sprintf(txtstr,"Exiting Program");
-            evbLCD->writeText(0,4,font_6x8,txtstr,strlen(txtstr));
-            exit(1);
-        }
-
-    } else {
-        printf("radio setup failed\n\r");
-        //exit(1);
-    }
-
+void PostJoinInit()
+{
     osDelay(200);
     evbBackLight->setPWM(NCP5623B::LED_3,16); // enable LED2 on EVB and set to 50% PWM
 
@@ -409,183 +376,395 @@
         evbLCD->writeText(0,4,font_6x8,txtstr,strlen(txtstr));
         exit(1);
     }
+}
+
+void ReadSensors(BoardSensorData &sensorData)
+{
+    MMA845x_DATA accel_data;
+    /*
+     * Test Accelerometer XYZ data ready bit to see if acquisition complete
+     */
+    do {
+        osDelay(100);			// allows other threads to process
+        result = evbAccel->getStatus();
+    } while ((result & MMA845x::XYZDR) == 0 );
+
+    /*
+     * Retrieve and print out accelerometer data
+     */
+    accel_data = evbAccel->getXYZ();
+
+    sprintf(txtstr,"Accelerometer");
+    evbLCD->writeText(0,0,font_6x8,txtstr,strlen(txtstr));
+    sprintf(txtstr, "x = %d", accel_data._x);
+    evbLCD->writeText(20,1,font_6x8,txtstr,strlen(txtstr));
+    sprintf(txtstr, "y = %d", accel_data._y);
+    evbLCD->writeText(20,2,font_6x8,txtstr,strlen(txtstr));
+    sprintf(txtstr, "z = %d", accel_data._z );
+    evbLCD->writeText(20,3,font_6x8,txtstr,strlen(txtstr));
+
+    sensorData->accel_x = accel_data._x;
+    sensorData->accel_y = accel_data._y;
+    sensorData->accel_z = accel_data._z;
+
+    // Update accelerometer state
+    evbLCD->startUpdate();
+    evbLCD->clearBuffer();
+    
+    // convert to simple position value for use in send/recv
+    if((accel_data._x > 500)&&(accel_data._z < 500))
+    {
+        if(position_value != 0x02)
+            position_changed = true;
+        position_value = 0x02;
+    }
+    else if((accel_data._x < 500)&&(accel_data._z > 500))
+    {
+        if(position_value != 0x01)
+            position_changed = true;
+        position_value = 0x01;
+    }
+    else
+    {
+        if(position_value != 0x00)
+            position_changed = true;
+        position_value= 0x00;
+    }
+
+    if(changed)
+        evbBackLight->setLEDCurrent(0);
+    /*
+     * Trigger a Pressure reading
+     */
+    evbBaro->setParameters(MPL3115A2::DATA_NORMAL, MPL3115A2::DM_BAROMETER, MPL3115A2::OR_16,
+                           MPL3115A2::AT_1);
+    evbBaro->triggerOneShot();
+
+    /*
+     * Test barometer device status to see if acquisition is complete
+     */
+    do {
+        osDelay(100);			// allows other threads to process
+        result = evbBaro->getStatus();
+    } while ((result & MPL3115A2::PTDR) == 0 );
+
+    /*
+     * Retrieve and print out barometric pressure
+     */
+    pressure = evbBaro->getBaroData() >> 12; // convert 32 bit signed to 20 bit unsigned value
+    num_whole = pressure >> 2;			// 18 bit integer significant
+    num_frac = (pressure & 0x3) * 25;		// 2 bit fractional  0.25 per bit
+    sensorData.pressure = pressure + (.25 * num_frac);
+
+    sprintf(txtstr,"Press=%ld.%02d Pa", num_whole, num_frac);
+    evbLCD->writeText(0,4,font_6x8,txtstr,strlen(txtstr));
+
+    /*
+     * Trigger a Altitude reading
+     */
+    evbBaro->setParameters(MPL3115A2::DATA_NORMAL, MPL3115A2::DM_ALTIMETER, MPL3115A2::OR_16,
+                           MPL3115A2::AT_1);
+    evbBaro->triggerOneShot();
+
+    /*
+     * Test barometer device status to see if acquisition is complete
+     */
+    do {
+        osDelay(100);			// allows other threads to process
+        result = evbBaro->getStatus();
+    } while ((result & MPL3115A2::PTDR) == 0 );
+
+    /*
+     * Retrieve and print out altitude and temperature
+     */
+    baro_data = evbBaro->getAllData(false);
+    baro_data._baro /= 4096;				// convert 32 bit signed to 20 bit signed value
+    num_whole = baro_data._baro / 16;		//	18 bit signed significant integer
+    num_frac = (baro_data._baro & 0xF) * 625 / 100;		// 4 bit fractional .0625 per bit
+    sprintf(txtstr,"Alti=%ld.%03d m", num_whole, num_frac);
+    evbLCD->writeText(0,5,font_6x8,txtstr,strlen(txtstr));
+    num_whole = baro_data._temp / 16;		// 8 bit signed significant integer
+    num_frac = (baro_data._temp & 0x0F) * 625 / 100;		// 4 bit fractional .0625 per bit
+    sensor.temperature = num_whole  + ((float)num_frac / 100);
+    sprintf(txtstr,"Temp=%ld.%03d C", num_whole, num_frac);
+    evbLCD->writeText(0,6,font_6x8,txtstr,strlen(txtstr));
+
+    /*
+     * retrieve and print out Ambient Light level
+     */
+    lux_data = evbAmbLight->getData();
+    num_whole = lux_data * 24 / 100;		// 16000 lux full scale .24 lux per bit
+    num_frac = lux_data * 24 % 100;
+    sprintf(txtstr, "Light=%ld.%02d lux", num_whole, num_frac );
+    evbLCD->writeText(0,7,font_6x8,txtstr,strlen(txtstr));
+
+    evbLCD->endUpdate();
+    printf("finished iteration %d\n\r",(++i));
+}
+
+uint32_t PrepareFrame(std::vector<uint8_t> frame, BoardSensorData &data)
+{
+    if (i % pckt_time != 0) 
+        return 0;
+
+    frame.clear();
+    // we will send a simple byte descriptor of the current position of the device: 01 is laying flat, 02 is vertically oriented
+    frame.push_back(0x00);
+
+    frame.push_back(position_value);
+    /*frame.push_back(0x0E);			// key for Current Acceleration 3-Axis Value
+    converts.f_s = accel_data._x *4;		// shift data 2 bits while retaining sign
+    frame.push_back(converts.t_u[1]);	// get 8 MSB of 14 bit value
+    converts.f_s = accel_data._y * 4;		// shift data 2 bits while retaining sign
+    frame.push_back(converts.t_u[1]);	// get 8 MSB of 14 bit value
+    converts.f_s = accel_data._z * 4;		// shift data 2 bits while retaining sign
+    frame.push_back(converts.t_u[1]);	// get 8 MSB of 14 bit value
+    */
+    /*
+    frame.push_back(0x08);			// key for Current Pressure Value
+    convertl.f_u = pressure;				// pressure data is 20 bits unsigned
+    frame.push_back(convertl.t_u[2]);
+    frame.push_back(convertl.t_u[1]);
+    frame.push_back(convertl.t_u[0]);
+    frame.push_back(0x05);			// key for Current Ambient Light Value
+    converts.f_u = lux_data;				// data is 16 bits unsigned
+    frame.push_back(converts.t_u[1]);
+    frame.push_back(converts.t_u[0]);
+    frame.push_back(0x0B);			// key for Current Temperature Value
+    converts.f_s = baro_data._temp;		// temperature is signed 12 bit
+    frame.push_back(converts.t_u[1]);
+    frame.push_back(converts.t_u[0]);
+*/
+
+    if((reflected_value != position_value)||(position_changed)||(0 == (i%100)))
+    {
+    }
+}
+
+#elif defined(MTDOT_UDK)
+
+void BoardInit()
+{ 
+    debugUART.baud(115200); 
+
+    // ST X-NUCLEO-IKS01A1 MEMS Shield
+    mems_shield = X_NUCLEO_IKS01A1::Instance(); 
+}
+
+void PostJoinInit() { }
+
+
+void ReadSensors(BoardSensorData &data)
+{
+    uint32_t ret = 0;
+    int32_t  accel_data[3];
+    
+   // Temperature
+   ret |= (!CALL_METH(mems_shield->pt_sensor, GetTemperature, &data.temperature, 0.0f) ? 0x0 : 0x1);
+
+   // Pressure
+   ret |= (!CALL_METH(mems_shield->pt_sensor, GetPressure, &data.pressure, 0.0f) ? 0x0 : 0x1);
+
+   // Accelerometer
+   MotionSensor *motionSensor = mems_shield->GetAccelerometer();
+   if( motionSensor != NULL)
+   {
+       motionSensor->Get_X_Axes(accel_data);
+
+       data.accel_x = accel_data[0];
+       data.accel_y = accel_data[1];
+       data.accel_z = accel_data[2];
+   }
+
+   printf("%s: Temperature=%f, Pressure=%f, x=%ld, y=%ld, z=%ld\r\n",__func__,
+           data.temperature, data.pressure, 
+           data.accel_x, data.accel_y, data.accel_z);
+
+   osDelay(5000);
+
+}
+
+uint32_t PrepareFrame(std::vector<uint8_t> frame, BoardSensorData &data)
+{
+    static uint8_t buffer[64];
+    uint16_t xyz_mask = 0;
+
+    // Sensor packet type serialized to the LMIC frame buffer
+    SensorPacket packet(buffer, sizeof(frame));
+
+    packet.setPrimarySensor(xyz_mask);
+    packet.setTemperature(data.temperature);
+    packet.setPressure(data.pressure);
+            
+    // Serialize  packet to LMIC transmit buffer
+    packet.serialize();
+
+    frame.assign(packet.payload(), packet.payload() + packet.length());
+
+    return frame.size();
+}
+
+
+
+#else
+#error Board type not defined!
+#endif
+
+void mDotConfigureAndJoin()
+{ 
+    bool ok;
+    
+    // get mDot handle
+    mdot_radio = mDot::getInstance();
+    if(mdot_radio == NULL)
+    { 
+        while(1) {
+            printf("radio setup failed\n\r");
+            osDelay(1000);
+        }
+    }
+
+    do{
+        ok = true; 
+
+        printf("\n\r setup mdot\n\r"); 
+        
+        // reset to default config so we know what state we're in
+        mdot_radio->resetConfig();
+        mdot_radio->setLogLevel(6);
+
+        mdot_radio->setAntennaGain(-3);
+
+        // Setting up LED1 as activity LED
+#ifdef MDOT_EVB
+        mdot_radio->setActivityLedPin(PB_0);
+        mdot_radio->setActivityLedEnable(true);
+#endif
+
+        // Read node ID
+        std::vector<uint8_t> mdot_EUI;
+        mdot_EUI = mdot_radio->getDeviceId();
+        printf("mDot EUI = ");
+
+        for (uint8_t i=0; i<mdot_EUI.size(); i++) {
+            printf("%02x ", mdot_EUI[i]);
+        }
+        printf("\n\r");
+
+
+// Setting up the mDot with network information.
+
+    /*
+     * This call sets up private or public mode on the MTDOT. Set the function to true if
+     * connecting to a public network
+     */
+       printf("setting Public Network Mode\r\n");
+       if ((mdot_ret = mdot_radio->setPublicNetwork(true)) != mDot::MDOT_OK) {
+           log_error(mdot_radio, "failed to set Public Network Mode", mdot_ret);
+       }
+       mdot_radio->setTxDataRate(mDot::DR0);
+       mdot_radio->setTxPower(14);
+       mdot_radio->setJoinRetries(1);
+       mdot_radio->setJoinMode(mDot::OTA);
+
+    /*
+     * Frequency sub-band is valid for NAM only and for Private networks should be set to a value
+     * between 1-8 that matches the the LoRa gateway setting. Public networks use sub-band 0 only.
+     * This function can be commented out for EU networks
+     */
+       printf("setting frequency sub band\r\n");
+        if ((mdot_ret = mdot_radio->setFrequencySubBand(config_frequency_sub_band)) != mDot::MDOT_OK) {
+            log_error(mdot_radio, "failed to set frequency sub band", mdot_ret);
+            ok = false;
+        }
+        
+        printf("setting ADR\r\n");
+        if ((mdot_ret = mdot_radio->setAdr(config_adr_on)) != mDot::MDOT_OK) {
+            log_error(mdot_radio, "failed to set ADR", mdot_ret);
+            ok = false;
+        }
+    /*
+     * setNetworkName is used for private networks.
+     * Use setNetworkID(AppID) for public networks
+     */ 
+        printf("setting network name\r\n");
+        if ((mdot_ret = mdot_radio->setNetworkId(config_app_id)) != mDot::MDOT_OK) {
+            log_error(mdot_radio, "failed to set network name", mdot_ret);
+            ok = false;
+        }
+
+    /*
+     * setNetworkPassphrase is used for private networks
+     * Use setNetworkKey for public networks
+     */ 
+        printf("setting network password\r\n");
+        if ((mdot_ret = mdot_radio->setNetworkKey(config_app_key)) != mDot::MDOT_OK) {
+            log_error(mdot_radio, "failed to set network password", mdot_ret);
+            ok = false;
+        } 
+    }while(ok == false);
+  
+  // attempt to join the network
+  printf("joining network\r\n");
+  while (((mdot_ret = mdot_radio->joinNetwork()) != mDot::MDOT_OK) && (!exit_program)) {
+      log_error(mdot_radio,"failed to join network:", mdot_ret);
+      if (mdot_radio->getFrequencyBand() == mDot::FB_868){
+          mdot_ret = mdot_radio->getNextTxMs();
+      }
+      else {
+          mdot_ret = 0;
+      } 
+
+      printf("delay = %lu\n\r",mdot_ret);
+      osDelay(mdot_ret + 10000);
+  } 
+}
+
+int main()
+{
+    BoardSensorData sensorData;
+    
+    // Board specific initialization
+    BoardInit(); 
+
+    // mDot configuration and join
+    //mDotConfigureAndJoin();
+
+    PostJoinInit();
+
+  /*
+   * Check for PB1 press during network join attempt
+   */
+  if (exit_program) {
+      printf("Exiting program\n\r");
+
+#ifdef MDOT_EVB
+      evbLCD->clearBuffer();
+      sprintf(txtstr,"Exiting Program");
+      evbLCD->writeText(0,4,font_6x8,txtstr,strlen(txtstr));
+#endif
+      exit(1);
+  } 
 
     /*
      * Main data acquisition loop
      */
-    pckt_time = 10;
-    i = 0;
-
     do {
-        evbLCD->startUpdate();
-        evbLCD->clearBuffer();
-
-        /*
-         * Test Accelerometer XYZ data ready bit to see if acquisition complete
-         */
-        do {
-            osDelay(100);			// allows other threads to process
-            result = evbAccel->getStatus();
-        } while ((result & MMA845x::XYZDR) == 0 );
-
-        /*
-         * Retrieve and print out accelerometer data
-         */
-        accel_data = evbAccel->getXYZ();
-
-        sprintf(txtstr,"Accelerometer");
-        evbLCD->writeText(0,0,font_6x8,txtstr,strlen(txtstr));
-        sprintf(txtstr, "x = %d", accel_data._x);
-        evbLCD->writeText(20,1,font_6x8,txtstr,strlen(txtstr));
-        sprintf(txtstr, "y = %d", accel_data._y);
-        evbLCD->writeText(20,2,font_6x8,txtstr,strlen(txtstr));
-        sprintf(txtstr, "z = %d", accel_data._z );
-        evbLCD->writeText(20,3,font_6x8,txtstr,strlen(txtstr));
-        // convert to simple position value for use in send/recv
-		if((accel_data._x > 500)&&(accel_data._z < 500))
-		{
-			if(position_value != 0x02)
-				position_changed = true;
-			position_value = 0x02;
-		}
-		else if((accel_data._x < 500)&&(accel_data._z > 500))
-		{
-			if(position_value != 0x01)
-				position_changed = true;
-			position_value = 0x01;
-		}
-		else
-		{
-			if(position_value != 0x00)
-				position_changed = true;
-			position_value= 0x00;
-		}
-		if(position_changed)
-			evbBackLight->setLEDCurrent(0);
-		
-        /*
-         * Trigger a Pressure reading
-         */
-        evbBaro->setParameters(MPL3115A2::DATA_NORMAL, MPL3115A2::DM_BAROMETER, MPL3115A2::OR_16,
-                               MPL3115A2::AT_1);
-        evbBaro->triggerOneShot();
-
-        /*
-         * Test barometer device status to see if acquisition is complete
-         */
-        do {
-            osDelay(100);			// allows other threads to process
-            result = evbBaro->getStatus();
-        } while ((result & MPL3115A2::PTDR) == 0 );
-
-        /*
-         * Retrieve and print out barometric pressure
-         */
-        pressure = evbBaro->getBaroData() >> 12; // convert 32 bit signed to 20 bit unsigned value
-        num_whole = pressure >> 2;			// 18 bit integer significant
-        num_frac = (pressure & 0x3) * 25;		// 2 bit fractional  0.25 per bit
-        sprintf(txtstr,"Press=%ld.%02d Pa", num_whole, num_frac);
-        evbLCD->writeText(0,4,font_6x8,txtstr,strlen(txtstr));
-
-        /*
-         * Trigger a Altitude reading
-         */
-        evbBaro->setParameters(MPL3115A2::DATA_NORMAL, MPL3115A2::DM_ALTIMETER, MPL3115A2::OR_16,
-                               MPL3115A2::AT_1);
-        evbBaro->triggerOneShot();
+        std::vector<uint8_t> frame;
+        
+        // Acquire sensor values
+        ReadSensors(sensorData);
+        
+        // Send sensor packets
+        if( PrepareFrame(frame, sensorData) > 0 ){
+            SendFrame( frame );
+        }
 
-        /*
-         * Test barometer device status to see if acquisition is complete
-         */
-        do {
-            osDelay(100);			// allows other threads to process
-            result = evbBaro->getStatus();
-        } while ((result & MPL3115A2::PTDR) == 0 );
+    } while(!exit_program);
 
-        /*
-         * Retrieve and print out altitude and temperature
-         */
-        baro_data = evbBaro->getAllData(false);
-        baro_data._baro /= 4096;				// convert 32 bit signed to 20 bit signed value
-        num_whole = baro_data._baro / 16;		//	18 bit signed significant integer
-        num_frac = (baro_data._baro & 0xF) * 625 / 100;		// 4 bit fractional .0625 per bit
-        sprintf(txtstr,"Alti=%ld.%03d m", num_whole, num_frac);
-        evbLCD->writeText(0,5,font_6x8,txtstr,strlen(txtstr));
-        num_whole = baro_data._temp / 16;		// 8 bit signed significant integer
-        num_frac = (baro_data._temp & 0x0F) * 625 / 100;		// 4 bit fractional .0625 per bit
-        sprintf(txtstr,"Temp=%ld.%03d C", num_whole, num_frac);
-        evbLCD->writeText(0,6,font_6x8,txtstr,strlen(txtstr));
-
-        /*
-         * retrieve and print out Ambient Light level
-         */
-        lux_data = evbAmbLight->getData();
-        num_whole = lux_data * 24 / 100;		// 16000 lux full scale .24 lux per bit
-        num_frac = lux_data * 24 % 100;
-        sprintf(txtstr, "Light=%ld.%02d lux", num_whole, num_frac );
-        evbLCD->writeText(0,7,font_6x8,txtstr,strlen(txtstr));
-
-        evbLCD->endUpdate();
-        printf("finished iteration %d\n\r",(++i));
 
-        if (i % pckt_time == 0) { // check packet counter will send packet every 2-5-10 data collection loops
-            mdot_data.clear();
-            // we will send a simple byte descriptor of the current position of the device: 01 is laying flat, 02 is vertically oriented
-            mdot_data.push_back(0x00);
-            mdot_data.push_back(position_value);
-			/*mdot_data.push_back(0x0E);			// key for Current Acceleration 3-Axis Value
-            converts.f_s = accel_data._x *4;		// shift data 2 bits while retaining sign
-            mdot_data.push_back(converts.t_u[1]);	// get 8 MSB of 14 bit value
-            converts.f_s = accel_data._y * 4;		// shift data 2 bits while retaining sign
-            mdot_data.push_back(converts.t_u[1]);	// get 8 MSB of 14 bit value
-            converts.f_s = accel_data._z * 4;		// shift data 2 bits while retaining sign
-            mdot_data.push_back(converts.t_u[1]);	// get 8 MSB of 14 bit value
-            */
-            /*
-            mdot_data.push_back(0x08);			// key for Current Pressure Value
-            convertl.f_u = pressure;				// pressure data is 20 bits unsigned
-            mdot_data.push_back(convertl.t_u[2]);
-            mdot_data.push_back(convertl.t_u[1]);
-            mdot_data.push_back(convertl.t_u[0]);
-            mdot_data.push_back(0x05);			// key for Current Ambient Light Value
-            converts.f_u = lux_data;				// data is 16 bits unsigned
-            mdot_data.push_back(converts.t_u[1]);
-            mdot_data.push_back(converts.t_u[0]);
-            mdot_data.push_back(0x0B);			// key for Current Temperature Value
-            converts.f_s = baro_data._temp;		// temperature is signed 12 bit
-            mdot_data.push_back(converts.t_u[1]);
-            mdot_data.push_back(converts.t_u[0]);
-*/
-			if((reflected_value  != position_value)||(position_changed)||(0 == (i%100)))
-			{
-	            if ((mdot_ret = mdot_radio->send(mdot_data)) != mDot::MDOT_OK) {
-	                log_error(mdot_radio, "failed to send", mdot_ret);
-	            } else {
-	                printf("successfully sent data to gateway\r\n");
-	                mdot_data.clear();
-	                if ((mdot_ret = mdot_radio->recv(mdot_data)) != mDot::MDOT_OK) {
-		                log_error(mdot_radio,"failed to recv:", mdot_ret);
-		            } else {
-		                printf("recv data: ");
-		                for(int i = 0;i < mdot_data.size();i++)
-		                	printf("%02X",mdot_data[i]);
-	                	printf("\r\n");
-	                	reflected_value = mdot_data[0];
-	                	if(reflected_value == position_value)
-	                	{
-	                		evbBackLight->setLEDCurrent(16);
-	                		position_changed = false;
-                		}
-	                	else
-	                		evbBackLight->setLEDCurrent(0);
-	                }
-	            }
-            }
-        }
-    } while(!exit_program /*&& (i < 65000)*/);
-
+#ifdef MDOT_EVB
     evbBaro->triggerOneShot();
-
     do {
         osDelay(200);			// allows other threads to process
         result = evbBaro->getStatus();
@@ -594,15 +773,15 @@
     baro_data = evbBaro->getAllData(true);
     printf ("minBaro=%ld maxBaro=%ld minTemp=%d maxTemp=%d\n\r", baro_data._minbaro, baro_data._maxbaro,
             baro_data._mintemp, baro_data._maxtemp);
-
-    printf("End of Test\n\r");
-
     evbLCD->clearBuffer();
     sprintf(txtstr,"Exiting Program");
     evbLCD->writeText(0,4,font_6x8,txtstr,strlen(txtstr));
+#endif
 
+    printf("End of Test\n\r");
 }
 
+#ifdef MDOT_EVB
 /*
  * Sets pb1_low flag. Slag is cleared in pb1_debounce thread
  */
@@ -677,6 +856,7 @@
         Thread::wait(5);
  	}
  }
+#endif
 
 /*
  *  Function that print clear text verion of mDot errors