/usr/include/Aria/ArGPS.h is in libaria-dev 2.8.0+repack-1.2.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 | /*
Adept MobileRobots Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
Copyright (C) 2011, 2012, 2013 Adept Technology
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program 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 General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
If you wish to redistribute ARIA under different terms, contact
Adept MobileRobots for information about a commercial version of ARIA at
robots@mobilerobots.com or
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
*/
#ifndef ARGPS_H
#define ARGPS_H
#include "ariaTypedefs.h"
#include "ArFunctor.h"
#include "ariaUtil.h"
#include "ArMutex.h"
#include "ArNMEAParser.h"
#include <math.h>
#include <string>
#include <vector>
class ArDeviceConnection; // for pointer in ArGPS
/** @brief GPS Device Interface
*
* Connects to GPS device over a serial port or other device connection and reads data.
* Supports GPS devices sending standard NMEA format data
* (specifically the GPRMC, GPGGA, GPGSA, GPGRME, and optionally GPGSV, PGRMZ, PGRME,
* HCHDG/T/M and GPHDG/T/M messages).
* If your GPS device supports several data formats or modes, select
* NMEA output in its configuration.
*
* The preferred method of creating and setting up a new ArGPS object is to use
* ArGPSConnector, which creates an instance of ArGPS or a subclass, and
* creates and opens its device connection, based on command-line parameters.
* (To manually create an ArGPS object, create an ArDeviceConnection instance
* and call setDeviceConnection(), then open that device connection and call
* connect().
*
* For either method, to get new data from the GPS, must call read() or readWithLock() periodically,
* ideally at a rate equal to or faster than your GPS sends data (usually one second).
* You can do this from a Sensor Intetrpretation Task in ArRobot, or a seperate thread.
* If you are calling read() from a loop in a new thread,
*
* Here is an example of calling readWithLock() from a sensor interpretation
* task. The integer argument given to the functor constructor is a milisecond timeout that
* is passed to readWithLock() and prevents it from blocking too long if it doesn't read any data.
* It is important to do this in a robot task, or the robot task cycle will be
* blocked and cause problems.
* @code
* ArRetFunctor1C<ArGPS, int, unsigned int> gpsReadFunc(myGPS, &ArGPS::readWithLock, 10);
* myRobot->addSensorInterpretationTask("GPS read", 100, &gpsReadFunc);
* @endcode
*
* If you use your own loop or thread, then it ought to include a call to ArUtil::sleep() for at least several hundred
* miliseconds to avoid starving other threads, since read() will return
* immediately if there is no data to read rather than blocking.
*
* For each piece of data provided by this class, there is a flag indicating
* whether it was received from the GPS and set. Not all GPS models return all
* kinds of information, or it may be disabled in some way in a GPS's internal
* configuration, or the GPS may not yet have started sending the data (e.g.
* still acquiring satellites). Also, not all data will be received by one call to read(),
* and especially immediately after connecting and starting to read data, it
* may take a few seconds for data to be obtained. Furthermore, it may take
* some time for the GPS to calculate data with full accuracy.
*
* @sa @ref gpsExample.cpp
* @sa @ref gpsRobotTaskExample.cpp
*
* This class is not inherently thread safe. Stored data is updated by read(), so
* if accessing from multiple threads, call lock() before calling any data
* accessor methods (methods starting with "get"), or read(), and call unlock()
* when done. You can also call readWithLock() to do a locked read in one
* function call.
*
* @note ArGPS only provides access to the data reported by a GPS. The position
* reported by a GPS is in degrees on the surface of the earth (WGS84 datum), not in the
* cartesian coordinate system used by the robot odometry or ArMap. You can use
* the subclasses of Ar3DPoint (ArLLACoords, etc) to convert between different
* geographical coordinate systems, which may help you match GPS coordinates to
* the robot pose coordinate system.
@ingroup OptionalClasses
@ingroup DeviceClasses
*/
class ArGPS {
public:
AREXPORT ArGPS();
virtual ~ArGPS() { }
/** @brief Set device connection to use */
void setDeviceConnection(ArDeviceConnection* deviceConn) { myDevice = deviceConn; }
/** @brief Return device connection in use (or NULL if none) */
ArDeviceConnection* getDeviceConnection() const { return myDevice; }
/** @brief Check that the device connection (e.g. serial port) is open, and
* that data is being received from GPS.
*
* Subclasses may override this method so that device-specific
* initialization commands may be sent.
*
* @return false if there is no device connection or the device connection
* is not open, or if there is an error sending device initialization
* commands, or if no data is received after calling read() every 100 ms
* for @a connectTimeout ms. Otherwise, return true.
*
* @sa blockingConnect()
*/
AREXPORT virtual bool connect(unsigned long connectTimeout = 20000);
/** Same as connect(). See connect(). */
bool blockingConnect(unsigned long connectTimeout = 20000) { return connect(connectTimeout); }
protected:
/** Block until data is read from GPS.
Waits by calling read() every 100 ms for @a timeout ms.
*/
AREXPORT bool waitForData(unsigned long timeout);
/** Subclasses may override to send device initialization/configuration
* commands and set up device-specific message handlers. (Default behavior
* is to do nothing and return true.)
*/
virtual bool initDevice() { return true; }
public:
/** @brief Flags to indicates what the read() method did.
* i.e. If nothing was done, then the
* result will be 0. To check a read() return result @a result to see if data was updated, use
* (result & ReadUpdated). To check if there was an error, use (result &
* ReadError).
*
* These happen to match the flags in ArNMEAParser.
*/
enum {
ReadFinished = ArNMEAParser::ParseFinished,
ReadError = ArNMEAParser::ParseError,
ReadData = ArNMEAParser::ParseData,
ReadUpdated = ArNMEAParser::ParseUpdated
} ReadFlags;
/** @brief Read some data from the device connection, and update stored data as complete messages are received.
* @param maxTime If nonzero, return when this time limit is reached, even if there is still data available to read. If zero, then don't return until all available data has been exhausted or an error occurs.
* Be careful setting this parameter to 0: read() could block for
* an arbitrary amount of time, even forever if for some reason data is recieved from
* the device faster than read() can read and parse it.
* @return A mask of ReadFlags codes, combined with bitwise or (|), or 0 if no attempt to read from the device occured (for example because the @a maxTime timeout was reached before the first attempt to read occured). The flags will include
* ReadError if there was as error reading from the device connection,
* ReadData if some data was read,
* ReadUpdated if data was read and a full message was successfully read and
* stored data was updated in ArGPS,
* ReadFinished if all available data was read.
*/
AREXPORT virtual int read(unsigned long maxTime = 0);
/** Calls lock(), calls read(maxTime), then calls unlock(). Note, this could
* end up keeping ArGPS locked until @a maxTime is reached, or for any amount
* of time if @a maxTime is 0, so watch out for that. */
int readWithLock(unsigned int maxTime) { lock(); int r = read(maxTime); unlock(); return r; }
/** Locks a mutex object contained by this class.
* No other method (except readWithLock()) in ArGPS locks or unlocks this
* mutex, it is provided for you to use when accessing ArGPS from multiple
* threads.
*/
void lock() { myMutex.lock(); }
/** Unlocks a mutex object contained by this class.
* No other method (except readWithLock()) in ArGPS locks or unlocks this
* mutex, it is provided for you to use when accessing ArGPS from multiple
* threads.
*/
void unlock() { myMutex.unlock(); }
/** @brief Set whether checksum sent with NMEA messages is ignored */
void setIgnoreChecksum(bool ignore) { myNMEAParser.setIgnoreChecksum(ignore); }
/** @brief Log last received data using ArLog. */
AREXPORT void logData() const;
/** Print basic navigation data on one line to standard output, with no newline at end. */
AREXPORT void printData(bool labels = true) const;
AREXPORT void printDataLabelsHeader() const;
/** Data accessors
* @brief Access the last received data from the GPS */
// @{
typedef enum {
NoFix, BadFix, GPSFix, DGPSFix, PPSFix,
RTKinFix, FloatRTKinFix, DeadReckFix,
ManualFix, SimulatedFix, UnknownFixType,
OmnistarConverging = FloatRTKinFix,
OmnistarConverged = RTKinFix
} FixType;
class Data {
public:
AREXPORT Data();
double latitude; ///< (from NMEA GPRMC)
double longitude; ///< (from NMEA GPRMC)
bool havePosition; ///< (from NMEA GPRMC)
ArTime timeGotPosition; ///< Local computer time when ArGPS class received the position message from the GPS. (From NMEA GPRMC)
double speed; ///< (From NMEA GPRMC, if provided)
bool haveSpeed; ///< (From NMEA GPRMC)
ArTime GPSPositionTimestamp; ///< Timestamp provided by GPS device along with latitude and longitude. (from NMEA GPRMC)
ArGPS::FixType fixType; ///< (from NMEA GPGGA)
unsigned short numSatellitesTracked;
double altitude; ///< receiver provides this based on GPS data. meters above sea level. (from NMEA GPGGA)
bool haveAltitude; //< (from NMEA GPGGA)
double altimeter; ///< from seperate altimeter (if receiver provides PGRMZ message). meters above sea level.
bool haveAltimeter;
unsigned short DGPSStationID; ///< (from NMEA GPGGA)
bool haveDGPSStation; ///< (from NMEA GPGGA)
double garminPositionError; ///< Error in meters, only some GPS devices provide this
bool haveGarminPositionError; ///< Error in meters, only some GPS devices provide this (PGRME)
double garminVerticalPositionError; ///< Error in meters, only some GPS devices provide this (PGRME)
bool haveGarminVerticalPositionError; ///< Error in meters, only some GPS devices provide this (PGRME)
double compassHeadingMag; ///< (from HCDHM message, if device provides it)
double compassHeadingTrue; ///< (from HCHDT, if device provides it)
bool haveCompassHeadingMag; ///< (from HCDHM message, if device provides it)
bool haveCompassHeadingTrue; ///< (from HCHDT message, if device provides it)
unsigned long compassMagCounter; ///< Incremented whenever @a compassHeadingMag is updated with new data
unsigned long compassTrueCounter; ///< Incremented whenever @a compassHeadingMag is updated with new data
bool haveHDOP; ///< Horizontal dilution of precision (from NMEA GPGGA)
double HDOP; ///< Horizontal dilution of precision (from NMEA GPGGA)
bool haveVDOP; ///< Vertical dilution of precision (from NMEA GPGGA)
double VDOP; ///< Vertical dilution of precision (from NMEA GPGGA)
bool havePDOP; ///< Combined dilution of precision (from NMEA GPGGA)
double PDOP; ///< Combined dilution of precision (from NMEA GPGGA)
bool qualityFlag; ///< Some GPS devices set this to false if data quality is below some thresholds.
double meanSNR; ///< Mean of satellite signal-noise ratios (dB)
bool haveSNR; ///< (from NMEA GPGSV)
double beaconSignalStrength; ///< dB (from NMEA GPMSS)
double beaconSNR; ///< dB (from NMEA GPMSS)
double beaconFreq; ///< kHz (from NMEA GPMSS)
unsigned short beaconBPS; ///< Bits/sec (from NMEA GPMSS)
unsigned short beaconChannel; ///< (from NMEA GPMSS)
bool haveBeaconInfo; ///< (from NMEA GPMSS)
double inputsRMS; ///< (from NMEA GPGST)
bool haveInputsRMS; ///< (from NMEA GPGST)
ArPose errorEllipse; ///< Ellipse shows standard deviation, in meters. Orientation is degrees from true north. (from NMEA GPGST)
bool haveErrorEllipse; ///< (from NMEA GPGST)
ArPose latLonError; ///< Std.deviation, meters. Theta is unused. May only be provided by the GPS in certain fix modes. Note, values could be inf or nan (GPS sends these in some situations). Use isinf() and isnan() to check.
bool haveLatLonError;
double altitudeError; ///< Std. deviation, meters. Note, value could be inf or nan (GPS sends these in some situations). use isinf() and isnan() to check.
bool haveAltitudeError;
};
/** Access all of the internally stored data directly. @see ArGPS::Data */
const ArGPS::Data& getCurrentDataRef() const { return myData; }
/** (from NMEA GPGGA) */
FixType getFixType() const { return myData.fixType; }
/** (from NMEA GPGGA) */
AREXPORT const char* getFixTypeName() const;
static AREXPORT const char* getFixTypeName(FixType type);
/** (from NMEA GPRMC) */
AREXPORT bool havePosition() const { return myData.havePosition; }
/** (from NMEA GPRMC) */
AREXPORT bool haveLatitude() const { return myData.havePosition; }
/** (from NMEA GPRMC) */
AREXPORT bool haveLongitude() const { return myData.havePosition; }
/** @return latitude in decimal degrees.
(from NMEA GPRMC) */
double getLatitude() const { return myData.latitude; }
/** @return longitude in decimal degrees.
(from NMEA GPRMC) */
double getLongitude() const { return myData.longitude; }
/** @return copy of an ArTime object set to the time that ArGPS read and received latitude and longitude data from the GPS.
(from NMEA GPRMC) */
ArTime getTimeReceivedPosition() const { return myData.timeGotPosition; }
/** (from NMEA GPRMC) */
bool haveSpeed() const { return myData.haveSpeed; }
/** @return GPS' measured speed converted to meters per second, if provided
(from NMEA GPRMC, if provided)
*/
double getSpeed() const { return myData.speed; }
/** Timestamp provided by GPS device along with position. (from NMEA GPRMC) */
ArTime getGPSPositionTimestamp() const { return myData.GPSPositionTimestamp; }
int getNumSatellitesTracked() const { return (int) myData.numSatellitesTracked; }
/** (from NMEA GPGGA) */
bool haveDGPSStation() const { return myData.haveDGPSStation; }
/** (from NMEA GPGGA) */
unsigned short getDGPSStationID() const { return myData.DGPSStationID; }
/** @return whether GPS provided a distance error estimation (from a
* Garmin-specific message PGRME, most GPS receivers will not provide this) */
bool haveGarminPositionError() const { return myData.haveGarminPositionError; }
/** GPS device's error estimation in meters (from a Garmin-specific message PGRME,
* most GPS receivers will not provide this)*/
double getGarminPositionError() const { return myData.garminPositionError; }
/** @return whether GPS provided an altitude error estimation (from a
* Garmin-specific message PGRME, most GPS receivers will not provide this) */
bool haveGarminVerticalPositionError() const { return myData.haveGarminVerticalPositionError; }
/** @return An altitude error estimation (from a Garmin-specific message PGRME,
* most GPS receivers will not provide this) */
double getGarminVerticalPositionError() const { return myData.garminVerticalPositionError; }
/** Have a compass heading value relative to magnetic north.
@note The GPS or compass device must be configured to send HCHDM messages
to receive compass data. Only some GPS receivers support this.
*/
bool haveCompassHeadingMag() const { return myData.haveCompassHeadingMag; }
/** Have a compass heading value relative to true north (using GPS/compass
device's configured declination).
@note The GPS or compass device must be configured to send HCHDT messages
to receive compass data. Only some GPS receivers support this.
*/
bool haveCompassHeadingTrue() const { return myData.haveCompassHeadingTrue; }
/** Heading from magnetic north
@note The GPS or compass device must be configured to send HCHDM messages
to receive compass data. Only some GPS receivers support this.
*/
double getCompassHeadingMag() const { return myData.compassHeadingMag; }
/** Heading from true north
@note The GPS or compass device must be configured to send HCHDT messages
to receive compass data. Only some GPS receivers support this.
*/
double getCompassHeadingTrue() const { return myData.compassHeadingTrue; }
/** Manually set compass value. */
void setCompassHeadingMag(double val) {
myData.haveCompassHeadingMag = true;
myData.compassHeadingMag = val;
myData.compassMagCounter++;
}
/** Manually set compass value. */
void setCompassHeadingTrue(double val) {
myData.haveCompassHeadingTrue = true;
myData.compassHeadingTrue = val;
myData.compassMagCounter++;
}
/** Manually set compass value. */
void setCompassHeadingMagWithLock(double val) { lock(); setCompassHeadingMag(val); unlock(); }
/** Manually set compass value. */
void setCompassHeadingTrueWithLock(double val) { lock(); setCompassHeadingTrue(val); unlock(); }
/// Altitude above sea level calculated from satellite positions (see also haveAltimiter()) (from NMEA GPGGA, if provided)
bool haveAltitude() const { return myData.haveAltitude; }
/// Altitude above sea level (meters), calculated from satellite positions (see also getAltimiter()) (from NMEA GPGGA, if provided)
double getAltitude() const { return myData.altitude; }
/// Some receivers may have an additional altitude from an altimiter (meters above sea level) (from PGRMZ, if receiver provides it)
bool haveAltimeter() const { return myData.haveAltimeter; }
/// Some receivers may have an additional altitude from an altimiter (meters above sea level) (from PGRMZ, if receiver provides it)
double getAltimeter() const { return myData.altimeter; }
/** (from NMEA GPGGA) */
bool haveHDOP() const { return myData.haveHDOP; }
/** (from NMEA GPGGA) */
double getHDOP() const { return myData.HDOP; }
/** (from NMEA GPGGA) */
bool haveVDOP() const { return myData.haveVDOP; }
/** (from NMEA GPGGA) */
double getVDOP() const { return myData.VDOP; }
/** (from NMEA GPGGA) */
bool havePDOP() const { return myData.havePDOP; }
/** (from NMEA GPGGA) */
double getPDOP() const { return myData.PDOP; }
/** (from NMEA GPGSV) */
bool haveSNR() const { return myData.haveSNR; }
/// dB (from NMEA GPGSV)
double getMeanSNR() const { return myData.meanSNR; }
/** Whether we have any DGPS stationary beacon info (from NMEA GPMSS) */
bool haveBeaconInfo() const { return myData.haveBeaconInfo; }
/** DGPS stationary beacon signal strength (dB) (from NMEA GPMSS) */
double getBeaconSignalStrength() const { return myData.beaconSignalStrength; }
/** DGPS stationary beacon signal to noise (dB) (from NMEA GPMSS) */
double getBeaconSNR() const { return myData.beaconSNR; }
/** DGPS stationary beacon frequency (kHz) (from NMEA GPMSS) */
double getBeaconFreq() const { return myData.beaconFreq; }
/** DGPS stationary beacon bitrate (bits per second) (from NMEA GPMSS) */
unsigned short getBecaonBPS() const { return myData.beaconBPS; }
/** DGPS stationary beacon channel (from NMEA GPMSS) */
unsigned short getBeaconChannel() const { return myData.beaconChannel; }
/** Whether we have a position error estimate (as standard deviations in latitude and longitude) (from NMEA GPGST) */
bool haveErrorEllipse() const { return myData.haveErrorEllipse; }
/** Standard deviation of position error (latitude and longitude), meters. Theta in ArPose is orientation of ellipse from true north, Y is the length of the major axis on that orientation, X the minor.
(from NMEA GPGST)
@note Values may be inf or NaN (if GPS supplies "Inf" or "NAN")
*/
ArPose getErrorEllipse() const {return myData.errorEllipse; }
/** Whether we have latitude or longitude error estimates (from NMEA GPGST) */
bool haveLatLonError() const { return myData.haveLatLonError; }
/** Standard deviation of latitude and longitude error, meters.
Theta value in ArPose is unused.
@note May only be provided by GPS in certain fix modes
(e.g. Trimble AgGPS provides it in Omnistar and RTK modes, but not in GPS
or DGPS modes).
@note Values may be inf or NaN (if GPS supplies "Inf" or "NAN")
(from NMEA GPGST)
*/
ArPose getLatLonError() const { return myData.latLonError; }
/** @copydoc getLatLonError() */
double getLatitudeError() const { return myData.latLonError.getX(); }
/** @copydoc getLatLonError() */
double getLongitudeError() const { return myData.latLonError.getY(); }
bool haveAltitudeError() const { return myData.haveAltitudeError; }
/// Standard deviation of altitude error, meters. (from NMEA GPGST, if provided)
double getAltitudeError() const { return myData.altitudeError; }
/// (from NMEA GPGST)
bool haveInputsRMS() const { return myData.haveInputsRMS; }
/// (from NMEA GPGST)
double getInputsRMS() const { return myData.inputsRMS; }
/** Set a handler for an NMEA message. Mostly for internal use or to be used
* by related classes, but you could use for ususual or custom messages
* emitted by a device that you wish to be handled outside of the ArGPS
* class.
*/
void addNMEAHandler(const char *message, ArNMEAParser::Handler *handler) { myNMEAParser.addHandler(message, handler); }
void removeNMEAHandler(const char *message) { myNMEAParser.removeHandler(message); }
void replaceNMEAHandler(const char *message, ArNMEAParser::Handler *handler) {
myNMEAParser.removeHandler(message);
myNMEAParser.addHandler(message, handler);
}
protected:
/* Most recent data values received, to return to user */
Data myData;
/* Utility to read a double floating point number out of a std::string, if possible.
* @return true if the string was nonempty and @a target was modified.
*/
bool readFloatFromString(const std::string& str, double* target, double(*convf)(double) = NULL) const;
/* Utility to read an unsigned short integer out of a std::string, if possible.
* @return true if the string was nonempty and @a target was modified.
*/
bool readUShortFromString(const std::string& str, unsigned short* target, unsigned short (*convf)(unsigned short) = NULL) const;
/* Utility to read a double from a member of a vector of strings, if it exists. */
bool readFloatFromStringVec(const std::vector<std::string>* vec, size_t i, double* target, double (*convf)(double) = NULL) const;
/* Utility to read a double from a member of a vector of strings, if it exists. */
bool readUShortFromStringVec(const std::vector<std::string>* vec, size_t i, unsigned short* target, unsigned short (*convf)(unsigned short) = NULL) const;
/* Utility to convert DDDMM.MMMM to decimal degrees */
static double gpsDegminToDegrees(double degmin);
/* Utility to convert US nautical knots to meters/sec */
static double knotsToMPS(double knots);
/** Convert meters per second to miles per hour */
static double mpsToMph(const double mps) { return mps * 2.23693629; }
/* Utility to convert meters to US feet */
static double metersToFeet(double m) { return m * 3.2808399; }
/* Utility to convert US feet to meters */
static double feetToMeters(double f) { return f / 3.2808399; }
/* Mutex */
ArMutex myMutex;
/* Connection info */
ArDeviceConnection *myDevice;
bool myCreatedOwnDeviceCon;
ArRetFunctorC<bool, ArGPS> myParseArgsCallback;
ArArgumentParser* myArgParser;
/* NMEA Parser */
ArNMEAParser myNMEAParser;
/* GPS message handlers */
void handleGPRMC(ArNMEAParser::Message msg);
ArFunctor1C<ArGPS, ArNMEAParser::Message> myGPRMCHandler;
void handleGPGGA(ArNMEAParser::Message msg);
ArFunctor1C<ArGPS, ArNMEAParser::Message> myGPGGAHandler;
void handlePGRME(ArNMEAParser::Message msg);
ArFunctor1C<ArGPS, ArNMEAParser::Message> myPGRMEHandler;
void handlePGRMZ(ArNMEAParser::Message msg);
ArFunctor1C<ArGPS, ArNMEAParser::Message> myPGRMZHandler;
void handleHCHDx(ArNMEAParser::Message msg);
ArFunctor1C<ArGPS, ArNMEAParser::Message> myHCHDxHandler;
void handleGPGSA(ArNMEAParser::Message msg);
ArFunctor1C<ArGPS, ArNMEAParser::Message> myGPGSAHandler;
void handleGPGSV(ArNMEAParser::Message msg);
ArFunctor1C<ArGPS, ArNMEAParser::Message> myGPGSVHandler;
/* For calculating SNR averages based on multiple GPGSV messages. */
unsigned int mySNRSum;
unsigned short mySNRNum;
void handleGPMSS(ArNMEAParser::Message msg);
ArFunctor1C<ArGPS, ArNMEAParser::Message> myGPMSSHandler;
void handleGPGST(ArNMEAParser::Message msg);
ArFunctor1C<ArGPS, ArNMEAParser::Message> myGPGSTHandler;
/* Set an ArTime object using a time read from a string as decimal seconds (SSS.SS) */
bool readTimeFromString(const std::string& s, ArTime* time) const;
/** Parse a GPRMC message (in @a msg) and place results in provided
* variables. (Can be used by subclasses to store results of GPRMC differently
* than normal.)
* @since Aria 2.7.2
*/
void parseGPRMC(const ArNMEAParser::Message &msg, double &latitudeResult, double &longitudeResult, bool &qualityFlagResult, bool &gotPosition, ArTime &timeGotPositionResult, ArTime &gpsTimestampResult, bool &gotSpeedResult, double &speedResult);
};
class ArRobotPacket;
class ArRobot;
/// @since Aria 2.7.4
class ArSimulatedGPS : public virtual ArGPS
{
bool myHaveDummyPosition;
ArRetFunctor1C<bool, ArSimulatedGPS, ArRobotPacket*> mySimStatHandlerCB;
ArRobot *myRobot;
public:
AREXPORT ArSimulatedGPS(ArRobot *robot = NULL);
AREXPORT virtual ~ArSimulatedGPS();
void setDummyPosition(double latitude, double longitude) {
myData.latitude = latitude;
myData.havePosition = true;
myData.longitude = longitude;
if(!myData.haveHDOP) myData.HDOP = 1.0;
myData.haveHDOP = true;
if(!myData.haveVDOP) myData.VDOP = 1.0;
myData.haveVDOP = true;
if(!myData.havePDOP) myData.PDOP = 1.0;
myData.havePDOP = true;
myData.fixType = SimulatedFix;
myHaveDummyPosition = true;
}
void clearDummyPosition() {
clearPosition();
myHaveDummyPosition = false;
}
void clearPosition() {
myData.havePosition = false;
myData.latitude = 0;
myData.longitude = 0;
myData.altitude = 0;
myData.HDOP = 0;
myData.VDOP = 0;
myData.PDOP = 0;
myData.fixType = NoFix;
}
void setDummyPosition(double latitude, double longitude, double altitude) {
myData.altitude = altitude;
setDummyPosition(latitude, longitude);
}
AREXPORT void setDummyPosition(ArArgumentBuilder *args);
void setDummyPositionFromArgs(ArArgumentBuilder *args) { setDummyPosition(args); } // non-overloaded function can be used in functors
AREXPORT virtual bool connect(unsigned long connectTimeout = 10000);
virtual bool initDevice() { return true; }
virtual int read(unsigned long maxTime = 0) {
if(myHaveDummyPosition)
{
myData.timeGotPosition.setToNow();
}
return ReadUpdated | ReadFinished;
}
private:
#ifndef SWIG
bool handleSimStatPacket(ArRobotPacket *pkt);
#endif
};
#endif // ifdef ARGPS_H
|