// This program simulates incoming sensor data on four serial ports using 4 GPS devices and a PC port.
// One of the GPS devices provides all nmea strings at 9600 baud at a 1Hz rate (cannot be modified),
// the other 3 GPS devices can be customized to send all nmea strings at different output rates (1,2,4,5,10Hz)
// ;gga and gvt only, or just gga at the same baud rate for all three (currently 115200 is the default). The PC
// port is used to feed a time-hack to the Teensy serial port at a 10Hz rate - to simulate yet another serial feed to the teensy.
//
// NTP/RTC:
// In setup(), prior to calling initNTPrtc() 8 times, the microsecond timer is started; the total time
// that it took to get ntp time 8 times is averaged, halved, and converted to counts; added to the counts, and
// the final counts is used to set the RTC. rtcThread() gets kicked off and reads the RTC periodically
// (less than 1ms) constantly updating rtcSeconds and rtcFracs. The serial threads grab
// rtcFracs when first character is read and both get written ahead of the nmea string that is written to the
// UDP socket.
// Currently, initNTPrtc() is called every minute and sets the RTC. While PPS is present, I'm not using it at this time for sync'ing time
// because I have to simulate this operation as if PPS is not available.
// notes:
// When using a TI MAX3232 RS232 driver, remember that channel 2 RS232 is inverted
//
// * Note about using chronyd on a linux system as NTP server:
// Make sure to set the "allow x.x.x.0/24" in /etc/chrony.conf to the appropriate subnet.
#include <EEPROM.h>
#include <imxrt.h>
#include <NativeEthernet.h>
#include <NativeEthernetUdp.h>
#include <TeensyThreads.h>
#include <TimeLib.h>
extern "C" uint32_t set_arm_clock(uint32_t frequency); // required prototype to set CPU clock
#define CLOCK_FREQ 816000000 // 720, 816, 912(c), 960(c), 1008(c) c=cooling required
//------------------------
// Serial vars
//------------------------
#define HWSERIAL1 Serial1 //rx = pin 0, tx = pin 1
#define HWSERIAL2 Serial2 //rx = pin 7, tx = pin 8
#define GPSHWSERIAL3 Serial3 //rx = pin 15, tx = pin 14 ; NAVSTAR
#define GPSHWSERIAL4 Serial4 //rx = pin 16, tx = pin 17
#define GPSHWSERIAL6 Serial6 //rx = pin 25, tx = pin 24
#define HWSERIAL7 Serial7 //rx = pin 28, tx = pin 29 ; PC
#define GPSHWSERIAL8 Serial8 //rx = pin 34, tx = pin 35
int defaultbaud3=9600, defaultbaud4=115200, defaultbaud6=115200;
int defaultbaud7=115200,defaultbaud8=115200, defaultbaudUSB=115200;
int defaultrate4=10, defaultrate6=10, defaultrate8=10;
int defaultnmea4=2, defaultnmea6=2, defaultnmea8=2; //0=all,1=gga,2=gga+gvt
String inData3, inData4, inData6, inData7, inData8;
char ser_s1[32], ser_s2[16];
char GPSbaud_4800[]="$PCAS01,0*1C\r\n"; // GPS CAS Instructions
char GPSbaud_9600[]="$PCAS01,1*1D\r\n";
char GPSbaud_19200[]="$PCAS01,2*1E\r\n";
char GPSbaud_38400[]="$PCAS01,3*1F\r\n";
char GPSbaud_57600[]="$PCAS01,4*18\r\n";
char GPSbaud_115200[]="$PCAS01,5*19\r\n";
char GPSupdatefreq_1Hz[]="$PCAS02,1000*2E\r\n";
char GPSupdatefreq_2Hz[]="$PCAS02,500*1A\r\n";
char GPSupdatefreq_4Hz[]="$PCAS02,250*18\r\n";
char GPSupdatefreq_5Hz[]="$PCAS02,200*1D\r\n";
char GPSupdatefreq_10Hz[]="$PCAS02,100*1E\r\n";
char GPSnmeaEnableALL[]="$PCAS03,1,1,1,1,1,1,1,1,1,1,1,1,1,1*02\r\n";
char GPSnmeaEnableGGA_only[]="$PCAS03,1,0,0,0,0,0,0,0,0,0,0,0,0,0*03\r\n";
char GPSnmeaEnableGGA_VTG_only[]="$PCAS03,1,0,0,0,0,1,0,0,0,0,0,0,0,0*02\r\n";
char GPSreset_hot[]="$PCAS10,0*1C\r\n";
char GPSreset_warm[]="$PCAS10,1*1D\r\n";
char GPSreset_cold[]="$PCAS10,2*1E\r\n";
char GPSreset_factory[]="$PCAS10,3*1F\r\n";
//------------------------
// Network vars
//------------------------
// Interface Settings
byte mac[] = {
0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xD5
};
IPAddress ip(10, 2, 69, 70); // Ignored when using dhcp
IPAddress bcastip(10,2,69,255);
IPAddress ptpMcastIp(224, 0, 1, 129);
IPAddress timeServer(10,2,69,199);
// Socket Settings (Interface can handle up to 8 sockets)
EthernetUDP Udp3; //chan 3 udp socket
EthernetUDP Udp4; //chan 4 udp socket
EthernetUDP Udp6; //chan 6 udp socket
EthernetUDP Udp7; //chan 7 udp socket
EthernetUDP Udp8; //chan 8 udp socket
EthernetUDP ntpUdp; //NTP socket
// UDP Source ports (49152 through 65535)
uint16_t udp_srcport3=49253, udp_srcport4=49254, udp_srcport6=49254;
uint16_t udp_srcport7=49257, udp_srcport8=49258, ntpudp_srcport=49250;
// UDP Destination ports (should eventually be selectable)
uint16_t udp_dstport3=10543, udp_dstport4=10544, udp_dstport6=10546;
uint16_t udp_dstport7=10547, udp_dstport8=10548, ntpudp_dstport=123;
const int NMEA_SIZE = 128;
const int NTP_PACKET_SIZE = 48;
const int NTP_WAIT_TIME = 5;
const int NTP_SYNC_FREQ = 59; //seconds
const int NTP_RTC_CAL = 60; // this number represents number of counts to add to value used to set the RTC. 1 cnt = ~30.5us
//------------------------
// General Purpose vars
//------------------------
const int rstALLpin = 32;
const byte PPS = 31; // Use pin 31 for PPS interrupt
int PPSpin = LOW;
const float SECS_PER_TIC_RTC = 1.0 / pow(2, 15);
const double SECS_PER_TIC_NTP = 1.0 / pow(2, 32);
unsigned int Tcsc, initLock=0;
unsigned long secsSince1900, highWord,lowWord, fracHighWord, fracLowWord, seventyYears = 2208988800UL;
uint32_t epoch, ntpFracSecs;
uint64_t rtcSeconds;
double rtcFracf;
int rtcFracint=0;
double rtcSecondsf, ntpFracf;
char T0secs[]="0000000000000000", T0fracs[]="9999999"; //time set to RTC
char rtcSecStr[]="0000000000000000", rtcFracStr[]="9999999"; //used by getRTC()
long double Trx,Ttx,Ttxrx;
byte packetBuffer[NTP_PACKET_SIZE];
bool ntpFail = false;
//Allocate some space in EEPROM for storing serial settings
unsigned int eeAddrBaudGPS4 = 0;
unsigned int eeAddrGPSupdatefreq4 = sizeof(GPSbaud_115200) + 1;
unsigned int eeAddrNmeaGPS4 = eeAddrGPSupdatefreq4 + sizeof(GPSupdatefreq_1Hz) + 1;
unsigned int eeAddrBaudGPS6 = eeAddrNmeaGPS4 + sizeof(GPSnmeaEnableALL) +1;
unsigned int eeAddrGPSupdatefreq6 = eeAddrBaudGPS6 + sizeof(GPSbaud_115200) + 1;
unsigned int eeAddrNmeaGPS6 = eeAddrGPSupdatefreq6 + sizeof(GPSupdatefreq_1Hz) + 1;
unsigned int eeAddrBaudGPS8 = eeAddrNmeaGPS6 + sizeof(GPSnmeaEnableALL) +1;
unsigned int eeAddrGPSupdatefreq8 = eeAddrBaudGPS8 + sizeof(GPSbaud_115200) + 1;
unsigned int eeAddrNmeaGPS8 = eeAddrGPSupdatefreq8 + sizeof(GPSupdatefreq_1Hz) + 1;
String eeGPSbaud4, eeGPSupdatefreq4, eeGPSnmea4, eeGPSbaud6, eeGPSupdatefreq6, eeGPSnmea6, eeGPSbaud8, eeGPSupdatefreq8, eeGPSnmea8;
// Timers/counters
elapsedMicros t_micros;
elapsedMillis t_millis;
elapsedMicros t_ntp_micros;
int clockset_cnt =0;
// semaphores
Threads::Mutex Udp_mutex;
Threads::Mutex serial_mutex;
//======================================================
// Custom Subroutines
//======================================================
void sendNTPpacket(IPAddress address) {
// set all bytes in the buffer to 0
memset(packetBuffer, 0, NTP_PACKET_SIZE);
// Initialize values needed to form an NTP packet
packetBuffer[0] = 0xE3; // LI, Version, Mode
packetBuffer[1] = 0x00; // Stratum, or type of clock
packetBuffer[2] = 0x04; // Polling Interval
packetBuffer[3] = 0xEC; // Peer Clock Precision (-20 => 954 nS )
packetBuffer[12] = 0x00;
packetBuffer[13] = 0x00;
packetBuffer[14] = 0x00;
packetBuffer[15] = 0x00;
// Send the packet to the NTP server
ntpUdp.beginPacket(address, 123); // NTP requests are sent to port 123
ntpUdp.write(packetBuffer, NTP_PACKET_SIZE);
ntpUdp.endPacket();
}
//-----------------------------------------
void getNTPTime() {
sendNTPpacket(timeServer); // send and NTP packet to the time server
epoch = 0;
t_millis = 0;
while(1) {
if (t_millis > 999) {
ntpFail = true;
break;
}
if (ntpUdp.parsePacket() == NTP_PACKET_SIZE) {
ntpUdp.read(packetBuffer, NTP_PACKET_SIZE); // read the packet into the buffer
// decode the "Transmit timestamp" from the packet
highWord = word(packetBuffer[40], packetBuffer[41]);
lowWord = word(packetBuffer[42], packetBuffer[43]);
secsSince1900 = highWord << 16 | lowWord;
epoch = secsSince1900 - seventyYears;
fracHighWord = word(packetBuffer[44], packetBuffer[45]);
fracLowWord = word(packetBuffer[46], packetBuffer[47]);
ntpFracSecs = fracHighWord << 16 | fracLowWord;
Serial.println("--------NTP Read----------");
Serial.print("NTP: ");
Serial.print(epoch);
Serial.print(" ");
ntpFracf = ntpFracSecs * SECS_PER_TIC_NTP;
Serial.printf("%lf\n", ntpFracf);
Serial.println("-----------------------------");
break;
}
}
}
//-----------------------------------------
void rtc_set_secs_and_frac(uint32_t secs, uint32_t frac)
{
// stop the RTC
SNVS_HPCR &= ~(SNVS_HPCR_RTC_EN | SNVS_HPCR_HP_TS);
while (SNVS_HPCR & SNVS_HPCR_RTC_EN); // wait
// stop the SRTC
SNVS_LPCR &= ~SNVS_LPCR_SRTC_ENV;
while (SNVS_LPCR & SNVS_LPCR_SRTC_ENV); // wait
// set the SRTC
SNVS_LPSRTCLR = ((secs & 0x1ffffUL) << 15) | (frac & 0x7fff);
SNVS_LPSRTCMR = secs >> 17;
// start the SRTC
SNVS_LPCR |= SNVS_LPCR_SRTC_ENV;
while (!(SNVS_LPCR & SNVS_LPCR_SRTC_ENV)); // wait
// start the RTC and sync it to the SRTC
SNVS_HPCR |= SNVS_HPCR_RTC_EN | SNVS_HPCR_HP_TS;
}
//-----------------------------------------
uint64_t rtc_get_64(void)
{
uint32_t hi1 = SNVS_HPRTCMR;
uint64_t tmp64;
uint32_t lo1 = SNVS_HPRTCLR;
while (1) {
uint32_t hi2 = SNVS_HPRTCMR;
uint32_t lo2 = SNVS_HPRTCLR;
if (lo1 == lo2 && hi1 == hi2) {
tmp64 = hi1;
return (tmp64 << 32) | lo1;
}
hi1 = hi2;
lo1 = lo2;
}
}
//-----------------------------------------
void printBinary(uint32_t num) {
for (int i = 3; i > -1; i--){
BinaryStrZeroPad((num >> (8*i)) & 0xFF, 7);
Serial .print(" ");
}
}
void BinaryStrZeroPad(int Number,char ZeroPadding){
//ZeroPadding = nth bit, e.g for a 16 bit number nth bit = 15
signed char i=ZeroPadding;
while(i>=0){
if((Number & (1<<i)) > 0) Serial.write('1');
else Serial.write('0');
--i;
}
}
//-----------------------------------------
void getRTC() {
uint64_t tmi = rtc_get_64();
rtcSeconds = tmi >> 15;
int rtcFracSecs = (tmi & 0x7fff);
rtcFracf = rtcFracSecs * SECS_PER_TIC_RTC;
rtcFracint = rtcFracf * 1E6;
sprintf(rtcSecStr, "%llu ", rtcSeconds);
sprintf(rtcFracStr, "%06d ", rtcFracint);
}
//-----------------------------------------
/*
void printUTCtime() {
unsigned int xepoch = rtcSeconds;
// print the hour, minute and second:
Serial.print("UTC: ");
Serial.printf("%d %d %d ", year(rtcSeconds), month(rtcSeconds), day(rtcSeconds));
Serial.print((xepoch % SECS_PER_DAY) / SECS_PER_HOUR); // print the hour (86400 equals secs per day)
Serial.print(':');
if (((xepoch % SECS_PER_HOUR) / SECS_PER_MIN) < 10) {
// In the first 10 minutes of each hour, we'll want a leading '0'
Serial.print('0');
}
Serial.print((xepoch % SECS_PER_HOUR) / SECS_PER_MIN); // print the minute
Serial.print(':');
if ((xepoch % SECS_PER_MIN) < 10) {
// In the first 10 seconds of each minute, we'll want a leading '0'
Serial.print('0');
}
Serial.println(xepoch % SECS_PER_MIN); // print the second
}
*/
//-----------------------------------------
void serial3Thread() {
while (1) {
char rtcFracStrTmp[NMEA_SIZE]="";
serial_mutex.lock();
while (GPSHWSERIAL3.available() > 0) {
char received = GPSHWSERIAL3.read();
if (strlen(rtcFracStrTmp) < strlen(rtcFracStr))
strcat(rtcFracStrTmp, rtcFracStr);
inData3 += received;
if (received == '\n') {
char chan3[NMEA_SIZE] = "Rx3: ";
strcat(chan3, rtcSecStr);
strcat(chan3, rtcFracStr);
char charBuf3[NMEA_SIZE];
//memset(charBuf3, 0, NMEA_SIZE);
inData3.toCharArray(charBuf3, NMEA_SIZE);
strcat(chan3, charBuf3);
Udp_mutex.lock();
Udp3.beginPacket(bcastip, udp_dstport3);
Udp3.write(chan3, strlen(chan3));
Udp3.endPacket();
Udp_mutex.unlock();
inData3 = "";
}
}
serial_mutex.unlock();
threads.yield();
}
}
//-----------------------------------------
void serial4Thread() {
while (1) {
char rtcFracStrTmp[NMEA_SIZE]="";
serial_mutex.lock();
while (GPSHWSERIAL4.available() > 0) {
char received = GPSHWSERIAL4.read();
if (strlen(rtcFracStrTmp) < strlen(rtcFracStr))
strcat(rtcFracStrTmp, rtcFracStr);
inData4 += received;
if (received == '\n') {
char chan4[NMEA_SIZE] = "Rx4: ";
strcat(chan4, rtcSecStr);
strcat(chan4, rtcFracStr);
char charBuf4[NMEA_SIZE];
//memset(charBuf4, 0, NMEA_SIZE);
inData4.toCharArray(charBuf4, NMEA_SIZE);
strcat(chan4, charBuf4);
Udp_mutex.lock();
Udp4.beginPacket(bcastip, udp_dstport4);
Udp4.write(chan4, strlen(chan4));
Udp4.endPacket();
Udp_mutex.unlock();
inData4 = "";
}
}
serial_mutex.unlock();
threads.yield();
}
}
//-----------------------------------------
void serial6Thread() {
while (1) {
char rtcFracStrTmp[NMEA_SIZE]="";
serial_mutex.lock();
while (GPSHWSERIAL6.available() > 0) {
char received = GPSHWSERIAL6.read();
if (strlen(rtcFracStrTmp) < strlen(rtcFracStr))
strcat(rtcFracStrTmp, rtcFracStr);
inData6 += received;
if (received == '\n') {
char chan6[NMEA_SIZE] = "Rx6: ";
strcat(chan6, rtcSecStr);
strcat(chan6, rtcFracStr);
char charBuf6[NMEA_SIZE];
//memset(charBuf6, 0, NMEA_SIZE);
inData6.toCharArray(charBuf6, NMEA_SIZE);
strcat(chan6, charBuf6);
Udp_mutex.lock();
Udp6.beginPacket(bcastip, udp_dstport6);
Udp6.write(chan6, strlen(chan6));
Udp6.endPacket();
Udp_mutex.unlock();
inData6 = "";
}
}
serial_mutex.unlock();
threads.yield();
}
}
//-----------------------------------------
void serial7Thread() {
while (1) {
char rtcFracStrTmp[NMEA_SIZE]="";
serial_mutex.lock();
while (HWSERIAL7.available() > 0) {
char received = HWSERIAL7.read();
if (strlen(rtcFracStrTmp) < strlen(rtcFracStr))
strcat(rtcFracStrTmp, rtcFracStr);
inData7 += received;
if (received == '\n') {
char chan7[NMEA_SIZE] = "Rx7: ";
strcat(chan7, rtcSecStr);
strcat(chan7, rtcFracStr);
char charBuf7[NMEA_SIZE];
//memset(charBuf7, 0, NMEA_SIZE);
inData7.toCharArray(charBuf7, NMEA_SIZE);
strcat(chan7, charBuf7);
Udp_mutex.lock();
Udp7.beginPacket(bcastip, udp_dstport7);
Udp7.write(chan7, strlen(chan7));
Udp7.endPacket();
Udp_mutex.unlock();
inData7 = "";
}
}
serial_mutex.unlock();
threads.yield();
}
}
//-----------------------------------------
void serial8Thread() {
while (1) {
char rtcFracStrTmp[NMEA_SIZE]="";
serial_mutex.lock();
while (GPSHWSERIAL8.available() > 0) {
char received = GPSHWSERIAL8.read();
if (strlen(rtcFracStrTmp) < strlen(rtcFracStr))
strcat(rtcFracStrTmp, rtcFracStr);
inData8 += received;
if (received == '\n') {
char chan8[NMEA_SIZE] = "Rx8: ";
strcat(chan8, rtcSecStr);
strcat(chan8, rtcFracStrTmp);
char charBuf8[NMEA_SIZE];
//memset(charBuf8, 0, NMEA_SIZE);
inData8.toCharArray(charBuf8, NMEA_SIZE);
strcat(chan8, charBuf8);
Udp_mutex.lock();
Udp8.beginPacket(bcastip, udp_dstport8);
Udp8.write(chan8, strlen(chan8));
Udp8.endPacket();
Udp_mutex.unlock();
inData8 = "";
}
}
serial_mutex.unlock();
threads.yield();
}
}
//-----------------------------------------
// Clear the EEPROM
void eepromCLR() {
for ( unsigned int i = 0 ; i < EEPROM.length() ; i++ )
EEPROM.write(i, 0);
digitalWrite(13, HIGH);
}
//-----------------------------------------
//--ISR for PPS -------------
void isrPPS() {
PPSpin = HIGH;
//Serial.println("PPS");
}
//-----------------------------------------
// Set GPS device baud rate
void GPSbaudSet(HardwareSerial *serport, int baud) {
serial_mutex.lock();
switch (baud) {
case 9600:
serport->print(GPSbaud_9600);
break;
case 19200:
serport->print(GPSbaud_19200);
break;
case 38400:
serport->print(GPSbaud_38400);
break;
case 56700:
serport->print(GPSbaud_57600);
break;
case 115200:
serport->print(GPSbaud_115200);
break;
default:
break;
}
serport->flush();
serport->end();
serport->begin(baud);
serial_mutex.unlock();
//this eeprom write means nothing at this point
//to make meaningful, need to write settings for all ports individually
//EEPROM.put(eeAddrBaud, GPSbaud_115200);
}
//-----------------------------------------
void GPSnmeaRateSet(HardwareSerial *serport, int rate) {
serial_mutex.lock();
switch (rate) {
case 1:
serport->print(GPSupdatefreq_1Hz);
break;
case 2:
serport->print(GPSupdatefreq_2Hz);
break;
case 4:
serport->print(GPSupdatefreq_4Hz);
break;
case 5:
serport->print(GPSupdatefreq_5Hz);
break;
case 10:
serport->print(GPSupdatefreq_10Hz);
break;
default:
break;
}
serport->flush();
serial_mutex.unlock();
}
//-----------------------------------------
// Hot reset; no init, all data in backup buffer is valid.
void GPShotreset() {
GPSHWSERIAL4.print(GPSreset_hot);
GPSHWSERIAL4.flush();
GPSHWSERIAL6.print(GPSreset_hot);
GPSHWSERIAL6.flush();
GPSHWSERIAL8.print(GPSreset_hot);
GPSHWSERIAL8.flush();
//delay(1000);
}
//-----------------------------------------
// Warm reset; no init, ephemeris is cleared.
void GPSwarmreset() {
GPSHWSERIAL4.print(GPSreset_warm);
GPSHWSERIAL4.flush();
GPSHWSERIAL6.print(GPSreset_warm);
GPSHWSERIAL6.flush();
GPSHWSERIAL8.print(GPSreset_warm);
GPSHWSERIAL8.flush();
//delay(1000);
}
//-----------------------------------------
// Cold reset; no init, all data except configuration in backup
// buffer is cleared.
void GPScoldreset() {
GPSHWSERIAL4.print(GPSreset_cold);
GPSHWSERIAL4.flush();
GPSHWSERIAL6.print(GPSreset_cold);
GPSHWSERIAL6.flush();
GPSHWSERIAL8.print(GPSreset_cold);
GPSHWSERIAL8.flush();
//delay(1000);
}
//-----------------------------------------
// Factory reset: output all NMEA strings 1Hz at 9600 baud
void GPSfactoryreset(HardwareSerial *serport) {
// loop through all baud rates sending the GPSreset_factory
// CAS-string each iteration.
serial_mutex.lock();
int i, br[] = {9600, 19200, 38400, 57600, 115200};
//Serial.println("Cycling through all baud rates...");
for (i = 0; i < 5; i++) {
//Serial.printf(" %d\n", br[i]);
serport->end();
serport->begin(br[i]);
serport->print(GPSreset_factory);
serport->flush();
}
delay(1000); // give it time to reset
//serport->print(GPSbaud_9600);
serport->end();
serport->begin(9600);
while (!serport) { ; }
serial_mutex.unlock();
}
//-----------------------------------------
void GPSnmeaString(HardwareSerial *serport, int nmea) {
switch (nmea) {
case 0:
serport->print(GPSnmeaEnableALL);
break;
case 1:
serport->print(GPSnmeaEnableGGA_only);
break;
case 2:
serport->print(GPSnmeaEnableGGA_VTG_only);
break;
}
serport->flush();
}
//-----------------------------------------
void debug_checksums() {
Serial.printf("%0*X\n", 2,get_checksum(GPSbaud_4800));
Serial.printf("%0*X\n", 2,get_checksum(GPSbaud_9600));
Serial.printf("%0*X\n", 2,get_checksum(GPSbaud_19200));
Serial.printf("%0*X\n", 2, get_checksum(GPSbaud_38400));
Serial.printf("%0*X\n", 2, get_checksum(GPSbaud_57600));
Serial.printf("%0*X\n", 2, get_checksum(GPSbaud_115200));
Serial.printf("%0*X\n", 2, get_checksum(GPSupdatefreq_1Hz));
Serial.printf("%0*X\n", 2, get_checksum(GPSupdatefreq_2Hz));
Serial.printf("%0*X\n", 2, get_checksum(GPSupdatefreq_4Hz));
Serial.printf("%0*X\n", 2, get_checksum(GPSupdatefreq_5Hz));
Serial.printf("%0*X\n", 2, get_checksum(GPSupdatefreq_10Hz));
Serial.printf("%0*X\n", 2, get_checksum(GPSnmeaEnableALL));
Serial.printf("%0*X\n", 2, get_checksum(GPSnmeaEnableGGA_only));
Serial.printf("%0*X\n", 2, get_checksum(GPSnmeaEnableGGA_VTG_only));
Serial.printf("%0*X\n", 2, get_checksum(GPSreset_hot));
Serial.printf("%0*X\n", 2, get_checksum(GPSreset_warm));
Serial.printf("%0*X\n", 2, get_checksum(GPSreset_cold));
Serial.printf("%0*X\n", 2, get_checksum(GPSreset_factory));
}
//-----------------------------------------
// Get NMEA checksum => XOR result of all chars in cmd string between $ and *
// this function returns the decimal checksum value (calling function needs
// to convert to hex)
int get_checksum(char *nmea_sentence) {
size_t i;
int checksum = 0;
// loop through all characters between "$" and "*"
for (i = 1; i < strlen(nmea_sentence) && nmea_sentence[i] != '*'; i++) {
checksum ^= nmea_sentence[i]; // XOR the character
}
return checksum;
}
//-----------------------------------------
void resetAllGPS() {
GPSfactoryreset(&GPSHWSERIAL4);
GPSfactoryreset(&GPSHWSERIAL6);
GPSfactoryreset(&GPSHWSERIAL8);
//eepromCLR();
Serial.println("Factory Reset for all GPS devices");
digitalWrite(13, LOW);
}
void eepromResetAllGPS() {
Serial.println("EEPROM: GPS baud is null; reseting GPS...");
GPSfactoryreset(&GPSHWSERIAL4);
GPSfactoryreset(&GPSHWSERIAL6);
GPSfactoryreset(&GPSHWSERIAL8);
GPSHWSERIAL4.end();
GPSHWSERIAL6.end();
GPSHWSERIAL8.end();
Serial.println("Stopping GPS serial");
Serial.println("Setting baud-rate to 9600");
GPSHWSERIAL4.begin(9600);
while (!GPSHWSERIAL4) { ; }
GPSHWSERIAL6.begin(9600);
while (!GPSHWSERIAL6) { ; }
GPSHWSERIAL8.begin(9600);
while (!GPSHWSERIAL8) { ; }
Serial.println("Setting baud-rate to default and default rate and strings");
GPSbaudSet(&GPSHWSERIAL4, defaultbaud4);
GPSbaudSet(&GPSHWSERIAL6, defaultbaud6);
GPSbaudSet(&GPSHWSERIAL8, defaultbaud8);
GPSnmeaRateSet(&GPSHWSERIAL4, 1);
GPSnmeaRateSet(&GPSHWSERIAL6, 1);
GPSnmeaRateSet(&GPSHWSERIAL8, 1);
}
//-----------------------------------------
void fetchEeprom() {
Serial.println("Fetching EEPROM data...");
EEPROM.get(eeAddrBaudGPS4, eeGPSbaud4);
EEPROM.get(eeAddrGPSupdatefreq4, eeGPSupdatefreq4);
EEPROM.get(eeAddrNmeaGPS4, eeGPSnmea4);
EEPROM.get(eeAddrBaudGPS6, eeGPSbaud6);
EEPROM.get(eeAddrGPSupdatefreq6, eeGPSupdatefreq6);
EEPROM.get(eeAddrNmeaGPS6, eeGPSnmea6);
EEPROM.get(eeAddrBaudGPS8, eeGPSbaud8);
EEPROM.get(eeAddrGPSupdatefreq8, eeGPSupdatefreq8);
EEPROM.get(eeAddrNmeaGPS8, eeGPSnmea8);
Serial.print("GPS4 baud = ");
Serial.println(eeGPSbaud4);
Serial.print("GPS4 rate = ");
Serial.println(eeGPSupdatefreq4);
Serial.print("GPS4 nmea = ");
Serial.println(eeGPSnmea4);
Serial.print("GPS6 baud = ");
Serial.println(eeGPSbaud6);
Serial.print("GPS6 rate = ");
Serial.println(eeGPSupdatefreq6);
Serial.print("GPS6 nmea = ");
Serial.println(eeGPSnmea6);
Serial.print("GPS8 baud = ");
Serial.println(eeGPSbaud8);
Serial.print("GPS8 rate = ");
Serial.println(eeGPSupdatefreq8);
Serial.print("GPS8 nmea = ");
Serial.println(eeGPSnmea8);
//EEPROM.put(eeAddrBaud, GPSbaud_57600);
//EEPROM.put(eeAddrGPSupdatefreq, GPSupdatefreq_10Hz);
//EEPROM.put(eeAddrNmea, GPSnmeaEnableALL);
}
//-----------------------------------------
void initNTPrtc() {
int ntpdelay = 0;
float delay2fracs17 = 0.0;
int fracs17new = 0;
t_ntp_micros = 0;
getNTPTime();
getNTPTime();
getNTPTime();
getNTPTime();
getNTPTime();
getNTPTime();
getNTPTime();
getNTPTime();
if (ntpFail == false) { // if any of the getNTPTime() functions failed, don't set the RTC
ntpdelay = (t_ntp_micros/8)/2;
delay2fracs17 = (ntpdelay / 1000000.0) / SECS_PER_TIC_RTC;
fracs17new = (ntpFracSecs >> 17) + delay2fracs17 + NTP_RTC_CAL;
rtc_set_secs_and_frac(epoch, fracs17new); // Set the rtc
sprintf(T0secs, "%lu", epoch);
sprintf(T0fracs, "%06d", (int)(fracs17new * SECS_PER_TIC_RTC * 1E6));
Serial.print("ntp avg delay (us) = ");
Serial.println(ntpdelay);
Serial.print("NTP_RTC_CAL = ");
Serial.println(NTP_RTC_CAL);
Serial.print("RTC Time Set = ");
Serial.print(T0secs);
Serial.print(" ");
Serial.println(T0fracs);
}
}
//-----------------------------------------
void rtcThread() {
while(1) {
getRTC();
threads.delay_us(10);
threads.yield();
}
}
//-----------------------------------------
//-----------------------------------------
//======================================================
// END Custom Subroutines
//======================================================
void setup() {
t_micros=0;
set_arm_clock (CLOCK_FREQ);
// Start ethernet interface and open UDP sockets
Ethernet.begin(mac);
ntpUdp.begin(ntpudp_srcport);
Udp3.begin(udp_srcport3);
Udp4.begin(udp_srcport4);
Udp6.begin(udp_srcport6);
Udp7.begin(udp_srcport7);
Udp8.begin(udp_srcport8);
// Configure serial ports
Serial.begin(defaultbaudUSB); // USB serial port
while (!Serial) { ; }
HWSERIAL7.begin(defaultbaud7); // PC serial port
while (!HWSERIAL7) { ; }
GPSHWSERIAL3.begin(defaultbaud3); // GPS3 serial port
while (!GPSHWSERIAL3) { ; }
GPSHWSERIAL4.begin(defaultbaud4); // GPS4 serial port
while (!GPSHWSERIAL4) { ; }
GPSHWSERIAL6.begin(defaultbaud6); // GPS6 serial port
while (!GPSHWSERIAL6) { ; }
GPSHWSERIAL8.begin(defaultbaud8); // GPS8 serial port
while (!GPSHWSERIAL8) { ; }
// Output some useful debug info
Serial.printf("CPU clock freq = %u\n",F_CPU_ACTUAL);
Serial.printf("MAC Address: %02X:%02X:%02X:%02X:%02X:%02X \n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
Serial.print("IP Address: ");
Serial.println(Ethernet.localIP());
Serial.printf("Time Server: ");
Serial.println(timeServer);
Serial.println();
// If pin 32 HIGH, perform a full reset
digitalWrite(13, LOW);
pinMode(rstALLpin, INPUT);
if (digitalRead(rstALLpin) == HIGH) {
Serial.println("GPS Reset pin is HIGH..");
//resetAllGPS(); //to be uncommented
}
fetchEeprom();
// for now, do a factory reset each time
resetAllGPS();
Serial.println("Configuring GPS serial ports...");
// Configure GPS serial ports
GPSnmeaRateSet(&GPSHWSERIAL4, defaultrate4);
GPSnmeaString(&GPSHWSERIAL4, defaultnmea4);
GPSbaudSet(&GPSHWSERIAL4, defaultbaud4);
GPSHWSERIAL4.end();
GPSHWSERIAL4.begin(defaultbaud4);
EEPROM.put(eeAddrBaudGPS4, String(defaultbaud4));
EEPROM.put(eeAddrGPSupdatefreq4, String(defaultrate4));
EEPROM.put(eeAddrNmeaGPS4, String(defaultnmea4));
GPSnmeaRateSet(&GPSHWSERIAL6, defaultrate6);
GPSnmeaString(&GPSHWSERIAL6, defaultnmea6);
GPSbaudSet(&GPSHWSERIAL6, defaultbaud6);
GPSHWSERIAL6.end();
GPSHWSERIAL6.begin(defaultbaud6);
EEPROM.put(eeAddrBaudGPS6, String(defaultbaud6));
EEPROM.put(eeAddrGPSupdatefreq6, String(defaultrate6));
EEPROM.put(eeAddrNmeaGPS6, String(defaultnmea6));
GPSnmeaRateSet(&GPSHWSERIAL8, defaultrate8);
GPSnmeaString(&GPSHWSERIAL8, defaultnmea8);
GPSbaudSet(&GPSHWSERIAL8, defaultbaud8);
GPSHWSERIAL8.end();
GPSHWSERIAL8.begin(defaultbaud8);
EEPROM.put(eeAddrBaudGPS8, String(defaultbaud8));
EEPROM.put(eeAddrGPSupdatefreq8, String(defaultrate8));
EEPROM.put(eeAddrNmeaGPS8, String(defaultnmea8));
Serial.println("Initializing RTC with NTP server...");
// use NTP server to initialize RTC
ntpFail = false; //make sure ntpFail = false before calling initNTPrtc()
initNTPrtc();
if (ntpFail == true) {
Serial.println("NTP failed");
}
// Start threads
threads.addThread(serial3Thread);
threads.addThread(serial4Thread);
threads.addThread(serial6Thread);
threads.addThread(serial7Thread);
threads.addThread(serial8Thread);
threads.addThread(rtcThread);
Serial.print("Init time = ");
Serial.print(t_micros / 1000000.0);
Serial.println(" secs");
Serial.println("Running...");
// Register interrupt on PPS pin
attachInterrupt(digitalPinToInterrupt(PPS), isrPPS, RISING);
t_millis = 0;
clockset_cnt = 0;
}
//======================================================
void loop() {
threads.yield();
if (t_millis > 998) {
clockset_cnt ++;
if (clockset_cnt > NTP_SYNC_FREQ) {
ntpFail = false;
initNTPrtc();
clockset_cnt = 0;
}
t_millis = 0;
}
}
