int do_read(int m_fd)
{
const size_t outlength = 80;
char outline[outlength];
char line[80];
const int ret = force_read(m_fd, line, 79);
line[ret] = '\0';
const char * delim = ",";
const char * field0 = strtok(line, delim);
const char * field1 = strtok(NULL, delim);
const char * field2 = strtok(NULL, delim);
const char * field3 = strtok(NULL, delim);
const char * field4 = strtok(NULL, delim);
if(field4 != NULL)
{
const float value_mbar = strtof(field4, NULL);
const float value_pascal = value_mbar * 100.0f;
const int snprintf_result = snprintf(outline, outlength, "$POV,Q,%f", value_pascal);
const uint8_t outline_checksum = nmea_checksum(outline);
fprintf(stdout, "%s*%02x\n", outline, outline_checksum);
fflush(stdout);
}
return ret;
}
/** Assemble an NMEA GPRMC message and send it out NMEA USARTs.
* NMEA RMC contains minimum GPS data
*
* \param nav_meas Pointer to navigation_measurement struct.
* \param soln Pointer to gnss_solution struct
* \param gps_t Pointer to the current GPS Time
*/
void nmea_gprmc(const navigation_measurement_t *nav_meas,
const gnss_solution *soln, const gps_time_t *gps_t)
{
/* NMEA Parameters
* Ex.
* $GPRMC,220516,A,5133.82,N,00042.24,W,173.8,231.8,130694,004.2,W*70
* | | | | | | | | | | | | |
* Command | | Lat N/S | | | | Date Stamp | W/E |
* Time (UTC) | Long W/E | True Course | Cksum
* Validity (A-OK) Speed Variation
* Variation is ignored as we have no way to maintain that information
* currently
*/
time_t unix_t;
struct tm t;
unix_t = gps2time(*gps_t);
gmtime_r(&unix_t, &t);
double frac_s = fmod(gps_t->tow, 1.0);
s16 lat_deg = R2D * (soln->pos_llh[0]);
double lat_min = MINUTES(soln->pos_llh[0]);
s16 lon_deg = R2D * (soln->pos_llh[1]);
double lon_min = MINUTES(soln->pos_llh[1]);
lat_deg = abs(lat_deg);
lon_deg = abs(lon_deg);
char lat_dir = soln->pos_llh[0] < 0 ? 'S' : 'N';
char lon_dir = soln->pos_llh[1] < 0 ? 'W' : 'E';
float velocity;
float x,y,z;
x = soln->vel_ned[0];
y = soln->vel_ned[1];
z = soln->vel_ned[2];
float course = atan2(y,x);
/* Conversion to magnitue knots */
velocity = MS2KNOTTS(x,y,z);
double az, el;
wgsecef2azel(nav_meas[0].sat_pos, soln->pos_ecef, &az, &el);
char buf[100];
u8 n = sprintf(buf,
"$GPRMC,%02d%02d%06.3f,A," /* Command, Time (UTC), Valid */
"%02d%010.7f,%c,%03d%010.7f,%c," /* Lat/Lon */
"%06.2f,%05.1f," /* Speed, Course */
"%02d%02d%02d," /* Date Stamp */
",", /* Variation */
t.tm_hour, t.tm_min, t.tm_sec + frac_s,
lat_deg, lat_min, lat_dir, lon_deg, lon_min, lon_dir,
velocity, course * R2D,
t.tm_mday, t.tm_mon, t.tm_year-100);
u8 sum = nmea_checksum(buf);
sprintf(buf + n, "*%02X\r\n", sum);
nmea_output(buf);
}
// If gga msg received do validate else return
bool update_gpgga_data(void)
{
bool return_value = false;
char gga_buffer[MAX_GPGGA_STR_SIZE];
if(return_string_with_identifier(xQueue_uart_nmea_receive_handle, gga_buffer, GPGGA_START_OF_MSG, GPGGA_END_OF_MSG, MAX_GPGGA_STR_SIZE, 10))
{
if(!nmea_checksum(gga_buffer))
{
if(xSemaphoreTake(xSemaphore_gga_msg_mutex_handle, 200))
{
if(nmea_gpgga_parse(gga_buffer, &gga_msg_global))
{
reset_gpgga_msg(&gga_msg_global);
}
else
{
return_value = true;
}
xSemaphoreGive( xSemaphore_gga_msg_mutex_handle );
}
}
}
return return_value;
}
uint8_t parseGGA(GPSData* gpsd, char* s)
{
if(!nmea_checksum(s)) return 0;
return 1;
}
int main(int argc, char *argv[]) {
if (argc<4) {
std::cout << "\nUsage: ./unpacker binary_string enable_nmea channel\n";
std::cout << "\nenable_nmea = 1 for full NMEA sentence, otherwise only payload is printed\n";
std::cout << "channel = A/B\n\n";
return 0;
}
int i;
char asciidata[255];
int len = strlen(argv[1]);
int enable_nmea = (int) *argv[2] - 48;
char channel = *argv[3];
std::ostringstream d_payload;
d_payload.str("");
for(i = 0; i < len/6; i++) {
asciidata[i] = unpack(argv[1], i*6, 6);
if(asciidata[i] > 39) asciidata[i] += 8;
asciidata[i] += 48;
}
if (enable_nmea)
d_payload << "!AIVDM,1,1,," << channel << ",";
for(int i = 0; i < len/6; i++)
d_payload << asciidata[i];
if (enable_nmea) {
d_payload << ",0"; //number of bits to fill out 6-bit boundary
char checksum = nmea_checksum(std::string(d_payload.str()));
d_payload << "*" << std::setw(2) << std::setfill('0') << std::hex << std::uppercase << int(checksum);
}
std::cout << std::string(d_payload.str()) << std::endl;
return 1;
}
/** Assemble a NMEA GPGSV message and send it out NMEA USARTs.
* NMEA GPGSV message contains GPS satellites in view.
*
* \param n_used Number of satellites currently being tracked.
* \param nav_meas Pointer to navigation_measurement struct.
* \param soln Pointer to gnss_solution struct.
*/
void nmea_gpgsv(u8 n_used, navigation_measurement_t *nav_meas,
gnss_solution *soln)
{
if (n_used == 0)
return;
u8 n_mess = (n_used + 3) / 4;
char buf[80];
char *buf0 = buf + sprintf(buf, "$GPGSV,%d,", n_mess);
char *bufp = buf0;
u8 n = 0;
double az, el;
for (u8 i = 0; i < n_mess; i++) {
bufp = buf0;
bufp += sprintf(bufp, "%d,%d", i + 1, n_used);
for (u8 j = 0; j < 4; j++) {
if (n < n_used) {
wgsecef2azel(nav_meas[n].sat_pos, soln->pos_ecef, &az, &el);
bufp += sprintf(
bufp, ",%02d,%02d,%03d,%02d",
nav_meas[n].prn + 1,
(u8)round(el * 180.0 / M_PI),
(u16)round(az * 180.0 / M_PI),
(u8)(10.0 * nav_meas[n].snr)
);
} else {
bufp += sprintf(bufp, ",,,,");
}
n++;
}
sprintf(bufp, "*%02X\r\n", nmea_checksum(buf));
nmea_output(buf);
}
}
/** Assemble an NMEA GPVTG message and send it out NMEA USARTs.
* NMEA VTG contains course and speed
*
* \param nav_meas Pointer to navigation_measurement struct.
* \param soln Pointer to gnss_solution struct
*/
void nmea_gpvtg(const navigation_measurement_t *nav_meas,
const gnss_solution *soln)
{
/* NMEA Parameters for GPVTG
* Ex.
* $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K
* | | | | | | | | |
* Command | 'T' | 'M' | 'N' | 'K'
* True Course | Speed (K) |
* Mag. course Speed (km/hr)
*/
double az, el;
wgsecef2azel(nav_meas[0].sat_pos, soln->pos_ecef, &az, &el);
float vknots, vkmhr;
float x,y,z;
x = soln->vel_ned[0];
y = soln->vel_ned[1];
z = soln->vel_ned[2];
float course = atan2(y,x);
/* Conversion to magnitue knots */
vknots = MS2KNOTTS(x,y,z);
/* Conversion to magnitue km/hr */
vkmhr = MS2KMHR(x,y,z);
char buf[80];
u8 n = sprintf(buf,
"$GPVTG,%05.1f,T," /* Command, course, */
",M," /* Magnetic Course (omitted) */
"%06.2f,N,%06.2f,K", /* Speed (knots, km/hr) */
course* R2D,
vknots, vkmhr);
u8 sum = nmea_checksum(buf);
sprintf(buf + n, "*%02X\r\n", sum);
nmea_output(buf);
}
/** Assemble a NMEA GPGGA message and send it out NMEA USARTs.
* NMEA GPGGA message contains Global Positioning System Fix Data.
*
* \param soln Pointer to gnss_solution struct.
* \param dops Pointer to dops_t struct.
*/
void nmea_gpgga(gnss_solution *soln, dops_t *dops)
{
time_t unix_t;
struct tm t;
unix_t = gps2time(soln->time);
gmtime_r(&unix_t, &t);
double frac_s = fmod(soln->time.tow, 1.0);
s8 lat_deg = (s8)((180.0 / M_PI) * soln->pos_llh[0]);
double lat_min = fabs(60 * ((180.0 / M_PI) * soln->pos_llh[0] - lat_deg));
s8 lon_deg = (s8)((180.0 / M_PI) * soln->pos_llh[1]);
double lon_min = fabs(60 * ((180.0 / M_PI) * soln->pos_llh[1] - lon_deg));
lat_deg = abs(lat_deg);
lon_deg = abs(lon_deg);
char lat_dir = soln->pos_llh[0] < 0 ? 'S' : 'N';
char lon_dir = soln->pos_llh[1] < 0 ? 'W' : 'E';
u8 fix_type = 1;
char buf[80];
u8 n = sprintf(buf,
"$GPGGA,%02d%02d%06.3f,"
"%02d%010.7f,%c,%03d%010.7f,%c,"
"%01d,%02d,%.1f,%1.f,M,,M,,",
t.tm_hour, t.tm_min, t.tm_sec + frac_s,
lat_deg, lat_min, lat_dir, lon_deg, lon_min, lon_dir,
fix_type, soln->n_used, dops->hdop, soln->pos_llh[2]
);
u8 sum = nmea_checksum(buf);
sprintf(buf + n, "*%02X\r\n", sum);
nmea_output(buf);
}
static void test_checksum(void)
{
struct checksum_test_t
{
const char * str;
uint8_t chk;
};
static const struct checksum_test_t TESTS[] =
{
{ "GPRMC,201034,A,4702.4040,N,00818.3281,E,0.0,328.4,260807,0.6,E,A", 0x17 },
{ "GPRMC,201124,A,4702.3947,N,00818.3372,E,0.3,328.4,260807,0.6,E,A", 0x10 },
{ "GPRMC,201126,A,4702.3944,N,00818.3381,E,0.0,328.4,260807,0.6,E,A", 0x1E },
};
unsigned int i;
uint8_t chk;
for (i = 0 ; i < sizeof(TESTS) / sizeof(TESTS[0]); ++i) {
chk = nmea_checksum(TESTS[i].str, TESTS[i].str + strlen(TESTS[i].str));
CU_ASSERT_EQUAL(chk, TESTS[i].chk);
}
}
int
nmea_sprintf(char *str, const char *fmt, ...)
{
int ret = 0;
char checksum;
va_list args;
va_start(args, fmt);
ret = vsnprintf(str, NMEA_MAX_LENGTH, fmt, args);
if (ret > 78)
{
*str = '\0';
return -1; /* insufficient space for checksum and CRLF*/
}
va_end(args);
checksum = nmea_checksum(str);
ret += snprintf(str+ret, NMEA_MAX_LENGTH-ret, "*%02X\x0d\x0a", checksum);
return ret;
}
/** Assemble a NMEA GPGSA message and send it out NMEA USARTs.
* NMEA GPGSA message contains DOP and active satellites.
*
* \param chans Pointer to tracking_channel_t struct.
* \param dops Pointer to dops_t struct.
*/
void nmea_gpgsa(tracking_channel_t *chans, dops_t *dops)
{
char buf[80] = "$GPGSA,A,3,";
char *bufp = buf + strlen(buf);
for (u8 i = 0; i < 12; i++) {
if (i < nap_track_n_channels && chans[i].state == TRACKING_RUNNING)
bufp += sprintf(bufp, "%02d,", chans[i].prn + 1);
else
*bufp++ = ',';
}
if (dops)
bufp += sprintf(bufp, "%.1f,%.1f,%.1f", dops->pdop, dops->hdop, dops->vdop);
else
bufp += sprintf(bufp, ",,");
u8 sum = nmea_checksum(buf);
sprintf(bufp, "*%02X\r\n", sum);
nmea_output(buf);
}
/** Assemble an NMEA GPGLL message and send it out NMEA USARTs.
* NMEA GLL contains course and speed
*
* \param soln Pointer to gnss_solution struct
* \param gpt_t Pointer to the current GPS Time
*/
void nmea_gpgll(const gnss_solution *soln, const gps_time_t *gps_t)
{
/* NMEA Parameters for GPGLL
* Ex.
* $GPGLL,5133.81,N,00042.25,W,225444,A*75
* | | | | | | |
* Command | N/S Lon E/W | Valid
* LAT UTC
*/
time_t unix_t;
struct tm t;
unix_t = gps2time(*gps_t);
gmtime_r(&unix_t, &t);
double frac_s = fmod(gps_t->tow, 1.0);
s16 lat_deg = R2D * (soln->pos_llh[0]);
double lat_min = MINUTES(soln->pos_llh[0]);
s16 lon_deg = R2D * (soln->pos_llh[1]);
double lon_min = MINUTES(soln->pos_llh[1]);
lat_deg = abs(lat_deg);
lon_deg = abs(lon_deg);
char lat_dir = soln->pos_llh[0] < 0 ? 'S' : 'N';
char lon_dir = soln->pos_llh[1] < 0 ? 'W' : 'E';
char buf[80];
u8 n = sprintf(buf,
"$GPGLL,"
"%02d%010.7f,%c,%03d%010.7f,%c," /* Lat/Lon */
"%02d%02d%06.3f,A", /* Time (UTC), Valid */
lat_deg, lat_min, lat_dir, lon_deg, lon_min, lon_dir,
t.tm_hour, t.tm_min, t.tm_sec + frac_s);
u8 sum = nmea_checksum(buf);
sprintf(buf + n, "*%02X\r\n", sum);
nmea_output(buf);
}
/** Assemble a NMEA GPGGA message and send it out NMEA USARTs.
* NMEA GPGGA message contains Global Positioning System Fix Data.
*
* \param soln Pointer to gnss_solution struct.
* \param dops Pointer to dops_t struct.
*/
void nmea_gpgga(const double pos_llh[3], const gps_time_t *gps_t, u8 n_used,
u8 fix_type, double hdop)
{
time_t unix_t;
struct tm t;
unix_t = gps2time(*gps_t);
gmtime_r(&unix_t, &t);
double frac_s = fmod(gps_t->tow, 1.0);
s16 lat_deg = R2D * (pos_llh[0]);
double lat_min = MINUTES(pos_llh[0]);
s16 lon_deg = R2D * (pos_llh[1]);
double lon_min = MINUTES(pos_llh[1]);
lat_deg = abs(lat_deg);
lon_deg = abs(lon_deg);
char lat_dir = pos_llh[0] < 0 ? 'S' : 'N';
char lon_dir = pos_llh[1] < 0 ? 'W' : 'E';
char buf[80];
u8 n = sprintf(buf,
"$GPGGA,%02d%02d%06.3f,"
"%02d%010.7f,%c,%03d%010.7f,%c,"
"%01d,%02d,%.1f,%.2f,M,,M,,",
t.tm_hour, t.tm_min, t.tm_sec + frac_s,
lat_deg, lat_min, lat_dir, lon_deg, lon_min, lon_dir,
fix_type, n_used, hdop, pos_llh[2]
);
u8 sum = nmea_checksum(buf);
sprintf(buf + n, "*%02X\r\n", sum);
nmea_output(buf);
}
unsigned int nmea_parse_gprmc(char *sentence, nmea_gprmc_t *data)
{
char sentence_checksum;
float lat_degrees;
float lat_minutes;
char lat_hemisphere;
float lon_degrees;
float lon_minutes;
char lon_hemisphere;
char *token;
static char *comma = ",";
char checksum_buffer[3];
unsigned int invalidity = 0;
/* Pre-amble of "$GPRMC" */
if(strncmp_P(sentence, PSTR("$GPRMC,"), 7) != 0)
return NMEA_INVALID_TYPE;
sentence_checksum = nmea_checksum(sentence);
/* Skip over the pre-amble */
sentence += 7;
/* Zero-fill the return data structure */
memset(data, 0, sizeof(nmea_gprmc_t));
/* Current UTC time */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%02hhd%02hhd%02hhd.%03hd"),
&data->hour,
&data->minute,
&data->second,
&data->millisecond
);
/* Status of fix: A = Valid; V = Invalid */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%c"),
&data->status
);
if(data->status != 'A')
invalidity |= NMEA_INVALID_STATUS;
/* Latitude degrees and minutes */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%02f%07f"),
&lat_degrees,
&lat_minutes
);
/* Latitude hemisphere */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%c"),
&lat_hemisphere
);
/* Combine latitude degrees and minutes and adjust for hemisphere */
data->latitude = (lat_degrees + (lat_minutes / 60.0))
* (lat_hemisphere == 'N' ? 1.0 : -1.0);
if(data->latitude < -90.0 || data->latitude > 90.0)
invalidity |= NMEA_INVALID_LATITUDE;
/* Longitude degrees and minutes */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%03f%07f"),
&lon_degrees,
&lon_minutes
);
/* Longitude hemisphere */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%c"),
&lon_hemisphere
);
/* Combine longitude degrees and minutes and adjust for hemisphere */
data->longitude = (lon_degrees + (lon_minutes / 60.0))
* (lon_hemisphere == 'E' ? 1.0 : -1.0);
if(data->longitude < -180.0 || data->longitude > 180.0)
invalidity |= NMEA_INVALID_LONGITUDE;
/* Speed in knots */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%f"),
&data->speed
);
/* Heading in degrees */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%f"),
&data->heading
);
/* Current UTC date */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%02hhd%02hhd%02hd"),
&data->date,
&data->month,
&data->year
);
/* Since 1980 is long past, if we see this year it's a good assumption
* that the data is invalid.
*/
if(data->year == 80)
invalidity |= NMEA_INVALID_DATE;
/* Adjust year based on GPS epoch of 1980 */
data->year += (data->year) < 80 ? 2000 : 1900;
/* Magnetic variation in degrees (unused) */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
/* Magnetic variation compass direction */
if((token = strsep(&sentence, comma)) == NULL)
return NMEA_INVALID_SENTENCE;
/* Mode: A = Autonomous operation; N = Data not valid */
if((token = strsep(&sentence, "*")) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%c"),
&data->mode
);
if(data->mode != 'A')
invalidity |= NMEA_INVALID_STATUS;
/* Checksum, one byte as a hexadecimal string */
if((token = strsep(&sentence, "\n")) == NULL)
return NMEA_INVALID_SENTENCE;
sscanf_P(token,
PSTR("%2s"),
checksum_buffer
);
/* Convert the checksum from a hexadecimal string */
data->checksum = strtoul(checksum_buffer, NULL, 16);
/* Verify that the checksum of the string matches the stored one */
if(data->checksum != sentence_checksum)
invalidity |= NMEA_INVALID_CHECKSUM;
return(invalidity);
}
// Function which will send an NMEA sentence to a Garmin GPS which
// will create a waypoint if the Garmin is set to NMEA-in/NMEA-out
// mode. The sentence looks like this:
//
// $GPWPL,4807.038,N,01131.000,E,WPTNME*31
//
// $GPWPL,4849.65,N,06428.53,W,0001*54
// $GPWPL,4849.70,N,06428.50,W,0002*50
//
// 4807.038,N Latitude
// 01131.000,E Longitude
// WPTNME Waypoint Name (stick to 6 chars for compatibility?)
// *31 Checksum, always begins with '*'
//
//
// Future implementation ideas:
//
// Create linked list of waypoints/location.
// Use the list to prevent multiple writes of the same waypoint if
// nothing has changed.
//
// Use the list to check distance of previously-written waypoints.
// If we're out of range, delete the waypoint and remove it from the
// list.
//
// Perhaps write the list to disk also. As we shut down, delete the
// waypoints (self-cleaning). As we come up, load them in again?
// We could also just continue cleaning out waypoints that are
// out-of-range since the last time we ran the program. That's
// probably a better solution.
//
void create_garmin_waypoint(long latitude,long longitude,char *call_sign) {
char short_callsign[10];
char lat_string[15];
char long_string[15];
char lat_char;
char long_char;
int i,j,len;
char out_string[80];
char out_string2[80];
convert_lat_l2s(latitude,
lat_string,
sizeof(lat_string),
CONVERT_HP_NOSP);
lat_char = lat_string[strlen(lat_string) - 1];
lat_string[strlen(lat_string) - 1] = '\0';
convert_lon_l2s(longitude,
long_string,
sizeof(long_string),
CONVERT_HP_NOSP);
long_char = long_string[strlen(long_string) - 1];
long_string[strlen(long_string) - 1] = '\0';
len = strlen(call_sign);
if (len > 9)
len = 9;
j = 0;
for (i = 0; i <= len; i++) { // Copy the '\0' as well
if (call_sign[i] != '-') { // We don't want the dash
short_callsign[j++] = call_sign[i];
}
}
short_callsign[6] = '\0'; // Truncate at 6 chars
// Convert to upper case. Garmin's don't seem to like lower
// case waypoint names
to_upper(short_callsign);
//fprintf(stderr,"Creating waypoint for %s:%s\n",call_sign,short_callsign);
xastir_snprintf(out_string, sizeof(out_string),
"$GPWPL,%s,%c,%s,%c,%s*",
lat_string,
lat_char,
long_string,
long_char,
short_callsign);
nmea_checksum(out_string);
xastir_snprintf(out_string2,
sizeof(out_string2),
"%s%s",
out_string,
checksum);
output_waypoint_data(out_string2);
//fprintf(stderr,"%s\n",out_string2);
}
