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); }