Diff: main.cpp

Revision:

33:e47306c32791

Parent:

31:f03c183e2bf6

Child:

34:341fb423e74b

--- a/main.cpp	Fri Aug 03 16:34:24 2018 +0000
+++ b/main.cpp	Wed Aug 15 22:16:56 2018 +0000
@@ -16,6 +16,8 @@
  */
 #include <stdio.h>
 #include "mbed.h"
+//#include "unsupported/USBDevice/USBSerial/USBSerial.h"
+#include "USBSerial.h"
 
 #include "lorawan/LoRaWANInterface.h"
 #include "lorawan/system/lorawan_data_structures.h"
@@ -91,35 +93,624 @@
  */
 static lorawan_app_callbacks_t callbacks;
 
-int mytime;
-int mybatt;
-double mylat;
-double mylon;
+int mytime = 0;
+int mybatt = 0;
+double mylat = 0;
+double mylon = 0;
+
+int16_t myAccX = 0;
+int16_t myAccY = 0;
+int16_t myAccZ = 0;
+int16_t myMagX = 0;
+int16_t myMagY = 0;
+int16_t myMagZ = 0;
+int16_t myOffX = 0;
+int16_t myOffY = 0;
+int16_t myOffZ = 0;
+int16_t myTemp = 0;
+
+int16_t accMinX = 0;
+int16_t accMinY = 0;
+int16_t accMinZ = 0;
+int16_t accMaxX = 0;
+int16_t accMaxY = 0;
+int16_t accMaxZ = 0;
+
+int16_t magMinX = 0;
+int16_t magMinY = 0;
+int16_t magMinZ = 0;
+int16_t magMaxX = 0;
+int16_t magMaxY = 0;
+int16_t magMaxZ = 0;
+
+#define NEOM8M_ADR_GPS 0x84
+#define LSM303_ADR_ACC 0x32
+#define LSM303_ADR_MAG 0x3C
 
-#define NEOM8M_ADR_GPS (0x42 >> 1)
-#define LSM303_ADR_ACC (0x32 >> 1)
-#define LSM303_ADR_MAG (0x3C >> 1)
+#define NEOM8M_REG_GPS_LENH 0xFD
+#define NEOM8M_REG_GPS_LENL 0xFE
+#define NEOM8M_REG_GPS_DATA 0xFE
+#define LSM303_REG_ACC_STATUS_REG_AUX_A 0x07
+#define LSM303_REG_ACC_OUT_TEMP_L_A 0x0C
+#define LSM303_REG_ACC_OUT_TEMP_H_A 0x0D
+#define LSM303_REG_ACC_WHO_AM_I_A 0x0F
+#define LSM303_REG_ACC_TEMP_CFG_REG_A 0x1F
+#define LSM303_REG_ACC_CTRL_REG1_A 0x20
+#define LSM303_REG_ACC_CTRL_REG2_A 0x21
+#define LSM303_REG_ACC_CTRL_REG3_A 0x22
+#define LSM303_REG_ACC_CTRL_REG4_A 0x23
+#define LSM303_REG_ACC_CTRL_REG5_A 0x24
+#define LSM303_REG_ACC_STATUS_REG_A 0x27
+#define LSM303_REG_ACC_OUT_X_L_A 0x28
+#define LSM303_REG_ACC_OUT_X_H_A 0x29
+#define LSM303_REG_ACC_OUT_Y_L_A 0x2A
+#define LSM303_REG_ACC_OUT_Y_H_A 0x2B
+#define LSM303_REG_ACC_OUT_Z_L_A 0x2C
+#define LSM303_REG_ACC_OUT_Z_H_A 0x2D
+#define LSM303_REG_ACC_INT1_CFG_A 0x30
+#define LSM303_REG_ACC_INT1_SRC_A 0x31
+#define LSM303_REG_ACC_INT1_THS_A 0x32
+#define LSM303_REG_ACC_INT1_DURATION_A 0x33
+//#define LSM303_REG_MAG_CRA_REG_M 0x00
+//#define LSM303_REG_MAG_MR_REG_M 0x02
+#define LSM303_REG_MAG_OFFSET_X_REG_L_M 0x45
+#define LSM303_REG_MAG_OFFSET_X_REG_H_M 0x46
+#define LSM303_REG_MAG_OFFSET_Y_REG_L_M 0x47
+#define LSM303_REG_MAG_OFFSET_Y_REG_H_M 0x48
+#define LSM303_REG_MAG_OFFSET_Z_REG_L_M 0x49
+#define LSM303_REG_MAG_OFFSET_Z_REG_H_M 0x4A
+#define LSM303_REG_MAG_WHO_AM_I_M 0x4F
+#define LSM303_REG_MAG_CFG_REG_A_M 0x60
+#define LSM303_REG_MAG_CFG_REG_C_M 0x62
+#define LSM303_REG_MAG_INT_CTRL_REG_M 0x63
+#define LSM303_REG_MAG_INT_SOURCE_REG_M 0x64
+#define LSM303_REG_MAG_INT_THS_L_REG_M 0x65
+#define LSM303_REG_MAG_INT_THS_H_REG_M 0x66
+#define LSM303_REG_MAG_STATUS_REG_M 0x67
+#define LSM303_REG_MAG_OUTX_L_REG_M 0x68
+#define LSM303_REG_MAG_OUTX_H_REG_M 0x69
+#define LSM303_REG_MAG_OUTY_L_REG_M 0x6A
+#define LSM303_REG_MAG_OUTY_H_REG_M 0x6B
+#define LSM303_REG_MAG_OUTZ_L_REG_M 0x6C
+#define LSM303_REG_MAG_OUTZ_H_REG_M 0x6D
+
 
 I2C i2c(PB_9, PB_8);
