Quick Deploy Sensor Cone

/*************************************************************************
 * Project: Quick Deloy Sensor Cone                                      * 
 * Team member: Hoa Phan, Thien Phung, Nghia Tran                        * 
 * Date: 6-Aug-2020                                                      *
**************************************************************************/
 /* Setting and dependencies                                                                                      
 * Preferences setting:                                                              
 * File -> Preferences
 * Paste the line below in to Additional Boards Manager URLs
 * https://grumpyoldpizza.github.io/ArduinoCore-stm32l0/package_stm32l0_boards_index.json 
 * 
 * Add Board package:
 * Tools -> Board: -> Boards Manager.... 
 * Search Tlera Corp STM32L0 Board
 * Install the board 
 *  
 * Include *.zip files:
 *  - HTS221-master
 *  - LIS2DW12-master
 *  - LIS2MDL-master
 *  - LPS22HH-master
 *  - LSM6DSO-master
 *  - LSM6DSOX-master
 *  - STTS751-master
 *  - CayenneLPP-master
 *  - MicroNMEA-master
 *  - X-NUCLEO-GNSS1A1-master
 *  - X-NUCLEO-IKS01A3-master
 *  
 * Error compilation: ‘NULL’ was not declared in this scope 
 * ### Temporary Manual Fix from owner                            
 * This issue has been fixed but not released yet, until then you will need to insert
 * three lines of code in a library file. Insert the following three lines below at 
 * line 30 in file `Callback.h` found at the path below.  
 *  ```
 *  #ifndef NULL
 *  #define NULL  0
 *  #endif
 * ```
 * Into the file found here:  
 * linux: /home/{user}/.arduino15/packages/TleraCorp/hardware/stm32l0/0.0.10/cores/arduino  
 * windows: C:\Users\\{User}\AppData\Local\Arduino15\packages\TleraCorp\hardware\stm32l0\0.0.10\cores\arduino  
 * windows alternative: C:\Users\\{user}\Documents\ArduinoData\packages\TleraCorp\hardware\stm32l0\0.0.10\cores\arduino  
 *
 * Functions list
 * void setup()
 * void setupLight() 
 * void setupConsole()
 * void setupMEMS()
 * void setupGPS()
 * void setupLoRa()
 * void async_timer_send()
 * void readMemsSensors()
 * void showMEMSData()
 * void readGPS()
 * void packData()
 * void gpsHardwareReset()
 * void readDistance()
 * void parseData()
 * void showDistance()
 * void calculateRollPitchHead()
 * void turnRedLightOn()
 * void turnRedLightOff()
 * void turnGreenLightOn()
 * void turnGreenLightOff()
 * void loop(void) 
******************************************************************************/

#include "LoRaWAN.h"
#include "TimerMillis.h"
#include <CayenneLPP.h>
#include <MicroNMEA.h>

#include <LSM6DSOSensor.h>
#include <LIS2DW12Sensor.h>
#include <LIS2MDLSensor.h>
#include <LPS22HHSensor.h>
#include <STTS751Sensor.h>
#include <HTS221Sensor.h>

const char *devEui = "AC9A4A0B44C49E1A";
const char *appEui = "A8809158BDA17B81";
const char *appKey = "957122A2EDD529701D2B8FF2DFF75AFE";

// Define
#define PI  3.14159
#define GPS_RESET_PIN   7
#define GPS_PPS_PIN     6
#define RED_LIGHT_PIN   12
#define GREEN_LIGHT_PIN 13

const uint32_t TX_INTERVAL = 2000;  // 2 seconds
TimerMillis timer_send;

// variables to hold MEMS Sensors from X-NUCLEO-IKS01A3 board
LSM6DSOSensor *AccGyr;
LIS2MDLSensor *Mag;
LPS22HHSensor *PressTemp;
HTS221Sensor *HumTemp;
float humidity = 0;
float temperature = 0;
float pressure = 0;
int32_t accelerometer[3];
int32_t gyroscope[3];
int32_t magnetometer[3];
int acc_x, acc_y, acc_z;
int mag_x, mag_y, mag_z;
float f_roll, f_pitch, f_head;
int angle_x, angle_y, angle_z;
int angle_roll, angle_pitch, angle_head;

