diff options
Diffstat (limited to 'nmeasource.cpp')
| -rw-r--r-- | nmeasource.cpp | 619 |
1 files changed, 619 insertions, 0 deletions
diff --git a/nmeasource.cpp b/nmeasource.cpp new file mode 100644 index 0000000..e9bb222 --- /dev/null +++ b/nmeasource.cpp @@ -0,0 +1,619 @@ +#include <cmath> +#include <QtCore/QDebug> +#include <QtCore/QDir> +#include <QtCore/QFile> +#include <QtPositioning/QNmeaPositionInfoSource> + +#include "nmeasource.h" + +NmeaSource::NmeaSource(QObject *parent) : + QObject(parent) +{ + m_possrc = QGeoPositionInfoSource::createDefaultSource(this); + m_satsrc = QGeoSatelliteInfoSource::createDefaultSource(this); + if (m_possrc) { + connect(m_possrc, SIGNAL(positionUpdated(QGeoPositionInfo)), + this, SLOT(handlePosition(QGeoPositionInfo))); + } else { +#ifdef QT_DEBUG + qDebug() << "Using test GPS source"; + QNmeaPositionInfoSource* nmea = new QNmeaPositionInfoSource(QNmeaPositionInfoSource::SimulationMode, this); + QFile* file = new QFile(QDir::home().absoluteFilePath("nmea.txt"), this); + if (file->open(QIODevice::ReadOnly | QIODevice::Text)) { + nmea->setDevice(file); + m_possrc = nmea; + connect(m_possrc, SIGNAL(positionUpdated(QGeoPositionInfo)), + this, SLOT(handlePosition(QGeoPositionInfo))); + } else { + qWarning() << "Could not open test GPS source"; + delete file; + delete nmea; + } +#else + qWarning() << "No valid GPS position source!"; +#endif + } + if (m_satsrc) { + connect(m_satsrc, SIGNAL(satellitesInUseUpdated(QList<QGeoSatelliteInfo>)), + this, SLOT(handleSatsInUse(QList<QGeoSatelliteInfo>))); + connect(m_satsrc, SIGNAL(satellitesInViewUpdated(QList<QGeoSatelliteInfo>)), + this, SLOT(handleSatsInView(QList<QGeoSatelliteInfo>))); + } +} + +NmeaSource::~NmeaSource() +{ +} + +void NmeaSource::start() +{ + m_last_satsinview = 0; + if (m_possrc) { + m_possrc->startUpdates(); + } + if (m_satsrc) { + m_satsrc->startUpdates(); + } +} + +void NmeaSource::stop() +{ + if (m_possrc) { + m_possrc->stopUpdates(); + } + if (m_satsrc) { + m_satsrc->stopUpdates(); + } +} + +QString NmeaSource::formatFixMode(const QGeoCoordinate &coord) +{ + switch (coord.type()) { + case QGeoCoordinate::Coordinate3D: + return QString::fromLatin1("3"); + case QGeoCoordinate::Coordinate2D: + return QString::fromLatin1("2"); + default: + return QString::fromLatin1("1"); + } +} + +QString NmeaSource::formatTimestamp(const QTime& time) +{ + return time.toString("HHmmss.zzz"); +} + +QString NmeaSource::formatDatestamp(const QDate& date) +{ + return date.toString("ddMMyy"); +} + +QString NmeaSource::formatLatitude(const QGeoCoordinate& coord) +{ + if (coord.isValid()) { + QLatin1Char fill('0'); + double degrees; + double minutes = modf(fabs(coord.latitude()), °rees) * 60; + return QString::fromLatin1("%1%2") + .arg(static_cast<int>(degrees), 2, 10, fill) + .arg(minutes, 7, 'f', 4, fill); + } else { + return QString(); + } +} + +QString NmeaSource::formatLatitudeNS(const QGeoCoordinate& coord) +{ + if (coord.isValid()) { + if (coord.latitude() >= 0.0) { + return QLatin1String("N"); + } else { + return QLatin1String("S"); + } + } else { + return QString(); + } +} + +QString NmeaSource::formatLongitude(const QGeoCoordinate& coord) +{ + if (coord.isValid()) { + QLatin1Char fill('0'); + double degrees; + double minutes = modf(fabs(coord.longitude()), °rees) * 60; + return QString::fromLatin1("%1%2") + .arg(static_cast<int>(degrees), 3, 10, fill) + .arg(minutes, 7, 'f', 4, fill); + } else { + return QString(); + } +} + +QString NmeaSource::formatLongitudeEW(const QGeoCoordinate& coord) +{ + if (coord.isValid()) { + if (coord.longitude() >= 0.0) { + return QString::fromLatin1("E"); + } else { + return QString::fromLatin1("W"); + } + } else { + return QString(); + } +} + +QString NmeaSource::formatAltitude(const QGeoCoordinate& coord) +{ + if (coord.type() == QGeoCoordinate::Coordinate3D) { + return QString::number(coord.altitude(), 'f', 1); + } else { + return QString(); + } +} + +QString NmeaSource::formatAltitudeUnits(const QGeoCoordinate& coord) +{ + if (coord.type() == QGeoCoordinate::Coordinate3D) { + return QString::fromLatin1("M"); + } else { + return QString(); + } +} + +QString NmeaSource::formatSpeedKnots(const QGeoPositionInfo& pos) +{ + if (pos.hasAttribute(QGeoPositionInfo::GroundSpeed)) { + double speed = pos.attribute(QGeoPositionInfo::GroundSpeed); // [m/s] + speed *= 1.943844; // [knots] + return QString::number(speed, 'f', 1); + } else { + return QString(); + } +} + +QString NmeaSource::formatSpeedKmH(const QGeoPositionInfo& pos) +{ + if (pos.hasAttribute(QGeoPositionInfo::GroundSpeed)) { + double speed = pos.attribute(QGeoPositionInfo::GroundSpeed); // [m/s] + speed *= 3.6; // [km/h] + return QString::number(speed, 'f', 1); + } else { + return QString(); + } +} + +QString NmeaSource::formatTrueCourse(const QGeoPositionInfo& pos) +{ + if (pos.hasAttribute(QGeoPositionInfo::Direction)) { + double course = pos.attribute(QGeoPositionInfo::Direction); // [deg] + return QString::number(course, 'f', 1); + } else { + return QString(); + } +} + +QString NmeaSource::formatMagCourse(const QGeoPositionInfo& pos) +{ + if (pos.hasAttribute(QGeoPositionInfo::Direction) + && pos.hasAttribute(QGeoPositionInfo::MagneticVariation)) { + double course = pos.attribute(QGeoPositionInfo::Direction); // [deg] + double magvar = pos.attribute(QGeoPositionInfo::MagneticVariation); // [+/-deg] + return QString::number(course + magvar, 'f', 1); + } else { + return QString(); + } +} + +QString NmeaSource::formatMagVariation(const QGeoPositionInfo& pos) +{ + if (pos.hasAttribute(QGeoPositionInfo::MagneticVariation)) { + double var = pos.attribute(QGeoPositionInfo::MagneticVariation); // [+/-deg] + return QString::number(fabs(var), 'f', 1); + } else { + return QString(); + } +} + +QString NmeaSource::formatMagVariationEW(const QGeoPositionInfo& pos) +{ + if (pos.hasAttribute(QGeoPositionInfo::MagneticVariation)) { + if (pos.attribute(QGeoPositionInfo::MagneticVariation) >= 0.0) { + return QString::fromLatin1("E"); + } else { + return QString::fromLatin1("W"); + } + } else { + return QString(); + } +} + +QString NmeaSource::formatHDOP(const QGeoPositionInfo& pos) +{ + if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) { + double eph = pos.attribute(QGeoPositionInfo::HorizontalAccuracy); // [m] + return QString::number(eph / H_UERE, 'f', 1); + } else { + return QString(); + } +} + +QString NmeaSource::formatVDOP(const QGeoPositionInfo& pos) +{ + if (pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) { + double epv = pos.attribute(QGeoPositionInfo::VerticalAccuracy); // [m] + return QString::number(epv / V_UERE, 'f', 1); + } else { + return QString(); + } +} + +QString NmeaSource::formatPDOP(const QGeoPositionInfo& pos) +{ + + if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy) + && pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) { + double eph = pos.attribute(QGeoPositionInfo::HorizontalAccuracy); // [m] + double epv = pos.attribute(QGeoPositionInfo::VerticalAccuracy); // [m] + double epe = sqrt(eph * eph + epv * epv); + return QString::number(epe / P_UERE, 'f', 1); + } else { + return QString(); + } +} + +QString NmeaSource::formatError(const QGeoPositionInfo& pos, + PositionAccuracyDirection which) +{ + switch (which) { + case Spherical: + if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy) + && pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) { + double eph = pos.attribute(QGeoPositionInfo::HorizontalAccuracy); // [m] + double epv = pos.attribute(QGeoPositionInfo::VerticalAccuracy); // [m] + double epe = sqrt(eph * eph + epv * epv); + return QString::number(epe, 'f', 1); + } else { + return QString(); + } + break; + case Horizontal: + if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) { + double eph = pos.attribute(QGeoPositionInfo::HorizontalAccuracy); // [m] + return QString::number(eph, 'f', 1); + } else { + return QString(); + } + break; + case Vertical: + if (pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) { + double epv = pos.attribute(QGeoPositionInfo::VerticalAccuracy); // [m] + return QString::number(epv, 'f', 1); + } else { + return QString(); + } + break; + } + return QString(); +} + +QString NmeaSource::formatErrorUnits(const QGeoPositionInfo& pos, + PositionAccuracyDirection which) +{ + switch (which) { + case Spherical: + if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy) + && pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) { + return QString::fromLatin1("M"); + } else { + return QString(); + } + break; + case Horizontal: + if (pos.hasAttribute(QGeoPositionInfo::HorizontalAccuracy)) { + return QString::fromLatin1("M"); + } else { + return QString(); + } + break; + case Vertical: + if (pos.hasAttribute(QGeoPositionInfo::VerticalAccuracy)) { + return QString::fromLatin1("M"); + } else { + return QString(); + } + break; + } + return QString(); +} + +QString NmeaSource::formatSatPrn(const QGeoSatelliteInfo &sat) +{ + return QString::fromLatin1("%1").arg(sat.satelliteIdentifier(), 2, 10, QLatin1Char('0')); +} + +QString NmeaSource::formatSatElev(const QGeoSatelliteInfo &sat) +{ + if (sat.hasAttribute(QGeoSatelliteInfo::Elevation)) { + int elv = lrint(sat.attribute(QGeoSatelliteInfo::Elevation)); + return QString::fromLatin1("%1").arg(elv, 2, 10, QLatin1Char('0')); + } else { + return QString(); + } +} + +QString NmeaSource::formatSatAz(const QGeoSatelliteInfo &sat) +{ + if (sat.hasAttribute(QGeoSatelliteInfo::Azimuth)) { + int az = lrint(sat.attribute(QGeoSatelliteInfo::Azimuth)); + return QString::fromLatin1("%1").arg(az, 3, 10, QLatin1Char('0')); + } else { + return QString(); + } +} +QString NmeaSource::formatSatSnr(const QGeoSatelliteInfo &sat) +{ + int snr = sat.signalStrength(); + if (snr >= 0) { + return QString::fromLatin1("%1").arg(snr, 2, 10, QLatin1Char('0')); + } else { + return QString(); + } + return QString::fromLatin1("%1").arg(sat.signalStrength(), 2, 10, QLatin1Char('0')); +} + +void NmeaSource::emitSentence(const QString &sentence) +{ + QByteArray data = sentence.toLatin1(); + uint checksum = 0; + foreach (const char& c, data) { + checksum ^= c; + } + +#if 0 + qDebug("%s", qPrintable(QString::fromLatin1("$%1*%2").arg(sentence). + arg(checksum, 2, 16, QLatin1Char('0')))); +#endif + + emit dataReady(QString::fromLatin1("$%1*%2\r\n") + .arg(sentence) + .arg(checksum, 2, 16, QLatin1Char('0'))); +} + +void NmeaSource::generateGPRMC(const QGeoPositionInfo &pos) +{ + QString sentence = + QString::fromLatin1("GPRMC,%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12") + // 1 = UTC of position fix + .arg(formatTimestamp(pos.timestamp().time())) + // 2 = Data status (A = Autonomous V = Warning) + .arg(pos.isValid() ? QLatin1Char('A') : QLatin1Char('V')) + // 3 = Latitude of fix + .arg(formatLatitude(pos.coordinate())) + // 4 = N or S + .arg(formatLatitudeNS(pos.coordinate())) + // 5 = Longitude of fix + .arg(formatLongitude(pos.coordinate())) + // 6 = E or W + .arg(formatLongitudeEW(pos.coordinate())) + // 7 = Speed over ground in knots + .arg(formatSpeedKnots(pos)) + // 8 = True track made good in degrees + .arg(formatTrueCourse(pos)) + // 9 = Date Stamp + .arg(formatDatestamp(pos.timestamp().date())) + // 10 = Magnetic variation in degrees + .arg(formatMagVariation(pos)) + // 11 = E or W + .arg(formatMagVariationEW(pos)) + // 12 = FAA mode indicator (A = Autonomous, N = Not valid) + .arg(pos.isValid() ? QLatin1Char('A') : QLatin1Char('N')) + ; + emitSentence(sentence); +} + +void NmeaSource::generateGPGGA(const QGeoPositionInfo &pos) +{ + QString sentence = + QString::fromLatin1("GPGGA,%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12,%13,%14") + // 1 = UTC of position fix + .arg(formatTimestamp(pos.timestamp().time())) + // 2 = Latitude of fix + .arg(formatLatitude(pos.coordinate())) + // 3 = N or S + .arg(formatLatitudeNS(pos.coordinate())) + // 4 = Longitude of fix + .arg(formatLongitude(pos.coordinate())) + // 5 = E or W + .arg(formatLongitudeEW(pos.coordinate())) + // 6 = Fix quality (0 = invalid, 1 = GPS) + .arg(pos.isValid() ? QLatin1Char('1') : QLatin1Char('0')) + // 7 = Number of satellites being tracked + .arg(m_last_satsinview, 2, 10, QLatin1Char('0')) + // 8 = Horizontal dillusion of position + .arg(formatHDOP(pos)) + // 9 = Altitude + .arg(formatAltitude(pos.coordinate())) + // 10 = Altitude units + .arg(formatAltitudeUnits(pos.coordinate())) + // 11 = Height of geoid (mean sea level) about WGS 84 + .arg(QString()) // TODO We could calculate it. Usefulness debatable. + // 12 = Units + .arg(QString()) + // 13 = Time in seconds since last DGPS update + .arg(QString()) + // 14 = DGPS station ID number + .arg(QString()) + ; + emitSentence(sentence); +} + +void NmeaSource::generateGPGLL(const QGeoPositionInfo &pos) +{ + QString sentence = + QString::fromLatin1("GPGLL,%1,%2,%3,%4,%5,%6,%7") + // 1 = Latitude of fix + .arg(formatLatitude(pos.coordinate())) + // 2 = N or S + .arg(formatLatitudeNS(pos.coordinate())) + // 3 = Longitude of fix + .arg(formatLongitude(pos.coordinate())) + // 4 = E or W + .arg(formatLongitudeEW(pos.coordinate())) + // 5 = UTC of fix + .arg(formatTimestamp(pos.timestamp().time())) + // 6 = Data valid (A = Active, V = Void) + .arg(pos.isValid() ? QLatin1Char('A') : QLatin1Char('V')) + // 7 = Mode indicator (A = Autonomous, N = Not valid) + .arg(pos.isValid() ? QLatin1Char('A') : QLatin1Char('N')) + ; + emitSentence(sentence); +} + +void NmeaSource::generateGPVTG(const QGeoPositionInfo &pos) +{ + QString sentence = + QString::fromLatin1("GPVTG,%1,%2,%3,%4,%5,%6,%7,%8") + // 1 = True track made good in degrees + .arg(formatTrueCourse(pos)) + // 2 = T + .arg(pos.hasAttribute(QGeoPositionInfo::Direction) + ? QString::fromLatin1("T") : QString()) + // 3 = Magnetic track made good + .arg(formatMagCourse(pos)) + // 4 = M + .arg((pos.hasAttribute(QGeoPositionInfo::Direction) + && pos.hasAttribute(QGeoPositionInfo::MagneticVariation)) + ? QString::fromLatin1("M") : QString()) + // 5 = Speed over ground in knots + .arg(formatSpeedKnots(pos)) + // 6 = N + .arg(pos.hasAttribute(QGeoPositionInfo::GroundSpeed) + ? QString::fromLatin1("N") : QString()) + // 7 = Mode indicator (A = Autonomous, N = Not valid) + .arg(formatSpeedKmH(pos)) + // 8 = K + .arg(pos.hasAttribute(QGeoPositionInfo::GroundSpeed) + ? QString::fromLatin1("K") : QString()) + ; + emitSentence(sentence); +} + +void NmeaSource::generatePGRME(const QGeoPositionInfo &pos) +{ + QString sentence = + QString::fromLatin1("PGRME,%1,%2,%3,%4,%5,%6") + // 1 = Horizontal error estimate + .arg(formatError(pos, Horizontal)) + // 2 = Units + .arg(formatErrorUnits(pos, Horizontal)) + // 3 = Vertical error estimate + .arg(formatError(pos, Vertical)) + // 4 = Units + .arg(formatErrorUnits(pos, Vertical)) + // 5 = Spherical error estimate + .arg(formatError(pos, Spherical)) + // 6 = Units + .arg(formatErrorUnits(pos, Spherical)) + ; + emitSentence(sentence); +} + +void NmeaSource::generateGPGSA(const QGeoPositionInfo& pos, const QList<QGeoSatelliteInfo>& info) +{ + const int n = info.size(); + QString sentence = + QString::fromLatin1("GPGSA,%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12,%13,%14,%15,%16,%17") + // 1 = Mode (A = Automatic) + .arg(QLatin1Char('A')) + // 2 = Fix mode + .arg(formatFixMode(pos.coordinate())) + // 3 - 14 = IDs of SVs used in position fix + .arg(n > 0 ? formatSatPrn(info[0]) : QString()) + .arg(n > 1 ? formatSatPrn(info[1]) : QString()) + .arg(n > 2 ? formatSatPrn(info[2]) : QString()) + .arg(n > 3 ? formatSatPrn(info[3]) : QString()) + .arg(n > 4 ? formatSatPrn(info[4]) : QString()) + .arg(n > 5 ? formatSatPrn(info[5]) : QString()) + .arg(n > 6 ? formatSatPrn(info[6]) : QString()) + .arg(n > 7 ? formatSatPrn(info[7]) : QString()) + .arg(n > 8 ? formatSatPrn(info[8]) : QString()) + .arg(n > 9 ? formatSatPrn(info[9]) : QString()) + .arg(n > 10 ? formatSatPrn(info[10]) : QString()) + .arg(n > 11 ? formatSatPrn(info[11]) : QString()) + // 15 = PDOP + .arg(formatPDOP(pos)) + // 16 = HDOP + .arg(formatHDOP(pos)) + // 17 = VDOP + .arg(formatVDOP(pos)) + ; + emitSentence(sentence); +} + +void NmeaSource::generateGPGSV(const QGeoPositionInfo& pos, const QList<QGeoSatelliteInfo>& info) +{ + const int nsats = info.size(); + const int nmsgs = nsats / 4; + int i; + + Q_UNUSED(pos); + + for (i = 0; i < nmsgs; i++) { + int s = i * 4; // Start from this satellite index) + QString sentence = + QString::fromLatin1("GPGSV,%1,%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12,%13,%14,%15,%16,%17,%18,%19") + // 1 = Number of sentences for full data + .arg(nmsgs) + // 2 = Sentence number + .arg(i) + // 3 = Total number of satellites in view + .arg(nsats) + // 4 - 14 = IDs of SVs used in position fix + .arg(nsats > s + 0 ? formatSatPrn(info[s + 0]) : QString()) + .arg(nsats > s + 0 ? formatSatElev(info[s + 0]) : QString()) + .arg(nsats > s + 0 ? formatSatAz(info[s + 0]) : QString()) + .arg(nsats > s + 0 ? formatSatSnr(info[s + 0]) : QString()) + .arg(nsats > s + 1 ? formatSatPrn(info[s + 1]) : QString()) + .arg(nsats > s + 1 ? formatSatElev(info[s + 1]) : QString()) + .arg(nsats > s + 1 ? formatSatAz(info[s + 1]) : QString()) + .arg(nsats > s + 1 ? formatSatSnr(info[s + 1]) : QString()) + .arg(nsats > s + 2 ? formatSatPrn(info[s + 2]) : QString()) + .arg(nsats > s + 2 ? formatSatElev(info[s + 2]) : QString()) + .arg(nsats > s + 2 ? formatSatAz(info[s + 2]) : QString()) + .arg(nsats > s + 2 ? formatSatSnr(info[s + 2]) : QString()) + .arg(nsats > s + 3 ? formatSatPrn(info[s + 3]) : QString()) + .arg(nsats > s + 3 ? formatSatElev(info[s + 3]) : QString()) + .arg(nsats > s + 3 ? formatSatAz(info[s + 3]) : QString()) + .arg(nsats > s + 3 ? formatSatSnr(info[s + 3]) : QString()) + ; + emitSentence(sentence); + } +} + +void NmeaSource::handlePosition(const QGeoPositionInfo &pos) +{ + generateGPRMC(pos); + + generateGPGGA(pos); + generateGPGLL(pos); + generateGPVTG(pos); + generatePGRME(pos); //Estimated Position Error + + //PGRMZ = Altitude info, PGRMM = Map datum? + //generateGPRMB? + //generateGPBOD? + //generateGPRTE? + + m_last_pos = pos; +} + +void NmeaSource::handleSatsInUse(const QList<QGeoSatelliteInfo> &info) +{ + generateGPGSA(m_last_pos, info); +} + +void NmeaSource::handleSatsInView(const QList<QGeoSatelliteInfo> &info) +{ + generateGPGSV(m_last_pos, info); + m_last_satsinview = info.count(); +} |