+InterruptIn accPin(PB_14);
+InterruptIn magPin(PA_10);
+
+char cfg;
+char ret;
+char rda = '\0';
+char cmd[2];
+char buf[83];
+int pos = 0;
+char *res;
+char sPass[26] = "[\u001b[32mPASS\u001b[0m]";
+char sFail[26] = "[\u001b[31mFAIL\u001b[0m]";
+int accShift = 0;
+int accScale = 0;
+int accEvent = 0;
+uint16_t accSFire = 0;
+uint16_t accHFire = 0;
+uint16_t accSLIRQ = 0;
+uint16_t accSHIRQ = 0;
+int magEvent = 0;
+uint16_t magSFire = 0;
+uint16_t magHFire = 0;
+uint16_t magSLIRQ = 0;
+uint16_t magSHIRQ = 0;
+
+char cmdSendLoop[9] = "SendLoop";
+
+void onAccIrq()
+{
+    accHFire++;
+}
+
+void onMagIrq()
+{
+    magHFire++;
+}
+
+void accDumpCfg()
+{
+    char start = LSM303_REG_ACC_CTRL_REG1_A;
+    int i;
+    for (i = 0; i < 6; i++)
+    {
+        cmd[0] = start + i;
+        i2c.write(LSM303_ADR_ACC, cmd, 1);
+        i2c.read(LSM303_ADR_ACC, &buf[i], 1);
+    }
+    printf("CFGACC: |%02X %02X %02X %02X %02X %02X|\r\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
+}
+
+void magDumpCfg()
+{
+    char start = LSM303_REG_MAG_CFG_REG_A_M;
+    int i;
+    for (i = 0; i < 3; i++)
+    {
+        cmd[0] = start + i;
+        i2c.write(LSM303_ADR_MAG, cmd, 1);
+        i2c.read(LSM303_ADR_MAG, &buf[i], 1);
+    }
+    printf("CFGMAG: |%02X %02X %02X|\r\n", buf[0], buf[1], buf[2]);
+}
+
+#define LEDR PB_7
+#define LEDG PB_5
+#define LEDB PB_6
+//#define LEDW PB_2
+
+DigitalOut myLedR(LEDR);
+DigitalOut myLedG(LEDG);
+DigitalOut myLedB(LEDB);
+//DigitalOut myLedW(LEDW);
+
+/*int myLedR = 0;
+int myLedG = 0;
+int myLedB = 0;
+int myLedW = 0;*/
+//uint8_t myFoo;
 
 /**
  * Entry point for application
  */
 int main (void)
 {
+    wait(4);
+    printf("\r\n-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-\r\n");
+    USBSerial serial;
+/*    printf("Turning the lights on...\r\n");
+    myLedR = 1;
+    myLedG = 1;
+    myLedB = 1;
+    myLedW = 1;
+    printf("The lights are on.\r\n");
+    wait(0.5);
+    printf("Turning the lights off...\r\n");
+    myLedR = 0;
+    myLedG = 0;
+    myLedB = 0;
+    myLedW = 0;
+    printf("The lights are off.\r\n");
+    wait(0.5);
+    printf("Turning the lights on...\r\n");
+    myLedR = 1;
+    myLedG = 1;
+    myLedB = 1;
+    myLedW = 1;
+    printf("The lights are on.\r\n");
+    wait(0.5);
+    printf("Turning the lights off...\r\n");
+    myLedR = 0;
+    myLedG = 0;
+    myLedB = 0;
+    myLedW = 0;
+    printf("The lights are off.\r\n");
+    printf("Initializing USBSerial.\r\n");
+    USBSerial serial;
+    printf("USBSerial initialized.\r\n");*/
+    int myFoo = 1;
+    while(1)
+    {
+        myLedR = 1;
+        //serial.printf("LED = Red\r\n");
+        wait(0.5);
+        myLedR = 0;
+        myLedG = 1;
+        //serial.printf("LED = Green\r\n");
+        wait(0.5);
+        myLedG = 0;
+        myLedB = 1;
+        //serial.printf("LED = Blue\r\n");
+        wait(0.5);
+        myLedB = 0;
+        //myLedW = 1;
+        //serial.printf("LED = White\r\n");
+        //wait(0.5);
+        //myLedW = 0;
+        if (myFoo % 6 == 0)
+        {
+            myLedR = 1;
+            myLedG = 1;
+            myLedB = 1;
+            //myLedW = 1;
+        }
+        myFoo++;
+    }
+    
+    
+    
     // setup tracing
     setup_trace();
 
     // stores the status of a call to LoRaWAN protocol
     lorawan_status_t retcode;
+    
+    wait(4);
 
     printf("\r\n- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -\r\n");
     
-    mytime = 0;
-    mybatt = 15;
-    mylat = 51.06509;
-    mylon = -114.08895;
+    magDumpCfg();
+    accDumpCfg();
+    
+    /* I2C init */
+    ret = 0x00;
+    
+    cmd[0] = LSM303_REG_MAG_WHO_AM_I_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &ret, 1);
+    res = (ret == 0x40 ? sPass : sFail);
+    printf("MAG WhoAmI: %02x %s\r\n", ret, res);
+    
+    cmd[0] = LSM303_REG_MAG_CFG_REG_A_M;
+    cmd[1] = 0x00;  // Mag = 10 Hz (high-resolution and continuous mode)
+    i2c.write(LSM303_ADR_MAG, cmd, 2);
+    cmd[0] = LSM303_REG_MAG_CFG_REG_C_M;
+    //cmd[1] = 0x01;  // Mag data-ready interrupt enable
+    cmd[1] = 0x40; // Mag enable interrupt on pin
+    i2c.write(LSM303_ADR_MAG, cmd, 2);
+    cmd[0] = LSM303_REG_MAG_CFG_REG_A_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &ret, 1);
+    printf("MAG RetVal: %02x\r\n", ret);
+    
+    cmd[0] = LSM303_REG_ACC_WHO_AM_I_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &ret, 1);
+    res = (ret == 0x33 ? sPass : sFail);
+    printf("ACC WhoAmI: %02x %s\r\n", ret, res);
+    
+    cmd[0] = LSM303_REG_ACC_CTRL_REG1_A;
+    cmd[1] = 0x57;  // Accel = 100 Hz (normal mode)
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG1_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &ret, 1);
+    printf("ACC RetVal: %02x\r\n", ret);
+    
+    // Enable High Resolution Mode
+    cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &ret, 1);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
+    cmd[1] = ret | 0x08;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+
+    // Enable Temp Sensor
+    cmd[0] = LSM303_REG_ACC_TEMP_CFG_REG_A;
+    cmd[1] = 0xC0;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &ret, 1);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
+    cmd[1] = ret | 0x80;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    
+    // Set Full Scale to 4g
+    /*
+    cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &ret, 1);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
+    //cmd[1] = ret | 0x30;              // 16g
+    //cmd[1] = (ret & ~0x10) | 0x20;    // 8g
+    cmd[1] = (ret & ~0x20) | 0x10;      // 4g
+    //cmd[1] = ret & ~0x30;             // 2g
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    */
+/*
+    // IRQ Init from Datasheet.
+    cmd[0] = LSM303_REG_ACC_CTRL_REG1_A;
+    cmd[1] = 0xA7;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG2_A;
+    cmd[1] = 0x00;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG3_A;
+    cmd[1] = 0x40;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
+    cmd[1] = 0x00;
+    //cmd[1] = 0x10;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG5_A;
+    cmd[1] = 0x08;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+*/
+
+    magDumpCfg();
+    accDumpCfg();
+
+    /*
+    // ACC INTERRUPT SETUP
+    // Enable Interrupt Pin
+    cmd[0] = LSM303_REG_ACC_CTRL_REG3_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &ret, 1);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG3_A;
+    cmd[1] = ret | 0x40;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    // Enable Interrupt Latch
+    cmd[0] = LSM303_REG_ACC_CTRL_REG5_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &ret, 1);
+    cmd[0] = LSM303_REG_ACC_CTRL_REG5_A;
+    cmd[1] = ret | 0x08;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+
+    // Set Threshold/Duration/Config
+    cmd[0] = LSM303_REG_ACC_INT1_THS_A;
+    //cmd[1] = 0x10;
+    //cmd[1] = 0x40;
+    //cmd[1] = 0x60;
+    cmd[1] = 0x7D;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    cmd[0] = LSM303_REG_ACC_INT1_DURATION_A;
+    cmd[1] = 0x00;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    cmd[0] = LSM303_REG_ACC_INT1_CFG_A;
+    cmd[1] = 0x2A;
+    //cmd[1] = 0x0A;
+    i2c.write(LSM303_ADR_ACC, cmd, 2);
+    //accPin.rise(&onAccIrq);
+    */
+    
 
