/** 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'; NMEA_SENTENCE_START(120); NMEA_SENTENCE_PRINTF("$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] ); NMEA_SENTENCE_DONE(); }
/** 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'; NMEA_SENTENCE_START(120); NMEA_SENTENCE_PRINTF("$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); NMEA_SENTENCE_DONE(); }
/** 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); }
/** Update GPS time estimate. * * This function may be called to update the GPS time estimate. If the time is * already known more precisely than the new estimate, the new estimate will be * ignored. * * This function should not be used to give an estimate with TIME_FINE quality * as this must be referenced to a particular value of the SwiftNAP timing * count. * * \param quality Quality of the time estimate. * \param t GPS time estimate. */ void set_time(time_quality_t quality, gps_time_t t) { if (quality > time_quality) { clock_state.t0_gps = t; clock_state.t0_gps.tow -= nap_timing_count() * RX_DT_NOMINAL; clock_state.t0_gps = normalize_gps_time(clock_state.t0_gps); time_quality = quality; time_t unix_t = gps2time(t); log_info("Time set to: %s (quality=%d)", ctime(&unix_t), quality); } }
/** Assemble an NMEA GPRMC message and send it out NMEA USARTs. * NMEA RMC contains minimum GPS data * * \param soln Pointer to gnss_solution struct * \param gps_t Pointer to the current GPS Time */ void nmea_gprmc(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); NMEA_SENTENCE_START(140); NMEA_SENTENCE_PRINTF( "$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 + 1, t.tm_year % 100); NMEA_SENTENCE_DONE(); }
/** 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); }
/** 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); }