// variables to hold GPS data from X-NUCLEO-GNSS1A1
float longitude_mdeg;
float latitude_mdeg;
long alt;

// MicroNMEA library structures
char nmeaBuffer[100];
MicroNMEA nmea(nmeaBuffer, sizeof(nmeaBuffer));
bool ledState = LOW;
volatile bool ppsTriggered = false;
void ppsHandler(void);
void ppsHandler(void) { ppsTriggered = true; }

// variable to hold the parsed data for distance sensor array
const byte numChars = 64;
char receivedChars[numChars];
char tempChars[numChars];      // temporary array fo use when parsing
char messageSensor[numChars] = {0};
int distanceA, distanceB, distanceC, distanceD = 0;
int distanceE, distanceF, distanceG, distanceH = 0;

boolean newData = false;

// Refer to serial devices by use
HardwareSerial &console = Serial;
HardwareSerial &gps = Serial1;

// Cayenne low power package
CayenneLPP lpp(51);

// Flag for uplink schedule
static volatile bool uplink_attempted;


// This flag control how frequent sensor read
int loopCnt = 0;

/******************************************************************************/
/* Setup and initialize                                                       */
/******************************************************************************/
void setup() {
  setupConsole();   // Share Serial for console and sonar sensor array
  setupLight();     // Setup output for red and green light control
  setupMEMS();      // Setup I2C for MEMS and envrironmental sensor interface
  setupGPS();       // Setup Serial1 and input/output pin for GPS interface
  setupLoRa();      // Setup frequency, account, credit, and connection
}
/********************************************************************/
/* Setup ouput pin for Lights                                       */                             
/********************************************************************/
void setupLight() {
  pinMode(RED_LIGHT_PIN, OUTPUT);
  digitalWrite(RED_LIGHT_PIN, LOW);
  pinMode(GREEN_LIGHT_PIN, OUTPUT);
  digitalWrite(GREEN_LIGHT_PIN, LOW);
}

/********************************************************************/
/* setup console and distance sensor array. The console and distance*/
/* sensor arrary share serial                                       */ 
/* Interface: Serial, baudrate: 115200                              */                              
/********************************************************************/
void setupConsole() {
  console.begin(115200);       
  delay(2000);
  Serial.print("\r\n"); 
  Serial.println("****************************************************"); 
  Serial.println("Project: Quick Deploy Sensor Cone");
  Serial.println("Date: 6-August-2020");
  Serial.println("Team members: Hoa Phan, Thien Phung, and Nghia Tran"); 
  Serial.println("****************************************************");            
  Serial.println("Console and traffic cone init: Done.");

}

/********************************************************************/
/* setup MEMS and environmental sensor                              */
/* Interface: I2C                                                   */                              
/********************************************************************/
void setupMEMS() {
  Serial.print("Mems and environmental sensor init...: ");
  Wire.begin();
  // Enable Sensors
  AccGyr = new LSM6DSOSensor (&Wire);
  AccGyr->Enable_X();
  AccGyr->Enable_G();

  Mag = new LIS2MDLSensor (&Wire);
  Mag->Enable();
  
  PressTemp = new LPS22HHSensor(&Wire);
  PressTemp->Enable();
  HumTemp = new HTS221Sensor (&Wire);
  HumTemp->Enable();
  Serial.print("Done.\r\n");
  
}