+    /*
+    // MAG INTERRUPT SETUP
+    cmd[0] = LSM303_REG_MAG_INT_THS_L_REG_M;
+    cmd[1] = 0xF4;
+    i2c.write(LSM303_ADR_MAG, cmd, 2);
+    cmd[0] = LSM303_REG_MAG_INT_THS_H_REG_M;
+    cmd[1] = 0x01;
+    i2c.write(LSM303_ADR_MAG, cmd, 2);
+    cmd[0] = LSM303_REG_MAG_INT_CTRL_REG_M;
+    cmd[1] = 0xE7;
+    i2c.write(LSM303_ADR_MAG, cmd, 2);
+    magPin.rise(&onMagIrq);
+    */
+    
+    
+    /*while(1)
+    {
+        time_t tNow = time(NULL);
+        cmd[0] = LSM303_REG_ACC_INT1_SRC_A;
+        i2c.write(LSM303_ADR_ACC, cmd, 1);
+        i2c.read(LSM303_ADR_ACC, &ret, 1);
+        printf("%08X - %02X - IRQ: %d\r\n", tNow, ret, irqFired);
+        wait(1);
+    }*/
+    
+    cfg = 0x00;
+    cmd[0] = LSM303_REG_ACC_CTRL_REG1_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &ret, 1);
+    cfg |= (ret & 0x08) >> 3;
+    cmd[0] = LSM303_REG_ACC_CTRL_REG4_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &ret, 1);
+    cfg |= (ret & 0x08) >> 2;
+    accScale = 1 << ((ret & 0x30) >> 4);
+    cmd[0] = LSM303_REG_MAG_CFG_REG_A_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &ret, 1);
+    cfg |= (ret & 0x10);
+    if (cfg & 0x01)
+        accShift = 8;
+    else if (cfg & 0x02)
+        accShift = 4;
+    else
+        accShift = 6;
+    printf("Quality: %02x AccShift: %d AccScale: %d\r\n", cfg, accShift, accScale);
+    
+    /*
+    while(1)
+    {
+        cmd[0] = LSM303_REG_ACC_STATUS_REG_AUX_A;
+        i2c.write(LSM303_ADR_ACC, cmd, 1);
+        i2c.read(LSM303_ADR_ACC, &rda, 1);
+        if (rda & 0x04)
+        {
+            cmd[0] = LSM303_REG_ACC_OUT_TEMP_L_A;
+            i2c.write(LSM303_ADR_ACC, cmd, 1);
+            i2c.read(LSM303_ADR_ACC, &buf[0], 1);
+            cmd[0] = LSM303_REG_ACC_OUT_TEMP_H_A;
+            i2c.write(LSM303_ADR_ACC, cmd, 1);
+            i2c.read(LSM303_ADR_ACC, &buf[1], 1);
+            myTemp = (int16_t)(buf[0] | (buf[1] << 8)) >> 6;
+            printf("TMP: |%02X %02X %02X| (%d)\r\n", rda, buf[0], buf[1], myTemp);
+        }
+    }
+    */
+    
+    /*
+    while(1)
+    {
+        cmd[0] = LSM303_REG_ACC_STATUS_REG_AUX_A | 0x80;
+        i2c.write(LSM303_ADR_ACC, cmd, 1);
+        i2c.read(LSM303_ADR_ACC, &buf[0], 7);
+        if (buf[0] & 0x04)
+        {
+            myTemp = (int16_t)(buf[5] | (buf[6] << 8)) >> 6;
+            printf("TMP: |%02X %02X %02X| (%d)\r\n", buf[0], buf[5], buf[6], myTemp);
+        }
+    }
+    */
+
+    /*
+    while(1)
+    {
+        cmd[0] = LSM303_REG_ACC_STATUS_REG_AUX_A;
+        i2c.write(LSM303_ADR_ACC, cmd, 1);
+        i2c.read(LSM303_ADR_ACC, &rda, 1);
+        if (rda & 0x04)
+        {
+            cmd[0] = LSM303_REG_ACC_OUT_TEMP_L_A | 0x80;
+            i2c.write(LSM303_ADR_ACC, cmd, 1);
+            i2c.read(LSM303_ADR_ACC, &buf[0], 2);
+            myTemp = (int16_t)(buf[0] | (buf[1] << 8)) >> 6;
+            printf("TMP: |%02X %02X %02X| (%d)\r\n", rda, buf[0], buf[1], myTemp);
+        }
+    }
+    */
+    
+    cmd[0] = LSM303_REG_ACC_STATUS_REG_AUX_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &rda, 1);
+    if (rda & 0x04)
+    {
+        cmd[0] = LSM303_REG_ACC_OUT_TEMP_L_A | 0x80;
+        i2c.write(LSM303_ADR_ACC, cmd, 1);
+        i2c.read(LSM303_ADR_ACC, &buf[0], 2);
+        myTemp = (int16_t)(buf[0] | (buf[1] << 8)) >> 6;
+        printf("TMP: |%02X %02X %02X| (%d)\r\n", rda, buf[0], buf[1], myTemp);
+    }
+
+/*
+    //wait(8);
+
+    while(1) {
+    cmd[0] = LSM303_REG_MAG_STATUS_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &rda, 1);
+    if (rda)
+    {
+    cmd[0] = LSM303_REG_MAG_OUTX_L_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[0], 1);
+    cmd[0] = LSM303_REG_MAG_OUTX_H_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[1], 1);
+    cmd[0] = LSM303_REG_MAG_OUTY_L_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[2], 1);
+    cmd[0] = LSM303_REG_MAG_OUTY_H_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[3], 1);
+    cmd[0] = LSM303_REG_MAG_OUTZ_L_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[4], 1);
+    cmd[0] = LSM303_REG_MAG_OUTZ_H_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[5], 1);
+    myMagX = (buf[0] | (buf[1] << 8));
+    myMagY = (buf[2] | (buf[3] << 8));
+    myMagZ = (buf[4] | (buf[5] << 8));
+    if (myMagX < magMinX)
+        magMinX = myMagX;
+    if (myMagY < magMinY)
+        magMinY = myMagY;
+    if (myMagZ < magMinZ)
+        magMinZ = myMagZ;
+    if (myMagX > magMaxX)
+        magMaxX = myMagX;
+    if (myMagY > magMaxY)
+        magMaxY = myMagY;
+    if (myMagZ > magMaxZ)
+        magMaxZ = myMagZ;
+    //printf("MAG: |%02X %02X %02X %02X %02X %02X| (%d,%d,%d)\r\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], myMagX, myMagY, myMagZ);
+    cmd[0] = LSM303_REG_MAG_INT_SOURCE_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &ret, 1);
+    if (ret & 0x01 && magEvent == 0 && ret & 0xFC)
+    {
+        magSFire++;
+        magEvent = 1;
+        magSHIRQ++;
+    }
+    else if (!(ret & 0x01) && magEvent == 1 && !(ret & 0xFC))
+    {
+        magSFire++;
+        magEvent = 0;
+        magSLIRQ++;
+    }
+    printf("M|%02X|%02X %02X %02X %02X %02X %02X|%*d,%*d,%*d|%*d,%*d,%*d|%*d,%*d,%*d|%02X|%02X/%02X %02X/%02X\r\n", rda, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], 6, myMagX, 6, myMagY, 6, myMagZ, 6, magMinX, 6, magMinY, 6, magMinZ, 6, magMaxX, 6, magMaxY, 6, magMaxZ, ret, magSHIRQ, magSLIRQ, magSFire, magHFire);
+    }
+
+    cmd[0] = LSM303_REG_ACC_STATUS_REG_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &rda, 1);
+    if (rda)
+    {
+    cmd[0] = LSM303_REG_ACC_OUT_X_L_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[0], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_X_H_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[1], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_Y_L_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[2], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_Y_H_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[3], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_Z_L_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[4], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_Z_H_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[5], 1);
+    myAccX = ((int16_t)(buf[0] | (buf[1] << 8)) >> accShift);
+    myAccY = ((int16_t)(buf[2] | (buf[3] << 8)) >> accShift);
+    myAccZ = ((int16_t)(buf[4] | (buf[5] << 8)) >> accShift);
+    if (myAccX < accMinX)
+        accMinX = myAccX;
+    if (myAccY < accMinY)
+        accMinY = myAccY;
+    if (myAccZ < accMinZ)
+        accMinZ = myAccZ;
+    if (myAccX > accMaxX)
+        accMaxX = myAccX;
+    if (myAccY > accMaxY)
+        accMaxY = myAccY;
+    if (myAccZ > accMaxZ)
+        accMaxZ = myAccZ;
+    //printf("ACC: |%02X %02X %02X %02X %02X %02X| (%d,%d,%d)\r\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], myAccX, myAccY, myAccZ);
+    cmd[0] = LSM303_REG_ACC_INT1_SRC_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &ret, 1);
+    if (ret & 0x40)
+    {
+        accSFire++;
+        if (accEvent == 1)
+        {
+            accEvent = 0;
+            accSLIRQ++;
+            cmd[0] = LSM303_REG_ACC_INT1_THS_A;
+            cmd[1] = 0x7D;
+            i2c.write(LSM303_ADR_ACC, cmd, 2);
+            cmd[0] = LSM303_REG_ACC_INT1_DURATION_A;
+            cmd[1] = 0x00;
+            i2c.write(LSM303_ADR_ACC, cmd, 2);
+            cmd[0] = LSM303_REG_ACC_INT1_CFG_A;
+            cmd[1] = 0x2A;
+            i2c.write(LSM303_ADR_ACC, cmd, 2);
+        }
+        else
+        {
+            accEvent = 1;
+            accSHIRQ++;
+            cmd[0] = LSM303_REG_ACC_INT1_THS_A;
+            cmd[1] = 0x50;
+            i2c.write(LSM303_ADR_ACC, cmd, 2);
+            cmd[0] = LSM303_REG_ACC_INT1_DURATION_A;
+            //cmd[1] = 0x7D;
+            cmd[1] = 0x03;
+            i2c.write(LSM303_ADR_ACC, cmd, 2);
+            cmd[0] = LSM303_REG_ACC_INT1_CFG_A;
+            cmd[1] = 0x95;
+            i2c.write(LSM303_ADR_ACC, cmd, 2);
+        }
+    }
+    printf("A|%02X|%02X %02X %02X %02X %02X %02X|%*d,%*d,%*d|%*d,%*d,%*d|%*d,%*d,%*d|%02X|%04X/%04X %04X/%04X\r\n", rda, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], 5, myAccX, 5, myAccY, 5, myAccZ, 5, accMinX, 5, accMinY, 5, accMinZ, 5, accMaxX, 5, accMaxY, 5, accMaxZ, ret, accSHIRQ, accSLIRQ, accSFire, accHFire);
+    }
+    }*/
+    
     // Initialize LoRaWAN stack
     if (lorawan.initialize(&ev_queue) != LORAWAN_STATUS_OK) {
         printf("\r\n LoRa initialization failed! \r\n");
@@ -127,7 +718,7 @@
     }
 
     printf("\r\n Mbed LoRaWANStack initialized \r\n");
