/*
nmea_gprmc()
NMEA Recommended Minimum Specific GPS/TRANSIT Data (RMC)
Caveats:
- Speed and course over ground are calculated from
the north/east velocity and may not be accurate
- Magnetic variation not available
*/
int nmea_gprmc(D800_Pvt_Data_Type *pvt, char *nmeastc) {
char buf[NMEA_BUF_SIZE];
char slat[NMEA_LATLON_SIZE], slon[NMEA_LATLON_SIZE];
char utctime[NMEA_UTC_SIZE], utcdate[NMEA_UTC_SIZE];
float speed, course;
unsigned char cksum;
nmea_getutc(pvt, utctime, utcdate);
/* latitude */
nmea_fmtlat(convert_to_float32(pvt->lat), slat);
/* longitude */
nmea_fmtlon(convert_to_float32(pvt->lon), slon);
/* speed over ground */
speed = sqrt(pvt->east*pvt->east + pvt->north*pvt->north) * 3.6 / KNOTS_TO_KMH;
/* course */
if (speed < 1.0) {
if (g_lastcourse >= 0)
course = g_lastcourse;
else
course = 0; /* too low to determine course */
} else {
course = atan2(pvt->east, pvt->north);
if (course < 0)
course += 2*G_PI;
course = rad2deg(course);
g_lastcourse = course; /* remember for later */
}
sprintf(buf, "GPRMC,%s,%c,%s,%s,%05.1f,%05.1f,%s,,", utctime,
(pvt->fix >= 2 && pvt->fix <= 5) ? 'A' : 'V',
slat, slon, (double)speed, (double)course, utcdate);
cksum = nmea_cksum(buf);
sprintf(nmeastc, "$%s*%02X\r\n", buf, cksum);
return 0;
}
void nmea_getutc(D800_Pvt_Data_Type *pvt, char *utctime, char *utcdate) {
float tmp;
int daysdiff;
float tow = convert_to_float32(pvt->tow);
/* UTC time of position fix */
tmp = tow - pvt->leap_scnds;
if (tmp < 0.0)
tmp += 86400.0;
daysdiff = 0;
while (tmp > 86400.0) {
tmp -= 86400.0;
daysdiff++;
}
if (utctime) {
int h, m, s;
h = ((int)tmp) / 3600;
m = ((int)tmp - h*3600) / 60;
s = ((int)tmp - h*3600 - m*60);
sprintf(utctime, "%02d%02d%02d", h, m, s);
}
if (utcdate) {
/* Garmin format: number of days since December 31, 1989 */
unsigned long jd = pvt->wn_days + daysdiff + 2447892;
unsigned long w, x, a, b, c, d, e, f;
unsigned long day, month, year;
w = (unsigned long)((jd - 1867216.25)/36524.25);
x = w/4;
a = jd + 1 + w - x;
b = a + 1524;
c = (unsigned long)((b - 122.1)/365.25);
d = (unsigned long)(365.25 * c);
e = (unsigned long)((b-d)/30.6001);
f = (unsigned long)(30.6001 * e);
day = b - d - f;
month = e - 1;
if (month > 12)
month -= 12;
year = c - 4716;
if (month == 1 || month == 2)
year++;
year -= 2000;
sprintf(utcdate, "%02ld%02ld%02ld", day, month, year);
}
}
IplImage* myReadImg2Lab(char* path) {
// load image, change to 32F CvMat for L*a*b* convertion
IplImage* src = cvLoadImage(path, CV_LOAD_IMAGE_UNCHANGED);
IplImage* src32F = convert_to_float32(src);
IplImage* img = cvCreateImage(cvGetSize(src32F), IPL_DEPTH_32F, 3);
cvCvtColor(src32F, img, CV_BGR2Lab);
cvReleaseImage(&src32F);
return img;
}
/*
nmea_gpgll()
NMEA Geographic Position (GLL)
*/
int nmea_gpgll(D800_Pvt_Data_Type *pvt, char *nmeastc) {
char buf[NMEA_BUF_SIZE];
char slat[NMEA_LATLON_SIZE], slon[NMEA_LATLON_SIZE];
char utctime[NMEA_UTC_SIZE];
unsigned char cksum;
nmea_getutc(pvt, utctime, NULL);
/* latitude */
nmea_fmtlat(convert_to_float32(pvt->lat), slat);
/* longitude */
nmea_fmtlon(convert_to_float32(pvt->lon), slon);
sprintf(buf, "GPGLL,%s,%s,%s,%c", slat, slon, utctime,
(pvt->fix >= 2 && pvt->fix <= 5) ? 'A' : 'V');
cksum = nmea_cksum(buf);
sprintf(nmeastc, "$%s*%02X\r\n", buf, cksum);
return 0;
}