/********************************************************************/
/* setup GPS                                                        */
/* Pinterface: Serial1, baudrate: 9600                               */
/* Pin 7: output for GPS reset                                      */
/* Pin 6: interrutp input for PPS                                   */                                 
/********************************************************************/
void setupGPS() {
  delay (3000);

  // Serial interface to GPS
  Serial.print("GPS init...: ");
  gps.begin(9600);
  
 
  // Start GPS the module
  Serial.print("1 ");
  pinMode(GPS_RESET_PIN, OUTPUT);
  digitalWrite(GPS_RESET_PIN, HIGH);
  gpsHardwareReset();
  delay(300);
  
  // Change the echoing messages to the ones recognized by the MicroNMEA library
  Serial.print("2 ");
  MicroNMEA::sendSentence(gps, "$PSTMSETPAR,1201,0x00000042");
  MicroNMEA::sendSentence(gps, "$PSTMSAVEPAR");

  // Reset the device so that the changes could take plaace
  MicroNMEA::sendSentence(gps, "$PSTMSRR");
  Serial.print("3 ");

  // clear serial buffer
  while (gps.available())
    gps.read();

  Serial.print("4 ");
  pinMode(GPS_PPS_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(GPS_PPS_PIN), ppsHandler, RISING); 
  Serial.print("Done.\r\n");
  delay(1000);
}


/********************************************************************/
/*  Configuration LoRa frequency and account                        */
/*  Frequency: 915 MHz                                              */
/*  SubBand: 2                                                      */
/********************************************************************/
void setupLoRa() {
  // US Region
  LoRaWAN.begin(US915);
  // Helium SubBand
  LoRaWAN.setSubBand(2);
  // Disable Adaptive Data Rate
  LoRaWAN.setADR(false);
  // Set Data Rate 1 - Max Payload 53 Bytes
  LoRaWAN.setDataRate(1);
  // Device IDs and Key
  LoRaWAN.joinOTAA(appEui, appKey, devEui);
  Serial.println("JOIN( )");
  
  while (!LoRaWAN.joined() && LoRaWAN.busy()) {
    Serial.println("JOINING( )");
    delay(5000);
  }
  Serial.println("JOINED( )");

  // Start Continuous Uplink Timer
  timer_send.start(async_timer_send, 0, TX_INTERVAL);
}

/*********************************************************************************************/
/* Send packet                                                                               */
/*********************************************************************************************/
void async_timer_send() {
  if (LoRaWAN.joined() && !LoRaWAN.busy()) {
     //Pack data getting ready to send
     packData();
     // Send Packet
     LoRaWAN.sendPacket(1, lpp.getBuffer(), lpp.getSize());
    uplink_attempted = true;
  }
}

/********************************************************************/
/* Read MEMS and enviromental sensor from X-NUCLEO-IKS01A3          */
/********************************************************************/
void readMemsSensors() {
  // Read humidity and temperature.
  // float humidity = 0;
  //float temperature = 0;
  HumTemp->GetHumidity(&humidity);
  HumTemp->GetTemperature(&temperature);

  // Read pressure and temperature.
 // float pressure = 0;
  PressTemp->GetPressure(&pressure);
  delay(1);
  // Read accelerometer and gyroscope.
 // int32_t accelerometer[3];
  //int32_t gyroscope[3];
  AccGyr->Get_X_Axes(accelerometer);
  AccGyr->Get_G_Axes(gyroscope);

  // Set max, min limit value for acc_x, acc_y, acc_z
  if(accelerometer[0] > 1000) acc_x = 1000;
  else if(accelerometer[0] < -1000) acc_x = -1000;
  else acc_x = accelerometer[0];
  
  if(accelerometer[1] > 1000) acc_y = 1000;
  else if(accelerometer[1] < -1000) acc_y = -1000;
  else acc_y = accelerometer[1];
  
  if(accelerometer[2] > 1000) acc_z = 1000;
  else if(accelerometer[2] < -1000) acc_z = -1000;
  else acc_z = accelerometer[2];
  
  // Read magnetometer
  //int32_t magnetometer[3];
  Mag->GetAxes(magnetometer);
  mag_x = magnetometer[0];
  mag_y = magnetometer[1];
  mag_z = magnetometer[2];
  /*
  // Clear Payload
  lpp.reset();
  
  // Pack Packload
  lpp.addTemperature(1, temperature);
  lpp.addRelativeHumidity(2, humidity);
  lpp.addBarometricPressure(3, pressure); 
  lpp.addAccelerometer(4, accelerometer[0], accelerometer[1], accelerometer[2]);
  lpp.addGyrometer(5, gyroscope[0], gyroscope[1], gyroscope[2]);

 */
}

