Revision: 9124
http://sourceforge.net/p/playerstage/svn/9124
Author: jpgr87
Date: 2013-10-06 17:06:38 +0000 (Sun, 06 Oct 2013)
Log Message:
-----------
Applied patch #666: GPS speed, course made good and time
Modified Paths:
--------------
code/player/trunk/client_libs/libplayerc/dev_gps.c
code/player/trunk/client_libs/libplayerc/playerc.h
code/player/trunk/client_libs/libplayerc++/gpsproxy.cc
code/player/trunk/client_libs/libplayerc++/playerc++.h
code/player/trunk/libplayerinterface/interfaces/013_gps.def
code/player/trunk/server/drivers/gps/garminnmea.cc
code/player/trunk/server/drivers/shell/dummy.cc
code/player/trunk/server/drivers/shell/readlog.cc
code/player/trunk/server/drivers/shell/writelog.cc
Modified: code/player/trunk/client_libs/libplayerc/dev_gps.c
===================================================================
--- code/player/trunk/client_libs/libplayerc/dev_gps.c 2013-04-13 00:16:00 UTC
(rev 9123)
+++ code/player/trunk/client_libs/libplayerc/dev_gps.c 2013-10-06 17:06:38 UTC
(rev 9124)
@@ -114,6 +114,9 @@
device->lon = gps_data->longitude / 1e7;
device->alt = gps_data->altitude / 1e3;
+ device->speed = gps_data->speed;
+ device->course = gps_data->course;
+
device->utm_e = gps_data->utm_e / 100.0;
device->utm_n = gps_data->utm_n / 100.0;
Modified: code/player/trunk/client_libs/libplayerc/playerc.h
===================================================================
--- code/player/trunk/client_libs/libplayerc/playerc.h 2013-04-13 00:16:00 UTC
(rev 9123)
+++ code/player/trunk/client_libs/libplayerc/playerc.h 2013-10-06 17:06:38 UTC
(rev 9124)
@@ -1755,6 +1755,13 @@
below. */
double alt;
+ /** Speed over ground, in m/s. */
+ double speed;
+
+ /** Course made good (heading if the robot moves along its longitudinal
+ * axis), in radians. */
+ double course;
+
/** UTM easting and northing (meters). */
double utm_e, utm_n;
Modified: code/player/trunk/client_libs/libplayerc++/gpsproxy.cc
===================================================================
--- code/player/trunk/client_libs/libplayerc++/gpsproxy.cc 2013-04-13
00:16:00 UTC (rev 9123)
+++ code/player/trunk/client_libs/libplayerc++/gpsproxy.cc 2013-10-06
17:06:38 UTC (rev 9124)
@@ -87,9 +87,10 @@
std::operator << (std::ostream &os, const PlayerCc::GpsProxy &c)
{
os << "#GPS (" << c.GetInterface() << ":" << c.GetIndex() << ")" <<
std::endl;
- os << "#lat|long|alt|utm_e|utm_n|err_horz|err_vert|num_sats|quality" <<
std::endl;
+ os <<
"#time|lat|long|alt|speed|course|utm_e|utm_n|err_horz|err_vert|num_sats|quality"
<< std::endl;
+ os << std::setw(14) << std::setprecision(13) << c.GetTime() << " ";
os << std::setw(11) << std::setprecision(10) << c.GetLatitude() << " " <<
std::setw(11) << std::setprecision(10) << c.GetLongitude() << " " <<
std::setw(6) << std::setprecision(5) << c.GetAltitude() << " " ;
-
+ os << std::setw(5) << std::setprecision(2) << c.GetSpeed() << " " <<
std::setw(6) << std::setprecision(5) << c.GetCourse() << " ";
os << std::setw(11) << std::setprecision(10) << c.GetUtmEasting() << " " <<
std::setw(11) << std::setprecision(10) << c.GetUtmNorthing() << " " <<
std::setw(6) << std::setprecision(5) << c.GetErrHorizontal() << " ";
os << std::setw(6) << std::setprecision(5) << c.GetErrVertical() << " " <<
setw(3) << c.GetSatellites() << " " << std::setw(3) << c.GetQuality() <<
std::endl;
return os;
Modified: code/player/trunk/client_libs/libplayerc++/playerc++.h
===================================================================
--- code/player/trunk/client_libs/libplayerc++/playerc++.h 2013-04-13
00:16:00 UTC (rev 9123)
+++ code/player/trunk/client_libs/libplayerc++/playerc++.h 2013-10-06
17:06:38 UTC (rev 9124)
@@ -876,6 +876,13 @@
/// Altitude, in meters.
double GetAltitude() const { return GetVar(mDevice->alt); };
+ /// Spped over ground, in m/s.
+ double GetSpeed() const { return GetVar(mDevice->speed); };
+
+ /** Course made good (heading if the robot moves along its longitudinal
+ * axis), in radians. */
+ double GetCourse() const { return GetVar(mDevice->course); };
+
/// Number of satellites in view.
uint32_t GetSatellites() const { return GetVar(mDevice->sat_count); };
Modified: code/player/trunk/libplayerinterface/interfaces/013_gps.def
===================================================================
--- code/player/trunk/libplayerinterface/interfaces/013_gps.def 2013-04-13
00:16:00 UTC (rev 9123)
+++ code/player/trunk/libplayerinterface/interfaces/013_gps.def 2013-10-06
17:06:38 UTC (rev 9124)
@@ -29,6 +29,11 @@
/** Altitude, in millimeters. Positive is above reference (e.g.,
sea-level), and negative is below. */
int32_t altitude;
+ /** Speed over ground, in meters/second . */
+ double speed;
+ /** Course made good (heading if the robot moves along its longitudinal
+ * axis), in radians. */
+ double course;
/** UTM WGS84 coordinates, easting [m] */
double utm_e;
/** UTM WGS84 coordinates, northing [m] */
Modified: code/player/trunk/server/drivers/gps/garminnmea.cc
===================================================================
--- code/player/trunk/server/drivers/gps/garminnmea.cc 2013-04-13 00:16:00 UTC
(rev 9123)
+++ code/player/trunk/server/drivers/gps/garminnmea.cc 2013-10-06 17:06:38 UTC
(rev 9124)
@@ -846,7 +846,7 @@
if(!strcmp(tmp,NMEA_GPGGA))
ParseGPGGA(ptr);
- // the RMC msg has the date and time
+ // the RMC msg has the date, time, speed and course
if(!strcmp(tmp,NMEA_GPRMC))
ParseGPRMC(ptr);
@@ -994,7 +994,7 @@
/*
- * Parse the GPRMC sentence, which has date/time
+ * Parse the GPRMC sentence, which has date/time and speed and course.
*/
int GarminNMEA::ParseGPRMC(const char *buf)
{
@@ -1002,6 +1002,7 @@
char field[32];
char tmp[8];
struct tm tms;
+ int usec;
time_t utc;
static bool short_time_field = 0;
@@ -1010,6 +1011,7 @@
memset(&tms, 0, sizeof(tms));
+ // Time
if(!(ptr = GetNextField(field, sizeof(field), ptr)))
return(-1);
@@ -1040,20 +1042,67 @@
tmp[2]='\0';
tms.tm_sec = atoi(tmp);
+ // Let's see if we have finer grained timing info.
+ if (strlen(field) > 6)
+ {
+ // field + 6 is '.', field + 7... is fractions of seconds.
+ int length = strlen(field) - 7;
+ if (length > 6)
+ length = 6; // At most microseconds.
+ strncpy(tmp, field + 7, length);
+ tmp[length] = '\0';
+ usec = atoi(tmp) * pow(10, 6 - length);
+ }
+ else
+ {
+ usec = 0;
+ }
+
+ // Validity
if(!(ptr = GetNextField(field, sizeof(field), ptr)))
return(-1);
+ // Latitude
if(!(ptr = GetNextField(field, sizeof(field), ptr)))
return(-1);
+ // North / south
if(!(ptr = GetNextField(field, sizeof(field), ptr)))
return(-1);
+ // Longitude
if(!(ptr = GetNextField(field, sizeof(field), ptr)))
return(-1);
+ // East / west
if(!(ptr = GetNextField(field, sizeof(field), ptr)))
return(-1);
+
+ // Speed
if(!(ptr = GetNextField(field, sizeof(field), ptr)))
return(-1);
+
+ if (strlen(field) == 0)
+ {
+ PLAYER_WARN("No speed information in string; ignoring");
+// return -1;
+ }
+ else
+ {
+ data.speed = atof(field) * 0.51444; // knots to m/sec
+ }
+
+ // Course
if(!(ptr = GetNextField(field, sizeof(field), ptr)))
return(-1);
+
+ if (strlen(field) == 0)
+ {
+ PLAYER_WARN("No course information in string; ignoring");
+// return -1;
+ }
+ else
+ {
+ data.course = atof(field) * M_PI / 180.0; // degrees to radians
+ }
+
+ // Date
if(!(ptr = GetNextField(field, sizeof(field), ptr)))
return(-1);
@@ -1081,11 +1130,12 @@
// tms.tm_hour, tms.tm_min, tms.tm_sec);
// Compute to time since the epoch. We only get it to the nearest
- // second, unfortunately.
+ // second, but microseconds possibly extracted from RMC string if it
+ // contains it.
utc = mktime(&tms);
data.time_sec = (uint32_t) utc;
- data.time_usec = (uint32_t) 0;
+ data.time_usec = (uint32_t) usec;
/* Dont write here
// Need to parse to sentences before write data
Modified: code/player/trunk/server/drivers/shell/dummy.cc
===================================================================
--- code/player/trunk/server/drivers/shell/dummy.cc 2013-04-13 00:16:00 UTC
(rev 9123)
+++ code/player/trunk/server/drivers/shell/dummy.cc 2013-10-06 17:06:38 UTC
(rev 9124)
@@ -356,6 +356,8 @@
data.latitude = 1e7;
data.longitude = 1e7;
data.altitude = 1e3;
+ data.course = 10;
+ data.speed = 5;
data.utm_e = 10.0;
data.utm_n = 10.0;
data.quality = 2;
Modified: code/player/trunk/server/drivers/shell/readlog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/readlog.cc 2013-04-13 00:16:00 UTC
(rev 9123)
+++ code/player/trunk/server/drivers/shell/readlog.cc 2013-10-06 17:06:38 UTC
(rev 9124)
@@ -1689,6 +1689,14 @@
data.quality = atoi(tokens[17]);
data.num_sats = atoi(tokens[18]);
+ // Read in course and heading, while staying backwards compatible with
+ // player 3.0
+ if (token_count == 21)
+ {
+ data.speed = atof(tokens[19]);
+ data.course = atof(tokens[20]);
+ }
+
this->Publish(id,type,subtype, (void*) &data, sizeof(data), &time);
return 0;
Modified: code/player/trunk/server/drivers/shell/writelog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/writelog.cc 2013-04-13 00:16:00 UTC
(rev 9123)
+++ code/player/trunk/server/drivers/shell/writelog.cc 2013-10-06 17:06:38 UTC
(rev 9124)
@@ -3010,6 +3010,8 @@
- err_vert (float): vertical error, in meters
- quality (int): quality of fix (0 = invalid, 1 = GPS fix, 2 = DGPS fix)
- num_sats (int): number of satellites in view
+ - speed (float): in meters per second
+ - course made good (float): in radians
*/
int WriteLog::WriteGps(player_msghdr_t* hdr, void *data)
{
@@ -3023,6 +3025,7 @@
"%.3f %.3f "
"%.3f %.3f %.3f %.3f "
"%d %d",
+ "%.3f %.3f",
(double)gdata->time_sec +
(double)gdata->time_sec * 1e-6,
(double)gdata->latitude / (1e7),
@@ -3035,7 +3038,9 @@
gdata->err_horz,
gdata->err_vert,
gdata->quality,
- gdata->num_sats);
+ gdata->num_sats,
+ (double)gdata->speed,
+ (double)gdata->course);
return (0);
default:
return (-1);
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
October Webinars: Code for Performance
Free Intel webinars can help you accelerate application performance.
Explore tips for MPI, OpenMP, advanced profiling, and more. Get the most from
the latest Intel processors and coprocessors. See abstracts and register >
http://pubads.g.doubleclick.net/gampad/clk?id=60134791&iu=/4140/ostg.clktrk
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
