mirror of
https://github.com/letscontrolit/ESPEasy.git
synced 2026-09-15 11:05:08 +00:00
617 lines
16 KiB
C++
617 lines
16 KiB
C++
/*
|
|
* Copyright (c) 2016 SODAQ. All rights reserved.
|
|
*
|
|
* This file is part of Sodaq_UBlox_GPS.
|
|
*
|
|
* Sodaq_UBlox_GPS 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 3 of the License, or (at
|
|
* your option) any later version.
|
|
*
|
|
* Sodaq_UBlox_GPS 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 Sodaq_UBlox_GPS. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <Arduino.h>
|
|
#include <Wire.h>
|
|
#include "Sodaq_UBlox_GPS.h"
|
|
|
|
#define DEBUG 1
|
|
#ifdef DEBUG
|
|
#define debugPrintLn(...) { if (this->_diagStream) this->_diagStream->println(__VA_ARGS__); }
|
|
#define debugPrint(...) { if (this->_diagStream) this->_diagStream->print(__VA_ARGS__); }
|
|
#else
|
|
#define debugPrintLn(...)
|
|
#define debugPrint(...)
|
|
#endif
|
|
|
|
static const size_t INPUT_BUFFER_SIZE = 128; // TODO Check UBlox manual ReceiverDescProtSpec
|
|
static const uint8_t UBlox_I2C_addr = 0x42;
|
|
|
|
#define GPS_ENABLE_ON HIGH
|
|
#define GPS_ENABLE_OFF LOW
|
|
|
|
Sodaq_UBlox_GPS sodaq_gps;
|
|
const char Sodaq_UBlox_GPS::_fieldSep = ',';
|
|
|
|
static inline bool is_timedout(uint32_t from, uint32_t nr_ms) __attribute__((always_inline));
|
|
static inline bool is_timedout(uint32_t from, uint32_t nr_ms)
|
|
{
|
|
return (millis() - from) > nr_ms;
|
|
}
|
|
|
|
Sodaq_UBlox_GPS::Sodaq_UBlox_GPS()
|
|
{
|
|
_enablePin = -1;
|
|
|
|
_diagStream = 0;
|
|
_addr = UBlox_I2C_addr;
|
|
|
|
_minNumOfLines = 0;
|
|
_minNumSatellites = 0;
|
|
|
|
resetValues();
|
|
|
|
_trans_active = false;
|
|
_inputBuffer = static_cast<char*>(malloc(INPUT_BUFFER_SIZE));
|
|
_inputBufferSize = INPUT_BUFFER_SIZE;
|
|
}
|
|
|
|
void Sodaq_UBlox_GPS::resetValues()
|
|
{
|
|
_seenLatLon = false;
|
|
_seenAlt = false;
|
|
_numSatellites = 0;
|
|
_lat = 0;
|
|
_lon = 0;
|
|
|
|
_seenTime = false;
|
|
_hh = 0;
|
|
_mm = 0;
|
|
_ss = 0;
|
|
_yy = 0;
|
|
_MM = 0;
|
|
_dd = 0;
|
|
}
|
|
|
|
void Sodaq_UBlox_GPS::init(int8_t enable_pin)
|
|
{
|
|
_enablePin = enable_pin;
|
|
Wire.begin();
|
|
digitalWrite(_enablePin, GPS_ENABLE_OFF);
|
|
pinMode(_enablePin, OUTPUT);
|
|
}
|
|
|
|
/*!
|
|
* Read the UBlox device until a fix is seen, or until
|
|
* a timeout has been reached.
|
|
*/
|
|
bool Sodaq_UBlox_GPS::scan(bool leave_on, uint32_t timeout)
|
|
{
|
|
bool retval = false;
|
|
uint32_t start = millis();
|
|
resetValues();
|
|
|
|
on();
|
|
delay(500); // TODO Is this needed?
|
|
|
|
size_t fix_count = 0;
|
|
while (!is_timedout(start, timeout)) {
|
|
if (!readLine()) {
|
|
// TODO Maybe quit?
|
|
continue;
|
|
}
|
|
parseLine(_inputBuffer);
|
|
|
|
// Which conditions are required to quit the scan?
|
|
if (_seenLatLon
|
|
&& _seenTime
|
|
&& _seenAlt
|
|
&& (_minNumSatellites == 0 || _numSatellites >= _minNumSatellites)) {
|
|
++fix_count;
|
|
if (fix_count >= _minNumOfLines) {
|
|
retval = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (_numSatellites > 0) {
|
|
debugPrintLn(String("[scan] num sats = ") + _numSatellites);
|
|
}
|
|
if (_seenTime) {
|
|
debugPrintLn(String("[scan] datetime = ") + getDateTimeString());
|
|
}
|
|
if (_seenLatLon) {
|
|
debugPrintLn(String("[scan] lat = ") + String(_lat, 7));
|
|
debugPrintLn(String("[scan] lon = ") + String(_lon, 7));
|
|
}
|
|
|
|
if (!leave_on) {
|
|
off();
|
|
}
|
|
return retval;
|
|
}
|
|
|
|
String Sodaq_UBlox_GPS::getDateTimeString()
|
|
{
|
|
return num2String(getYear(), 4)
|
|
+ num2String(getMonth(), 2)
|
|
+ num2String(getDay(), 2)
|
|
+ num2String(getHour(), 2)
|
|
+ num2String(getMinute(), 2)
|
|
+ num2String(getSecond(), 2);
|
|
}
|
|
|
|
bool Sodaq_UBlox_GPS::parseLine(const char * line)
|
|
{
|
|
//debugPrintLn(String("= ") + line);
|
|
if (!computeCrc(line, false)) {
|
|
// Redo the check, with logging
|
|
computeCrc(line, true);
|
|
return false;
|
|
}
|
|
String data = line + 1;
|
|
data.remove(data.length() - 3, 3); // Strip checksum *<hex><hex>
|
|
|
|
if (data.startsWith("GPGGA")) {
|
|
return parseGPGGA(data);
|
|
}
|
|
|
|
if (data.startsWith("GPGSA")) {
|
|
return parseGPGSA(data);
|
|
}
|
|
|
|
if (data.startsWith("GPRMC")) {
|
|
return parseGPRMC(data);
|
|
}
|
|
|
|
if (data.startsWith("GPGSV")) {
|
|
return parseGPGSV(data);
|
|
}
|
|
|
|
if (data.startsWith("GPGLL")) {
|
|
return parseGPGLL(data);
|
|
}
|
|
|
|
if (data.startsWith("GPVTG")) {
|
|
return parseGPVTG(data);
|
|
}
|
|
|
|
if (data.startsWith("GPTXT")) {
|
|
return parseGPTXT(data);
|
|
}
|
|
|
|
debugPrintLn(String("?? >> ") + line);
|
|
return false;
|
|
}
|
|
|
|
/*!
|
|
* Read the coordinates using $GPGGA
|
|
* See also section 24.3 of u-blox 7, Receiver Description. Document number: GPS.G7-SW-12001-B
|
|
*
|
|
* See section "Decode of selected position sentences" at
|
|
* http://www.gpsinformation.org/dale/nmea.htm
|
|
* The most important NMEA sentences include the GGA which provides the
|
|
* current Fix data, the RMC which provides the minimum gps sentences
|
|
* information, and the GSA which provides the Satellite status data.
|
|
*
|
|
* 0 $GPGGA
|
|
* 1 time hhmmss.ss UTC time
|
|
* 2 lat ddmm.mmmmm Latitude (degrees & minutes)
|
|
* 3 NS char North/South indicator
|
|
* 4 long dddmm.mmmmm Longitude (degrees & minutes)
|
|
* 5 EW char East/West indicator
|
|
* 6 quality digit Quality indicator for position fix: 0 No Fix, 6 Estimated, 1 Auto GNSS, 2 Diff GNSS
|
|
* 7 numSV num Number of satellites used
|
|
* 8 HDOP num Horizontal Dilution of Precision
|
|
* 9 alt num Altitude above mean sea level
|
|
* 10 uAlt char Altitude units: meters
|
|
* 11 sep num Geoid separation: difference between geoid and mean sea level
|
|
* 12 uSep char Separation units: meters
|
|
* 13 diffAge num Age of differential corrections
|
|
* 14 diffStation num ID of station providing differential corrections
|
|
* 15 checksum 2 hex digits
|
|
*/
|
|
bool Sodaq_UBlox_GPS::parseGPGGA(const String & line)
|
|
{
|
|
debugPrintLn("parseGPGGA");
|
|
debugPrintLn(String(">> ") + line);
|
|
if (getField(line, 6) != "0") {
|
|
_lat = convertDegMinToDecDeg(getField(line, 2));
|
|
if (getField(line, 3) == "S") {
|
|
_lat = -_lat;
|
|
}
|
|
_lon = convertDegMinToDecDeg(getField(line, 4));
|
|
if (getField(line, 5) == "W") {
|
|
_lon = -_lon;
|
|
}
|
|
_seenLatLon = true;
|
|
|
|
_hdop = getField(line, 8).toFloat();
|
|
if(getField(line, 10) == "M") {
|
|
_alt = getField(line, 9).toFloat();
|
|
_seenAlt = true;
|
|
}
|
|
}
|
|
|
|
_numSatellites = getField(line, 7).toInt();
|
|
return true;
|
|
}
|
|
|
|
/*!
|
|
* Parse GPGSA line
|
|
* GNSS DOP and Active Satellites
|
|
*/
|
|
bool Sodaq_UBlox_GPS::parseGPGSA(const String & line)
|
|
{
|
|
// Not (yet) used
|
|
debugPrintLn("parseGPGSA");
|
|
debugPrintLn(String(">> ") + line);
|
|
return false;
|
|
}
|
|
|
|
/*!
|
|
* Read the coordinates using $GPRMC
|
|
* See also section 24.13 of u-blox 7, Receiver Description. Document number: GPS.G7-SW-12001-B
|
|
*
|
|
* 0 $GPRMC
|
|
* 1 time hhmmss.ss UTC time
|
|
* 2 status char Status, V = Navigation receiver warning, A = Data valid
|
|
* 3 lat ddmm.mmmmm Latitude (degrees & minutes)
|
|
* 4 NS char North/South
|
|
* 5 long dddmm.mmmmm Longitude (degrees & minutes)
|
|
* 6 EW char East/West
|
|
* 7 spd num Speed over ground
|
|
* 8 cog num Course over ground
|
|
* 9 date ddmmyy Date in day, month, year format
|
|
* 10 mv num Magnetic variation value
|
|
* 11 mvEW char Magnetic variation E/W indicator
|
|
* 12 posMode char Mode Indicator: 'N' No Fix, 'E' Estimate, 'A' Auto GNSS, 'D' Diff GNSS
|
|
* 13 checksum 2 hex digits Checksum
|
|
*/
|
|
bool Sodaq_UBlox_GPS::parseGPRMC(const String & line)
|
|
{
|
|
debugPrintLn("parseGPRMC");
|
|
debugPrintLn(String(">> ") + line);
|
|
|
|
if (getField(line, 2) == "A" && getField(line, 12) != "N") {
|
|
_lat = convertDegMinToDecDeg(getField(line, 3));
|
|
if (getField(line, 4) == "S") {
|
|
_lat = -_lat;
|
|
}
|
|
_lon = convertDegMinToDecDeg(getField(line, 5));
|
|
if (getField(line, 6) == "W") {
|
|
_lon = -_lon;
|
|
}
|
|
_seenLatLon = true;
|
|
}
|
|
|
|
String time = getField(line, 1);
|
|
String date = getField(line, 9);
|
|
setDateTime(date, time);
|
|
|
|
return true;
|
|
}
|
|
|
|
/*!
|
|
* Parse GPGSV line
|
|
* See also section 24.12 of u-blox 7, Receiver Description. Document number: GPS.G7-SW-12001-B
|
|
*
|
|
* 0 $GPGSV
|
|
* 1 numMsg digit Number of messages, total number of GSV messages being output
|
|
* 2 msgNum digit Number of this message
|
|
* 3 numSV num Number of satellites in view
|
|
*
|
|
* 4 sv num Satellite ID
|
|
* 5 elv num Elevation (range 0-90)
|
|
* 6 az num Azimuth, (range 0-359)
|
|
* 7 cno num Signal strength (C/N0, range 0-99), blank when not tracking
|
|
*
|
|
* fields 4..7 are repeated for each satellite in this message
|
|
*/
|
|
bool Sodaq_UBlox_GPS::parseGPGSV(const String & line)
|
|
{
|
|
debugPrintLn("parseGPGSV");
|
|
debugPrintLn(String(">> ") + line);
|
|
|
|
// We could/should only use msgNum == 1. However, all messages should have
|
|
// the same numSV.
|
|
_numSatellites = getField(line, 3).toInt();
|
|
|
|
return true;
|
|
}
|
|
|
|
/*!
|
|
* Parse GPGLL line
|
|
* Latitude and longitude, with time of position fix and status
|
|
*/
|
|
bool Sodaq_UBlox_GPS::parseGPGLL(const String & line)
|
|
{
|
|
// Not (yet) used
|
|
debugPrintLn("parseGPGLL");
|
|
debugPrintLn(String(">> ") + line);
|
|
return false;
|
|
}
|
|
|
|
/*!
|
|
* Parse GPVTG line
|
|
* Course over ground and Ground speed
|
|
*/
|
|
bool Sodaq_UBlox_GPS::parseGPVTG(const String & line)
|
|
{
|
|
// Not (yet) used
|
|
debugPrintLn("parseGPVTG");
|
|
debugPrintLn(String(">> ") + line);
|
|
return false;
|
|
}
|
|
|
|
/*!
|
|
* Parse GPTXT line
|
|
* See also section 24.13 of u-blox 7, Receiver Description. Document number: GPS.G7-SW-12001-B
|
|
* $GPTXT,01,01,02,ANTSTATUS=INIT*25
|
|
* ...
|
|
* $GPTXT,01,01,02,ANTSTATUS=OK*3B
|
|
*
|
|
* 0 $GPTXT
|
|
* 1 numMsg num Total number of messages in this transmission
|
|
* 2 msgNum num Message number in this transmission
|
|
* 3 msgType num Text identifier, 00: Error, 01: Warning, 02: Notice, 07: User
|
|
* 4 text string Any ASCII text
|
|
* 13 checksum 2 hex digits Checksum
|
|
*/
|
|
bool Sodaq_UBlox_GPS::parseGPTXT(const String & line)
|
|
{
|
|
//debugPrintLn("parseGPTXT");
|
|
//debugPrintLn(String(">> ") + line);
|
|
debugPrintLn(String("TXT: \"") + getField(line, 4) + "\"");
|
|
return true;
|
|
}
|
|
|
|
/*!
|
|
* Compute and verify the checksum
|
|
*
|
|
* Each line must start with '$'
|
|
* Each line must end with '*' <hex> <hex>
|
|
*/
|
|
bool Sodaq_UBlox_GPS::computeCrc(const char * line, bool do_logging)
|
|
{
|
|
if (do_logging) {
|
|
debugPrint(line);
|
|
}
|
|
size_t len = strlen(line);
|
|
if (len < 4) {
|
|
if (do_logging) {
|
|
debugPrint(" Invalid short: ");
|
|
debugPrintLn(len);
|
|
}
|
|
return false;
|
|
}
|
|
if (line[0] != '$') {
|
|
if (do_logging) {
|
|
debugPrintLn(" Invalid$");
|
|
}
|
|
return false;
|
|
}
|
|
if (line[len - 3] != '*') {
|
|
if (do_logging) {
|
|
debugPrintLn(" Invalid*");
|
|
}
|
|
return false;
|
|
}
|
|
|
|
uint8_t crc = 0;
|
|
for (size_t i = 1; i < len - 3; ++i) {
|
|
crc ^= line[i];
|
|
}
|
|
|
|
uint8_t crc1 = getHex2(line, len - 2);
|
|
if (crc != crc1) {
|
|
if (do_logging) {
|
|
debugPrint(" INVALID CRC ");
|
|
debugPrint(crc1, HEX);
|
|
debugPrint(" EXPECTED ");
|
|
debugPrintLn(crc, HEX);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
//debugSerial.println(" OK");
|
|
return true;
|
|
}
|
|
|
|
uint8_t Sodaq_UBlox_GPS::getHex2(const char * s, size_t index)
|
|
{
|
|
uint8_t val = 0;
|
|
char c;
|
|
c = s[index];
|
|
if (c >= '0' && c <= '9') {
|
|
val += c - '0';
|
|
} else if (c >= 'a' && c <= 'f') {
|
|
val += c - 'a' + 10;
|
|
} else if (c >= 'A' && c <= 'F') {
|
|
val += c - 'A' + 10;
|
|
}
|
|
val <<= 4;
|
|
c = s[++index];
|
|
if (c >= '0' && c <= '9') {
|
|
val += c - '0';
|
|
} else if (c >= 'a' && c <= 'f') {
|
|
val += c - 'a' + 10;
|
|
} else if (c >= 'A' && c <= 'F') {
|
|
val += c - 'A' + 10;
|
|
}
|
|
return val;
|
|
}
|
|
|
|
String Sodaq_UBlox_GPS::num2String(int num, size_t width)
|
|
{
|
|
String out;
|
|
out = num;
|
|
while (out.length() < width) {
|
|
out = String("0") + out;
|
|
}
|
|
return out;
|
|
}
|
|
|
|
String Sodaq_UBlox_GPS::getField(const String & data, int index)
|
|
{
|
|
int found = 0;
|
|
int strIndex[] = { 0, -1 };
|
|
int maxIndex = data.length() - 1;
|
|
|
|
for (int i = 0; i <= maxIndex && found <= index; i++) {
|
|
if (data.charAt(i) == _fieldSep || i == maxIndex) {
|
|
found++;
|
|
strIndex[0] = strIndex[1] + 1;
|
|
strIndex[1] = (i == maxIndex) ? i + 1 : i;
|
|
}
|
|
}
|
|
|
|
return found > index ? data.substring(strIndex[0], strIndex[1]) : "";
|
|
}
|
|
|
|
/*
|
|
* Convert lat/long degree-minute format to decimal-degrees
|
|
*
|
|
* This code is from
|
|
* http://arduinodev.woofex.net/2013/02/06/adafruit_gps_forma/
|
|
*
|
|
* According to the NMEA Standard, Latitude and Longitude are output in the format Degrees, Minutes and
|
|
* (Decimal) Fractions of Minutes. To convert to Degrees and Fractions of Degrees, or Degrees, Minutes, Seconds
|
|
* and Fractions of seconds, the 'Minutes' and 'Fractional Minutes' parts need to be converted. In other words: If
|
|
* the GPS Receiver reports a Latitude of 4717.112671 North and Longitude of 00833.914843 East, this is
|
|
* Latitude 47 Degrees, 17.112671 Minutes
|
|
* Longitude 8 Degrees, 33.914843 Minutes
|
|
* or
|
|
* Latitude 47 Degrees, 17 Minutes, 6.76026 Seconds
|
|
* Longitude 8 Degrees, 33 Minutes, 54.89058 Seconds
|
|
* or
|
|
* Latitude 47.28521118 Degrees
|
|
* Longitude 8.56524738 Degrees
|
|
*/
|
|
double Sodaq_UBlox_GPS::convertDegMinToDecDeg(const String & data)
|
|
{
|
|
double degMin = data.toFloat();
|
|
double min = 0.0;
|
|
double decDeg = 0.0;
|
|
|
|
//get the minutes, fmod() requires double
|
|
min = fmod((double) degMin, 100.0);
|
|
|
|
//rebuild coordinates in decimal degrees
|
|
degMin = (int) (degMin / 100);
|
|
decDeg = degMin + (min / 60);
|
|
|
|
return decDeg;
|
|
}
|
|
|
|
void Sodaq_UBlox_GPS::setDateTime(const String & date, const String & time)
|
|
{
|
|
if (time.length() == 9 && date.length() == 6) {
|
|
_hh = time.substring(0, 2).toInt();
|
|
_mm = time.substring(2, 4).toInt();
|
|
_ss = time.substring(4, 6).toInt();
|
|
_dd = date.substring(0, 2).toInt();
|
|
_MM = date.substring(2, 4).toInt();
|
|
_yy = date.substring(4, 6).toInt();
|
|
_seenTime = true;
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* Read one NMEA frame
|
|
*/
|
|
bool Sodaq_UBlox_GPS::readLine(uint32_t timeout)
|
|
{
|
|
if (!_inputBuffer) {
|
|
return false;
|
|
}
|
|
|
|
uint32_t start = millis();
|
|
char c;
|
|
char *ptr = _inputBuffer;
|
|
size_t cnt = 0;
|
|
*ptr = '\0';
|
|
|
|
c = 0;
|
|
while (!is_timedout(start, timeout)) {
|
|
c = (char)read();
|
|
if (c == '$') {
|
|
break;
|
|
}
|
|
}
|
|
if (c != '$') {
|
|
return false;
|
|
}
|
|
*ptr++ = c;
|
|
++cnt;
|
|
|
|
c = 0;
|
|
while (!is_timedout(start, timeout)) {
|
|
c = (char)read();
|
|
if (c == '\r') {
|
|
continue;
|
|
}
|
|
if (c == '\n') {
|
|
break;
|
|
}
|
|
if (cnt < _inputBufferSize - 1) {
|
|
*ptr++ = c;
|
|
++cnt;
|
|
}
|
|
}
|
|
*ptr = '\0';
|
|
if (c != '\n') {
|
|
return false;
|
|
}
|
|
endTransmission();
|
|
return true;
|
|
}
|
|
|
|
uint8_t Sodaq_UBlox_GPS::read()
|
|
{
|
|
beginTransmission();
|
|
|
|
uint8_t b = 0xFF;
|
|
uint8_t nr_bytes;
|
|
nr_bytes = Wire.requestFrom(_addr, 1, false);
|
|
if (nr_bytes == 1) {
|
|
b = Wire.read();
|
|
}
|
|
return b;
|
|
}
|
|
|
|
void Sodaq_UBlox_GPS::beginTransmission()
|
|
{
|
|
if (_trans_active) {
|
|
return;
|
|
}
|
|
Wire.beginTransmission(_addr);
|
|
_trans_active = true;
|
|
}
|
|
|
|
void Sodaq_UBlox_GPS::endTransmission()
|
|
{
|
|
if (!_trans_active) {
|
|
return;
|
|
}
|
|
Wire.endTransmission();
|
|
_trans_active = false;
|
|
}
|
|
|
|
void Sodaq_UBlox_GPS::on()
|
|
{
|
|
digitalWrite(_enablePin, GPS_ENABLE_ON);
|
|
}
|
|
|
|
void Sodaq_UBlox_GPS::off()
|
|
{
|
|
digitalWrite(_enablePin, GPS_ENABLE_OFF);
|
|
}
|