/********************************************************************/
/* Read MEMS and enviromental sensor from X-NUCLEO-IKS01A3          */
/********************************************************************/
/*void showMEMSData(){
  // Debug Print Data 
  Serial.print("\r\n");
  Serial.print("| Hum[%]: ");
  Serial.print(humidity, 2);
  Serial.print(" | Temp[C]: ");
  Serial.print(temperature, 2);
  Serial.print(" | Pres[hPa]: ");
  Serial.print(pressure, 2);
  Serial.print(" | Acc[mg]: ");
  Serial.print(acc_x);//(accelerometer[0]);
  Serial.print(" ");
  Serial.print(accelerometer[1]);
  Serial.print(" ");
  Serial.print(accelerometer[2]);
  Serial.print(" | Gyr[mdps]: ");
  Serial.print(gyroscope[0]);
  Serial.print(" ");
  Serial.print(gyroscope[1]);
  Serial.print(" ");
  Serial.print(gyroscope[2]);
  Serial.print(" | Mag[mT]: ");
  Serial.print(" ");
  Serial.print(magnetometer[0]);
  Serial.print(" ");
  Serial.print(magnetometer[1]);
  Serial.print(" ");
  Serial.println(magnetometer[2]);
  
  Serial.print("\r\n");
}
*/

/********************************************************************/
/* Read MEMS and enviromental sensor from X-NUCLEO-IKS01A3          */
/********************************************************************/
void showMEMSData(){
  // Debug Print Data 
  Serial.print("\r\n");
  Serial.print("Hum[%]: ");
  Serial.print(humidity, 2);
  Serial.print(" Temp[C]: ");
  Serial.print(temperature, 2);
  Serial.print(" Pres[hPa]: ");
  Serial.print(pressure, 2);
  Serial.print(" Acc[mg]: ");
  Serial.print(acc_x);//(accelerometer[0]);
  Serial.print(" ");
  Serial.print(accelerometer[1]);
  Serial.print(" ");
  Serial.print(accelerometer[2]);
  Serial.print(" Gyr[mdps]: ");
  Serial.print(gyroscope[0]);
  Serial.print(" ");
  Serial.print(gyroscope[1]);
  Serial.print(" ");
  Serial.print(gyroscope[2]);
  Serial.print(" Mag[uT]: ");
  Serial.print(" ");
  Serial.print(magnetometer[0]);
  Serial.print(" ");
  Serial.print(magnetometer[1]);
  Serial.print(" ");
  Serial.println(magnetometer[2]);
  
  Serial.print("\r\n");
}


/********************************************************************/
/* Read GPS data from X-NUCLEO-GNSS1A1                              */
/********************************************************************/
void readGPS() {
  // If a message is received
  if (ppsTriggered) {
    ppsTriggered = false;

   // Clear Payload
   // lpp.reset();

    latitude_mdeg = nmea.getLatitude();
    longitude_mdeg = nmea.getLongitude();
    nmea.getAltitude(alt);

  //   lpp.addGPS(6, latitude_mdeg / 1000000, longitude_mdeg / 1000000,
  //             alt / 1000);

    nmea.clear();
  }

  // While the message isn't complete
  while (!ppsTriggered && gps.available()) {
    // Fetch the character one by one
    char c = gps.read();
    //Serial.print(c);
    // Pass the character to the library
    nmea.process(c);
  }
}


