74 lines
1.8 KiB
C++
74 lines
1.8 KiB
C++
#include <gps.h>
|
|
|
|
byte CFG_RST[12] = {0xb5, 0x62, 0x06, 0x04, 0x04, 0x00, 0x00, 0x00, 0x01,0x00, 0x0F, 0x66};
|
|
|
|
Gps::Gps() {
|
|
Serial.begin(9600);
|
|
Serial.setTimeout(2);
|
|
// Serial.write(CFG_RST, sizeof(CFG_RST)); // Soft Reset GPS on Start
|
|
satsInView.begin(tGps, "GPGSV", 3); // NMEA Sentence GPGSV, Element 3 (Sat in View)
|
|
}
|
|
|
|
void Gps::encode()
|
|
{
|
|
int data;
|
|
unsigned long previousMillis = millis();
|
|
|
|
while((previousMillis + 100) > millis())
|
|
{
|
|
while (Serial.available() )
|
|
tGps.encode((char)Serial.read());
|
|
}
|
|
//Serial.println("");
|
|
}
|
|
|
|
bool Gps::buildPacket(uint8_t txBuffer[9])
|
|
{
|
|
LatitudeBinary = ((tGps.location.lat() + 90) / 180.0) * 16777215;
|
|
LongitudeBinary = ((tGps.location.lng() + 180) / 360.0) * 16777215;
|
|
|
|
txBuffer[0] = ( LatitudeBinary >> 16 ) & 0xFF;
|
|
txBuffer[1] = ( LatitudeBinary >> 8 ) & 0xFF;
|
|
txBuffer[2] = LatitudeBinary & 0xFF;
|
|
|
|
txBuffer[3] = ( LongitudeBinary >> 16 ) & 0xFF;
|
|
txBuffer[4] = ( LongitudeBinary >> 8 ) & 0xFF;
|
|
txBuffer[5] = LongitudeBinary & 0xFF;
|
|
|
|
altitudeGps = tGps.altitude.meters();
|
|
txBuffer[6] = ( altitudeGps >> 8 ) & 0xFF;
|
|
txBuffer[7] = altitudeGps & 0xFF;
|
|
|
|
hdopGps = tGps.hdop.value()/10;
|
|
txBuffer[8] = hdopGps & 0xFF;
|
|
|
|
return true;
|
|
}
|
|
|
|
void Gps::gdisplay(uint16_t dispBuffer[])
|
|
{
|
|
dispBuffer[0] = tGps.hdop.value()/10;
|
|
dispBuffer[1] = atoi(satsInView.value());
|
|
dispBuffer[2] = tGps.satellites.value();
|
|
dispBuffer[3] = tGps.altitude.meters();
|
|
dispBuffer[4] = tGps.speed.kmph();
|
|
}
|
|
|
|
bool Gps::checkGpsFix()
|
|
{
|
|
encode();
|
|
if (tGps.location.isValid() &&
|
|
tGps.location.age() < 4000 &&
|
|
tGps.hdop.isValid() &&
|
|
tGps.hdop.value() <= 600 &&
|
|
tGps.hdop.age() < 4000 &&
|
|
tGps.altitude.isValid() &&
|
|
tGps.altitude.age() < 4000 )
|
|
{
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
} |