Browse Source

recoding

display_support
Andreas Peters 2 years ago
parent
commit
72e6cab12e
6 changed files with 1212 additions and 271 deletions
  1. 9
    0
      README.md
  2. 2
    2
      lib/LibAPRS/AFSK.cpp
  3. 503
    0
      lib/TinyGPS++/TinyGPS++.cpp
  4. 273
    0
      lib/TinyGPS++/TinyGPS++.h
  5. 83
    269
      src/main.cpp
  6. 342
    0
      src/main.cpp.bak

+ 9
- 0
README.md View File

@@ -1 +1,10 @@
# Arduino-APRS-Tracker

## Configuration

Before you start to use it, change the callsign in the main.cpp please.

```
char callSign[] = "CALLSING";
char comment[] = "CALLSIGN APRS Tracker";
```

+ 2
- 2
lib/LibAPRS/AFSK.cpp View File

@@ -53,8 +53,8 @@ void AFSK_hw_init(void) {
_BV(ADPS2);

AFSK_DAC_INIT();
LED_TX_INIT();
LED_RX_INIT();
//LED_TX_INIT();
//LED_RX_INIT();
}

void AFSK_init(Afsk *afsk) {

+ 503
- 0
lib/TinyGPS++/TinyGPS++.cpp View File

@@ -0,0 +1,503 @@
/*
TinyGPS++ - a small GPS library for Arduino providing universal NMEA parsing
Based on work by and "distanceBetween" and "courseTo" courtesy of Maarten Lamers.
Suggestion to add satellites, courseTo(), and cardinal() by Matt Monson.
Location precision improvements suggested by Wayne Holder.
Copyright (C) 2008-2013 Mikal Hart
All rights reserved.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/

#include "TinyGPS++.h"

#include <string.h>
#include <ctype.h>
#include <stdlib.h>

#define _GPRMCterm "GPRMC"
#define _GNRMCterm "GNRMC"
#define _GPGGAterm "GPGGA"
#define _GNGGAterm "GNGGA"

TinyGPSPlus::TinyGPSPlus()
: parity(0)
, isChecksumTerm(false)
, curSentenceType(GPS_SENTENCE_OTHER)
, curTermNumber(0)
, curTermOffset(0)
, sentenceHasFix(false)
, customElts(0)
, customCandidates(0)
, encodedCharCount(0)
, sentencesWithFixCount(0)
, failedChecksumCount(0)
, passedChecksumCount(0)
{
term[0] = '\0';
}

//
// public methods
//

bool TinyGPSPlus::encode(char c)
{
++encodedCharCount;

switch(c)
{
case ',': // term terminators
parity ^= (uint8_t)c;
case '\r':
case '\n':
case '*':
{
bool isValidSentence = false;
if (curTermOffset < sizeof(term))
{
term[curTermOffset] = 0;
isValidSentence = endOfTermHandler();
}
++curTermNumber;
curTermOffset = 0;
isChecksumTerm = c == '*';
return isValidSentence;
}
break;

case '$': // sentence begin
curTermNumber = curTermOffset = 0;
parity = 0;
curSentenceType = GPS_SENTENCE_OTHER;
isChecksumTerm = false;
sentenceHasFix = false;
return false;

default: // ordinary characters
if (curTermOffset < sizeof(term) - 1)
term[curTermOffset++] = c;
if (!isChecksumTerm)
parity ^= c;
return false;
}

return false;
}

//
// internal utilities
//
int TinyGPSPlus::fromHex(char a)
{
if (a >= 'A' && a <= 'F')
return a - 'A' + 10;
else if (a >= 'a' && a <= 'f')
return a - 'a' + 10;
else
return a - '0';
}

// static
// Parse a (potentially negative) number with up to 2 decimal digits -xxxx.yy
int32_t TinyGPSPlus::parseDecimal(const char *term)
{
bool negative = *term == '-';
if (negative) ++term;
int32_t ret = 100 * (int32_t)atol(term);
while (isdigit(*term)) ++term;
if (*term == '.' && isdigit(term[1]))
{
ret += 10 * (term[1] - '0');
if (isdigit(term[2]))
ret += term[2] - '0';
}
return negative ? -ret : ret;
}

// static
// Parse degrees in that funny NMEA format DDMM.MMMM
void TinyGPSPlus::parseDegrees(const char *term, RawDegrees &deg)
{
uint32_t leftOfDecimal = (uint32_t)atol(term);
uint16_t minutes = (uint16_t)(leftOfDecimal % 100);
uint32_t multiplier = 10000000UL;
uint32_t tenMillionthsOfMinutes = minutes * multiplier;

deg.deg = (int16_t)(leftOfDecimal / 100);

while (isdigit(*term))
++term;

if (*term == '.')
while (isdigit(*++term))
{
multiplier /= 10;
tenMillionthsOfMinutes += (*term - '0') * multiplier;
}

deg.billionths = (5 * tenMillionthsOfMinutes + 1) / 3;
deg.negative = false;
}

#define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number)

// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
bool TinyGPSPlus::endOfTermHandler()
{
// If it's the checksum term, and the checksum checks out, commit
if (isChecksumTerm)
{
byte checksum = 16 * fromHex(term[0]) + fromHex(term[1]);
if (checksum == parity)
{
passedChecksumCount++;
if (sentenceHasFix)
++sentencesWithFixCount;

switch(curSentenceType)
{
case GPS_SENTENCE_GPRMC:
date.commit();
time.commit();
if (sentenceHasFix)
{
location.commit();
speed.commit();
course.commit();
}
break;
case GPS_SENTENCE_GPGGA:
time.commit();
if (sentenceHasFix)
{
location.commit();
altitude.commit();
}
satellites.commit();
hdop.commit();
break;
}

// Commit all custom listeners of this sentence type
for (TinyGPSCustom *p = customCandidates; p != NULL && strcmp(p->sentenceName, customCandidates->sentenceName) == 0; p = p->next)
p->commit();
return true;
}

else
{
++failedChecksumCount;
}

return false;
}

// the first term determines the sentence type
if (curTermNumber == 0)
{
if (!strcmp(term, _GPRMCterm) || !strcmp(term, _GNRMCterm))
curSentenceType = GPS_SENTENCE_GPRMC;
else if (!strcmp(term, _GPGGAterm) || !strcmp(term, _GNGGAterm))
curSentenceType = GPS_SENTENCE_GPGGA;
else
curSentenceType = GPS_SENTENCE_OTHER;

// Any custom candidates of this sentence type?
for (customCandidates = customElts; customCandidates != NULL && strcmp(customCandidates->sentenceName, term) < 0; customCandidates = customCandidates->next);
if (customCandidates != NULL && strcmp(customCandidates->sentenceName, term) > 0)
customCandidates = NULL;

return false;
}

if (curSentenceType != GPS_SENTENCE_OTHER && term[0])
switch(COMBINE(curSentenceType, curTermNumber))
{
case COMBINE(GPS_SENTENCE_GPRMC, 1): // Time in both sentences
case COMBINE(GPS_SENTENCE_GPGGA, 1):
time.setTime(term);
break;
case COMBINE(GPS_SENTENCE_GPRMC, 2): // GPRMC validity
sentenceHasFix = term[0] == 'A';
break;
case COMBINE(GPS_SENTENCE_GPRMC, 3): // Latitude
case COMBINE(GPS_SENTENCE_GPGGA, 2):
location.setLatitude(term);
break;
case COMBINE(GPS_SENTENCE_GPRMC, 4): // N/S
case COMBINE(GPS_SENTENCE_GPGGA, 3):
location.rawNewLatData.negative = term[0] == 'S';
break;
case COMBINE(GPS_SENTENCE_GPRMC, 5): // Longitude
case COMBINE(GPS_SENTENCE_GPGGA, 4):
location.setLongitude(term);
break;
case COMBINE(GPS_SENTENCE_GPRMC, 6): // E/W
case COMBINE(GPS_SENTENCE_GPGGA, 5):
location.rawNewLngData.negative = term[0] == 'W';
break;
case COMBINE(GPS_SENTENCE_GPRMC, 7): // Speed (GPRMC)
speed.set(term);
break;
case COMBINE(GPS_SENTENCE_GPRMC, 8): // Course (GPRMC)
course.set(term);
break;
case COMBINE(GPS_SENTENCE_GPRMC, 9): // Date (GPRMC)
date.setDate(term);
break;
case COMBINE(GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA)
sentenceHasFix = term[0] > '0';
break;
case COMBINE(GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA)
satellites.set(term);
break;
case COMBINE(GPS_SENTENCE_GPGGA, 8): // HDOP
hdop.set(term);
break;
case COMBINE(GPS_SENTENCE_GPGGA, 9): // Altitude (GPGGA)
altitude.set(term);
break;
}

// Set custom values as needed
for (TinyGPSCustom *p = customCandidates; p != NULL && strcmp(p->sentenceName, customCandidates->sentenceName) == 0 && p->termNumber <= curTermNumber; p = p->next)
if (p->termNumber == curTermNumber)
p->set(term);

return false;
}

/* static */
double TinyGPSPlus::distanceBetween(double lat1, double long1, double lat2, double long2)
{
// returns distance in meters between two positions, both specified
// as signed decimal-degrees latitude and longitude. Uses great-circle
// distance computation for hypothetical sphere of radius 6372795 meters.
// Because Earth is no exact sphere, rounding errors may be up to 0.5%.
// Courtesy of Maarten Lamers
double delta = radians(long1-long2);
double sdlong = sin(delta);
double cdlong = cos(delta);
lat1 = radians(lat1);
lat2 = radians(lat2);
double slat1 = sin(lat1);
double clat1 = cos(lat1);
double slat2 = sin(lat2);
double clat2 = cos(lat2);
delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
delta = sq(delta);
delta += sq(clat2 * sdlong);
delta = sqrt(delta);
double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
delta = atan2(delta, denom);
return delta * 6372795;
}

double TinyGPSPlus::courseTo(double lat1, double long1, double lat2, double long2)
{
// returns course in degrees (North=0, West=270) from position 1 to position 2,
// both specified as signed decimal-degrees latitude and longitude.
// Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
// Courtesy of Maarten Lamers
double dlon = radians(long2-long1);
lat1 = radians(lat1);
lat2 = radians(lat2);
double a1 = sin(dlon) * cos(lat2);
double a2 = sin(lat1) * cos(lat2) * cos(dlon);
a2 = cos(lat1) * sin(lat2) - a2;
a2 = atan2(a1, a2);
if (a2 < 0.0)
{
a2 += TWO_PI;
}
return degrees(a2);
}

const char *TinyGPSPlus::cardinal(double course)
{
static const char* directions[] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"};
int direction = (int)((course + 11.25f) / 22.5f);
return directions[direction % 16];
}

void TinyGPSLocation::commit()
{
rawLatData = rawNewLatData;
rawLngData = rawNewLngData;
lastCommitTime = millis();
valid = updated = true;
}

void TinyGPSLocation::setLatitude(const char *term)
{
TinyGPSPlus::parseDegrees(term, rawNewLatData);
}

void TinyGPSLocation::setLongitude(const char *term)
{
TinyGPSPlus::parseDegrees(term, rawNewLngData);
}

double TinyGPSLocation::lat()
{
updated = false;
double ret = rawLatData.deg + rawLatData.billionths / 1000000000.0;
return rawLatData.negative ? -ret : ret;
}

double TinyGPSLocation::lng()
{
updated = false;
double ret = rawLngData.deg + rawLngData.billionths / 1000000000.0;
return rawLngData.negative ? -ret : ret;
}

void TinyGPSDate::commit()
{
date = newDate;
lastCommitTime = millis();
valid = updated = true;
}

void TinyGPSTime::commit()
{
time = newTime;
lastCommitTime = millis();
valid = updated = true;
}

void TinyGPSTime::setTime(const char *term)
{
newTime = (uint32_t)TinyGPSPlus::parseDecimal(term);
}

void TinyGPSDate::setDate(const char *term)
{
newDate = atol(term);
}

uint16_t TinyGPSDate::year()
{
updated = false;
uint16_t year = date % 100;
return year + 2000;
}

uint8_t TinyGPSDate::month()
{
updated = false;
return (date / 100) % 100;
}

uint8_t TinyGPSDate::day()
{
updated = false;
return date / 10000;
}

uint8_t TinyGPSTime::hour()
{
updated = false;
return time / 1000000;
}

uint8_t TinyGPSTime::minute()
{
updated = false;
return (time / 10000) % 100;
}

uint8_t TinyGPSTime::second()
{
updated = false;
return (time / 100) % 100;
}

uint8_t TinyGPSTime::centisecond()
{
updated = false;
return time % 100;
}

void TinyGPSDecimal::commit()
{
val = newval;
lastCommitTime = millis();
valid = updated = true;
}

void TinyGPSDecimal::set(const char *term)
{
newval = TinyGPSPlus::parseDecimal(term);
}

void TinyGPSInteger::commit()
{
val = newval;
lastCommitTime = millis();
valid = updated = true;
}

void TinyGPSInteger::set(const char *term)
{
newval = atol(term);
}

TinyGPSCustom::TinyGPSCustom(TinyGPSPlus &gps, const char *_sentenceName, int _termNumber)
{
begin(gps, _sentenceName, _termNumber);
}

void TinyGPSCustom::begin(TinyGPSPlus &gps, const char *_sentenceName, int _termNumber)
{
lastCommitTime = 0;
updated = valid = false;
sentenceName = _sentenceName;
termNumber = _termNumber;
memset(stagingBuffer, '\0', sizeof(stagingBuffer));
memset(buffer, '\0', sizeof(buffer));

// Insert this item into the GPS tree
gps.insertCustom(this, _sentenceName, _termNumber);
}

void TinyGPSCustom::commit()
{
strcpy(this->buffer, this->stagingBuffer);
lastCommitTime = millis();
valid = updated = true;
}

void TinyGPSCustom::set(const char *term)
{
strncpy(this->stagingBuffer, term, sizeof(this->stagingBuffer));
}

void TinyGPSPlus::insertCustom(TinyGPSCustom *pElt, const char *sentenceName, int termNumber)
{
TinyGPSCustom **ppelt;

for (ppelt = &this->customElts; *ppelt != NULL; ppelt = &(*ppelt)->next)
{
int cmp = strcmp(sentenceName, (*ppelt)->sentenceName);
if (cmp < 0 || (cmp == 0 && termNumber < (*ppelt)->termNumber))
break;
}

pElt->next = *ppelt;
*ppelt = pElt;
}

+ 273
- 0
lib/TinyGPS++/TinyGPS++.h View File

@@ -0,0 +1,273 @@
/*
TinyGPS++ - a small GPS library for Arduino providing universal NMEA parsing
Based on work by and "distanceBetween" and "courseTo" courtesy of Maarten Lamers.
Suggestion to add satellites, courseTo(), and cardinal() by Matt Monson.
Location precision improvements suggested by Wayne Holder.
Copyright (C) 2008-2013 Mikal Hart
All rights reserved.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/

#ifndef __TinyGPSPlus_h
#define __TinyGPSPlus_h

#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include <limits.h>

#define _GPS_VERSION "0.95" // software version of this library
#define _GPS_MPH_PER_KNOT 1.15077945
#define _GPS_MPS_PER_KNOT 0.51444444
#define _GPS_KMPH_PER_KNOT 1.852
#define _GPS_MILES_PER_METER 0.00062137112
#define _GPS_KM_PER_METER 0.001
#define _GPS_FEET_PER_METER 3.2808399
#define _GPS_MAX_FIELD_SIZE 15

struct RawDegrees
{
uint16_t deg;
uint32_t billionths;
bool negative;
public:
RawDegrees() : deg(0), billionths(0), negative(false)
{}
};

struct TinyGPSLocation
{
friend class TinyGPSPlus;
public:
bool isValid() const { return valid; }
bool isUpdated() const { return updated; }
uint32_t age() const { return valid ? millis() - lastCommitTime : (uint32_t)ULONG_MAX; }
const RawDegrees &rawLat() { updated = false; return rawLatData; }
const RawDegrees &rawLng() { updated = false; return rawLngData; }
double lat();
double lng();

TinyGPSLocation() : valid(false), updated(false)
{}

private:
bool valid, updated;
RawDegrees rawLatData, rawLngData, rawNewLatData, rawNewLngData;
uint32_t lastCommitTime;
void commit();
void setLatitude(const char *term);
void setLongitude(const char *term);
};

struct TinyGPSDate
{
friend class TinyGPSPlus;
public:
bool isValid() const { return valid; }
bool isUpdated() const { return updated; }
uint32_t age() const { return valid ? millis() - lastCommitTime : (uint32_t)ULONG_MAX; }

uint32_t value() { updated = false; return date; }
uint16_t year();
uint8_t month();
uint8_t day();

TinyGPSDate() : valid(false), updated(false), date(0)
{}

private:
bool valid, updated;
uint32_t date, newDate;
uint32_t lastCommitTime;
void commit();
void setDate(const char *term);
};

struct TinyGPSTime
{
friend class TinyGPSPlus;
public:
bool isValid() const { return valid; }
bool isUpdated() const { return updated; }
uint32_t age() const { return valid ? millis() - lastCommitTime : (uint32_t)ULONG_MAX; }

uint32_t value() { updated = false; return time; }
uint8_t hour();
uint8_t minute();
uint8_t second();
uint8_t centisecond();

TinyGPSTime() : valid(false), updated(false), time(0)
{}

private:
bool valid, updated;
uint32_t time, newTime;
uint32_t lastCommitTime;
void commit();
void setTime(const char *term);
};

struct TinyGPSDecimal
{
friend class TinyGPSPlus;
public:
bool isValid() const { return valid; }
bool isUpdated() const { return updated; }
uint32_t age() const { return valid ? millis() - lastCommitTime : (uint32_t)ULONG_MAX; }
int32_t value() { updated = false; return val; }

TinyGPSDecimal() : valid(false), updated(false), val(0)
{}

private:
bool valid, updated;
uint32_t lastCommitTime;
int32_t val, newval;
void commit();
void set(const char *term);
};

struct TinyGPSInteger
{
friend class TinyGPSPlus;
public:
bool isValid() const { return valid; }
bool isUpdated() const { return updated; }
uint32_t age() const { return valid ? millis() - lastCommitTime : (uint32_t)ULONG_MAX; }
uint32_t value() { updated = false; return val; }

TinyGPSInteger() : valid(false), updated(false), val(0)
{}

private:
bool valid, updated;
uint32_t lastCommitTime;
uint32_t val, newval;
void commit();
void set(const char *term);
};

struct TinyGPSSpeed : TinyGPSDecimal
{
double knots() { return value() / 100.0; }
double mph() { return _GPS_MPH_PER_KNOT * value() / 100.0; }
double mps() { return _GPS_MPS_PER_KNOT * value() / 100.0; }
double kmph() { return _GPS_KMPH_PER_KNOT * value() / 100.0; }
};

struct TinyGPSCourse : public TinyGPSDecimal
{
double deg() { return value() / 100.0; }
};

struct TinyGPSAltitude : TinyGPSDecimal
{
double meters() { return value() / 100.0; }
double miles() { return _GPS_MILES_PER_METER * value() / 100.0; }
double kilometers() { return _GPS_KM_PER_METER * value() / 100.0; }
double feet() { return _GPS_FEET_PER_METER * value() / 100.0; }
};

class TinyGPSPlus;
class TinyGPSCustom
{
public:
TinyGPSCustom() {};
TinyGPSCustom(TinyGPSPlus &gps, const char *sentenceName, int termNumber);
void begin(TinyGPSPlus &gps, const char *_sentenceName, int _termNumber);

bool isUpdated() const { return updated; }
bool isValid() const { return valid; }
uint32_t age() const { return valid ? millis() - lastCommitTime : (uint32_t)ULONG_MAX; }
const char *value() { updated = false; return buffer; }

private:
void commit();
void set(const char *term);

char stagingBuffer[_GPS_MAX_FIELD_SIZE + 1];
char buffer[_GPS_MAX_FIELD_SIZE + 1];
unsigned long lastCommitTime;
bool valid, updated;
const char *sentenceName;
int termNumber;
friend class TinyGPSPlus;
TinyGPSCustom *next;
};

class TinyGPSPlus
{
public:
TinyGPSPlus();
bool encode(char c); // process one character received from GPS
TinyGPSPlus &operator << (char c) {encode(c); return *this;}

TinyGPSLocation location;
TinyGPSDate date;
TinyGPSTime time;
TinyGPSSpeed speed;
TinyGPSCourse course;
TinyGPSAltitude altitude;
TinyGPSInteger satellites;
TinyGPSDecimal hdop;

static const char *libraryVersion() { return _GPS_VERSION; }

static double distanceBetween(double lat1, double long1, double lat2, double long2);
static double courseTo(double lat1, double long1, double lat2, double long2);
static const char *cardinal(double course);

static int32_t parseDecimal(const char *term);
static void parseDegrees(const char *term, RawDegrees &deg);

uint32_t charsProcessed() const { return encodedCharCount; }
uint32_t sentencesWithFix() const { return sentencesWithFixCount; }
uint32_t failedChecksum() const { return failedChecksumCount; }
uint32_t passedChecksum() const { return passedChecksumCount; }

private:
enum {GPS_SENTENCE_GPGGA, GPS_SENTENCE_GPRMC, GPS_SENTENCE_OTHER};

// parsing state variables
uint8_t parity;
bool isChecksumTerm;
char term[_GPS_MAX_FIELD_SIZE];
uint8_t curSentenceType;
uint8_t curTermNumber;
uint8_t curTermOffset;
bool sentenceHasFix;

// custom element support
friend class TinyGPSCustom;
TinyGPSCustom *customElts;
TinyGPSCustom *customCandidates;
void insertCustom(TinyGPSCustom *pElt, const char *sentenceName, int index);

// statistics
uint32_t encodedCharCount;
uint32_t sentencesWithFixCount;
uint32_t failedChecksumCount;
uint32_t passedChecksumCount;

// internal utilities
int fromHex(char a);
bool endOfTermHandler();
};

#endif // def(__TinyGPSPlus_h)

+ 83
- 269
src/main.cpp View File

@@ -1,310 +1,124 @@
#include "Arduino.h"
#include <SoftwareSerial.h>
#include "TinyGPS.h"
#include "TinyGPS++.h"
#include "LibAPRS.h"

//UBLOX-TX sends data to the Arduino-RX
//UBLOX-RX receives data from the Arduino-TX

static const int RXPin = 4, TXPin = 3;
static const int RXPin = 10, TXPin = 8;
static const uint32_t SerialBaud = 9600;
static const uint32_t GPSBaud = 9600;

// basic setup for the APRS radio section.
#define ADC_REFERENCE REF_5V
#define OPEN_SQUELCH false
char callSign[] = "DC6AP";
char comment[] = "DC6AP APRS Tracker";

boolean gotPacket = false;
AX25Msg incomingPacket;
uint8_t *packetData;
TinyGPS gps;

// set up a couple variables for when we last transmitted.
// aprs spec is around 10 minutes or so (faster if your speed is faster)
// but probably not more than once per minute.
long lastUpdate=0;
int Updatedelay=600; //delay between packets in seconds.
#define ADC_REFERENCE REF_5V
#define OPEN_SQUELCH false
// The TinyGPS++ object
TinyGPSPlus gps;

// The serial connection to the GPS device
SoftwareSerial ss(RXPin, TXPin);

static void smartdelay(unsigned long ms);
static void print_float(float val, float invalid, int len, int prec);
static void print_int(unsigned long val, unsigned long invalid, int len);
static void print_date(TinyGPS &gps);
char *ftoa(char *a, double f, int precision);
void processPacket();
void locationUpdate(float flat,float flon);
// GPS Test Data
const char *gpsStream =
"$GPRMC,045103.000,A,3014.1984,N,09749.2872,W,0.67,161.46,030913,,,A*7C\r\n"
"$GPGGA,045104.000,3014.1985,N,09749.2873,W,1,09,1.2,211.6,M,-22.5,M,,0000*62\r\n"
"$GPRMC,045200.000,A,3014.3820,N,09748.9514,W,36.88,65.02,030913,,,A*77\r\n"
"$GPGGA,045201.000,3014.3864,N,09748.9411,W,1,10,1.2,200.8,M,-22.5,M,,0000*6C\r\n"
"$GPRMC,045251.000,A,3014.4275,N,09749.0626,W,0.51,217.94,030913,,,A*7D\r\n"
"$GPGGA,045252.000,3014.4273,N,09749.0628,W,1,09,1.3,206.9,M,-22.5,M,,0000*6F\r\n";

static void smartDelay(unsigned long ms);
static void displayInfo();

void setup() {
// initialize serial output.
// 9600 is mandatory. the aprs stuff can't hack a faster rate
// even though it doesn't directly use this.
Serial.begin(SerialBaud);
Serial.println(F("Sats HDOP Latitude Longitude Fix Date Time Date Alt "));
Serial.println(F(" (deg) (deg) Age Age (m) "));
Serial.println(F("--------------------------------------------------------------------"));

// initialize serial to talk with the GPS.
// 9600 is mandatory.
ss.begin(GPSBaud);
// Initialise APRS library - This starts the mode
APRS_init(ADC_REFERENCE, OPEN_SQUELCH);
// You must at a minimum configure your callsign and SSID
// you may also not use this software without a valid license
APRS_setCallsign("DC6AP", 1);
APRS_printSettings();
Serial.print(F("Free RAM: ")); Serial.println(freeMemory());
}


void loop() {

float fLat, fLon;
unsigned long age, date = 0;

gps.f_get_position(&fLat, &fLon, &age);
Serial.begin(SerialBaud);
while(!Serial);

print_float(fLat, TinyGPS::GPS_INVALID_F_ANGLE, 10, 6);
print_float(fLon, TinyGPS::GPS_INVALID_F_ANGLE, 11, 6);
print_int(age, TinyGPS::GPS_INVALID_AGE, 5);
print_date(gps);

if ((fLat != TinyGPS::GPS_INVALID_F_ANGLE ) && ( fLon != TinyGPS::GPS_INVALID_F_ANGLE )){
// check for any new packets.
smartdelay(250);
processPacket();
smartdelay(250);
locationUpdate(fLat,fLon);
}
ss.begin(GPSBaud);
delay(1000);
Serial.println("Setup Complete!");

smartdelay(1000);
// check for packets.
processPacket();
}
APRS_init(ADC_REFERENCE, OPEN_SQUELCH);
APRS_setCallsign(callSign, 1);
APRS_setSymbol('n');
APRS_printSettings();
Serial.print(F("Free RAM: ")); Serial.println(freeMemory());

// TODO: have to rewrite
static void print_int(unsigned long val, unsigned long invalid, int len) {
char sz[32];
if (val == invalid)
strcpy(sz, "*******");
else
sprintf(sz, "%ld", val);
sz[len] = 0;
for (int i=strlen(sz); i<len; ++i)
sz[i] = ' ';
if (len > 0)
sz[len-1] = ' ';
Serial.print(sz);
smartdelay(0);
Serial.println(F("Sats HDOP Latitude Longitude Fix Date Time Date Alt "));
Serial.println(F(" (deg) (deg) Age Age (m) "));
Serial.println(F("--------------------------------------------------------------------"));
}

static void print_date(TinyGPS &gps) {
int year;
byte month, day, hour, minute, second, hundredths;
unsigned long age;
gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
if (age == TinyGPS::GPS_INVALID_AGE)
Serial.print(F("********** ******** "));
else
{
char sz[32];
sprintf(sz, "%02d/%02d/%02d %02d:%02d:%02d ",
month, day, year, hour, minute, second);
Serial.print(sz);
void displayInfo() {
Serial.print(F("Location: "));
if (gps.location.isValid()) {
Serial.print(gps.location.lat(), 6);
Serial.print(F(","));
Serial.print(gps.location.lng(), 6);
} else {
Serial.print(F("INVALID"));
}
print_int(age, TinyGPS::GPS_INVALID_AGE, 5);
smartdelay(0);
}


//basically delay without stopping the processor.
// the library still needs to be able to decode incoming packets.
static void smartdelay(unsigned long ms) {
unsigned long start = millis();
do {
while (ss.available())
gps.encode(ss.read());
} while (millis() - start < ms);
}

void locationUpdate(float flat,float flon) {
// Let's first set our latitude and longtitude.
// These should be in NMEA format!


// flat, negative == Southern hemisphere
// flon, negative == western hemisphere
// for instance... 43.646808, -116.286437
//nmea then is:
// 43 .. (.646808*60),N
// 116 .. (.286437*60),W (w since it was negative)
// APRS chokes when you send a ,N though...
// it also chokes when you send more than 2 decimal places.

int temp;
char y[13];
char z[13];

// CONVERT to NMEA.
if (flat<0){
temp=-(int)flat;
flat=temp*100 - (flat+temp)*60;
ftoa((char*)&y,flat,3);
//z[10]=',';
if (flat > 10000) {
y[8]='S';
y[9]=0;
} else {
y[7]='S';
y[8]=0;
}
Serial.print(F(" Date/Time: "));
if (gps.date.isValid()) {
Serial.print(gps.date.month());
Serial.print(F("/"));
Serial.print(gps.date.day());
Serial.print(F("/"));
Serial.print(gps.date.year());
} else {
temp=(int)flat;
flat=temp*100 + (flat-temp)*60;
ftoa((char*)&y,flat,3);
//z[10]=',';
if (flat > 10000) {
y[8]='N';
y[9]=0;
} else {
y[7]='N';
y[8]=0;
}
}
APRS_setLat(y);
Serial.println();
Serial.println(F("Location Update:"));
Serial.print(F("Lat: "));
Serial.println(y);
if (flon<0){
temp=-(int)flon;
flon=temp*100 - (flon+temp)*60;
ftoa((char*)&z,flon,3);
//z[10]=',';
if (flon > 10000){
z[8]='W';
z[9]=0;
} else {
z[7]='W';
z[8]=0;
}
Serial.print(F("INVALID"));
}

Serial.print(F(" "));
if (gps.time.isValid()) {
if (gps.time.hour() < 10) Serial.print(F("0"));
Serial.print(gps.time.hour());
Serial.print(F(":"));
if (gps.time.minute() < 10) Serial.print(F("0"));
Serial.print(gps.time.minute());
Serial.print(F(":"));
if (gps.time.second() < 10) Serial.print(F("0"));
Serial.print(gps.time.second());
Serial.print(F("."));
if (gps.time.centisecond() < 10) Serial.print(F("0"));
Serial.print(gps.time.centisecond());
} else {
temp=(int)flon;
flon=temp*100 + (flon-temp)*60;
ftoa((char*)&z,flon,3);
//z[10]=',';
if (flon > 10000){
z[8]='E';
z[9]=0;
} else {
z[7]='E';
z[8]=0;
}
}
APRS_setLon(z);
// done converting to NMEA.
Serial.print(F("INVALID"));
}

Serial.print(F("Lon:"));
Serial.println(z);
// We can optionally set power/height/gain/directivity
// information. These functions accept ranges
// from 0 to 10, directivity 0 to 9.
// See this site for a calculator:
// http://www.aprsfl.net/phgr.php
// LibAPRS will only add PHG info if all four variables
// are defined!
//APRS_setPower(2);
//APRS_setHeight(4);
//APRS_setGain(7);
//APRS_setDirectivity(0);
// We'll define a comment string
char *comment = " JWAprsTrkr";
// And send the update
APRS_sendLoc(comment, strlen(comment));
Serial.println();
}

// Here's a function to process incoming packets
// Remember to call this function often, so you
// won't miss any packets due to one already
// waiting to be processed
void processPacket() {
if (gotPacket) {
gotPacket = false;
Serial.print(F("Rcvd APRS packet. SRC: "));
Serial.print(incomingPacket.src.call);
Serial.print(F("-"));
Serial.print(incomingPacket.src.ssid);
Serial.print(F(". DST: "));
Serial.print(incomingPacket.dst.call);
Serial.print(F("-"));
Serial.print(incomingPacket.dst.ssid);
Serial.print(F(". Data: "));

for (int i = 0; i < incomingPacket.len; i++) {
Serial.write(incomingPacket.info[i]);
}
Serial.println(F(""));

// Remeber to free memory for our buffer!
free(packetData);

// You can print out the amount of free
// RAM to check you don't have any memory
// leaks
Serial.print(F("Free RAM: ")); Serial.println(freeMemory());
}
void loop() {
smartDelay(2500);
displayInfo();
}

// converts float to char*
char *ftoa(char *a, double f, int precision){
long p[] = {0,10,100,1000,10000,100000,1000000,10000000,100000000};
char *ret = a;
long heiltal = (long)f;
itoa(heiltal, a, 10);
while (*a != '\0') a++;
*a++ = '.';
long desimal = abs((long)((f - heiltal) * p[precision]));
itoa(desimal, a, 10);
return ret;
// basically delay without stopping the processor.
static void smartDelay(unsigned long ms) {
unsigned long start = millis();
do {
while (ss.available()) {
char c = ss.read();
gps.encode(c);
//Serial.write(c);
}
} while (millis() - start < ms);
}

static void print_float(float val, float invalid, int len, int prec)
{
if (val == invalid)
{
while (len-- > 1)
Serial.print(F("*"));
Serial.print(F(" "));
}
else
{
Serial.print(val, prec);
int vi = abs((int)val);
int flen = prec + (val < 0.0 ? 2 : 1); // . and -
flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
for (int i=flen; i<len; ++i)
Serial.print(F(" "));
}
smartdelay(0);
}

void aprs_msg_callback(struct AX25Msg *msg) {
// If we already have a packet waiting to be

+ 342
- 0
src/main.cpp.bak View File

@@ -0,0 +1,342 @@
#include "Arduino.h"
#include <SoftwareSerial.h>
#include "TinyGPS.h"
#include "LibAPRS.h"

//UBLOX-TX sends data to the Arduino-RX
//UBLOX-RX receives data from the Arduino-TX

static const int RXPin = 4, TXPin = 3;
static const uint32_t SerialBaud = 9600;
static const uint32_t GPSBaud = 9600;
char callSign[] = "DC6AP";
char comment[] = "DC6AP APRS Tracker";

// basic setup for the APRS radio section.
#define ADC_REFERENCE REF_5V
#define OPEN_SQUELCH false

boolean gotPacket = false;
AX25Msg incomingPacket;
uint8_t *packetData;
TinyGPS gps;

// set up a couple variables for when we last transmitted.
// aprs spec is around 10 minutes or so (faster if your speed is faster)
// but probably not more than once per minute.
long lastUpdate=0;
int Updatedelay=600; //delay between packets in seconds.

// The serial connection to the GPS device
SoftwareSerial ss(RXPin, TXPin);

static void smartDelay(unsigned long ms);
static void print_float(float val, float invalid, int len, int prec);
static void print_int(unsigned long val, unsigned long invalid, int len);
static void print_date(TinyGPS &gps);
char *ftoa(char *a, double f, int precision);
void processPacket();
void locationUpdate(float flat,float flon);
void cardinalPointSN(float latitude, char *coord);


void setup() {
// initialize serial output.
// 9600 is mandatory. the aprs stuff can't hack a faster rate
Serial.begin(SerialBaud);
Serial.println(F("Sats HDOP Latitude Longitude Fix Date Time Date Alt "));
Serial.println(F(" (deg) (deg) Age Age (m) "));
Serial.println(F("--------------------------------------------------------------------"));

// initialize serial to talk with the GPS.
// 9600 is mandatory.
ss.begin(GPSBaud);
// Initialise APRS library - This starts the mode
APRS_init(ADC_REFERENCE, OPEN_SQUELCH);
// You must at a minimum configure your callsign and SSID
// you may also not use this software without a valid license

APRS_setCallsign(callSign, 1);
APRS_printSettings();
Serial.print(F("Free RAM: ")); Serial.println(freeMemory());
}


void loop() {
float fLat, fLon;
unsigned long age = 0;
while (ss.available()) {
Serial.write(ss.read());
//gps.encode(ss.read());
//displayInfo();
}
gps.f_get_position(&fLat, &fLon, &age);

print_float(fLat, TinyGPS::GPS_INVALID_F_ANGLE, 10, 6);
print_float(fLon, TinyGPS::GPS_INVALID_F_ANGLE, 11, 6);
print_int(age, TinyGPS::GPS_INVALID_AGE, 5);
print_date(gps);

if ((fLat != TinyGPS::GPS_INVALID_F_ANGLE ) && ( fLon != TinyGPS::GPS_INVALID_F_ANGLE )){
// check for any new packets.
smartDelay(250);
processPacket();
smartDelay(250);
locationUpdate(fLat,fLon);
}

smartDelay(1000);
// check for packets.
processPacket();
}

// TODO: have to rewrite
static void print_int(unsigned long val, unsigned long invalid, int len) {
char sz[32];
if (val == invalid)
strcpy(sz, "*******");
else
sprintf(sz, "%ld", val);
sz[len] = 0;
for (int i=strlen(sz); i<len; ++i)
sz[i] = ' ';
if (len > 0)
sz[len-1] = ' ';
Serial.print(sz);
smartDelay(0);
}

// TODO: have to rewrite
static void print_date(TinyGPS &gps) {
int year;
byte month, day, hour, minute, second, hundredths;
unsigned long age;
gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
if (age == TinyGPS::GPS_INVALID_AGE)
Serial.print(F("********** ******** "));
else
{
char sz[32];
sprintf(sz, "%02d/%02d/%02d %02d:%02d:%02d ",
month, day, year, hour, minute, second);
Serial.print(sz);
}
print_int(age, TinyGPS::GPS_INVALID_AGE, 5);
smartDelay(0);
}


// basically delay without stopping the processor.
static void smartDelay(unsigned long ms) {
unsigned long start = millis();
do {
while (ss.available())
gps.encode(ss.read());
} while (millis() - start < ms);
}

void cardinalPoint(int temp, float latlong, char* coord, char compa) {
latlong =temp * 100 - (latlong + temp)*60;
ftoa((char*)&coord, latlong, 3);
if (latlong > 10000) {
coord[8]=compa;
coord[9]=0;
} else {
coord[7]=compa;
coord[8]=0;
}
}

void locationUpdate(float latitude,float longitude) {
int temp;
char y[13];
char z[13];

// CONVERT to NMEA.
if (latitude<0){
//cardinalPoint(-(int)latitute, latitude, &y, 'S');
temp=-(int)latitude;
latitude=temp*100 - (latitude+temp)*60;
ftoa((char*)&y,latitude,3);
//z[10]=',';
if (latitude > 10000) {
y[8]='S';
y[9]=0;
} else {
y[7]='S';
y[8]=0;
}
} else {
//cardinalPoint(-(int)latitute, latitude, &y, 'N');
temp=(int)latitude;
latitude=temp*100 + (latitude-temp)*60;
ftoa((char*)&y,latitude,3);
//z[10]=',';
if (latitude > 10000) {
y[8]='N';
y[9]=0;
} else {
y[7]='N';
y[8]=0;
}
}
APRS_setLat(y);
Serial.println();
Serial.println(F("Location Update:"));
Serial.print(F("Lat: "));
Serial.println(y);
if (longitude<0){
//cardinalPoint(-(int)longitude, latitude, &z, 'W');
temp=-(int)longitude;
longitude=temp*100 - (longitude+temp)*60;
ftoa((char*)&z,longitude,3);
//z[10]=',';
if (longitude > 10000){
z[8]='W';
z[9]=0;
} else {
z[7]='W';
z[8]=0;
}
} else {
//cardinalPoint((int)longitude, latitude, &z, 'E');
temp=(int)longitude;
longitude=temp*100 + (longitude-temp)*60;
ftoa((char*)&z,longitude,3);
//z[10]=',';
if (longitude > 10000){
z[8]='E';
z[9]=0;
} else {
z[7]='E';
z[8]=0;
}
}
APRS_setLon(z);
// done converting to NMEA.

Serial.print(F("Lon:"));
Serial.println(z);
// We can optionally set power/height/gain/directivity
// information. These functions accept ranges
// from 0 to 10, directivity 0 to 9.
// See this site for a calculator:
// http://www.aprsfl.net/phgr.php
// LibAPRS will only add PHG info if all four variables
// are defined!
//APRS_setPower(2);
//APRS_setHeight(4);
//APRS_setGain(7);
//APRS_setDirectivity(0);
// And send the update
APRS_sendLoc(comment, strlen(comment));
}

// Here's a function to process incoming packets
// Remember to call this function often, so you
// won't miss any packets due to one already
// waiting to be processed
void processPacket() {
if (gotPacket) {
gotPacket = false;
Serial.print(F("Rcvd APRS packet. SRC: "));
Serial.print(incomingPacket.src.call);
Serial.print(F("-"));
Serial.print(incomingPacket.src.ssid);
Serial.print(F(". DST: "));
Serial.print(incomingPacket.dst.call);
Serial.print(F("-"));
Serial.print(incomingPacket.dst.ssid);
Serial.print(F(". Data: "));

for (int i = 0; i < (int) incomingPacket.len; i++) {
Serial.write(incomingPacket.info[i]);
}
Serial.println(F(""));

// Remeber to free memory for our buffer!
free(packetData);

// You can print out the amount of free
// RAM to check you don't have any memory
// leaks
Serial.print(F("Free RAM: ")); Serial.println(freeMemory());
}
}

// converts float to char*
char *ftoa(char *a, double f, int precision) {
long p[] = {0,10,100,1000,10000,100000,1000000,10000000,100000000};
char *ret = a;
long heiltal = (long)f;
itoa(heiltal, a, 10);
while (*a != '\0') a++;
*a++ = '.';
long desimal = abs((long)((f - heiltal) * p[precision]));
itoa(desimal, a, 10);
return ret;
}

// TODO: have to rewrite
static void print_float(float val, float invalid, int len, int prec)
{
if (val == invalid)
{
while (len-- > 1)
Serial.print(F("*"));
Serial.print(F(" "));
}
else
{
Serial.print(val, prec);
int vi = abs((int)val);
int flen = prec + (val < 0.0 ? 2 : 1); // . and -
flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
for (int i=flen; i<len; ++i)
Serial.print(F(" "));
}
smartDelay(0);
}

void aprs_msg_callback(struct AX25Msg *msg) {
// If we already have a packet waiting to be
// processed, we must drop the new one.
if (!gotPacket) {
// Set flag to indicate we got a packet
gotPacket = true;

// The memory referenced as *msg is volatile
// and we need to copy all the data to a
// local variable for later processing.
memcpy(&incomingPacket, msg, sizeof(AX25Msg));

// We need to allocate a new buffer for the
// data payload of the packet. First we check
// if there is enough free RAM.
if (freeMemory() > (int) msg->len) {
packetData = (uint8_t*)malloc(msg->len);
memcpy(packetData, msg->info, msg->len);
incomingPacket.info = packetData;
} else {
// We did not have enough free RAM to receive
// this packet, so we drop it.
gotPacket = false;
}
}
}

Loading…
Cancel
Save