/********************************************************/
/* Pack data for Cayenne lLP                            */
/********************************************************/
void packData(){
  // Clear Payload
  lpp.reset();
  
  // Pack Packload
  lpp.addTemperature(1, temperature);
  lpp.addRelativeHumidity(2, humidity);
  lpp.addBarometricPressure(3, pressure); 
  lpp.addAccelerometer(4, accelerometer[0], accelerometer[1], accelerometer[2]);
  lpp.addGyrometer(5, gyroscope[0], gyroscope[1], gyroscope[2]);
  lpp.addGPS(6, latitude_mdeg / 1000000, longitude_mdeg / 1000000, alt / 1000);

}
/********************************************************/
/* Reset GPS module                                     */
/********************************************************/
void gpsHardwareReset() {
  // Empty input buffer
  while (gps.available())
    gps.read();

  // reset the device
  digitalWrite(GPS_RESET_PIN, LOW);
  delay(50);
  digitalWrite(GPS_RESET_PIN, HIGH);

  // wait for reset to apply
  delay(4000);
}

/****************************************************************************************************/
/* Read Distance sensor array module.                                                               */
/* Data sent from distance sensor array is a string via serial 115200bps format as below            */ 
/* New set of distance is update every half of second                                               */                                                
/* <Sonar, distanceA, distanceB, distanceC, distanceD, distanceE, distanceF, distanceG, distanceH>  */
/* Example: <Sonar,1234,2345,3245,325,2344,886,12887,1123>                                          */
/* Ref: https://forum.arduino.cc/index.php?topic=396450.0                                           */
/****************************************************************************************************/
void readDistance(){
  static boolean recvInProgress = false;
  static byte ndx = 0;
  char startMarker = '<';
  char endMarker = '>';
  char rc;

  while (Serial.available() > 0 && newData == false){
    rc = Serial.read();

    if (recvInProgress == true) {
      if (rc != endMarker) {
        receivedChars[ndx] = rc;
        ndx++;
        if (ndx >= numChars) {
          ndx = numChars - 1;          
        }
      }
      else {
        receivedChars[ndx] = '\0'; // terminate the string
        recvInProgress = false;
        ndx = 0;
        newData = true;
      }
    }

    else if (rc == startMarker) {
      recvInProgress = true;
    }
  }
}

/*********************************************************************************************/
/* Parse Distance Sensor Data                                                                 */
/* There eight distance senor A, B, C, D, E, F, G, and H                                      */
/* Ref: https://forum.arduino.cc/index.php?topic=396450.0                                     */
/**********************************************************************************************/
void parseData() {  //split the data into its parts
  char * strtokIndx;  // this is used by strtok() as an index

  strtokIndx = strtok(tempChars, ",");    // get the first part - the string
  strcpy(messageSensor, strtokIndx);      // copy it to messageFromSensor
  
  strtokIndx = strtok(NULL, ",");         // this continues where the previous call left off
  distanceA = atoi(strtokIndx);       // convert this part to an integer
  strtokIndx = strtok(NULL, ",");         // this continues where the previous call left off
  distanceB = atoi(strtokIndx);       // convert this part to an integer
  strtokIndx = strtok(NULL, ",");         // this continues where the previous call left off
  distanceC = atoi(strtokIndx);       // convert this part to an integer
  strtokIndx = strtok(NULL, ",");         // this continues where the previous call left off
  distanceD = atoi(strtokIndx);       // convert this part to an integer
  strtokIndx = strtok(NULL, ",");         // this continues where the previous call left off
  distanceE = atoi(strtokIndx);       // convert this part to an integer
  strtokIndx = strtok(NULL, ",");         // this continues where the previous call left off
  distanceF = atoi(strtokIndx);       // convert this part to an integer
  strtokIndx = strtok(NULL, ",");         // this continues where the previous call left off
  distanceG = atoi(strtokIndx);       // convert this part to an integer
  strtokIndx = strtok(NULL, ",");         // this continues where the previous call left off
  distanceH = atoi(strtokIndx);       // convert this part to an integer
  
}
/*********************************************************************************************/
/* Show Distance Sensor Data                                                                 */
/* There eight distance senor A, B, C, D, E, F, G, and H                                      */
/**********************************************************************************************/
void showDistance() {
  Serial.print("\r\n");
  Serial.print(messageSensor);
  Serial.print(" ");
  Serial.print("A:");
  Serial.print(distanceA);
  Serial.print(" B:");
  Serial.print(distanceB);
  Serial.print(" C:");
  Serial.print(distanceC);
  Serial.print(" D:");
  Serial.print(distanceD);
  Serial.print(" E:");
  Serial.print(distanceE);
  Serial.print(" F:");
  Serial.print(distanceF);
  Serial.print(" G:");
  Serial.print(distanceG); 
  Serial.print(" H:");
  Serial.print(distanceH); 
  Serial.print("\r\n");
}