-    //printf("MBED_CONF_LORA_APP_PORT: %d", MBED_CONF_LORA_APP_PORT);
+    printf("MBED_CONF_LORA_APP_PORT: %d", MBED_CONF_LORA_APP_PORT);
 
     // prepare application callbacks
     callbacks.events = mbed::callback(lora_event_handler);
@@ -152,9 +743,7 @@
     printf("\r\n Adaptive data  rate (ADR) - Enabled \r\n");
 
     retcode = lorawan.connect();
-    /*if (retcode == LORAWAN_STATUS_CONNECT_IN_PROGRESS) {
-        printf("Retcode = Connecting.");
-    }*/
+
     if (retcode == LORAWAN_STATUS_OK ||
         retcode == LORAWAN_STATUS_CONNECT_IN_PROGRESS) {
     } else {
@@ -178,6 +767,7 @@
     uint16_t packet_len;
     int16_t retcode;
     float sensor_value;
+    bool gpsDone = false;
 
     if (ds1820.begin()) {
         ds1820.startConversion();
@@ -189,35 +779,230 @@
         return;
     }
 
-    //packet_len = sprintf((char*) tx_buffer, "Dummy Sensor Value is %3.1f",
-    //                sensor_value);
-    //packet_len = sprintf((char*) tx_buffer, "VAL=%3.1f", sensor_value);
-    //packet_len = sprintf((char*) tx_buffer, "%d,%d,%f,%f", mytime, mybatt, mylat, mylon);
-    //printf("BUF: |%s|", tx_buffer);
     time_t tNow = time(NULL);
     printf("Clock: %d\r\n", tNow);
-    mytime = tNow;
-    char cmd[2];
+
+    ret = 0xFF;
+    cmd[0] = 0xFF;
+    i2c.write(NEOM8M_ADR_GPS, cmd, 1);
+    while(!gpsDone)
+    {
+        while (ret == 0xFF)
+        {
+            i2c.read(NEOM8M_ADR_GPS, &ret, 1);
+        }   
+        while (ret != 0xFF)
+        {
+            buf[pos++] = ret;
+            i2c.read(NEOM8M_ADR_GPS, &ret, 1);
+            if (ret == '\r')
+            {
+                i2c.read(NEOM8M_ADR_GPS, &ret, 1);
+                if (ret == '\n')
+                {
+                    buf[pos] = 0x00;
+                    /* NMEA Validation */
+                    uint16_t crc = 0;
+                    char clr;
+                    if (buf[0] == '$' && buf[pos-3] == '*')
+                    {
+                        int i;
+                        for (i = 1; i < pos-3; i++)
+                        {
+                            crc ^= buf[i];
+                        }
+                    }
+                    if (crc == ((buf[pos-2] < 'A' ? buf[pos-2] - '0' : buf[pos-2] - '7') << 4 | (buf[pos-1] < 'A' ? buf[pos-1] - '0' : buf[pos-1] - '7')))
+                        clr = '2'; // 2 = Green
+                    else
+                        clr = '1'; // 1 = Red
+                    printf("GPS: [\u001b[3%cm%02X\u001b[0m] |%s|\r\n", clr, crc, buf);
+                    // Global Positioning System Fix Data
+                    if(strncmp(buf, "$GNGGA", 6) == 0)
+                    {
+                        printf("GNGGA> ");
+                        //sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
+                        //pc.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);
+                        float fldTim, fldAlt;
+                        double fldLat, fldLon;
+                        char fldN_S, fldE_W;
+                        int fldFix, fldSat;
+                        sscanf(buf, "$GNGGA,%f,%lf,%c,%lf,%c,%d,%d,%*f,%f", &fldTim, &fldLat, &fldN_S, &fldLon, &fldE_W, &fldFix, &fldSat, &fldAlt);
+                        printf("Sec: %.2f, Lat: %.5f %c, Lon: %.5f %c, Fix: %d, Sat: %d, Alt: %.1f M\r\n", fldTim, fldLat, fldN_S, fldLon, fldE_W, fldFix, fldSat, fldAlt);
+                        if (clr == '2')
+                        {
+                            mylat = fldLat / (fldN_S == 'S' ? -100 : 100);
+                            mylon = fldLon / (fldE_W == 'W' ? -100 : 100);
+                            mytime = (uint32_t)fldTim;
+                            mybatt = (fldSat & 0xF0 ? 0x0F : fldSat & 0x0F);
+                        }
+                    }
+                    // Satellite status
+                    if(strncmp(buf, "$GNGSA", 6) == 0)
+                    {
+                        printf("GNGSA> ");
+                        //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);
+                        char fldTyp;
+                        int fldDim, fldSat;
+                        sscanf(buf, "$GNGSA,%c,%d,%d", &fldTyp, &fldDim, &fldSat);
+                        printf("Typ: %c, Pos: %d, Sat: %d\r\n", fldTyp, fldDim, fldSat);
+                    }
+                    // Geographic position, Latitude and Longitude
+                    if(strncmp(buf, "$GNGLL", 6) == 0)
+                    {
+                        printf("GNGLL> ");
+                        //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);
+                        float fldTim;
+                        double fldLat, fldLon;
+                        char fldN_S, fldE_W;
+                        sscanf(buf, "$GNGLL,%lf,%c,%lf,%c,%f", &fldLat, &fldN_S, &fldLon, &fldE_W, &fldTim);
+                        printf("Lat: %.5f %c, Lon: %.5f %c, Sec: %.2f\r\n", fldLat, fldN_S, fldLon, fldE_W, fldTim);
+                    }
+                    // Geographic position, Latitude and Longitude
+                    if(strncmp(buf, "$GNRMC", 6) == 0)
+                    {
+                        printf("GNRMC> ");
+                        //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);
+                        float fldTim, fldSpd;
+                        double fldLat, fldLon;
+                        char fldSts, fldN_S, fldE_W;
+                        int fldDat;
+                        sscanf(buf, "$GNRMC,%f,%c,%lf,%c,%lf,%c,%f,,%d", &fldTim, &fldSts, &fldLat, &fldN_S, &fldLon, &fldE_W, &fldSpd, &fldDat);
+                        printf("Sec: %.2f, Sts: %c, Lat: %.5f %c, Lon: %.5f %c, Spd: %.3f, Dat: %06d\r\n", fldTim, fldSts, fldLat, fldN_S, fldLon, fldE_W, fldSpd, fldDat);
+                    }
+                    pos = 0;
+                    i2c.read(NEOM8M_ADR_GPS, &ret, 1);
+                }
+                else
+                {
+                    printf("WARN: Expected '\n', received '%02x'.\r\n", ret);
+                }
+            }
+            else if (pos == 82)
+            {
+                buf[pos] = 0x00;
+                printf("GPS: |%s| ...\r\n", buf);
+                pos = 0;
+                i2c.read(NEOM8M_ADR_GPS, &ret, 1);
+            }
+        }
+        buf[pos] = 0x00;
+        gpsDone = true;
+    }
+    if (pos > 0)
+        printf("GPS: |%s|\r\n", buf);
+
     /*
-    char buf[1024];
-    buf[0] = 0x00;
-    buf[1] = 0x00;
-    buf[2] = 0x00;
+    cmd[0] = LSM303_REG_ACC_STATUS_REG_AUX_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &rda, 1);
+    cmd[0] = LSM303_REG_ACC_OUT_TEMP_L_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[0], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_TEMP_H_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[1], 1);
+    myTemp = (buf[0] | (buf[1] << 8));
+    printf("TMP: |%02X %02X %02X| (%d)\r\n", rda, buf[0], buf[1], myTemp);
     */
-    char buf;
-    //cmd[0] = 0xFF;
-    //i2c.write(NEOM8M_ADR_GPS, cmd, 1);
-    //i2c.read(NEOM8M_ADR_GPS, buf, 1024);
-    cmd[0] = 0x20;
-    cmd[1] = 0x57;
-    i2c.write(LSM303_ADR_ACC, cmd, 2);
-    cmd[0] = 0x20;
+    
+    cmd[0] = LSM303_REG_ACC_STATUS_REG_AUX_A;
     i2c.write(LSM303_ADR_ACC, cmd, 1);
-    i2c.read(LSM303_ADR_ACC, &buf, 1);
-    //i2c.read(LSM303_ADR_ACC, buf, 1);
-    printf("Return Value: %02x", buf);
+    i2c.read(LSM303_ADR_ACC, &rda, 1);
+    if (rda & 0x04)
+    {
+        cmd[0] = LSM303_REG_ACC_OUT_TEMP_L_A | 0x80;
+        i2c.write(LSM303_ADR_ACC, cmd, 1);
+        i2c.read(LSM303_ADR_ACC, &buf[0], 2);
+        myTemp = (int16_t)(buf[0] | (buf[1] << 8)) >> 6;
+        printf("TMP: |%02X %02X %02X| (%d)\r\n", rda, buf[0], buf[1], myTemp);
+    }
+
+    cmd[0] = LSM303_REG_MAG_STATUS_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &rda, 1);
+
+    cmd[0] = LSM303_REG_MAG_OUTX_L_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[0], 1);
+    cmd[0] = LSM303_REG_MAG_OUTX_H_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[1], 1);
+    cmd[0] = LSM303_REG_MAG_OUTY_L_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[2], 1);
+    cmd[0] = LSM303_REG_MAG_OUTY_H_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[3], 1);
+    cmd[0] = LSM303_REG_MAG_OUTZ_L_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[4], 1);
+    cmd[0] = LSM303_REG_MAG_OUTZ_H_REG_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[5], 1);
+    myMagX = (buf[0] | (buf[1] << 8));
+    myMagY = (buf[2] | (buf[3] << 8));
+    myMagZ = (buf[4] | (buf[5] << 8));
+    printf("MAG: |%02X %02X %02X %02X %02X %02X %02X| (%d,%d,%d)\r\n", rda, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], myMagX, myMagY, myMagZ);
+    
+    /*
+    cmd[0] = LSM303_REG_MAG_OFFSET_X_REG_L_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[0], 1);
+    cmd[0] = LSM303_REG_MAG_OFFSET_X_REG_H_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[1], 1);
+    cmd[0] = LSM303_REG_MAG_OFFSET_Y_REG_L_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[2], 1);
+    cmd[0] = LSM303_REG_MAG_OFFSET_Y_REG_H_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[3], 1);
+    cmd[0] = LSM303_REG_MAG_OFFSET_Z_REG_L_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[4], 1);
+    cmd[0] = LSM303_REG_MAG_OFFSET_Z_REG_H_M;
+    i2c.write(LSM303_ADR_MAG, cmd, 1);
+    i2c.read(LSM303_ADR_MAG, &buf[5], 1);
+    myOffX = (buf[0] | (buf[1] << 8));
+    myOffY = (buf[2] | (buf[3] << 8));
+    myOffZ = (buf[4] | (buf[5] << 8));
+    printf("OFF: |%02X %02X %02X %02X %02X %02X| (%d,%d,%d)\r\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], myOffX, myOffY, myOffZ);
+    */
+    
+    cmd[0] = LSM303_REG_ACC_STATUS_REG_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &rda, 1);
+    
+    cmd[0] = LSM303_REG_ACC_OUT_X_L_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[0], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_X_H_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[1], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_Y_L_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[2], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_Y_H_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[3], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_Z_L_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[4], 1);
+    cmd[0] = LSM303_REG_ACC_OUT_Z_H_A;
+    i2c.write(LSM303_ADR_ACC, cmd, 1);
+    i2c.read(LSM303_ADR_ACC, &buf[5], 1);
+    myAccX = ((int16_t)(buf[0] | (buf[1] << 8)) >> accShift);
+    myAccY = ((int16_t)(buf[2] | (buf[3] << 8)) >> accShift);
+    myAccZ = ((int16_t)(buf[4] | (buf[5] << 8)) >> accShift);
+    printf("ACC: |%02X %02X %02X %02X %02X %02X %02X| (%d,%d,%d)\r\n", rda, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], myAccX, myAccY, myAccZ);
+    
     int ilat = (int)(mylat * 100000);
     int ilon = (int)(mylon * 100000);
+    printf("TIM: %d, SAT: %d, LAT: %d, LON: %d\r\n", mytime, mybatt, ilat, ilon);
     packet_len = 11;
     tx_buffer[0] = (mytime >> 24) & 0xFF;
     tx_buffer[1] = (mytime >> 16) & 0xFF;
@@ -234,8 +1019,6 @@
     int i;
     for (i = 0; i < packet_len; i++) { printf("%02x", tx_buffer[i]); }
     printf("|\r\n");
-    mytime++;
-    if (mybatt == 0) { mybatt = 15; } else { mybatt--; }
     retcode = lorawan.send(MBED_CONF_LORA_APP_PORT, tx_buffer, packet_len,
                            MSG_CONFIRMED_FLAG);
 
@@ -279,7 +1062,17 @@
 
     printf("\r\n Data Length: %d\r\n", retcode);
 
+    int startLoop = 0;
+    if (strncmp((char *)rx_buffer, cmdSendLoop, 8) == 0)
+    {
+        printf("SendLoop Command Received!\r\n");
+        startLoop = 1;
+    }
+
     memset(rx_buffer, 0, sizeof(rx_buffer));
+    
+    if (startLoop)
+        send_message();
 }
 
 /**