int main()
{
const char *buff[] = {
"$GPRMC,173843,A,3349.896,N,11808.521,W,000.0,360.0,230108,013.4,E*69\r\n",
"$GPGGA,111609.14,5001.27,N,3613.06,E,3,08,0.0,10.2,M,0.0,M,0.0,0000*70\r\n",
"$GPGSV,2,1,08,01,05,005,80,02,05,050,80,03,05,095,80,04,05,140,80*7f\r\n",
"$GPGSV,2,2,08,05,05,185,80,06,05,230,80,07,05,275,80,08,05,320,80*71\r\n",
"$GPGSA,A,3,01,02,03,04,05,06,07,08,00,00,00,00,0.0,0.0,0.0*3a\r\n",
"$GPRMC,111609.14,A,5001.27,N,3613.06,E,11.2,0.0,261206,0.0,E*50\r\n",
"$GPVTG,217.5,T,208.8,M,000.00,N,000.01,K*4C\r\n"
};
int it;
nmeaINFO info;
nmeaPARSER parser;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
for(it = 0; it < 6; ++it)
nmea_parse(&parser, buff[it], (int)strlen(buff[it]), &info);
nmea_parser_destroy(&parser);
return 0;
}
int main()
{
const char *buff =
"$GPGGA,073044.000,2235.8106,N,11359.9026,E,1,8,1.01,143.7,M,-2.4,M,,*41\r\n\
$GPRMC,070344.000,A,2235.8106,N,11359.9026,E,0.17,220.01,200916,,,A*6A\r\n";
int it;
nmeaINFO info;
nmeaPARSER parser;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
// for(it = 0; it < 1; ++it)
nmea_parse(&parser, buff, (int)strlen(buff), &info);
printf("gps time:\t%04d-%02d-%02d %02d:%02d:%02d\n",
info.utc.year + 1900,
info.utc.mon + 1,
info.utc.day,
info.utc.hour + 8,
info.utc.min,
info.utc.sec);
printf("gps latitude:\t%f\n", info.lat);
printf("gps longitude:\t%f\n", info.lon);
printf("gps altitude:\t%f\n", info.elv);
nmea_parser_destroy(&parser);
return 0;
}
/**
* static int Gps_set(void * _self, va_list *app) - Perform a new gps acquisition.
* \param *_self - pointer to the device
* \param *app - none
* \return:
<LI><Breturn = 0:</B>when the GPS device is connected on a Grove nest DIG port, with or without a valid connection </LI>
<LI><Breturn = -1:</B>when the GPS device is not connected on a Grove nest DIG port. </LI>
</UL>
*/
static int Gps_set(void *_self,va_list *app)
{
struct Gps *self = _self;
char gps_sentences[70];
unsigned int timer = 65000;
flag = 1;
nmea_zero_INFO(&self->info);
nmea_parser_init(&self->parser);
UARTOn(self->uart_module);
UARTFlush(self->uart_module);
vTaskDelay(50);
while((UARTBufferSize(self->uart_module)<3) && timer)
timer--;
if(!timer)
return -1;
timer = 500;
while((gps_sentences[0] != '$') && timer)
{
timer--;
while(UARTBufferSize(self->uart_module)<1);
UARTRead(self->uart_module,gps_sentences,1);
if(gps_sentences[0] == '$')
{
while(UARTBufferSize(self->uart_module) < 5);
UARTRead(self->uart_module,(gps_sentences+1),5); //Reads all the chars in the
if((gps_sentences[4] == 'M'))
{
while(UARTBufferSize(self->uart_module) < 65);
UARTRead(self->uart_module,(gps_sentences+6),64); //Reads all the chars in the
gps_sentences[70] = '\0';
int i = 0;
i = nmea_parse(&self->parser, gps_sentences, (int)strlen(gps_sentences), &self->info);
if(i)
{
flag = 0;
self->lat = nmea_ndeg2degree (self->info.lat);
self->lon = -nmea_ndeg2degree (self->info.lon);
self->year = self->info.utc.year+1900;
self->mon = self->info.utc.mon;
self->day = self->info.utc.day;
self->hour = self->info.utc.hour;
self->min = self->info.utc.min;
self->sec = self->info.utc.sec;
self->speed = self->info.speed;
break;
}
}
}
gps_sentences[0]= 0;
}
nmea_parser_destroy(&self->parser);
UARTOff(self->uart_module);
return 0;
}
int main()
{
static const char * filename = "../../samples/parse_file/gpslog.txt";
nmeaINFO info;
nmeaPARSER parser;
FILE *file;
char buff[2048];
int size, it = 0;
nmeaPOS dpos;
file = fopen(filename, "rb");
if(!file) {
printf("Could not open file %s\n", filename);
return -1;
}
nmea_property()->trace_func = &trace;
nmea_property()->error_func = &error;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
/*
while(1)
{
*/
while(!feof(file))
{
size = (int)fread(&buff[0], 1, 100, file);
nmea_parse(&parser, &buff[0], size, &info);
nmea_info2pos(&info, &dpos);
printf(
"%03d, Lat: %f, Lon: %f, Sig: %d, Fix: %d\n",
it++, dpos.lat, dpos.lon, info.sig, info.fix
);
}
fseek(file, 0, SEEK_SET);
/*
}
*/
nmea_parser_destroy(&parser);
fclose(file);
return 0;
}
int main()
{
const char *buff[] = {
"$GPGGA,000000,2508.644,N,12145.244,E,8,00,99.9,00000,M,00000,M,,*4A\r\n\r\n",
"$SDDBT,0008.2,f,0002.5,M,0001.4,F*3E\r\n\r\n"
};
const char *buff2[] = {
"$GPDTM,W84,,0.0000,N,0.0000,E,0.0000,W84*5F\r\n",
"$GPAPB,V,V,-0.00,L,N,V,V,090.2,T,0009,090.2,T,090.2,T,N*6F\r\n",
"$GPBOD,090.2,T,090.2,M,0009,0000*4E\r\n",
"$GPBWC,000000,2508.956,N,12146.687,E,090.2,T,090.2,M,000.27,N,N*74\r\n",
"$GPGGA,000000,2508.957,N,12146.385,E,0,00,99.9,00000,M,00000,M,,*40\r\n",
"$GPGBS,,,,,,,,*41\r\n",
"$GPRMC,000000,V,2508.957,N,12146.385,E,00.0,000.0,010105,,,N*57\r\n",
"$GPVTG,000.0,T,,,00.0,N,00.0,K,N*4F\r\n",
"$GPRMC,000000,V,2515.791,N,12145.337,E,00.0,251.7,010105,,,S*49\r\n"
};
int it;
for(it = 0; it < 2; ++it)
{
nmeaINFO info;
nmeaPARSER parser;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
nmea_parse(&parser, buff[it], (int)strlen(buff[it]), &info);
printf("parse: %s\n", buff[it]);
printf("declination: %f\n", info.declination);
printf("utc.min: %f\n", info.utc.min);
printf("HDOP: %f\n", info.HDOP);
printf("VDOP: %f\n", info.VDOP);
printf("lat: %f\n", info.lat);
printf("lon: %f\n", info.lon);
printf("elv: %f\n", info.elv);
printf("depth: %f\n", info.depthinfo.depth_M);
nmea_parser_destroy(&parser);
}
return 0;
}
void deinit_gps() {
nmea_parser_destroy(&parserGPS);
}
int main(int argc, char *argv[])
{
int it = 0;
int done = 0;
nmeaINFO info;
nmeaPARSER parser;
int select_result = -1; // value returned frome select()
struct timeval select_timeout;
int nfds = 0;
fd_set rset;
char rs232_device;
int bytes_read;
char rs232_buffer[1024];
char *token;
char nmea_message[256];
if(argc < 2)
{
printf("Usage: %s <file to parse>\n", argv[0]);
return -1;
}
rs232_device = open(argv[1], O_RDWR | O_NOCTTY);
if(rs232_device < 0)
perror("com_open");
// Select UART2_TX and set it as output
printf("Setting tx. . .\n");
sprintf(rs232_buffer, "echo 11 > /sys/kernel/debug/omap_mux/spi0_d0");
if(system(rs232_buffer) < 0)
perror("setting tx");
// Select UART1_RX and set it as input pulled up
printf("Setting rx. . .\n");
sprintf(rs232_buffer, "echo 39 > /sys/kernel/debug/omap_mux/spi0_sclk");
if(system(rs232_buffer) < 0)
perror("setting rx");
const char *buff[] = {
"$GPRMC,173843,A,3349.896,N,11808.521,W,000.0,360.0,230108,013.4,E*69\r\n",
"$GPGGA,111609.14,5001.27,N,3613.06,E,3,08,0.0,10.2,M,0.0,M,0.0,0000*70\r\n",
"$GPGSV,2,1,08,01,05,005,80,02,05,050,80,03,05,095,80,04,05,140,80*7f\r\n",
"$GPGSV,2,2,08,05,05,185,80,06,05,230,80,07,05,275,80,08,05,320,80*71\r\n",
"$GPGSA,A,3,01,02,03,04,05,06,07,08,00,00,00,00,0.0,0.0,0.0*3a\r\n",
"$GPRMC,111609.14,A,5001.27,N,3613.06,E,11.2,0.0,261206,0.0,E*50\r\n",
"$GPVTG,217.5,T,208.8,M,000.00,N,000.01,K*4C\r\n"
};
select_timeout.tv_sec = 1;
select_timeout.tv_usec = 0;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
while(!done)
{
fflush(stdout);
FD_ZERO(&rset);
if(rs232_device > 0)
{
FD_SET(rs232_device, &rset);
nfds = max(nfds, rs232_device);
}
select_result = select(nfds + 1, &rset, NULL, NULL, NULL);
if(select_result == -1 && errno == EAGAIN)
{
perror("select");
continue;
}
if(select_result == -1)
{
perror("main:");
return 1;
}
if(rs232_device > 0)
{
if(FD_ISSET(rs232_device, &rset))
{
//bytes_read = read(rs232_device, rs232_buffer, sizeof(rs232_buffer));
bytes_read = rs232_read(rs232_device);
if(bytes_read > 0)
{
if(rs232_check_last_char('\n'))
{
bytes_read = rs232_unload_rx(rs232_buffer);
if(bytes_read > 0)
{
rs232_buffer[bytes_read] = 0;
token = strtok(rs232_buffer, "\n");
while(token != NULL)
{
sprintf(nmea_message, "%s\n", token);
nmea_parse(&parser, nmea_message, (int)strlen(nmea_message), &info);
if(it > 0)
{
printf("\033[15A");
}
else
it++;
printf("Time: %i/%i/%i %i:%i:%i.%i\n", info.utc.day, info.utc.mon + 1, info.utc.year + 1900, info.utc.hour, info.utc.min, info.utc.sec, info.utc.hsec);
printf("Signal: %i\n", info.sig);
printf("Operating Mode: %i\n", info.fix);
printf("Position Diluition of Precision: %f\n", info.PDOP);
printf("Horizontal Diluition of Precision: %f\n", info.HDOP);
printf("Vertical Diluition of Precisione: %f\n", info.VDOP);
printf("Latitude: %f\n", info.lat);
printf("Longitude: %f\n", info.lon);
printf("Elevation: %f m\n", info.elv);
printf("Speed: %f km/h\n", info.speed);
printf("Direction: %f degrees\n", info.direction);
printf("Magnetic variation degrees: %f\n", info.declination);
printf("\nSatellite: \tin view: %i\n\t\tin use: %i\n", info.satinfo.inview, info.satinfo.inuse);
token = strtok(NULL, "\n");
}
}
}
}
}
}
//int smask; /**< Mask specifying types of packages from which data have been obtained */
//nmeaTIME utc; /**< UTC of position */
//int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */
//int fix; /**< Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */
//double PDOP; /**< Position Dilution Of Precision */
//double HDOP; /**< Horizontal Dilution Of Precision */
//double VDOP; /**< Vertical Dilution Of Precision */
//double lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */
//double lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */
//double elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */
//double speed; /**< Speed over the ground in kilometers/hour */
//double direction; /**< Track angle in degrees True */
//double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */
//nmeaSATINFO satinfo; /**< Satellites information */
/*it++;
if(it > 6)
done = 1;
select_timeout.tv_sec = 1;
select_timeout.tv_usec = 0;*/
}
nmea_parser_destroy(&parser);
return 0;
}
int parse_gps(char * database, int tripid) {
int retval;
// Open database connection
retval = sqlite3_open(database, &handle);
if (retval) {
printf("GPS Parsing: Database connection failed: %d\n", retval);
return -1;
}
sqlite3_exec(handle, "PRAGMA synchronous = NORMAL", 0, 0, 0);
sqlite3_exec(handle, "PRAGMA journal_mode = MEMORY", 0, 0, 0);
// Select data from database
char selectquery[100];
sprintf(selectquery,
"SELECT Gps_ID, RawData FROM GpsData WHERE Trip_ID = %d", tripid);
sqlite3_stmt *stmt;
retval = sqlite3_prepare_v2(handle, selectquery, -1, &stmt, 0);
if (retval) {
printf("GPS Parsing: Selecting data from DB Failed: %d\n", retval);
printf("Query: %s\n", selectquery);
return -1;
}
// Initialize NMEA parser
nmeaINFO info;
nmeaPARSER parser;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
// Begin SQL transaction
sqlite3_exec(handle, "BEGIN TRANSACTION", 0, 0, 0);
// Step through SELECT query
while (1) {
retval = sqlite3_step(stmt);
if (retval == SQLITE_ROW) {
// Gather row from database
int gpsid = (int) sqlite3_column_int(stmt, 0);
char *rawdata = (char*) sqlite3_column_text(stmt, 1);
int len = (int) strlen(rawdata);
// Parse NMEA data
nmea_parse(&parser, rawdata, len, &info);
// Convert time format to C_time
struct tm converttime;
converttime.tm_year = info.utc.year;
converttime.tm_mon = info.utc.mon;
converttime.tm_mday = info.utc.day;
converttime.tm_hour = info.utc.hour;
converttime.tm_min = info.utc.min;
converttime.tm_sec = info.utc.sec;
// Update row with NMEA data
char query[200];
sprintf(
query,
"UPDATE GpsData SET UTC = %f, Fix = %d, Latitude = %f, Longitude = %f, Speed = %f, Direction = %f, Declination = %f WHERE Gps_ID = %d", (float) mktime(&converttime), info.fix, nmea_ndeg2degree(info.lat), nmea_ndeg2degree(info.lon), info.speed, info.direction, info.declination, gpsid);
retval = sqlite3_exec(handle, query, 0, 0, 0);
if (retval) {
printf("GPS Parsing: Updating data in DB Failed: %d\n", retval);
return -1;
}
} else if (retval == SQLITE_DONE) {
break;
} else {
printf("GPS Parsing: SQL error whilst reading rows: %d\n", retval);
return -1;
}
}
// End SQL transaction
sqlite3_exec(handle, "END TRANSACTION", 0, 0, 0);
// Remove unnecesary data
char removequery[100];
sprintf(removequery,
"DELETE FROM GpsData WHERE RawData NOT LIKE '$GPRMC%%'");
retval = sqlite3_exec(handle, removequery, 0, 0, 0);
if (retval) {
printf("GPS Parsing: Removing data from DB Failed: %d\n", retval);
printf("Query: %s\n", removequery);
return -1;
}
// Destroy the evidence!
nmea_parser_destroy(&parser);
sqlite3_close(handle);
return 0;
}
void *nmea_loop(void *args)
{
/* Set thread name */
prctl(PR_SET_NAME, "gps nmea read", getpid());
/* Retrieve file descriptor and thread flag */
struct arg_struct *arguments = (struct arg_struct *)args;
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
/* Initialize gps stuff */
nmeaINFO info_d;
nmeaINFO *info = &info_d;
char gps_rx_buffer[NMEA_BUFFER_SIZE];
/* gps parser (nmea) */
nmeaPARSER parser;
nmea_parser_init(&parser);
nmea_zero_INFO(info);
/* advertise GPS topic */
struct vehicle_gps_position_s nmea_gps_d = {.counter=0};
nmea_gps = &nmea_gps_d;
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps);
while (!(*thread_should_exit)) {
/* Parse a message from the gps receiver */
uint8_t read_res = read_gps_nmea(fd, gps_rx_buffer, NMEA_BUFFER_SIZE, info, &parser);
if (0 == read_res) {
/* convert data, ready it for publishing */
/* convert nmea utc time to usec */
struct tm timeinfo;
timeinfo.tm_year = info->utc.year;
timeinfo.tm_mon = info->utc.mon;
timeinfo.tm_mday = info->utc.day;
timeinfo.tm_hour = info->utc.hour;
timeinfo.tm_min = info->utc.min;
timeinfo.tm_sec = info->utc.sec;
time_t epoch = mktime(&timeinfo);
// printf("%d.%d.%d %d:%d:%d:%d\n", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, info->utc.hsec);
nmea_gps->timestamp = hrt_absolute_time();
nmea_gps->time_gps_usec = (uint64_t)((epoch)*1000000 + (info->utc.hsec)*10000);
nmea_gps->fix_type = (uint8_t)info->fix;
nmea_gps->lat = (int32_t)(ndeg2degree(info->lat) * 1e7f);
nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7f);
nmea_gps->alt = (int32_t)(info->elv * 1000.0f);
nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling
nmea_gps->epv = (uint16_t)(info->VDOP * 100); //TODO:test scaling
nmea_gps->vel = (uint16_t)(info->speed * 1000 / 36); //*1000/3600*100
nmea_gps->cog = (uint16_t)info->direction*100; //nmea: degrees float, ubx/mavlink: degrees*1e2
nmea_gps->satellites_visible = (uint8_t)info->satinfo.inview;
int i = 0;
/* Write info about individual satellites */
for (i = 0; i < 12; i++) {
nmea_gps->satellite_prn[i] = (uint8_t)info->satinfo.sat[i].id;
nmea_gps->satellite_used[i] = (uint8_t)info->satinfo.sat[i].in_use;
nmea_gps->satellite_elevation[i] = (uint8_t)info->satinfo.sat[i].elv;
nmea_gps->satellite_azimuth[i] = (uint8_t)info->satinfo.sat[i].azimuth;
nmea_gps->satellite_snr[i] = (uint8_t)info->satinfo.sat[i].sig;
}
if (nmea_gps->satellites_visible > 0) {
nmea_gps->satellite_info_available = 1;
} else {
nmea_gps->satellite_info_available = 0;
}
nmea_gps->counter_pos_valid++;
nmea_gps->timestamp = hrt_absolute_time();
nmea_gps->counter++;
pthread_mutex_lock(nmea_mutex);
nmea_state->last_message_timestamp = hrt_absolute_time();
pthread_mutex_unlock(nmea_mutex);
/* publish new GPS position */
orb_publish(ORB_ID(vehicle_gps_position), gps_handle, nmea_gps);
} else if (read_res == 2) { //termination
/* de-advertise */
close(gps_handle);
break;
}
}
//destroy gps parser
nmea_parser_destroy(&parser);
if(gps_verbose) printf("[gps] nmea loop is going to terminate\n");
return NULL;
}
void *nmea_watchdog_loop(void *args)
{
/* Set thread name */
prctl(PR_SET_NAME, "gps nmea watchdog", getpid());
bool nmea_healthy = false;
uint8_t nmea_fail_count = 0;
uint8_t nmea_success_count = 0;
bool once_ok = false;
/* Retrieve file descriptor and thread flag */
struct arg_struct *arguments = (struct arg_struct *)args;
//int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
while (!(*thread_should_exit)) {
// printf("nmea_watchdog_loop : while ");
/* if we have no update for a long time print warning (in nmea mode there is no reconfigure) */
pthread_mutex_lock(nmea_mutex);
uint64_t timestamp_now = hrt_absolute_time();
bool all_okay = true;
if (timestamp_now - nmea_state->last_message_timestamp > NMEA_WATCHDOG_CRITICAL_TIME_MICROSECONDS) {
all_okay = false;
}
pthread_mutex_unlock(nmea_mutex);
if (!all_okay) {
/* gps error */
nmea_fail_count++;
// printf("nmea error, nmea_fail_count=%u\n", nmea_fail_count);
// fflush(stdout);
/* If we have too many failures and another mode or baud should be tried, exit... */
if ((gps_mode_try_all == true || gps_baud_try_all == true) && (nmea_fail_count >= NMEA_HEALTH_FAIL_COUNTER_LIMIT) && (nmea_healthy == false) && once_ok == false) {
if (gps_verbose) printf("\t[gps] no NMEA module found\n");
gps_mode_success = false;
break;
}
if (nmea_healthy && nmea_fail_count >= NMEA_HEALTH_FAIL_COUNTER_LIMIT) {
printf("\t[gps] ERROR: NMEA GPS module stopped responding\n");
// global_data_send_subsystem_info(&nmea_present_enabled);
mavlink_log_critical(mavlink_fd, "[gps] NMEA module stopped responding\n");
nmea_healthy = false;
nmea_success_count = 0;
}
fflush(stdout);
sleep(1);
} else {
/* gps healthy */
// printf("\t[gps] nmea success\n");
nmea_success_count++;
if (!nmea_healthy && nmea_success_count >= NMEA_HEALTH_SUCCESS_COUNTER_LIMIT) {
printf("[gps] NMEA module found, status ok (baud=%d)\r\n", current_gps_speed);
// global_data_send_subsystem_info(&nmea_present_enabled_healthy);
mavlink_log_info(mavlink_fd, "[gps] NMEA module found, status ok\n");
nmea_healthy = true;
nmea_fail_count = 0;
once_ok = true;
}
}
usleep(NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS);
}
if(gps_verbose) printf("[gps] nmea watchdog loop is going to terminate\n");
close(mavlink_fd);
return NULL;
}
int main()
{
const char *buff[] = {
"$GPRMC,213916.199,A,4221.0377,N,07102.9778,W,0.00,,010207,,,A*6A\r\n",
"$GPRMC,213917.199,A,4221.0510,N,07102.9549,W,0.23,175.43,010207,,,A*77\r\n",
"$GPRMC,213925.000,A,4221.1129,N,07102.9146,W,0.00,,010207,,,A*68\r\n",
"$GPRMC,111609.14,A,5001.27,N,3613.06,E,11.2,0.0,261206,0.0,E*50\r\n"
};
nmeaPOS pos[NUM_POINTS], pos_moved[NUM_POINTS][2];
double dist[NUM_POINTS][2];
double azimuth[NUM_POINTS][2], azimuth_moved[NUM_POINTS];
int result[2];
int it = 0;
nmeaPARSER parser;
nmea_parser_init(&parser);
for(it = 0; it < NUM_POINTS; ++it)
{
nmeaINFO info;
nmea_zero_INFO(&info);
(void)nmea_parse(&parser, buff[it], (int)strlen(buff[it]), &info);
nmea_info2pos(&info, &pos[it]);
}
nmea_parser_destroy(&parser);
for(it = 0; it < NUM_POINTS; ++it)
{
dist[it][0] = nmea_distance(&pos[0], &pos[it]);
dist[it][1] = nmea_distance_ellipsoid(
&pos[0], &pos[it], &azimuth[it][0], &azimuth[it][1]
);
}
for(it = 0; it < NUM_POINTS; ++it)
{
result[0] = nmea_move_horz(&pos[0], &pos_moved[it][0], azimuth[it][0], dist[it][0]);
result[1] = nmea_move_horz_ellipsoid(
&pos[0], &pos_moved[it][1], azimuth[it][0], dist[it][0], &azimuth_moved[it]
);
}
/* Output of results */
printf("Coordinate points:\n");
for(it = 0; it < NUM_POINTS; ++it)
{
printf(
"P%d in radians: lat:%9.6lf lon:%9.6lf \tin degree: lat:%+010.6lf° lon:%+011.6lf°\n",
it, pos[it].lat, pos[it].lon, nmea_radian2degree(pos[it].lat), nmea_radian2degree(pos[it].lon)
);
}
printf("\nCalculation results:\n");
for(it = 0; it < NUM_POINTS; ++it)
{
printf("\n");
printf("Distance P0 to P%d\ton spheroid: %14.3lf m\n", it, dist[it][0]);
printf("Distance P0 to P%d\ton ellipsoid: %14.3lf m\n", it, dist[it][1]);
printf("Azimuth P0 to P%d\tat start: %8.3lf°\tat end: %8.3lf°\n", it, nmea_radian2degree(azimuth[it][0]), nmea_radian2degree(azimuth[it][1]));
printf("Move P0 to P%d\t \tAzimuth at end: %8.3lf°\n", it, nmea_radian2degree(azimuth_moved[it]));
printf("Move P0 to P%d\ton spheroid: %3s lat:%+010.6lf° lon:%+011.6lf°\n", it, result[0] == 1 ? "OK" : "nOK", nmea_radian2degree(pos_moved[it][0].lat), nmea_radian2degree(pos_moved[it][0].lon));
printf("Move P0 to P%d\ton ellipsoid: %3s lat:%+010.6lf° lon:%+011.6lf°\n", it, result[0] == 1 ? "OK" : "nOK", nmea_radian2degree(pos_moved[it][1].lat), nmea_radian2degree(pos_moved[it][1].lon));
printf("Move P0 to P%d\toriginal: lat:%+010.6lf° lon:%+011.6lf°\n", it, nmea_radian2degree(pos[it].lat), nmea_radian2degree(pos[it].lon));
}
return 0;
}
// Destructor
GpsParser::~GpsParser()
{
// Destroy parser
nmea_parser_destroy(&_parser);
}