/*********************************************************************************************/
/* Calculate  Roll, Pitch, Yaw (heading)                                                     */
/* Orignal source: ST Software package: x-cube-mems1, file motion_ec_manager.c               */
/* This function requires data from 3 axis of accelerometer and 3 axis of magnetometer       *
/*********************************************************************************************/
void calculateRollPitchHead() {
  
  float fNormAcc,fSinRoll,fCosRoll,fSinPitch,fCosPitch;
  float fTiltedX,fTiltedY;
  float fAcc[3];
  float f_acc_x, f_acc_y, f_acc_z;
  float f_mag_x, f_mag_y, f_mag_z;
  int i;

   /* Rescale the accelerometer radings (in order to increase accuracy in the following norm computation) */
  // Board on horizonal(on test bench) orientation config
  // Accelerometer data  
 /* f_acc_x = (float) acc_x/1000.0;
  f_acc_y = (float) acc_y/1000.0;
  f_acc_z = (float) acc_z/1000.0;

  // Magnetometer data
  f_mag_x = (float) mag_x/1000.0;
  f_mag_y = (float) mag_y/1000.0;
  f_mag_z = (float) mag_z/1000.0;
*/

  //Board on vertial (on pole) orienation config
  // Accelerometer data  
  f_acc_x = (float) acc_z/1000.0;
  f_acc_y = (float) acc_y/1000.0;
  f_acc_z = (float) acc_x/1000.0;

  // Magnetometer data
  f_mag_x = (float) mag_z/1000.0;
  f_mag_y = (float) mag_y/1000.0;
  f_mag_z = (float) mag_x/1000.0;



  // Test print out
  //Serial.println(f_acc_x); Serial.println(f_acc_y); Serial.println(f_acc_z);
  //Serial.println(f_mag_x); Serial.println(f_mag_y); Serial.println(f_mag_z);
  
    /* Compute the scaled acceleration vector norm */
  fNormAcc = sqrt(pow(f_acc_x,2) + pow(f_acc_y,2) + pow(f_acc_z,2));

  /* Compute some useful parameters for the g vector rotation matrix */
  fSinRoll = f_acc_y/sqrt(pow(f_acc_y,2) + pow(f_acc_z,2));
  fCosRoll = sqrt(1.0 - fSinRoll * fSinRoll);
  fSinPitch = - f_acc_x/fNormAcc;
  fCosPitch = sqrt(1.0 - fSinPitch * fSinPitch);
  
  /* Apply the rotation matrix to the magnetic field vector to obtain the X and Y components on the earth plane */
  fTiltedX = f_mag_x * fCosPitch + f_mag_z * fSinPitch;
  fTiltedY = f_mag_x * fSinRoll * fSinPitch + f_mag_y * fCosRoll - f_mag_z * fSinRoll * fCosPitch;
 
    
  /* return the heading angle expressed in degree */
  f_head = -atan2(fTiltedY, fTiltedX);
  f_head = f_head * 180.0/PI;
  f_head = floor(f_head * 100.0 + 0.5) / 100.0; /* Rounds number to two decimal digits */
  f_head = (f_head < 0.0) ? (f_head + 360.0) : f_head; /* Change negative value to be in range <0,360) */
  
  /* return the roll and pitch angles */
  f_roll  = atan2(fSinRoll,fCosRoll);
  f_pitch = atan2(fSinPitch,fCosPitch);

  /* Make roll, pitch, head into integer */
  angle_roll = (int)(f_roll * 180.0/PI);
  angle_pitch = (int) (f_pitch * 180.0/PI);
  angle_head = (int) f_head;
  
}


