diff --git a/arduino/gps-tracker2/config.h b/arduino/gps-tracker2/config.h index 7eb5915..5f33fb6 100644 --- a/arduino/gps-tracker2/config.h +++ b/arduino/gps-tracker2/config.h @@ -1,14 +1,32 @@ -#define GSM_APN "internet" -#define GSM_USERNAME "" -#define GSM_PASSWORD "" +/*define mqtt brocker + * https://customer.cloudmqtt.com/login + * register your server (for free 10 connections allowed) + * set direct access to server - config as standard owntrack config + * create user i.e. piesek + * create topic for user RW access owntracks/# + * + * + */ +//soft serial buffer +#define _SS_MAX_RX_BUFF 128 +//gsm +#define GSM_APN "internet" +#define GSM_USERNAME "" +#define GSM_PASSWORD "" -#define AIO_SERVER "broker.mqttdashboard.com" -#define AIO_SERVERPORT 1883 -// #define AIO_USERNAME "" -// #define AIO_PASSWORD "" -#define TOPIC "owntracks/piesek/yasmina" -#define TID "yb" // "tracker-ID which is used by the auto-faces feature to display, say, initials of a user." -#define SPEED_TR 2 // bellow that number = 0 -#define DELAY 10 // seconds +//#define AIO_SERVER "m20.cloudmqtt.com" +//#define AIO_SERVER "broker.mqttdashboard.com" +//#define AIO_SERVER "broker.hivemq.com" +#define AIO_SERVER "xxx.xxx.xxx.xxx" +#define AIO_SERVERPORT 1883 +#define AIO_USERNAME "piesek" +#define AIO_PASSWORD "xxxxxxxxx" +#define TOPIC "owntracks/piesek/yasmina" +#define TID "yb" // "tracker-ID which is used by the auto-faces feature to display, say, initials of a user." +#define SPEED_TR 3 // bellow that number = 0 +#define HEARTBIT 120 // there ic multiply by DELAY and sending heartbit - set for every 30 min when movement trying is 5 sec. +#define DELAY 10 // seconds + ~5 +#define MOVEMENT 50.0 //metters moved to report +//#define DEBUG diff --git a/arduino/gps-tracker2/fonahelper.cpp b/arduino/gps-tracker2/fonahelper.cpp index e4619b3..c253bcf 100644 --- a/arduino/gps-tracker2/fonahelper.cpp +++ b/arduino/gps-tracker2/fonahelper.cpp @@ -1,128 +1,178 @@ -#include -#include +#include "config.h" #include "Adafruit_FONA.h" +#include -#define halt(s) { Serial.println(F( s )); while(1); } +/* +//prog mem helper - change buffer size for topic if needed +const char stringBuffer[30]; +const char* getString(const char* str[30]) { +strcpy_P(stringBuffer, (const char*)str); +delay(50); +return stringBuffer; +} +*/ + extern Adafruit_FONA fona; extern SoftwareSerial fonaSS; -char utime[14]; -int YY; -int MM; -int DD; -int hh; -int mm; -int ss; +//extern SoftwareSerial *fonaSerial; + // flags byte gps_enabled = 0; boolean FONAconnect(const __FlashStringHelper *apn, const __FlashStringHelper *username, const __FlashStringHelper *password) { - Watchdog.reset(); + - //Serial.println(F("Initializing FONA....(May take 3 seconds)")); - - //fonaSS.begin(*fonaSerial); // if you're using software serial - fonaSS.begin(9600); + while (!fonaSS); + fonaSS.begin(9800); + delay(50); + + if (! fona.begin(fonaSS)) { // can also try fona.begin(Serial1) + delay(50); +#ifdef DEBUG Serial.println(F("Couldn't find FONA")); + delay(50); +#endif return false; + } else { + + // to test } - fonaSS.println("AT+CMEE=2"); - //Serial.println(F("FONA is OK")); - Watchdog.reset(); - // Serial.println(F("Checking for network...")); + + while (!fonaSS); + + + byte net =0; while (fona.getNetworkStatus() != 1) { delay(500); +#ifdef DEBUG + Serial.println(F("Waiting for network ...")); +#endif + delay(15000); + while ( !fonaSS); + net++; + if (net == 20) { + fonaSS.end(); + asm volatile ( "jmp 0"); + } } - Watchdog.reset(); - delay(5000); // wait a few seconds to stabilize connection - Watchdog.reset(); + + //delay(20000); // wait a few seconds to stabilize connection fona.setGPRSNetworkSettings(apn, username, password); - //Serial.println(F("Disabling GPRS")); + while (!fonaSS); fona.enableGPRS(false); - - Watchdog.reset(); - delay(5000); // wait a few seconds to stabilize connection - Watchdog.reset(); - + delay(500); // wait a few seconds to stabilize connection +#ifdef DEBUG Serial.println(F("Enabling GPRS")); + delay(50); +#endif + while (!fonaSS); if (!fona.enableGPRS(true)) { - //Serial.println(F("Failed to turn GPRS on")); +#ifdef DEBUG + Serial.println(F("Failed to turn GPRS on")); + delay(50); +#endif return false; } - - Watchdog.reset(); + while (!fonaSS); + fona.enableRTC(1); return true; } - boolean GPS() { + if (gps_enabled == 0 ) { - //Serial.println(F("Enabling GPS")); - if (!fona.enableGPS(true)) { - Serial.println(F("Failed to turn GPS on")); +#ifdef DEBUG + Serial.println(F("Enabling GPS")); + delay(50); +#endif + while ( !fonaSS); + fona.enableGPS(false); + delay(500); + while ( !fonaSS); + if(!fona.enableGPS(true)) { +#ifdef DEBUG + Serial.println(F("Failed to turn GPS on")); + delay(50); +#endif return false; - } - else { + } else { gps_enabled = 1; + delay(10000); } } - - Watchdog.reset(); - delay(5000); // wait a few seconds to stabilize connection - Watchdog.reset(); - - Watchdog.reset(); - //Serial.println(F("Looking for satelites")); byte stat; + while ( !fonaSS); stat = fona.GPSstatus(); + delay(500); if (stat < 0 or stat == 0 or stat == 1) { +#ifdef DEBUG Serial.println(F("Not fixed!")); + delay(50); +#endif return false; - } - if (stat >= 2) { + + } + if (stat >= 2) { +#ifdef DEBUG Serial.println(F("GPS Fixed")); - } + delay(50); +#endif + return true; + } else { + return false; + } - Watchdog.reset(); - - return true; +return false; } void GPS_Data(float *fdata, int *idata) { - - - float latitude, longitude, speed_kph, heading, altitude; - boolean gps_success = fona.getGPS(&latitude, &longitude, &altitude, &speed_kph, &heading, &utime[0]); - sscanf(utime,"%04d%02d%02d%02d%02d%02d",&YY,&MM,&DD,&hh,&mm,&ss); - - - // put data into array - fdata[0] = latitude; - fdata[1] = longitude; - fdata[2] = speed_kph; - fdata[3] = altitude; - fdata[4] = heading; - idata[0] = hh; - idata[1] = mm; - idata[2] = ss; - idata[3] = DD; - idata[4] = MM; - idata[5] = YY; + char utime[14]; + //int YY; + //int MM; + //int DD; + //int hh; + //int mm; + //int ss; + float latitude, longitude, speed_kph,cog; + //, speed_kph, heading, altitude; + //boolean gps_success = fona.getGPS(&fdata[0], &fdata[1], &fdata[3], &fdata[2], &fdata[4], &utime[0]); + while (!fonaSS); + boolean gps_success = fona.getGPS(&latitude,&longitude, &fdata[3], &speed_kph, &cog, &utime[0]); + delay(50); + //sscanf(utime,"%04d%02d%02d%02d%02d%02d",&YY,&MM,&DD,&hh,&mm,&ss); + sscanf(utime,"%04d%02d%02d%02d%02d%02d",&idata[5],&idata[4],&idata[3],&idata[0],&idata[1],&idata[2]); + delay(50); + //idata[0] = hh; + //idata[1] = mm; + //idata[2] = ss; + //idata[3] = DD; + //idata[4] = MM; + //idata[5] = YY; + if (&gps_success) { - //delete utime; + + fdata[0]=latitude; + fdata[1]=longitude; + fdata[2]=speed_kph; + fdata[4]=cog; + //sendATcommand("AT+CGATT=1",1000); + delay(50); + return &fdata; } } + diff --git a/arduino/gps-tracker2/gps-tracker2.ino b/arduino/gps-tracker2/gps-tracker2.ino index 0eaf2dc..9e23c0e 100644 --- a/arduino/gps-tracker2/gps-tracker2.ino +++ b/arduino/gps-tracker2/gps-tracker2.ino @@ -1,3 +1,4 @@ + /*************************************************** Adafruit MQTT Library FONA Example @@ -18,230 +19,494 @@ MIT license, all text above must be included in any redistribution ****************************************************/ -/* - TODO: - reconnect GPRS -*/ #include "config.h" #include -//#include "Time.h" -#include -#include #include "Adafruit_FONA.h" #include "Adafruit_MQTT.h" #include "Adafruit_MQTT_FONA.h" -#include -//#include -//#include +#include "ArduinoJson.h" +#include + +/*************************** FONA Params ***********************************/ + +//#undef FONA_DEFAULT_TIMEOUT_MS +//#define FONA_DEFAULT_TIMEOUT_MS 600 +//#undef MQTT_CONN_KEEPALIVE +//#define MQTT_CONN_KEEPALIVE 1800 +//#undef MQTT_FONA_INTERAVAILDELAY +//#define MQTT_FONA_INTERAVAILDELAY 200 +//#undef MQTT_FONA_QUERYDELAY +//#define MQTT_FONA_QUERYDELAY 600 /*************************** FONA Pins ***********************************/ -// GSm pins #define GSM_RX 2 #define GSM_TX 3 #define GSM_RST 4 // pin number 16 in sim808 throug diode to ground https://cdn-shop.adafruit.com/datasheets/SIM808_Hardware+Design_V1.00.pdf max 4.3 V SoftwareSerial fonaSS = SoftwareSerial(GSM_TX, GSM_RX); -//SoftwareSerial *FONASERIAL = &fonaSS; -Adafruit_FONA fona = Adafruit_FONA(GSM_RST); -//Adafruit_FONA *FONA = &fona; - -//int ctime[6]; -//char Lat[10]; -//char Lon[10]; -//char Alt[10]; -//uint16_t Course; -//uint16_t Speed; -//char Date[14]; +//oftwareSerial *fonaSerial = &fonaSS; +Adafruit_FONA fona = Adafruit_FONA(GSM_RST); -//byte offset = 0; -StaticJsonBuffer<120> jsonBuffer; +int readytosend = 0; +uint16_t reporter = 0; +char geodata[125]; -// global - a tak nie powinno się robić -JsonObject& root = jsonBuffer.createObject(); -//JsonObject *ROOT = &root; -char geodata[120]; /************ Global State (you don't need to change this!) ******************/ - +//const __FlashStringHelper *MQTT_TOPIC; +//#define MQTT_DEBUG // Setup the FONA MQTT class by passing in the FONA class and MQTT server and login details. -Adafruit_MQTT_FONA mqtt(&fona, AIO_SERVER, AIO_SERVERPORT); -//Adafruit_MQTT_FONA *MQTT(&mqtt); -// You don't need to change anything below this line! -#define halt(s) { Serial.println(F( s )); while(1); } +Adafruit_MQTT_FONA mqtt(&fona,AIO_SERVER,AIO_SERVERPORT,AIO_USERNAME,AIO_PASSWORD); +//Adafruit_MQTT_FONA mqtt(&fona,AIO_SERVER,AIO_SERVERPORT); // FONAconnect is a helper function that sets up the FONA and connects to // the GPRS network. See the fonahelper.cpp tab above for the source! boolean FONAconnect(const __FlashStringHelper *apn, const __FlashStringHelper *username, const __FlashStringHelper *password); boolean GPS(); - +//reset +void(* resetFunc) (void) = 0; /****************************** Feeds ***************************************/ - // Setup a feed called 'photocell' for publishing. -Adafruit_MQTT_Publish feed = Adafruit_MQTT_Publish(&mqtt, TOPIC); -//Adafruit_MQTT_Publish *FEED = &feed; +Adafruit_MQTT_Publish feed = Adafruit_MQTT_Publish(&mqtt,TOPIC); + + /*************************** Sketch Code ************************************/ -// How many transmission failures in a row we're willing to be ok with before reset -byte txfailures = 0; -#define MAXTXFAILURES 3 void setup() { - while (!Serial); - - // Watchdog is optional! - //Watchdog.enable(10000); - Serial.begin(115200); - - Serial.println(F("GPS Tracker")); - - Watchdog.reset(); - delay(5000); // wait a few seconds to stabilize connection - Watchdog.reset(); - + while (!Serial); + fonaSS.begin(9600); + while (!fonaSS); + //delay(20); + //fonaSS.println("ATZ"); + fona.begin(fonaSS); + delay(100); + //while (!fonaSS); + //fonaSS.println("AT&F1"); + //delay(500); + //fonaSS.println("AT&W0"); + //delay(500); + //clearbuffer(); +#ifdef DEBUG + Serial.println(F("GPS Tracker starting ...")); +#endif +#ifndef DEBUG + Serial.println(F("Debug is off")); +#endif + + delay(10000); // wait a few seconds to stabilize connection + + while ( !fonaSS); // Initialise the FONA module while (! FONAconnect(F(GSM_APN), F(GSM_USERNAME), F(GSM_PASSWORD))) { - Serial.println(F("Retrying FONA GSM")); - } - while (! GPS()) { - Serial.println(F("Retrying FONA GPS")); - } +#ifdef DEBUG + Serial.println(F("Retrying FONA GPRS ...")); +#endif + delay(15000); + while ( !fonaSS); +} + + delay(10000); + while ( !fonaSS); + byte trying =0; + while (! GPS() ) { +#ifdef DEBUG + Serial.println(F("Retrying FONA GPS ...")); +#endif + delay(15000); + while ( !fonaSS); + trying++; + if (trying == 20)resetFunc(); + } +#ifdef DEBUG Serial.println(F("Connected to Cellular!")); +#endif - Watchdog.reset(); - delay(5000); // wait a few seconds to stabilize connection - Watchdog.reset(); + + delay(10000); // wait a few seconds to stabilize connection + } void loop() { -// Serial.println(freeMemory()); - prepareData(); - // Make sure to reset watchdog every loop iteration! - Watchdog.reset(); + while ( !fonaSS); + fonaSS.flush(); + delay(50); + Serial.flush(); + delay(50); + + +//-------------main function not generate when no GPS, GPRS, GSM + +do{ + //clearbuffer(); + delay(DELAY*1000); // wait a few seconds to stabilize connection + while (!fonaSS); + fona.deleteSMS(1); + delay(500); +#ifdef DEBUG + Serial.println(F("Network avaliable GPS fixed GPRS connected, processing ...")); + delay(50); +#endif + //clearbuffer(); + prepareData(); + + +if (readytosend == 1) { + + //clearbuffer(); + MQTT_connect(); + //delay(50); - // Ensure the connection to the MQTT server is alive (this will make the first - // connection and automatically reconnect when disconnected). See the MQTT_connect - // function definition further below. - MQTT_connect(); + + // Now we can publish stuff! + //clearbuffer(); + int check =3; + do { + while (!fonaSS); + //clearbuffer(); + if (! feed.publish(geodata)) { +#ifdef DEBUG + Serial.println(F("Sent Failed")); + delay(50); +#endif + check--; + //clearbuffer(); + MQTT_connect(); + if (check == 0) return; + delay(1000); + } else { +#ifdef DEBUG + Serial.println(F("Sent OK!")); + delay(50); +#endif + //Serial.println(freeMemory()); + check = 0; + readytosend = 0; + if (check == 0) return; + delay(1000); + } + } while (check != 0); + + + +} else { +#ifdef DEBUG + Serial.print(F("No movement of ")); + Serial.print(MOVEMENT); + Serial.println(F(" meters waiting ...")); + delay(50); +#endif + readytosend = 0; + + + while ( !fonaSS); + mqtt.ping(3); + delay(50); + while (!fonaSS); + if (!fona.TCPconnected()){ + return; + } +} + + //clearbuffer(); + - Watchdog.reset(); - // Now we can publish stuff! + + + while ( !fonaSS); + } while(fonaSS && fona.GPRSstate() == 1 && fona.GPSstatus() > 1 ); +#ifdef DEBUG + Serial.println(F("No GPS fixed or GPRS connected, waiting for restart ...")); +#endif + delay(1000); + while ( !fonaSS); + if( fona.GPRSstate() != 1 ){ +#ifdef DEBUG + Serial.println(F("No network !!! traing to bring up ...")); + delay(50); +#endif + //asm volatile ( "jmp 0"); + //albo tak ale reset przed setup void(* resetFunc) (void) = 0; wywolanie resetFunc(); + fonaSS.end(); + resetFunc(); //reset + } + + while (!fonaSS); + if (fona.GPSstatus() <= 1){ +#ifdef DEBUG + Serial.println(F("Restarting FONA GPS - not fixed ...")); + delay(50); +#endif + byte retry=0; + while (! GPS() ) { +#ifdef DEBUG + Serial.println(F("Trying up FONA GPS ...")); +#endif + delay(10000); + while (!fonaSS); + retry++; + if (retry == 10) { + fonaSS.end(); + resetFunc();//asm volatile ( "jmp 0"); //reset + } + } + } + - //Serial.print(F("\nSending ")); - //Serial.print(F("...")); - - if (! feed.publish(geodata)) { +} + + +/* +void clearbuffer(){ + // Read all available serial input to flush pending data. + uint16_t timeoutloop = 0; - Serial.println(F("Sent Failed")); - txfailures++; - } else { - Serial.println(F("Sent OK!")); - txfailures = 0; - } + while (!fonaSS); + fonaSS.peek(); + while (timeoutloop++ < 60) { + while(fonaSS.available()) { + fonaSS.read(); + timeoutloop = 0; // If char was received reset the timer + } + delay(1); + } - Watchdog.reset(); - delay(DELAY*1000); // wait a few seconds to stabilize connection - Watchdog.reset(); - } - +*/ // Function to connect and reconnect as necessary to the MQTT server. // Should be called in the loop function and it will take care if connecting. + + + void MQTT_connect() { + while ( !fonaSS); byte ret; // Stop if already connected. - if (mqtt.connected()) { + if (mqtt.connected()) { + return; - } - - //Serial.print(F("Connecting to MQTT... ")); - - while ((ret = mqtt.connect()) != 0) { // connect will return 0 for connected - //Serial.println(mqtt.connectErrorString(ret)); - Serial.println(F("Retrying MQTT connection in 10 seconds...")); - mqtt.disconnect(); - delay(10000); // wait 8 seconds - } + } +#ifdef DEBUG + Serial.println(F("Connecting to MQTT ... ")); + delay(50); +#endif + while (!fonaSS); + byte test=0; + while ((ret = mqtt.connect()) != 0 ) { // connect will return 0 for connected + /* + switch (ret) { + case 1: Serial.println(F("Wrong protocol")); break; + case 2: Serial.println(F("ID rejected")); break; + case 3: Serial.println(F("Server unavail")); break; + case 4: Serial.println(F("Bad user/pass")); break; + case 5: Serial.println(F("Not authed")); break; + case 6: Serial.println(F("Failed to subscribe")); break; + default: Serial.println(F("Connection failed")); break; + } + */ + delay(50); + //Serial.println(mqtt.connectErrorString(ret)); + + mqtt.disconnect(); +#ifdef DEBUG + Serial.println(F("Retrying MQTT connection in 5 seconds ...")); +#endif + delay(5000); // wait 2 seconds + while (!fonaSS); + test ++; + if (test == 20) { + fonaSS.end(); + resetFunc();//asm volatile ( "jmp 0"); + } + + } +#ifdef DEBUG Serial.println(F("MQTT Connected!")); - //if (! Serial.println(geodata)) { - // Serial.println(F("Failed")); - // } else { - // Serial.println(F("data OK!")); - - // } - + delay(50); +#endif } + void prepareData() { - + +static float lastmovement_W = 0; +static float lastmovement_L = 0; + + int vbat; int ctime[6]; float gps_data[5]; + while (!fonaSS); + if (! fona.getBattVoltage(&vbat)) vbat = 0; + delay(50); + while (!fonaSS); void GPS_Data(float *fdata, int *idata); - GPS_Data(gps_data, ctime); - //dtostrf(gps_data[0], 6, 6, &Lat[0]); - //dtostrf(gps_data[1], 6, 6, &Lon[0]); - //dtostrf(gps_data[2], 6, 2, Speed); - //Speed = gps_data[2]; - //dtostrf(gps_data[3], 6, 2, Alt); - //dtostrf(gps_data[4], 6, 2, Course); - //Course = gps_data[4]; - // prepare data to unixtime + //GPS_Data(gps_data, ctime); + while (!fonaSS); + GPS_Data(&gps_data[0], &ctime[0]); + delay(50); setTime(ctime[0], ctime[1], ctime[2], ctime[3], ctime[4], ctime[5]); - // timezone - //adjustTime(offset * SECS_PER_HOUR); - // unixtime to string - //snprintf(&Date[0], 14, "%lu", now()); + //setTime(&ctime[0], &ctime[1], &ctime[2], &ctime[3], &ctime[4], &ctime[5]); + delay(20); time_t uxdate = now(); - //Serial.println(test); + delay(20); + //if (! fona.getBattPercent(&vbat)) vbat = 0; - //(int(gps_data[2]) >= 2 ? 0 : int(gps_data[2])); - - //int _speed = int(gps_data[2]); - //if (_speed < SPEED_TR) _speed = 0; - int vbat; - //if (! fona.getBattPercent(&vbat)) vbat = 0; - if (! fona.getBattVoltage(&vbat)) vbat = 0; - //fona.getBattVoltage - //} else { - // Serial.print(F("VPct = ")); Serial.print(vbat); Serial.println(F("%")); //} // {"tst":1476474031,"acc":1000,"_type":"location","alt":140,"lon":-90.48259734672334,"vac":10,"p":100.160530090332,"lat":38.75410327670748,"batt":100,"tid":"JT"} // Owntracks API: // http://owntracks.org/booklet/tech/json/ - //root["_type"] = "location"; - root.set("_type","location"); - //root["acc"] = 10; - root.set("acc",3); - //root["alt"] = atof(Alt); - // root.set("alt",atof(Alt)); - //root["batt"] = 100; - root.set("batt",vbat); - //root["cog"] = atof(Course); - root.set("cog",int(gps_data[4])); - //root["lat"] = Lat; - root.set("lat",(gps_data[0]),6); - //root["lon"] = Lon; - root.set("lon",(gps_data[1]),6); - //root["tid"] = TID; - root.set("tid",TID); - //root["vel"] = _speed; - root.set("vel",(int(gps_data[2]) <= 2 ? 0 : int(gps_data[2]))); - //root["tst"] = Date; - //ROOT->set("tst",atol(Date)); - root.set("tst",uxdate); - root.printTo(geodata, 120); + float converter[2]; + converter[0]=gps_data[0]; + converter[1]=gps_data[1]; + + float movement = 0; + movement = distanceCoordinates(converter[0],converter[1],lastmovement_W,lastmovement_L); + if ((int)movement > 1000 or (int)movement < 0 ) movement = MOVEMENT + 1.0; //1000 means more than 200 km/h, it is added before first initialosation of last loc. + StaticJsonBuffer<125> jsonBuffer; + //memset(jsonBuffer, 0, sizeof(jsonBuffer)); + //delay(50); + JsonObject& root = jsonBuffer.createObject(); + delay(100); + + + root.set("_type","location"); + root.set("acc",int(movement)); + root.set("batt",vbat); + root.set("cog",int(gps_data[4])); + delay(2); + root.set("lat",converter[0],7); + delay(2); + root.set("lon",converter[1],7); + + //root.set("lat",gps_data[0],7); + //delay(2); + //root.set("lon",gps_data[1],7); + + root.set("tid",TID); + root.set("vel",(int(gps_data[2]) <= SPEED_TR ? 0 : int(gps_data[2]))); + delay(50); + root.set("tst",uxdate); + delay(2); + memset(geodata, 0, sizeof(geodata)); + delay(50); + root.printTo(geodata, 125); + delay(50); + + + //if ((((gps_data[0]) - lastmovement_W ) > float(0.0010000)) or (((gps_data[1]) - lastmovement_L ) > float(0.0010000)) or reporter == HEARTBIT) { + // if (((converter[0] - lastmovement_W ) > float(0.0010000)) or ((converter[1] - lastmovement_L ) > float(0.0010000)) or reporter == HEARTBIT) { + + if ( movement > MOVEMENT or reporter == HEARTBIT){ + reporter=0; + readytosend = 1; + +//byte stat = fona.GPSstatus(); + + //if (stat < 0 or stat == 0 or stat == 1) { + + //Serial.println(F("Not fixed!")); + //if (fona.getNetworkStatus() == 1) { + + //float lat,lon; + //fona.getGSMLoc(&lat, &lon); + + //Serial.print("GSMLoc lat:"); + //Serial.println(lat, 7); + //Serial.print("GSMLoc long:"); + //Serial.println(lon, 7); + + //} + lastmovement_W = converter[0]; + lastmovement_L = converter[1]; + +/* +lastmovement_W = gps_data[0]; +delay(50); +lastmovement_L = gps_data[1]; +delay(50); +*/ + + delete[] converter,movement,root; + + } else { + + readytosend = 0; + + // reporter every xxx min - position reported even not moved + reporter++; + //simulator for test only + //float adding = random(0,2) * 0.0010000; + //lastmovement_W = converter[0] + adding; + //lastmovement_L = converter[1] + adding; + + + } +#ifdef DEBUG + Serial.println(); + Serial.print(F(" Latitude OLD: ")); + delay(50); + Serial.print(lastmovement_W,7); + Serial.println(); + Serial.print(F(" Longitude OLD: ")); + delay(50); + Serial.print(lastmovement_L,7); + Serial.println(); + Serial.println(); + Serial.print(F(" Seconds waiting: ")); + delay(50); + Serial.print((HEARTBIT * (DELAY +5))); + Serial.print(F(" seconds before heartbit: ")); + delay(50); + Serial.println( ((HEARTBIT * (DELAY +5)) - (reporter *(DELAY +5)))); + Serial.println(); +#endif + delete[] converter,movement,root; + +} + +// Calculate distance between two points +//distanceCoordinates(latitude, longitude, initialLatitude, initialLongitude); +float distanceCoordinates(float flat1, float flon1, float flat2, float flon2) { + + // Variables + float dist_calc=0; + float dist_calc2=0; + float diflat=0; + float diflon=0; + + // Calculations + diflat = radians(flat2-flat1); + flat1 = radians(flat1); + flat2 = radians(flat2); + diflon = radians((flon2)-(flon1)); + + dist_calc = (sin(diflat/2.0)*sin(diflat/2.0)); + dist_calc2 = cos(flat1); + dist_calc2*=cos(flat2); + dist_calc2*=sin(diflon/2.0); + dist_calc2*=sin(diflon/2.0); + dist_calc +=dist_calc2; + dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc))); + + dist_calc*=6371000.0; //Converting to meters + + return dist_calc; } + + +