#include "TeensyThreads.h"
#include "Streaming.h"
#include "UBLOX.h"
#include <Wire.h>
// a uBlox object, which is on Teensy hardware
// serial port 3
UBLOX gps(1);
// the uBlox data structure, UBX-NAV-PVT
gpsData uBloxData;
struct GPS {
unsigned long iTOW; ///< [ms], GPS time of the navigation epoch
unsigned char utcDay; ///< [day], Day of month, range 1..31 (UTC)
unsigned char utcHour; ///< [hour], Hour of day, range 0..23 (UTC)
unsigned char utcMin; ///< [min], Minute of hour, range 0..59 (UTC)
unsigned char utcSec; ///< [s], Seconds of minute, range 0..60 (UTC)
long utcNano; ///< [ns], Fraction of second, range -1e9 .. 1e9 (UTC)
unsigned char fixType; ///< [ND], GNSSfix Type: 0: no fix, 1: dead reckoning only,
///< 2: 2D-fix, 3: 3D-fix, 4: GNSS + dead reckoning combined,
///< 5: time only fix
unsigned char numSV; ///< [ND], Number of satellites used in Nav Solution
double lon; ///< [deg], Longitude
double lat; ///< [deg], Latitude
double hMSL; ///< [m], Height above mean sea level
double velN; ///< [m/s], NED north velocity
double velE; ///< [m/s], NED east velocity
double velD; ///< [m/s], NED down velocity
double gSpeed; ///< [m/s], Ground Speed (2-D)
double heading; ///< [deg], Heading of motion (2-D)
double pDOP; ///< [ND], Position DOP
bool upDated;
} rtk;
Threads::Mutex drdy_lock;
int IMU, GPS;
#define cout Serial
void setup() {
// serial to display data
cout.begin(115200);
// starting communication with the GPS
// receiver at 115200 baud
gps.begin(115200);
Serial1.setRX(27);
// put your setup code here, to run once:
GPS = threads.addThread(readGPS);
threads.setTimeSlice(0, 1);
threads.setTimeSlice(GPS, 50);
if (threads.getState(GPS) == Threads::RUNNING) Serial.println("GPS thread started");
rtk.upDated = false;
}
void loop() {
if(rtk.upDated) {
cout << rtk.utcHour << "\t" << rtk.utcMin << "\t" << rtk.utcSec << "\t" << rtk.utcNano << " ---->> ";
cout << "Lat: " << _FLOAT(rtk.lat,10) << " Lon: " << _FLOAT(rtk.lon,10) << endl;
cout << " ------------------------- " << endl;
rtk.upDated = false;
}
}
void readGPS(){
while(1){
{ Threads::Scope scope(drdy_lock);
if(gps.read(&uBloxData)) {
rtk.iTOW = uBloxData.iTOW;
rtk.utcDay = uBloxData.utcDay;
rtk.utcHour = uBloxData.utcHour;
rtk.utcMin = uBloxData.utcMin;
rtk.utcSec = uBloxData.utcSec;
rtk.utcNano = uBloxData.utcNano;
rtk.fixType = uBloxData.fixType;
rtk.numSV = uBloxData.numSV;
rtk.pDOP = uBloxData.pDOP;
rtk.lat = uBloxData.lat;
rtk.lon = uBloxData.lon;
rtk.heading = uBloxData.heading;
rtk.gSpeed = uBloxData.gSpeed;
rtk.hMSL = uBloxData.hMSL;
rtk.velN = uBloxData.velN;
rtk.velE = uBloxData.velE;
rtk.velD = uBloxData.velD;
rtk.upDated = true;
}
}
}
}