/*********************************************************************************************/
/* Turn ON/OFF Red/Green Light                                                                               */
/*********************************************************************************************/
void turnRedLightOn(){
     digitalWrite(RED_LIGHT_PIN, HIGH);
}
void turnRedLightOff(){
     digitalWrite(RED_LIGHT_PIN, LOW);
}
void turnGreenLightOn(){
     digitalWrite(GREEN_LIGHT_PIN, HIGH);
}
void turnGreenLightOff(){
     digitalWrite(GREEN_LIGHT_PIN, LOW);
}
/*********************************************************************************************/
/* Main loop                                                                                 */
/*********************************************************************************************/

void loop(void) {
  if (uplink_attempted) {
    Serial.println("***************************************************************");
/*    Serial.print("TRANSMIT( ");
    Serial.print("TimeOnAir: ");
    Serial.print(LoRaWAN.getTimeOnAir());
    Serial.print(", NextTxTime: ");
    Serial.print(LoRaWAN.getNextTxTime());
    Serial.print(", MaxPayloadSize: ");
    Serial.print(LoRaWAN.getMaxPayloadSize());
    Serial.print(", DR: ");
    Serial.print(LoRaWAN.getDataRate());
    Serial.print(", TxPower: ");
    Serial.print(LoRaWAN.getTxPower(), 1);
    Serial.print("dbm, UpLinkCounter: ");
    Serial.print(LoRaWAN.getUpLinkCounter());
    Serial.print(", DownLinkCounter: ");
    Serial.print(LoRaWAN.getDownLinkCounter());
//  Serial.println(" )");
    //latitude = nmea.getLatitude();
*/    //longitude = nmea.getLongitude();
    Serial.print("Latitude (deg): ");
    //Serial.print(latitude / 1000000., 6);
    Serial.print(latitude_mdeg / 1000000., 6);
    Serial.print(" Longitude (deg): ");
    //Serial.print(longitude / 1000000., 6);
    Serial.print(longitude_mdeg / 1000000., 6);
    Serial.print("  Altitude (m): ");
    //if (nmea.getAltitude(alt))
    Serial.println(alt / 1000., 3);
    //else
     // Serial.println("not available");

    // Print sensor data
    showMEMSData();

    // Print roll, pitch, heading
    Serial.print("Roll: ");
    Serial.print(angle_roll);
    Serial.print(" Pitch: ");
    Serial.print(angle_pitch);
    Serial.print(" Head: "); 
    Serial.println(angle_head);

    // Print distance from 8-sensor array
    showDistance();
    Serial.print("\r\n");
    
    uplink_attempted = false;
  }

     // Read MEMS and environmental sensor
      readMemsSensors();
      
      // Calculate roll, pitch, and heading
      calculateRollPitchHead();

     // Read GPS data for lat, long, and altitude
      readGPS();
  
     // Read distance from array sensor module
      readDistance();
      if (newData == true) {
        strcpy(tempChars, receivedChars);
          // this tempoary copy is nessessary to protec the original data
          // because strtok(P used in parseData() replaces the commas with \0
        parseData();
        newData = false;  
      }

     // The array of distance sensor allow to detect an object(s) in 8 directions in range of 2 meters
     // This information can be used to monitor the motion of object and be update on the cloud. The distance
     // data combine with heading information calculate above allows to know there is/are object(s) in direction
     // relative to earth north pole.
     // For now a simple senario of detection object(s)
     // If there no object in 1.5m, light turn off red and turn on green
     // If there is/are objects in 1.5m in any directionion, turn on red light and turn off green light
     
    if( distanceA < 1500 || distanceB < 1500 || distanceC < 1500 || distanceD < 1500 ||
        distanceE < 1500 || distanceF < 1500 || distanceG < 1500 || distanceG < 1500)
      {
        turnGreenLightOff();
        turnRedLightOn();
      }
    else
      {
        turnGreenLightOn();
        turnRedLightOff();
      }
  
  }