Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

Removed old GPS app

Browse Source
This commit is contained in:
Lorenz Meier
2013-02-17 13:07:40 +01:00
parent 2d1009a897
commit 9fb024b1f2
36 changed files with 0 additions and 6718 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/gps/.context
View File
apps/gps/.gitignore
-3
View File
@@ -1,3 +0,0 @@
include
mavlink-*
pymavlink-*
apps/gps/Makefile
-59
View File
@@ -1,59 +0,0 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Makefile to build the GPS receiver application
#
APPNAME = gps
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
CSRCS = gps.c \
ubx.c \
mtk.c \
nmea_helper.c \
nmealib/context.c \
nmealib/generate.c \
nmealib/generator.c \
nmealib/gmath.c \
nmealib/info.c \
nmealib/parse.c \
nmealib/parser.c \
nmealib/sentence.c \
nmealib/time.c \
nmealib/tok.c
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
include $(APPDIR)/mk/app.mk
apps/gps/gps.c
-589
View File
File diff suppressed because it is too large Load Diff
apps/gps/gps.h
-18
View File
@@ -1,18 +0,0 @@
/*
* gps.h
*
* Created on: Mar 8, 2012
* Author: thomasgubler
*/
#ifndef GPS_H_
#define GPS_H
#include <stdbool.h>
struct arg_struct {
int *fd_ptr;
bool *thread_should_exit_ptr;
};
#endif /* GPS_H_ */
apps/gps/mtk.c
-432
View File
@@ -1,432 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* @file MTK custom binary (3DR) protocol implementation */
#include "gps.h"
#include "mtk.h"
#include <nuttx/config.h>
#include <unistd.h>
#include <sys/prctl.h>
#include <pthread.h>
#include <poll.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
#define MTK_HEALTH_SUCCESS_COUNTER_LIMIT 2
#define MTK_HEALTH_FAIL_COUNTER_LIMIT 2
// XXX decrease this substantially, it should be only a few dozen bytes max.
#warning XXX trying 128 for now
#define MTK_BUFFER_SIZE 128
pthread_mutex_t *mtk_mutex;
gps_bin_mtk_state_t *mtk_state;
static struct vehicle_gps_position_s *mtk_gps;
extern bool gps_mode_try_all;
extern bool gps_mode_success;
extern bool terminate_gps_thread;
extern bool gps_baud_try_all;
extern bool gps_verbose;
extern int current_gps_speed;
void mtk_decode_init(void)
{
mtk_state->ck_a = 0;
mtk_state->ck_b = 0;
mtk_state->rx_count = 0;
mtk_state->decode_state = MTK_DECODE_UNINIT;
mtk_state->print_errors = false;
}
void mtk_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b)
{
*(ck_a) = *(ck_a) + b;
*(ck_b) = *(ck_b) + *(ck_a);
// printf("Checksum now: %d\n",*(ck_b));
}
int mtk_parse(uint8_t b, char *gps_rx_buffer)
{
// printf("b=%x\n",b);
// Debug output to telemetry port
// PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, &b, 1);
if (mtk_state->decode_state == MTK_DECODE_UNINIT) {
if (b == 0xd0) {
mtk_state->decode_state = MTK_DECODE_GOT_CK_A;
}
} else if (mtk_state->decode_state == MTK_DECODE_GOT_CK_A) {
if (b == 0xdd) {
mtk_state->decode_state = MTK_DECODE_GOT_CK_B;
} else {
// Second start symbol was wrong, reset state machine
mtk_decode_init();
}
} else if (mtk_state->decode_state == MTK_DECODE_GOT_CK_B) {
// Add to checksum
if (mtk_state->rx_count < 33) mtk_checksum(b, &(mtk_state->ck_a), &(mtk_state->ck_b));
// Fill packet buffer
gps_rx_buffer[mtk_state->rx_count] = b;
(mtk_state->rx_count)++;
// printf("Rx count: %d\n",mtk_state->rx_count);
uint8_t ret = 0;
/* Packet size minus checksum */
if (mtk_state->rx_count >= 35) {
gps_bin_mtk_packet_t *packet = (gps_bin_mtk_packet_t *) gps_rx_buffer;
/* Check if checksum is valid */
if (mtk_state->ck_a == packet->ck_a && mtk_state->ck_b == packet->ck_b) {
mtk_gps->lat = packet->latitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7
mtk_gps->lon = packet->longitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7
mtk_gps->alt = (int32_t)(packet->msl_altitude * 10); // conversion from centimeters to millimeters, and from uint32_t to int16_t
mtk_gps->fix_type = packet->fix_type;
mtk_gps->eph = packet->hdop;
mtk_gps->epv = 65535; //unknown in mtk custom mode
mtk_gps->vel = packet->ground_speed;
mtk_gps->cog = (uint16_t)packet->heading; //mtk: degrees *1e2, mavlink/ubx: degrees *1e2
mtk_gps->satellites_visible = packet->satellites;
/* convert time and date information to unix timestamp */
struct tm timeinfo; //TODO: test this conversion
uint32_t timeinfo_conversion_temp;
timeinfo.tm_mday = packet->date * 1e-4;
timeinfo_conversion_temp = packet->date - timeinfo.tm_mday * 1e4;
timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1;
timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100;
timeinfo.tm_hour = packet->utc_time * 1e-7;
timeinfo_conversion_temp = packet->utc_time - timeinfo.tm_hour * 1e7;
timeinfo.tm_min = timeinfo_conversion_temp * 1e-5;
timeinfo_conversion_temp -= timeinfo.tm_min * 1e5;
timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3;
timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
time_t epoch = mktime(&timeinfo);
mtk_gps->timestamp = hrt_absolute_time();
mtk_gps->time_gps_usec = epoch * 1e6; //TODO: test this
mtk_gps->time_gps_usec += timeinfo_conversion_temp * 1e3;
mtk_gps->counter_pos_valid++;
mtk_gps->timestamp = hrt_absolute_time();
// printf("%lu; %lu; %d.%d.%d %d:%d:%d:%d\n", packet->date, packet->utc_time,timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, timeinfo_conversion_temp);
pthread_mutex_lock(mtk_mutex);
// printf("Write timestamp /n");
mtk_state->last_message_timestamp = hrt_absolute_time();
pthread_mutex_unlock(mtk_mutex);
ret = 1;
// printf("found package\n");
} else {
if (gps_verbose) printf("[gps] Checksum invalid\r\n");
ret = 0;
}
// Reset state machine to decode next packet
mtk_decode_init();
// printf("prepared for next state\n");
return ret;
}
}
return 0; // no valid packet found
}
int read_gps_mtk(int *fd, char *gps_rx_buffer, int buffer_size) // returns 1 if the thread should terminate
{
// printf("in read_gps_mtk\n");
uint8_t ret = 0;
uint8_t c;
int rx_count = 0;
int gpsRxOverflow = 0;
struct pollfd fds;
fds.fd = *fd;
fds.events = POLLIN;
// This blocks the task until there is something on the buffer
while (1) {
//check if the thread should terminate
if (terminate_gps_thread == true) {
// printf("terminate_gps_thread=%u ", terminate_gps_thread);
// printf("exiting mtk thread\n");
// fflush(stdout);
ret = 1;
break;
}
if (poll(&fds, 1, 1000) > 0) {
if (read(*fd, &c, 1) > 0) {
// printf("Read %x\n",c);
if (rx_count >= buffer_size) {
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxOverflow++;
rx_count = 0;
mtk_decode_init();
if (gps_verbose) printf("[gps] Buffer full\r\n");
} else {
//gps_rx_buffer[rx_count] = c;
rx_count++;
}
int msg_read = mtk_parse(c, gps_rx_buffer);
if (msg_read > 0) {
// printf("Found sequence\n");
break;
}
} else {
break;
}
} else {
break;
}
}
return ret;
}
int configure_gps_mtk(int *fd)
{
int success = 0;
size_t result_write;
result_write = write(*fd, MEDIATEK_REFRESH_RATE_10HZ, strlen(MEDIATEK_REFRESH_RATE_10HZ));
if (result_write != strlen(MEDIATEK_REFRESH_RATE_10HZ)) {
printf("[gps] Set update speed to 10 Hz failed\r\n");
success = 1;
} else {
if (gps_verbose) printf("[gps] Attempted to set update speed to 10 Hz..\r\n");
}
//set custom mode
result_write = write(*fd, MEDIATEK_CUSTOM_BINARY_MODE, strlen(MEDIATEK_CUSTOM_BINARY_MODE));
if (result_write != strlen(MEDIATEK_CUSTOM_BINARY_MODE)) {
//global_data_send_subsystem_info(&mtk_present);
printf("[gps] Set MTK custom mode failed\r\n");
success = 1;
} else {
//global_data_send_subsystem_info(&mtk_present_enabled);
if (gps_verbose) printf("[gps] Attempted to set MTK custom mode..\r\n");
}
return success;
}
void *mtk_loop(void *args)
{
// int oldstate;
// pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, oldstate);
//
// printf("in mtk loop\n");
/* Set thread name */
prctl(PR_SET_NAME, "gps mtk 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 */
// int buffer_size = 1000;
// char * gps_rx_buffer = malloc(buffer_size*sizeof(char));
char gps_rx_buffer[MTK_BUFFER_SIZE];
/* set parameters for mtk custom */
if (configure_gps_mtk(fd) != 0) {
printf("[gps] Could not write serial port..\r\n");
/* Write shared variable sys_status */
//global_data_send_subsystem_info(&mtk_present);
} else {
if (gps_verbose) printf("[gps] Configuration finished, awaiting GPS data..\r\n");
/* Write shared variable sys_status */
//global_data_send_subsystem_info(&mtk_present_enabled);
}
/* advertise GPS topic */
struct vehicle_gps_position_s mtk_gps_d;
mtk_gps = &mtk_gps_d;
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), mtk_gps);
while (!(*thread_should_exit)) {
/* Parse a message from the gps receiver */
if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) {
/* publish new GPS position */
orb_publish(ORB_ID(vehicle_gps_position), gps_handle, mtk_gps);
} else {
/* de-advertise */
close(gps_handle);
break;
}
}
close(gps_handle);
if(gps_verbose) printf("[gps] mtk loop is going to terminate\n");
return NULL;
}
void *mtk_watchdog_loop(void *args)
{
// printf("in mtk watchdog loop\n");
fflush(stdout);
/* Set thread name */
prctl(PR_SET_NAME, "gps mtk watchdog", 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;
bool mtk_healthy = false;
uint8_t mtk_fail_count = 0;
uint8_t mtk_success_count = 0;
bool once_ok = false;
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
while (!(*thread_should_exit)) {
fflush(stdout);
/* if we have no update for a long time reconfigure gps */
pthread_mutex_lock(mtk_mutex);
uint64_t timestamp_now = hrt_absolute_time();
bool all_okay = true;
if (timestamp_now - mtk_state->last_message_timestamp > MTK_WATCHDOG_CRITICAL_TIME_MICROSECONDS) {
all_okay = false;
}
pthread_mutex_unlock(mtk_mutex);
if (!all_okay) {
// printf("mtk unhealthy\n");
mtk_fail_count++;
/* gps error */
// if (err_skip_counter == 0)
// {
// printf("[gps] GPS module not connected or not responding..\n");
// err_skip_counter = 20;
// }
// err_skip_counter--;
// printf("gps_mode_try_all =%u, mtk_fail_count=%u, mtk_healthy=%u, once_ok=%u\n", gps_mode_try_all, mtk_fail_count, mtk_healthy, once_ok);
/* 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) && (mtk_fail_count >= MTK_HEALTH_FAIL_COUNTER_LIMIT) && (mtk_healthy == false) && once_ok == false) {
if (gps_verbose) printf("[gps] Connection attempt failed, no MTK module found\r\n");
gps_mode_success = false;
break;
}
if (mtk_healthy && mtk_fail_count >= MTK_HEALTH_FAIL_COUNTER_LIMIT) {
printf("[gps] ERROR: MTK GPS module stopped responding\r\n");
// global_data_send_subsystem_info(&mtk_present_enabled);
mavlink_log_critical(mavlink_fd, "[gps] MTK module stopped responding\n");
mtk_healthy = false;
mtk_success_count = 0;
}
/* trying to reconfigure the gps configuration */
configure_gps_mtk(fd);
fflush(stdout);
} else {
/* gps healthy */
mtk_success_count++;
mtk_fail_count = 0;
once_ok = true; // XXX Should this be true on a single success, or on same criteria as mtk_healthy?
if (!mtk_healthy && mtk_success_count >= MTK_HEALTH_SUCCESS_COUNTER_LIMIT) {
printf("[gps] MTK module found, status ok (baud=%d)\r\n", current_gps_speed);
/* MTK never has sat info */
// XXX Check if lock makes sense here
mtk_gps->satellite_info_available = 0;
// global_data_send_subsystem_info(&mtk_present_enabled_healthy);
mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n");
mtk_healthy = true;
}
}
usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);
}
if(gps_verbose) printf("[gps] mtk watchdog is going to terminate\n");
close(mavlink_fd);
return NULL;
}
apps/gps/mtk.h
-98
View File
@@ -1,98 +0,0 @@
/*
* mtk.h
*
* Created on: Mar 6, 2012
* Author: thomasgubler
*/
#ifndef MTK_H_
#define MTK_H_
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <stdbool.h>
#include <unistd.h>
#include <pthread.h>
//Definition for mtk custom mode
#define MEDIATEK_REFRESH_RATE_4HZ "$PMTK220,250*29\r\n" //refresh rate - 4Hz - 250 milliseconds
#define MEDIATEK_REFRESH_RATE_5HZ "$PMTK220,200*2C\r\n"
#define MEDIATEK_REFRESH_RATE_10HZ "$PMTK220,100*2F\r\n" //refresh rate - 10Hz - 100 milliseconds
#define MEDIATEK_FACTORY_RESET "$PMTK104*37\r\n" //clear current settings
#define MEDIATEK_CUSTOM_BINARY_MODE "$PGCMD,16,0,0,0,0,0*6A\r\n"
#define MEDIATEK_FULL_COLD_RESTART "$PMTK104*37\r\n"
//#define NMEA_GGA_ENABLE "$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0*27\r\n" //Set GGA messages
//definitions for watchdog
#define MTK_WATCHDOG_CRITICAL_TIME_MICROSECONDS 2000000
#define MTK_WATCHDOG_WAIT_TIME_MICROSECONDS 800000
// ************
// the structure of the binary packet
typedef struct {
uint8_t payload; ///< Number of payload bytes
int32_t latitude; ///< Latitude in degrees * 10^7
int32_t longitude; ///< Longitude in degrees * 10^7
uint32_t msl_altitude; ///< MSL altitude in meters * 10^2
uint32_t ground_speed; ///< FIXME SPEC UNCLEAR
int32_t heading;
uint8_t satellites;
uint8_t fix_type;
uint32_t date;
uint32_t utc_time;
uint16_t hdop;
uint8_t ck_a;
uint8_t ck_b;
} __attribute__((__packed__)) type_gps_bin_mtk_packet;
typedef type_gps_bin_mtk_packet gps_bin_mtk_packet_t;
enum MTK_DECODE_STATES {
MTK_DECODE_UNINIT = 0,
MTK_DECODE_GOT_CK_A = 1,
MTK_DECODE_GOT_CK_B = 2
};
typedef struct {
union {
uint16_t ck;
struct {
uint8_t ck_a;
uint8_t ck_b;
};
};
uint8_t decode_state;
// bool new_data;
// uint8_t fix;
bool print_errors;
int16_t rx_count;
uint64_t last_message_timestamp;
} __attribute__((__packed__)) type_gps_bin_mtk_state;
typedef type_gps_bin_mtk_state gps_bin_mtk_state_t;
extern pthread_mutex_t *mtk_mutex;
extern gps_bin_mtk_state_t *mtk_state;
void mtk_decode_init(void);
void mtk_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b);
int mtk_parse(uint8_t b, char *gps_rx_buffer);
int read_gps_mtk(int *fd, char *gps_rx_buffer, int buffer_size);
int configure_gps_mtk(int *fd);
void *mtk_loop(void *args);
void *mtk_watchdog_loop(void *args);
#endif /* MTK_H_ */
apps/gps/nmea_helper.c
-345
View File
@@ -1,345 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* @file NMEA protocol implementation */
#include "gps.h"
#include "nmea_helper.h"
#include <sys/prctl.h>
#include <unistd.h>
#include <poll.h>
#include <fcntl.h>
#include <unistd.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
#include <drivers/drv_hrt.h>
#define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2
#define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2
#define NMEA_BUFFER_SIZE 1000
pthread_mutex_t *nmea_mutex;
gps_bin_nmea_state_t *nmea_state;
static struct vehicle_gps_position_s *nmea_gps;
extern bool gps_mode_try_all;
extern bool gps_mode_success;
extern bool terminate_gps_thread;
extern bool gps_baud_try_all;
extern bool gps_verbose;
extern int current_gps_speed;
int read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser)
{
int ret = 1;
char c;
int start_flag = 0;
int found_cr = 0;
int rx_count = 0;
int gpsRxOverflow = 0;
struct pollfd fds;
fds.fd = *fd;
fds.events = POLLIN;
// NMEA or SINGLE-SENTENCE GPS mode
while (1) {
//check if the thread should terminate
if (terminate_gps_thread == true) {
// printf("terminate_gps_thread=%u ", terminate_gps_thread);
// printf("exiting mtk thread\n");
// fflush(stdout);
ret = 2;
break;
}
if (poll(&fds, 1, 1000) > 0) {
if (read(*fd, &c, 1) > 0) {
// detect start while acquiring stream
// printf("Char = %c\n", c);
if (!start_flag && (c == '$')) {
start_flag = 1;
found_cr = 0;
rx_count = 0;
} else if (!start_flag) { // keep looking for start sign
continue;
}
if (rx_count >= buffer_size) {
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxOverflow++;
start_flag = 0;
found_cr = 0;
rx_count = 0;
if (gps_verbose) printf("\t[gps] Buffer full\n");
} else {
// store chars in buffer
gps_rx_buffer[rx_count] = c;
rx_count++;
}
// look for carriage return CR
if (start_flag && c == 0x0d) {
found_cr = 1;
}
// and then look for line feed LF
if (start_flag && found_cr && c == 0x0a) {
// parse one NMEA line, use buffer up to rx_count
if (nmea_parse(parser, gps_rx_buffer, rx_count, info) > 0) {
ret = 0;
}
break;
}
} else {
break;
}
} else {
break;
}
}
// As soon as one NMEA message has been parsed, we break out of the loop and end here
return(ret);
}
/**
* \brief Convert NDEG (NMEA degree) to fractional degree
*/
float ndeg2degree(float val)
{
float deg = ((int)(val / 100));
val = deg + (val - deg * 100) / 60;
return val;
}
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;
}
apps/gps/nmea_helper.h
-47
View File
@@ -1,47 +0,0 @@
/*
* nmea_helper.h
*
* Created on: Mar 15, 2012
* Author: thomasgubler
*/
#ifndef NMEA_H_
#define NMEA_H_
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "nmealib/nmea/nmea.h"
//definitions for watchdog
#define NMEA_WATCHDOG_CRITICAL_TIME_MICROSECONDS 2000000
#define NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS 800000
typedef struct {
uint64_t last_message_timestamp;
} __attribute__((__packed__)) type_gps_bin_nmea_state;
typedef type_gps_bin_nmea_state gps_bin_nmea_state_t;
extern gps_bin_nmea_state_t *nmea_state;
extern pthread_mutex_t *nmea_mutex;
int read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser);
void *nmea_loop(void *arg);
void *nmea_watchdog_loop(void *arg);
/**
* \brief Convert NDEG (NMEA degree) to fractional degree
*/
float ndeg2degree(float val);
void nmea_init(void);
#endif /* NMEA_H_ */
apps/gps/nmealib/LICENSE.TXT
-506
View File
File diff suppressed because it is too large Load Diff
apps/gps/nmealib/README.TXT
-26
View File
@@ -1,26 +0,0 @@
NMEA library see: http://nmea.sourceforge.net/
Disclaimer
The National Marine Electronics Association (NMEA) has developed a specification that defines the interface between various pieces of marine electronic equipment. The standard permits marine electronics to send information to computers and to other marine equipment.
Most computer programs that provide real time position information understand and expect data to be in NMEA format. This data includes the complete PVT (position, velocity, time) solution computed by the GPS receiver. The idea of NMEA is to send a line of data called a sentence that is totally self contained and independent from other sentences. All NMEA sentences is sequences of ACSII symbols begins with a '$' and ends with a carriage return/line feed sequence and can be no longer than 80 characters of visible text (plus the line terminators).
Introduction
We present library in 'C' programming language for work with NMEA protocol. Small and easy to use. The library build on different compilers under different platforms (see below). The code was tested in real projects. Just download and try...
Features
- Analysis NMEA sentences and granting GPS data in C structures
- Generate NMEA sentences
- Supported sentences: GPGGA, GPGSA, GPGSV, GPRMC, GPVTG
- Multilevel architecture of algorithms
- Additional functions of geographical mathematics and work with navigation data
Supported (tested) platforms
- Microsoft Windows (MS Visual Studio 8.0, GCC)
- Windows Mobile, Windows CE (MS Visual Studio 8.0)
- UNIX (GCC)
Licence: LGPL
apps/gps/nmealib/context.c
-67
View File
@@ -1,67 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: context.c 17 2008-03-11 11:56:11Z xtimor $
*
*/
#include "nmea/context.h"
#include <string.h>
#include <stdarg.h>
#include <stdio.h>
nmeaPROPERTY * nmea_property(void)
{
static nmeaPROPERTY prop = {
0, 0, NMEA_DEF_PARSEBUFF
};
return &prop;
}
void nmea_trace(const char *str, ...)
{
int size;
va_list arg_list;
char buff[NMEA_DEF_PARSEBUFF];
nmeaTraceFunc func = nmea_property()->trace_func;
if(func)
{
va_start(arg_list, str);
size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list);
va_end(arg_list);
if(size > 0)
(*func)(&buff[0], size);
}
}
void nmea_trace_buff(const char *buff, int buff_size)
{
nmeaTraceFunc func = nmea_property()->trace_func;
if(func && buff_size)
(*func)(buff, buff_size);
}
void nmea_error(const char *str, ...)
{
int size;
va_list arg_list;
char buff[NMEA_DEF_PARSEBUFF];
nmeaErrorFunc func = nmea_property()->error_func;
if(func)
{
va_start(arg_list, str);
size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list);
va_end(arg_list);
if(size > 0)
(*func)(&buff[0], size);
}
}
apps/gps/nmealib/generate.c
-229
View File
@@ -1,229 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: generate.c 17 2008-03-11 11:56:11Z xtimor $
*
*/
#include "nmea/tok.h"
#include "nmea/sentence.h"
#include "nmea/generate.h"
#include "nmea/units.h"
#include <string.h>
#include <stdlib.h>
#include <math.h>
int nmea_gen_GPGGA(char *buff, int buff_sz, nmeaGPGGA *pack)
{
return nmea_printf(buff, buff_sz,
"$GPGGA,%02d%02d%02d.%02d,%07.4f,%C,%07.4f,%C,%1d,%02d,%03.1f,%03.1f,%C,%03.1f,%C,%03.1f,%04d",
pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec,
pack->lat, pack->ns, pack->lon, pack->ew,
pack->sig, pack->satinuse, pack->HDOP, pack->elv, pack->elv_units,
pack->diff, pack->diff_units, pack->dgps_age, pack->dgps_sid);
}
int nmea_gen_GPGSA(char *buff, int buff_sz, nmeaGPGSA *pack)
{
return nmea_printf(buff, buff_sz,
"$GPGSA,%C,%1d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%03.1f,%03.1f,%03.1f",
pack->fix_mode, pack->fix_type,
pack->sat_prn[0], pack->sat_prn[1], pack->sat_prn[2], pack->sat_prn[3], pack->sat_prn[4], pack->sat_prn[5],
pack->sat_prn[6], pack->sat_prn[7], pack->sat_prn[8], pack->sat_prn[9], pack->sat_prn[10], pack->sat_prn[11],
pack->PDOP, pack->HDOP, pack->VDOP);
}
int nmea_gen_GPGSV(char *buff, int buff_sz, nmeaGPGSV *pack)
{
return nmea_printf(buff, buff_sz,
"$GPGSV,%1d,%1d,%02d,"
"%02d,%02d,%03d,%02d,"
"%02d,%02d,%03d,%02d,"
"%02d,%02d,%03d,%02d,"
"%02d,%02d,%03d,%02d",
pack->pack_count, pack->pack_index + 1, pack->sat_count,
pack->sat_data[0].id, pack->sat_data[0].elv, pack->sat_data[0].azimuth, pack->sat_data[0].sig,
pack->sat_data[1].id, pack->sat_data[1].elv, pack->sat_data[1].azimuth, pack->sat_data[1].sig,
pack->sat_data[2].id, pack->sat_data[2].elv, pack->sat_data[2].azimuth, pack->sat_data[2].sig,
pack->sat_data[3].id, pack->sat_data[3].elv, pack->sat_data[3].azimuth, pack->sat_data[3].sig);
}
int nmea_gen_GPRMC(char *buff, int buff_sz, nmeaGPRMC *pack)
{
return nmea_printf(buff, buff_sz,
"$GPRMC,%02d%02d%02d.%02d,%C,%07.4f,%C,%07.4f,%C,%03.1f,%03.1f,%02d%02d%02d,%03.1f,%C,%C",
pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec,
pack->status, pack->lat, pack->ns, pack->lon, pack->ew,
pack->speed, pack->direction,
pack->utc.day, pack->utc.mon + 1, pack->utc.year - 100,
pack->declination, pack->declin_ew, pack->mode);
}
int nmea_gen_GPVTG(char *buff, int buff_sz, nmeaGPVTG *pack)
{
return nmea_printf(buff, buff_sz,
"$GPVTG,%.1f,%C,%.1f,%C,%.1f,%C,%.1f,%C",
pack->dir, pack->dir_t,
pack->dec, pack->dec_m,
pack->spn, pack->spn_n,
pack->spk, pack->spk_k);
}
void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack)
{
nmea_zero_GPGGA(pack);
pack->utc = info->utc;
pack->lat = fabs(info->lat);
pack->ns = ((info->lat > 0)?'N':'S');
pack->lon = fabs(info->lon);
pack->ew = ((info->lon > 0)?'E':'W');
pack->sig = info->sig;
pack->satinuse = info->satinfo.inuse;
pack->HDOP = info->HDOP;
pack->elv = info->elv;
}
void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack)
{
int it;
nmea_zero_GPGSA(pack);
pack->fix_type = info->fix;
pack->PDOP = info->PDOP;
pack->HDOP = info->HDOP;
pack->VDOP = info->VDOP;
for(it = 0; it < NMEA_MAXSAT; ++it)
{
pack->sat_prn[it] =
((info->satinfo.sat[it].in_use)?info->satinfo.sat[it].id:0);
}
}
int nmea_gsv_npack(int sat_count)
{
int pack_count = (int)ceil(((float)sat_count) / NMEA_SATINPACK);
if(0 == pack_count)
pack_count = 1;
return pack_count;
}
void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx)
{
int sit, pit;
nmea_zero_GPGSV(pack);
pack->sat_count = (info->satinfo.inview <= NMEA_MAXSAT)?info->satinfo.inview:NMEA_MAXSAT;
pack->pack_count = nmea_gsv_npack(pack->sat_count);
if(pack->pack_count == 0)
pack->pack_count = 1;
if(pack_idx >= pack->pack_count)
pack->pack_index = pack_idx % pack->pack_count;
else
pack->pack_index = pack_idx;
for(pit = 0, sit = pack->pack_index * NMEA_SATINPACK; pit < NMEA_SATINPACK; ++pit, ++sit)
pack->sat_data[pit] = info->satinfo.sat[sit];
}
void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack)
{
nmea_zero_GPRMC(pack);
pack->utc = info->utc;
pack->status = ((info->sig > 0)?'A':'V');
pack->lat = fabs(info->lat);
pack->ns = ((info->lat > 0)?'N':'S');
pack->lon = fabs(info->lon);
pack->ew = ((info->lon > 0)?'E':'W');
pack->speed = info->speed / NMEA_TUD_KNOTS;
pack->direction = info->direction;
pack->declination = info->declination;
pack->declin_ew = 'E';
pack->mode = ((info->sig > 0)?'A':'N');
}
void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack)
{
nmea_zero_GPVTG(pack);
pack->dir = info->direction;
pack->dec = info->declination;
pack->spn = info->speed / NMEA_TUD_KNOTS;
pack->spk = info->speed;
}
int nmea_generate(
char *buff, int buff_sz,
const nmeaINFO *info,
int generate_mask
)
{
int gen_count = 0, gsv_it, gsv_count;
int pack_mask = generate_mask;
nmeaGPGGA gga;
nmeaGPGSA gsa;
nmeaGPGSV gsv;
nmeaGPRMC rmc;
nmeaGPVTG vtg;
if(!buff)
return 0;
while(pack_mask)
{
if(pack_mask & GPGGA)
{
nmea_info2GPGGA(info, &gga);
gen_count += nmea_gen_GPGGA(buff + gen_count, buff_sz - gen_count, &gga);
pack_mask &= ~GPGGA;
}
else if(pack_mask & GPGSA)
{
nmea_info2GPGSA(info, &gsa);
gen_count += nmea_gen_GPGSA(buff + gen_count, buff_sz - gen_count, &gsa);
pack_mask &= ~GPGSA;
}
else if(pack_mask & GPGSV)
{
gsv_count = nmea_gsv_npack(info->satinfo.inview);
for(gsv_it = 0; gsv_it < gsv_count && buff_sz - gen_count > 0; ++gsv_it)
{
nmea_info2GPGSV(info, &gsv, gsv_it);
gen_count += nmea_gen_GPGSV(buff + gen_count, buff_sz - gen_count, &gsv);
}
pack_mask &= ~GPGSV;
}
else if(pack_mask & GPRMC)
{
nmea_info2GPRMC(info, &rmc);
gen_count += nmea_gen_GPRMC(buff + gen_count, buff_sz - gen_count, &rmc);
pack_mask &= ~GPRMC;
}
else if(pack_mask & GPVTG)
{
nmea_info2GPVTG(info, &vtg);
gen_count += nmea_gen_GPVTG(buff + gen_count, buff_sz - gen_count, &vtg);
pack_mask &= ~GPVTG;
}
else
break;
if(buff_sz - gen_count <= 0)
break;
}
return gen_count;
}
apps/gps/nmealib/generator.c
-399
View File
@@ -1,399 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: generator.c 17 2008-03-11 11:56:11Z xtimor $
*
*/
#include "nmea/gmath.h"
#include "nmea/generate.h"
#include "nmea/generator.h"
#include "nmea/context.h"
#include <string.h>
#include <stdlib.h>
#if defined(NMEA_WIN) && defined(_MSC_VER)
# pragma warning(disable: 4100) /* unreferenced formal parameter */
#endif
float nmea_random(float min, float max)
{
static float rand_max = MAX_RAND;//RAND_MAX; //nuttx defines MAX_RAND instead of RAND_MAX
float rand_val = rand();
float bounds = max - min;
return min + (rand_val * bounds) / rand_max;
}
/*
* low level
*/
int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info)
{
int RetVal = 1; int smask = info->smask;
nmeaGENERATOR *igen = gen;
nmea_zero_INFO(info);
info->smask = smask;
info->lat = NMEA_DEF_LAT;
info->lon = NMEA_DEF_LON;
while(RetVal && igen)
{
if(igen->init_call)
RetVal = (*igen->init_call)(igen, info);
igen = igen->next;
}
return RetVal;
}
int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
int RetVal = 1;
if(gen->loop_call)
RetVal = (*gen->loop_call)(gen, info);
if(RetVal && gen->next)
RetVal = nmea_gen_loop(gen->next, info);
return RetVal;
}
int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info)
{
int RetVal = 1;
if(gen->reset_call)
RetVal = (*gen->reset_call)(gen, info);
return RetVal;
}
void nmea_gen_destroy(nmeaGENERATOR *gen)
{
if(gen->next)
{
nmea_gen_destroy(gen->next);
gen->next = 0;
}
if(gen->destroy_call)
(*gen->destroy_call)(gen);
free(gen);
}
void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen)
{
if(to->next)
nmea_gen_add(to->next, gen);
else
to->next = gen;
}
int nmea_generate_from(
char *buff, int buff_sz,
nmeaINFO *info,
nmeaGENERATOR *gen,
int generate_mask
)
{
int retval;
if(0 != (retval = nmea_gen_loop(gen, info)))
retval = nmea_generate(buff, buff_sz, info, generate_mask);
return retval;
}
/*
* NOISE generator
*/
int nmea_igen_noise_init(nmeaGENERATOR *gen, nmeaINFO *info)
{
return 1;
}
int nmea_igen_noise_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
int it;
int in_use;
info->sig = (int)nmea_random(1, 3);
info->PDOP = nmea_random(0, 9);
info->HDOP = nmea_random(0, 9);
info->VDOP = nmea_random(0, 9);
info->fix = (int)nmea_random(2, 3);
info->lat = nmea_random(0, 100);
info->lon = nmea_random(0, 100);
info->speed = nmea_random(0, 100);
info->direction = nmea_random(0, 360);
info->declination = nmea_random(0, 360);
info->elv = (int)nmea_random(-100, 100);
info->satinfo.inuse = 0;
info->satinfo.inview = 0;
for(it = 0; it < 12; ++it)
{
info->satinfo.sat[it].id = it;
info->satinfo.sat[it].in_use = in_use = (int)nmea_random(0, 3);
info->satinfo.sat[it].elv = (int)nmea_random(0, 90);
info->satinfo.sat[it].azimuth = (int)nmea_random(0, 359);
info->satinfo.sat[it].sig = (int)(in_use?nmea_random(40, 99):nmea_random(0, 40));
if(in_use)
info->satinfo.inuse++;
if(info->satinfo.sat[it].sig > 0)
info->satinfo.inview++;
}
return 1;
}
int nmea_igen_noise_reset(nmeaGENERATOR *gen, nmeaINFO *info)
{
return 1;
}
/*
* STATIC generator
*/
int nmea_igen_static_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
nmea_time_now(&info->utc);
return 1;
};
int nmea_igen_static_reset(nmeaGENERATOR *gen, nmeaINFO *info)
{
info->satinfo.inuse = 4;
info->satinfo.inview = 4;
info->satinfo.sat[0].id = 1;
info->satinfo.sat[0].in_use = 1;
info->satinfo.sat[0].elv = 50;
info->satinfo.sat[0].azimuth = 0;
info->satinfo.sat[0].sig = 99;
info->satinfo.sat[1].id = 2;
info->satinfo.sat[1].in_use = 1;
info->satinfo.sat[1].elv = 50;
info->satinfo.sat[1].azimuth = 90;
info->satinfo.sat[1].sig = 99;
info->satinfo.sat[2].id = 3;
info->satinfo.sat[2].in_use = 1;
info->satinfo.sat[2].elv = 50;
info->satinfo.sat[2].azimuth = 180;
info->satinfo.sat[2].sig = 99;
info->satinfo.sat[3].id = 4;
info->satinfo.sat[3].in_use = 1;
info->satinfo.sat[3].elv = 50;
info->satinfo.sat[3].azimuth = 270;
info->satinfo.sat[3].sig = 99;
return 1;
}
int nmea_igen_static_init(nmeaGENERATOR *gen, nmeaINFO *info)
{
info->sig = 3;
info->fix = 3;
nmea_igen_static_reset(gen, info);
return 1;
}
/*
* SAT_ROTATE generator
*/
int nmea_igen_rotate_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
int it;
int count = info->satinfo.inview;
float deg = 360 / (count?count:1);
float srt = (count?(info->satinfo.sat[0].azimuth):0) + 5;
nmea_time_now(&info->utc);
for(it = 0; it < count; ++it)
{
info->satinfo.sat[it].azimuth =
(int)((srt >= 360)?srt - 360:srt);
srt += deg;
}
return 1;
};
int nmea_igen_rotate_reset(nmeaGENERATOR *gen, nmeaINFO *info)
{
int it;
float deg = 360 / 8;
float srt = 0;
info->satinfo.inuse = 8;
info->satinfo.inview = 8;
for(it = 0; it < info->satinfo.inview; ++it)
{
info->satinfo.sat[it].id = it + 1;
info->satinfo.sat[it].in_use = 1;
info->satinfo.sat[it].elv = 5;
info->satinfo.sat[it].azimuth = (int)srt;
info->satinfo.sat[it].sig = 80;
srt += deg;
}
return 1;
}
int nmea_igen_rotate_init(nmeaGENERATOR *gen, nmeaINFO *info)
{
info->sig = 3;
info->fix = 3;
nmea_igen_rotate_reset(gen, info);
return 1;
}
/*
* POS_RANDMOVE generator
*/
int nmea_igen_pos_rmove_init(nmeaGENERATOR *gen, nmeaINFO *info)
{
info->sig = 3;
info->fix = 3;
info->direction = info->declination = 0;
info->speed = 20;
return 1;
}
int nmea_igen_pos_rmove_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
nmeaPOS crd;
info->direction += nmea_random(-10, 10);
info->speed += nmea_random(-2, 3);
if(info->direction < 0)
info->direction = 359 + info->direction;
if(info->direction > 359)
info->direction -= 359;
if(info->speed > 40)
info->speed = 40;
if(info->speed < 1)
info->speed = 1;
nmea_info2pos(info, &crd);
nmea_move_horz(&crd, &crd, info->direction, info->speed / 3600);
nmea_pos2info(&crd, info);
info->declination = info->direction;
return 1;
};
int nmea_igen_pos_rmove_destroy(nmeaGENERATOR *gen)
{
return 1;
};
/*
* generator create
*/
nmeaGENERATOR * __nmea_create_generator(int type, nmeaINFO *info)
{
nmeaGENERATOR *gen = 0;
switch(type)
{
case NMEA_GEN_NOISE:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_noise_init;
gen->loop_call = &nmea_igen_noise_loop;
gen->reset_call = &nmea_igen_noise_reset;
}
break;
case NMEA_GEN_STATIC:
case NMEA_GEN_SAT_STATIC:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_static_init;
gen->loop_call = &nmea_igen_static_loop;
gen->reset_call = &nmea_igen_static_reset;
}
break;
case NMEA_GEN_SAT_ROTATE:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_rotate_init;
gen->loop_call = &nmea_igen_rotate_loop;
gen->reset_call = &nmea_igen_rotate_reset;
}
break;
case NMEA_GEN_POS_RANDMOVE:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_pos_rmove_init;
gen->loop_call = &nmea_igen_pos_rmove_loop;
gen->destroy_call = &nmea_igen_pos_rmove_destroy;
}
break;
case NMEA_GEN_ROTATE:
gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info);
nmea_gen_add(gen, __nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info));
break;
};
return gen;
}
nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info)
{
nmeaGENERATOR *gen = __nmea_create_generator(type, info);
if(gen)
nmea_gen_init(gen, info);
return gen;
}
void nmea_destroy_generator(nmeaGENERATOR *gen)
{
nmea_gen_destroy(gen);
}
#if defined(NMEA_WIN) && defined(_MSC_VER)
# pragma warning(default: 4100)
#endif
apps/gps/nmealib/gmath.c
-376
View File
@@ -1,376 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: gmath.c 17 2008-03-11 11:56:11Z xtimor $
*
*/
/*! \file gmath.h */
#include "nmea/gmath.h"
#include <math.h>
#include <float.h>
/**
* \fn nmea_degree2radian
* \brief Convert degree to radian
*/
float nmea_degree2radian(float val)
{ return (val * NMEA_PI180); }
/**
* \fn nmea_radian2degree
* \brief Convert radian to degree
*/
float nmea_radian2degree(float val)
{ return (val / NMEA_PI180); }
/**
* \brief Convert NDEG (NMEA degree) to fractional degree
*/
float nmea_ndeg2degree(float val)
{
float deg = ((int)(val / 100));
val = deg + (val - deg * 100) / 60;
return val;
}
/**
* \brief Convert fractional degree to NDEG (NMEA degree)
*/
float nmea_degree2ndeg(float val)
{
float int_part;
float fra_part;
fra_part = modf(val, &int_part);
val = int_part * 100 + fra_part * 60;
return val;
}
/**
* \fn nmea_ndeg2radian
* \brief Convert NDEG (NMEA degree) to radian
*/
float nmea_ndeg2radian(float val)
{ return nmea_degree2radian(nmea_ndeg2degree(val)); }
/**
* \fn nmea_radian2ndeg
* \brief Convert radian to NDEG (NMEA degree)
*/
float nmea_radian2ndeg(float val)
{ return nmea_degree2ndeg(nmea_radian2degree(val)); }
/**
* \brief Calculate PDOP (Position Dilution Of Precision) factor
*/
float nmea_calc_pdop(float hdop, float vdop)
{
return sqrt(pow(hdop, 2) + pow(vdop, 2));
}
float nmea_dop2meters(float dop)
{ return (dop * NMEA_DOP_FACTOR); }
float nmea_meters2dop(float meters)
{ return (meters / NMEA_DOP_FACTOR); }
/**
* \brief Calculate distance between two points
* \return Distance in meters
*/
float nmea_distance(
const nmeaPOS *from_pos, /**< From position in radians */
const nmeaPOS *to_pos /**< To position in radians */
)
{
float dist = ((float)NMEA_EARTHRADIUS_M) * acos(
sin(to_pos->lat) * sin(from_pos->lat) +
cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon)
);
return dist;
}
/**
* \brief Calculate distance between two points
* This function uses an algorithm for an oblate spheroid earth model.
* The algorithm is described here:
* http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
* \return Distance in meters
*/
float nmea_distance_ellipsoid(
const nmeaPOS *from_pos, /**< From position in radians */
const nmeaPOS *to_pos, /**< To position in radians */
float *from_azimuth, /**< (O) azimuth at "from" position in radians */
float *to_azimuth /**< (O) azimuth at "to" position in radians */
)
{
/* All variables */
float f, a, b, sqr_a, sqr_b;
float L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
float sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda;
int remaining_steps;
float sqr_u, A, B, delta_sigma;
/* Check input */
//NMEA_ASSERT(from_pos != 0);
//NMEA_ASSERT(to_pos != 0);
if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon))
{ /* Identical points */
if ( from_azimuth != 0 )
*from_azimuth = 0;
if ( to_azimuth != 0 )
*to_azimuth = 0;
return 0;
} /* Identical points */
/* Earth geometry */
f = NMEA_EARTH_FLATTENING;
a = NMEA_EARTH_SEMIMAJORAXIS_M;
b = (1 - f) * a;
sqr_a = a * a;
sqr_b = b * b;
/* Calculation */
L = to_pos->lon - from_pos->lon;
phi1 = from_pos->lat;
phi2 = to_pos->lat;
U1 = atan((1 - f) * tan(phi1));
U2 = atan((1 - f) * tan(phi2));
sin_U1 = sin(U1);
sin_U2 = sin(U2);
cos_U1 = cos(U1);
cos_U2 = cos(U2);
/* Initialize iteration */
sigma = 0;
sin_sigma = sin(sigma);
cos_sigma = cos(sigma);
cos_2_sigmam = 0;
sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
sqr_cos_alpha = 0;
lambda = L;
sin_lambda = sin(lambda);
cos_lambda = cos(lambda);
delta_lambda = lambda;
remaining_steps = 20;
while ((delta_lambda > 1e-12) && (remaining_steps > 0))
{ /* Iterate */
/* Variables */
float tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev;
/* Calculation */
tmp1 = cos_U2 * sin_lambda;
tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;
sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2);
cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
tan_sigma = sin_sigma / cos_sigma;
sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;
cos_alpha = cos(asin(sin_alpha));
sqr_cos_alpha = cos_alpha * cos_alpha;
cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
lambda_prev = lambda;
sigma = asin(sin_sigma);
lambda = L +
(1 - C) * f * sin_alpha
* (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)));
delta_lambda = lambda_prev - lambda;
if ( delta_lambda < 0 ) delta_lambda = -delta_lambda;
sin_lambda = sin(lambda);
cos_lambda = cos(lambda);
remaining_steps--;
} /* Iterate */
/* More calculation */
sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
delta_sigma = B * sin_sigma * (
cos_2_sigmam + B / 4 * (
cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
));
/* Calculate result */
if ( from_azimuth != 0 )
{
float tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
*from_azimuth = atan(tan_alpha_1);
}
if ( to_azimuth != 0 )
{
float tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
*to_azimuth = atan(tan_alpha_2);
}
return b * A * (sigma - delta_sigma);
}
/**
* \brief Horizontal move of point position
*/
int nmea_move_horz(
const nmeaPOS *start_pos, /**< Start position in radians */
nmeaPOS *end_pos, /**< Result position in radians */
float azimuth, /**< Azimuth (degree) [0, 359] */
float distance /**< Distance (km) */
)
{
nmeaPOS p1 = *start_pos;
int RetVal = 1;
distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
azimuth = nmea_degree2radian(azimuth);
end_pos->lat = asin(
sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth));
end_pos->lon = p1.lon + atan2(
sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat));
if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon))
{
end_pos->lat = 0; end_pos->lon = 0;
RetVal = 0;
}
return RetVal;
}
/**
* \brief Horizontal move of point position
* This function uses an algorithm for an oblate spheroid earth model.
* The algorithm is described here:
* http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
*/
int nmea_move_horz_ellipsoid(
const nmeaPOS *start_pos, /**< Start position in radians */
nmeaPOS *end_pos, /**< (O) Result position in radians */
float azimuth, /**< Azimuth in radians */
float distance, /**< Distance (km) */
float *end_azimuth /**< (O) Azimuth at end position in radians */
)
{
/* Variables */
float f, a, b, sqr_a, sqr_b;
float phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1;
float tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B;
float sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma;
int remaining_steps;
float tmp1, phi2, lambda, C, L;
/* Check input */
//NMEA_ASSERT(start_pos != 0);
//NMEA_ASSERT(end_pos != 0);
if (fabs(distance) < 1e-12)
{ /* No move */
*end_pos = *start_pos;
if ( end_azimuth != 0 ) *end_azimuth = azimuth;
return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
} /* No move */
/* Earth geometry */
f = NMEA_EARTH_FLATTENING;
a = NMEA_EARTH_SEMIMAJORAXIS_M;
b = (1 - f) * a;
sqr_a = a * a;
sqr_b = b * b;
/* Calculation */
phi1 = start_pos->lat;
tan_U1 = (1 - f) * tan(phi1);
cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1);
sin_U1 = tan_U1 * cos_U1;
s = distance;
alpha1 = azimuth;
sin_alpha1 = sin(alpha1);
cos_alpha1 = cos(alpha1);
tan_sigma1 = tan_U1 / cos_alpha1;
sigma1 = atan2(tan_U1, cos_alpha1);
sin_alpha = cos_U1 * sin_alpha1;
sqr_cos_alpha = 1 - sin_alpha * sin_alpha;
cos_alpha = sqrt(sqr_cos_alpha);
sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
/* Initialize iteration */
sigma_initial = s / (b * A);
sigma = sigma_initial;
sin_sigma = sin(sigma);
cos_sigma = cos(sigma);
cos_2_sigmam = cos(2 * sigma1 + sigma);
sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
delta_sigma = 0;
sigma_prev = 2 * NMEA_PI;
remaining_steps = 20;
while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0))
{ /* Iterate */
cos_2_sigmam = cos(2 * sigma1 + sigma);
sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
sin_sigma = sin(sigma);
cos_sigma = cos(sigma);
delta_sigma = B * sin_sigma * (
cos_2_sigmam + B / 4 * (
cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
));
sigma_prev = sigma;
sigma = sigma_initial + delta_sigma;
remaining_steps --;
} /* Iterate */
/* Calculate result */
tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1);
phi2 = atan2(
sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
(1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1)
);
lambda = atan2(
sin_sigma * sin_alpha1,
cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
);
C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
L = lambda -
(1 - C) * f * sin_alpha * (
sigma + C * sin_sigma *
(cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))
);
/* Result */
end_pos->lon = start_pos->lon + L;
end_pos->lat = phi2;
if ( end_azimuth != 0 )
{
*end_azimuth = atan2(
sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
);
}
return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
}
/**
* \brief Convert position from INFO to radians position
*/
void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos)
{
pos->lat = nmea_ndeg2radian(info->lat);
pos->lon = nmea_ndeg2radian(info->lon);
}
/**
* \brief Convert radians position to INFOs position
*/
void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info)
{
info->lat = nmea_radian2ndeg(pos->lat);
info->lon = nmea_radian2ndeg(pos->lon);
}
apps/gps/nmealib/info.c
-21
View File
@@ -1,21 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: info.c 17 2008-03-11 11:56:11Z xtimor $
*
*/
#include <string.h>
#include "nmea/info.h"
void nmea_zero_INFO(nmeaINFO *info)
{
memset(info, 0, sizeof(nmeaINFO));
nmea_time_now(&info->utc);
info->sig = NMEA_SIG_BAD;
info->fix = NMEA_FIX_BAD;
}
apps/gps/nmealib/nmea/config.h
-51
View File
@@ -1,51 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: config.h 17 2008-03-11 11:56:11Z xtimor $
*
*/
#ifndef __NMEA_CONFIG_H__
#define __NMEA_CONFIG_H__
#define NMEA_VERSION ("0.5.3")
#define NMEA_VERSION_MAJOR (0)
#define NMEA_VERSION_MINOR (5)
#define NMEA_VERSION_PATCH (3)
#define NMEA_CONVSTR_BUF (256)
#define NMEA_TIMEPARSE_BUF (256)
#if defined(WINCE) || defined(UNDER_CE)
# define NMEA_CE
#endif
#if defined(WIN32) || defined(NMEA_CE)
# define NMEA_WIN
#else
# define NMEA_UNI
#endif
#if defined(NMEA_WIN) && (_MSC_VER >= 1400)
# pragma warning(disable: 4996) /* declared deprecated */
#endif
#if defined(_MSC_VER)
# define NMEA_POSIX(x) _##x
# define NMEA_INLINE __inline
#else
# define NMEA_POSIX(x) x
# define NMEA_INLINE inline
#endif
#if !defined(NDEBUG) && !defined(NMEA_CE)
# include <assert.h>
# define NMEA_ASSERT(x) assert(x)
#else
# define NMEA_ASSERT(x)
#endif
#endif /* __NMEA_CONFIG_H__ */
apps/gps/nmealib/nmea/context.h
-44
View File
@@ -1,44 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: context.h 4 2007-08-27 13:11:03Z xtimor $
*
*/
#ifndef __NMEA_CONTEXT_H__
#define __NMEA_CONTEXT_H__
#include "config.h"
#define NMEA_DEF_PARSEBUFF (1024)
#define NMEA_MIN_PARSEBUFF (256)
#ifdef __cplusplus
extern "C" {
#endif
typedef void (*nmeaTraceFunc)(const char *str, int str_size);
typedef void (*nmeaErrorFunc)(const char *str, int str_size);
typedef struct _nmeaPROPERTY
{
nmeaTraceFunc trace_func;
nmeaErrorFunc error_func;
int parse_buff_size;
} nmeaPROPERTY;
nmeaPROPERTY * nmea_property(void);
void nmea_trace(const char *str, ...);
void nmea_trace_buff(const char *buff, int buff_size);
void nmea_error(const char *str, ...);
#ifdef __cplusplus
}
#endif
#endif /* __NMEA_CONTEXT_H__ */
apps/gps/nmealib/nmea/generate.h
-44
View File
@@ -1,44 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: generate.h 4 2007-08-27 13:11:03Z xtimor $
*
*/
#ifndef __NMEA_GENERATE_H__
#define __NMEA_GENERATE_H__
#include "sentence.h"
#ifdef __cplusplus
extern "C" {
#endif
int nmea_generate(
char *buff, int buff_sz, /* buffer */
const nmeaINFO *info, /* source info */
int generate_mask /* mask of sentence`s (e.g. GPGGA | GPGSA) */
);
int nmea_gen_GPGGA(char *buff, int buff_sz, nmeaGPGGA *pack);
int nmea_gen_GPGSA(char *buff, int buff_sz, nmeaGPGSA *pack);
int nmea_gen_GPGSV(char *buff, int buff_sz, nmeaGPGSV *pack);
int nmea_gen_GPRMC(char *buff, int buff_sz, nmeaGPRMC *pack);
int nmea_gen_GPVTG(char *buff, int buff_sz, nmeaGPVTG *pack);
void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack);
void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack);
void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack);
void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack);
int nmea_gsv_npack(int sat_count);
void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx);
#ifdef __cplusplus
}
#endif
#endif /* __NMEA_GENERATE_H__ */
apps/gps/nmealib/nmea/generator.h
-79
View File
@@ -1,79 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: generator.h 4 2007-08-27 13:11:03Z xtimor $
*
*/
#ifndef __NMEA_GENERATOR_H__
#define __NMEA_GENERATOR_H__
#include "info.h"
#ifdef __cplusplus
extern "C" {
#endif
/*
* high level
*/
struct _nmeaGENERATOR;
enum nmeaGENTYPE
{
NMEA_GEN_NOISE = 0,
NMEA_GEN_STATIC,
NMEA_GEN_ROTATE,
NMEA_GEN_SAT_STATIC,
NMEA_GEN_SAT_ROTATE,
NMEA_GEN_POS_RANDMOVE,
NMEA_GEN_LAST
};
struct _nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info);
void nmea_destroy_generator(struct _nmeaGENERATOR *gen);
int nmea_generate_from(
char *buff, int buff_sz, /* buffer */
nmeaINFO *info, /* source info */
struct _nmeaGENERATOR *gen, /* generator */
int generate_mask /* mask of sentence`s (e.g. GPGGA | GPGSA) */
);
/*
* low level
*/
typedef int (*nmeaNMEA_GEN_INIT)(struct _nmeaGENERATOR *gen, nmeaINFO *info);
typedef int (*nmeaNMEA_GEN_LOOP)(struct _nmeaGENERATOR *gen, nmeaINFO *info);
typedef int (*nmeaNMEA_GEN_RESET)(struct _nmeaGENERATOR *gen, nmeaINFO *info);
typedef int (*nmeaNMEA_GEN_DESTROY)(struct _nmeaGENERATOR *gen);
typedef struct _nmeaGENERATOR
{
void *gen_data;
nmeaNMEA_GEN_INIT init_call;
nmeaNMEA_GEN_LOOP loop_call;
nmeaNMEA_GEN_RESET reset_call;
nmeaNMEA_GEN_DESTROY destroy_call;
struct _nmeaGENERATOR *next;
} nmeaGENERATOR;
int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info);
int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info);
int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info);
void nmea_gen_destroy(nmeaGENERATOR *gen);
void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen);
#ifdef __cplusplus
}
#endif
#endif /* __NMEA_GENERATOR_H__ */
apps/gps/nmealib/nmea/gmath.h
-92
View File
@@ -1,92 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: gmath.h 17 2008-03-11 11:56:11Z xtimor $
*
*/
#ifndef __NMEA_GMATH_H__
#define __NMEA_GMATH_H__
#include "info.h"
#define NMEA_PI (3.141592653589793) /**< PI value */
#define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */
#define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */
#define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */
#define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */
#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */
#define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */
#define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */
#ifdef __cplusplus
extern "C" {
#endif
/*
* degree VS radian
*/
float nmea_degree2radian(float val);
float nmea_radian2degree(float val);
/*
* NDEG (NMEA degree)
*/
float nmea_ndeg2degree(float val);
float nmea_degree2ndeg(float val);
float nmea_ndeg2radian(float val);
float nmea_radian2ndeg(float val);
/*
* DOP
*/
float nmea_calc_pdop(float hdop, float vdop);
float nmea_dop2meters(float dop);
float nmea_meters2dop(float meters);
/*
* positions work
*/
void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos);
void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info);
float nmea_distance(
const nmeaPOS *from_pos,
const nmeaPOS *to_pos
);
float nmea_distance_ellipsoid(
const nmeaPOS *from_pos,
const nmeaPOS *to_pos,
float *from_azimuth,
float *to_azimuth
);
int nmea_move_horz(
const nmeaPOS *start_pos,
nmeaPOS *end_pos,
float azimuth,
float distance
);
int nmea_move_horz_ellipsoid(
const nmeaPOS *start_pos,
nmeaPOS *end_pos,
float azimuth,
float distance,
float *end_azimuth
);
#ifdef __cplusplus
}
#endif
#endif /* __NMEA_GMATH_H__ */
apps/gps/nmealib/nmea/info.h
-112
View File
@@ -1,112 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: info.h 10 2007-11-15 14:50:15Z xtimor $
*
*/
/*! \file */
#ifndef __NMEA_INFO_H__
#define __NMEA_INFO_H__
#include "time.h"
#define NMEA_SIG_BAD (0)
#define NMEA_SIG_LOW (1)
#define NMEA_SIG_MID (2)
#define NMEA_SIG_HIGH (3)
#define NMEA_FIX_BAD (1)
#define NMEA_FIX_2D (2)
#define NMEA_FIX_3D (3)
#define NMEA_MAXSAT (12)
#define NMEA_SATINPACK (4)
#define NMEA_NSATPACKS (NMEA_MAXSAT / NMEA_SATINPACK)
#define NMEA_DEF_LAT (5001.2621)
#define NMEA_DEF_LON (3613.0595)
#ifdef __cplusplus
extern "C" {
#endif
/**
* Position data in fractional degrees or radians
*/
typedef struct _nmeaPOS
{
float lat; /**< Latitude */
float lon; /**< Longitude */
} nmeaPOS;
/**
* Information about satellite
* @see nmeaSATINFO
* @see nmeaGPGSV
*/
typedef struct _nmeaSATELLITE
{
int id; /**< Satellite PRN number */
int in_use; /**< Used in position fix */
int elv; /**< Elevation in degrees, 90 maximum */
int azimuth; /**< Azimuth, degrees from true north, 000 to 359 */
int sig; /**< Signal, 00-99 dB */
} nmeaSATELLITE;
/**
* Information about all satellites in view
* @see nmeaINFO
* @see nmeaGPGSV
*/
typedef struct _nmeaSATINFO
{
int inuse; /**< Number of satellites in use (not those in view) */
int inview; /**< Total number of satellites in view */
nmeaSATELLITE sat[NMEA_MAXSAT]; /**< Satellites information */
} nmeaSATINFO;
/**
* Summary GPS information from all parsed packets,
* used also for generating NMEA stream
* @see nmea_parse
* @see nmea_GPGGA2info, nmea_...2info
*/
typedef struct _nmeaINFO
{
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) */
float PDOP; /**< Position Dilution Of Precision */
float HDOP; /**< Horizontal Dilution Of Precision */
float VDOP; /**< Vertical Dilution Of Precision */
float lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */
float lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */
float elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */
float speed; /**< Speed over the ground in kilometers/hour */
float direction; /**< Track angle in degrees True */
float declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */
nmeaSATINFO satinfo; /**< Satellites information */
} nmeaINFO;
void nmea_zero_INFO(nmeaINFO *info);
#ifdef __cplusplus
}
#endif
#endif /* __NMEA_INFO_H__ */
apps/gps/nmealib/nmea/nmea.h
-25
View File
@@ -1,25 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: nmea.h 17 2008-03-11 11:56:11Z xtimor $
*
*/
#ifndef __NMEA_H__
#define __NMEA_H__
#include "./config.h"
#include "./units.h"
#include "./gmath.h"
#include "./info.h"
#include "./sentence.h"
#include "./generate.h"
#include "./generator.h"
#include "./parse.h"
#include "./parser.h"
#include "./context.h"
#endif /* __NMEA_H__ */
apps/gps/nmealib/nmea/parse.h
-39
View File
@@ -1,39 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: parse.h 4 2007-08-27 13:11:03Z xtimor $
*
*/
#ifndef __NMEA_PARSE_H__
#define __NMEA_PARSE_H__
#include "sentence.h"
#ifdef __cplusplus
extern "C" {
#endif
int nmea_pack_type(const char *buff, int buff_sz);
int nmea_find_tail(const char *buff, int buff_sz, int *res_crc);
int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack);
int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack);
int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack);
int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack);
int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack);
void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info);
void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info);
void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info);
void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info);
void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info);
#ifdef __cplusplus
}
#endif
#endif /* __NMEA_PARSE_H__ */
apps/gps/nmealib/nmea/parser.h
-59
View File
@@ -1,59 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: parser.h 4 2007-08-27 13:11:03Z xtimor $
*
*/
#ifndef __NMEA_PARSER_H__
#define __NMEA_PARSER_H__
#include "info.h"
#ifdef __cplusplus
extern "C" {
#endif
/*
* high level
*/
typedef struct _nmeaPARSER
{
void *top_node;
void *end_node;
unsigned char *buffer;
int buff_size;
int buff_use;
} nmeaPARSER;
int nmea_parser_init(nmeaPARSER *parser);
void nmea_parser_destroy(nmeaPARSER *parser);
int nmea_parse(
nmeaPARSER *parser,
const char *buff, int buff_sz,
nmeaINFO *info
);
/*
* low level
*/
int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz);
int nmea_parser_top(nmeaPARSER *parser);
int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr);
int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr);
int nmea_parser_drop(nmeaPARSER *parser);
int nmea_parser_buff_clear(nmeaPARSER *parser);
int nmea_parser_queue_clear(nmeaPARSER *parser);
#ifdef __cplusplus
}
#endif
#endif /* __NMEA_PARSER_H__ */
apps/gps/nmealib/nmea/sentence.h
-128
View File
@@ -1,128 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: sentence.h 17 2008-03-11 11:56:11Z xtimor $
*
*/
/*! \file */
#ifndef __NMEA_SENTENCE_H__
#define __NMEA_SENTENCE_H__
#include "info.h"
#ifdef __cplusplus
extern "C" {
#endif
/**
* NMEA packets type which parsed and generated by library
*/
enum nmeaPACKTYPE
{
GPNON = 0x0000, /**< Unknown packet type. */
GPGGA = 0x0001, /**< GGA - Essential fix data which provide 3D location and accuracy data. */
GPGSA = 0x0002, /**< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values. */
GPGSV = 0x0004, /**< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values. */
GPRMC = 0x0008, /**< RMC - Recommended Minimum Specific GPS/TRANSIT Data. */
GPVTG = 0x0010 /**< VTG - Actual track made good and speed over ground. */
};
/**
* GGA packet information structure (Global Positioning System Fix Data)
*/
typedef struct _nmeaGPGGA
{
nmeaTIME utc; /**< UTC of position (just time) */
float lat; /**< Latitude in NDEG - [degree][min].[sec/60] */
char ns; /**< [N]orth or [S]outh */
float lon; /**< Longitude in NDEG - [degree][min].[sec/60] */
char ew; /**< [E]ast or [W]est */
int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */
int satinuse; /**< Number of satellites in use (not those in view) */
float HDOP; /**< Horizontal dilution of precision */
float elv; /**< Antenna altitude above/below mean sea level (geoid) */
char elv_units; /**< [M]eters (Antenna height unit) */
float diff; /**< Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level. '-' = geoid is below WGS-84 ellipsoid) */
char diff_units; /**< [M]eters (Units of geoidal separation) */
float dgps_age; /**< Time in seconds since last DGPS update */
int dgps_sid; /**< DGPS station ID number */
} nmeaGPGGA;
/**
* GSA packet information structure (Satellite status)
*/
typedef struct _nmeaGPGSA
{
char fix_mode; /**< Mode (M = Manual, forced to operate in 2D or 3D; A = Automatic, 3D/2D) */
int fix_type; /**< Type, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */
int sat_prn[NMEA_MAXSAT]; /**< PRNs of satellites used in position fix (null for unused fields) */
float PDOP; /**< Dilution of precision */
float HDOP; /**< Horizontal dilution of precision */
float VDOP; /**< Vertical dilution of precision */
} nmeaGPGSA;
/**
* GSV packet information structure (Satellites in view)
*/
typedef struct _nmeaGPGSV
{
int pack_count; /**< Total number of messages of this type in this cycle */
int pack_index; /**< Message number */
int sat_count; /**< Total number of satellites in view */
nmeaSATELLITE sat_data[NMEA_SATINPACK];
} nmeaGPGSV;
/**
* RMC packet information structure (Recommended Minimum sentence C)
*/
typedef struct _nmeaGPRMC
{
nmeaTIME utc; /**< UTC of position */
char status; /**< Status (A = active or V = void) */
float lat; /**< Latitude in NDEG - [degree][min].[sec/60] */
char ns; /**< [N]orth or [S]outh */
float lon; /**< Longitude in NDEG - [degree][min].[sec/60] */
char ew; /**< [E]ast or [W]est */
float speed; /**< Speed over the ground in knots */
float direction; /**< Track angle in degrees True */
float declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */
char declin_ew; /**< [E]ast or [W]est */
char mode; /**< Mode indicator of fix type (A = autonomous, D = differential, E = estimated, N = not valid, S = simulator) */
} nmeaGPRMC;
/**
* VTG packet information structure (Track made good and ground speed)
*/
typedef struct _nmeaGPVTG
{
float dir; /**< True track made good (degrees) */
char dir_t; /**< Fixed text 'T' indicates that track made good is relative to true north */
float dec; /**< Magnetic track made good */
char dec_m; /**< Fixed text 'M' */
float spn; /**< Ground speed, knots */
char spn_n; /**< Fixed text 'N' indicates that speed over ground is in knots */
float spk; /**< Ground speed, kilometers per hour */
char spk_k; /**< Fixed text 'K' indicates that speed over ground is in kilometers/hour */
} nmeaGPVTG;
void nmea_zero_GPGGA(nmeaGPGGA *pack);
void nmea_zero_GPGSA(nmeaGPGSA *pack);
void nmea_zero_GPGSV(nmeaGPGSV *pack);
void nmea_zero_GPRMC(nmeaGPRMC *pack);
void nmea_zero_GPVTG(nmeaGPVTG *pack);
#ifdef __cplusplus
}
#endif
#endif /* __NMEA_SENTENCE_H__ */
apps/gps/nmealib/nmea/time.h
-47
View File
@@ -1,47 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: time.h 4 2007-08-27 13:11:03Z xtimor $
*
*/
/*! \file */
#ifndef __NMEA_TIME_H__
#define __NMEA_TIME_H__
#include "config.h"
#ifdef __cplusplus
extern "C" {
#endif
/**
* Date and time data
* @see nmea_time_now
*/
typedef struct _nmeaTIME
{
int year; /**< Years since 1900 */
int mon; /**< Months since January - [0,11] */
int day; /**< Day of the month - [1,31] */
int hour; /**< Hours since midnight - [0,23] */
int min; /**< Minutes after the hour - [0,59] */
int sec; /**< Seconds after the minute - [0,59] */
int hsec; /**< Hundredth part of second - [0,99] */
} nmeaTIME;
/**
* \brief Get time now to nmeaTIME structure
*/
void nmea_time_now(nmeaTIME *t);
#ifdef __cplusplus
}
#endif
#endif /* __NMEA_TIME_H__ */
apps/gps/nmealib/nmea/tok.h
-31
View File
@@ -1,31 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: tok.h 4 2007-08-27 13:11:03Z xtimor $
*
*/
#ifndef __NMEA_TOK_H__
#define __NMEA_TOK_H__
#include "config.h"
#include <string.h>
#ifdef __cplusplus
extern "C" {
#endif
int nmea_calc_crc(const char *buff, int buff_sz);
int nmea_atoi(const char *str, int str_sz, int radix);
float nmea_atof(const char *str, int str_sz);
int nmea_printf(char *buff, int buff_sz, const char *format, ...);
int nmea_scanf(const char *buff, int buff_sz, const char *format, ...);
#ifdef __cplusplus
}
#endif
#endif /* __NMEA_TOK_H__ */
apps/gps/nmealib/nmea/units.h
-30
View File
@@ -1,30 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: units.h 4 2007-08-27 13:11:03Z xtimor $
*
*/
#ifndef __NMEA_UNITS_H__
#define __NMEA_UNITS_H__
#include "config.h"
/*
* Distance units
*/
#define NMEA_TUD_YARDS (1.0936) /**< Yeards, meter * NMEA_TUD_YARDS = yard */
#define NMEA_TUD_KNOTS (1.852) /**< Knots, kilometer / NMEA_TUD_KNOTS = knot */
#define NMEA_TUD_MILES (1.609) /**< Miles, kilometer / NMEA_TUD_MILES = mile */
/*
* Speed units
*/
#define NMEA_TUS_MS (3.6) /**< Meters per seconds, (k/h) / NMEA_TUS_MS= (m/s) */
#endif /* __NMEA_UNITS_H__ */
apps/gps/nmealib/parse.c
-501
View File
File diff suppressed because it is too large Load Diff
apps/gps/nmealib/parser.c
-400
View File
@@ -1,400 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: parser.c 17 2008-03-11 11:56:11Z xtimor $
*
*/
/**
* \file parser.h
*/
#include "nmea/tok.h"
#include "nmea/parse.h"
#include "nmea/parser.h"
#include "nmea/context.h"
#include <string.h>
#include <stdlib.h>
typedef struct _nmeaParserNODE
{
int packType;
void *pack;
struct _nmeaParserNODE *next_node;
} nmeaParserNODE;
/*
* high level
*/
/**
* \brief Initialization of parser object
* @return true (1) - success or false (0) - fail
*/
int nmea_parser_init(nmeaPARSER *parser)
{
int resv = 0;
int buff_size = nmea_property()->parse_buff_size;
//NMEA_ASSERT(parser);
if(buff_size < NMEA_MIN_PARSEBUFF)
buff_size = NMEA_MIN_PARSEBUFF;
memset(parser, 0, sizeof(nmeaPARSER));
if(0 == (parser->buffer = malloc(buff_size)))
nmea_error("Insufficient memory!");
else
{
parser->buff_size = buff_size;
resv = 1;
}
return resv;
}
/**
* \brief Destroy parser object
*/
void nmea_parser_destroy(nmeaPARSER *parser)
{
//NMEA_ASSERT(parser && parser->buffer);
free(parser->buffer);
nmea_parser_queue_clear(parser);
memset(parser, 0, sizeof(nmeaPARSER));
}
/**
* \brief Analysis of buffer and put results to information structure
* @return Number of packets wos parsed
*/
int nmea_parse(
nmeaPARSER *parser,
const char *buff, int buff_sz,
nmeaINFO *info
)
{
int ptype, nread = 0;
void *pack = 0;
//NMEA_ASSERT(parser && parser->buffer);
nmea_parser_push(parser, buff, buff_sz);
while(GPNON != (ptype = nmea_parser_pop(parser, &pack)))
{
nread++;
switch(ptype)
{
case GPGGA:
nmea_GPGGA2info((nmeaGPGGA *)pack, info);
break;
case GPGSA:
nmea_GPGSA2info((nmeaGPGSA *)pack, info);
break;
case GPGSV:
nmea_GPGSV2info((nmeaGPGSV *)pack, info);
break;
case GPRMC:
nmea_GPRMC2info((nmeaGPRMC *)pack, info);
break;
case GPVTG:
nmea_GPVTG2info((nmeaGPVTG *)pack, info);
break;
};
free(pack);
}
return nread;
}
/*
* low level
*/
int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz)
{
int nparsed = 0, crc, sen_sz, ptype;
nmeaParserNODE *node = 0;
//NMEA_ASSERT(parser && parser->buffer);
/* clear unuse buffer (for debug) */
/*
memset(
parser->buffer + parser->buff_use, 0,
parser->buff_size - parser->buff_use
);
*/
/* add */
if(parser->buff_use + buff_sz >= parser->buff_size)
nmea_parser_buff_clear(parser);
memcpy(parser->buffer + parser->buff_use, buff, buff_sz);
parser->buff_use += buff_sz;
/* parse */
for(;;node = 0)
{
sen_sz = nmea_find_tail(
(const char *)parser->buffer + nparsed,
(int)parser->buff_use - nparsed, &crc);
if(!sen_sz)
{
if(nparsed)
memcpy(
parser->buffer,
parser->buffer + nparsed,
parser->buff_use -= nparsed);
break;
}
else if(crc >= 0)
{
ptype = nmea_pack_type(
(const char *)parser->buffer + nparsed + 1,
parser->buff_use - nparsed - 1);
if(0 == (node = malloc(sizeof(nmeaParserNODE))))
goto mem_fail;
node->pack = 0;
switch(ptype)
{
case GPGGA:
if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
goto mem_fail;
node->packType = GPGGA;
if(!nmea_parse_GPGGA(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPGGA *)node->pack))
{
free(node);
node = 0;
}
break;
case GPGSA:
if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
goto mem_fail;
node->packType = GPGSA;
if(!nmea_parse_GPGSA(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPGSA *)node->pack))
{
free(node);
node = 0;
}
break;
case GPGSV:
if(0 == (node->pack = malloc(sizeof(nmeaGPGSV))))
goto mem_fail;
node->packType = GPGSV;
if(!nmea_parse_GPGSV(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPGSV *)node->pack))
{
free(node);
node = 0;
}
break;
case GPRMC:
if(0 == (node->pack = malloc(sizeof(nmeaGPRMC))))
goto mem_fail;
node->packType = GPRMC;
if(!nmea_parse_GPRMC(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPRMC *)node->pack))
{
free(node);
node = 0;
}
break;
case GPVTG:
if(0 == (node->pack = malloc(sizeof(nmeaGPVTG))))
goto mem_fail;
node->packType = GPVTG;
if(!nmea_parse_GPVTG(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPVTG *)node->pack))
{
free(node);
node = 0;
}
break;
default:
free(node);
node = 0;
break;
};
if(node)
{
if(parser->end_node)
((nmeaParserNODE *)parser->end_node)->next_node = node;
parser->end_node = node;
if(!parser->top_node)
parser->top_node = node;
node->next_node = 0;
}
}
nparsed += sen_sz;
}
return nparsed;
mem_fail:
if(node)
free(node);
nmea_error("Insufficient memory!");
return -1;
}
/**
* \brief Analysis of buffer and keep results into parser
* @return Number of bytes wos parsed from buffer
*/
int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz)
{
int nparse, nparsed = 0;
do
{
if(buff_sz > parser->buff_size)
nparse = parser->buff_size;
else
nparse = buff_sz;
nparsed += nmea_parser_real_push(
parser, buff, nparse);
buff_sz -= nparse;
} while(buff_sz);
return nparsed;
}
/**
* \brief Get type of top packet keeped into parser
* @return Type of packet
* @see nmeaPACKTYPE
*/
int nmea_parser_top(nmeaPARSER *parser)
{
int retval = GPNON;
nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
//NMEA_ASSERT(parser && parser->buffer);
if(node)
retval = node->packType;
return retval;
}
/**
* \brief Withdraw top packet from parser
* @return Received packet type
* @see nmeaPACKTYPE
*/
int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr)
{
int retval = GPNON;
nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
//NMEA_ASSERT(parser && parser->buffer);
if(node)
{
*pack_ptr = node->pack;
retval = node->packType;
parser->top_node = node->next_node;
if(!parser->top_node)
parser->end_node = 0;
free(node);
}
return retval;
}
/**
* \brief Get top packet from parser without withdraw
* @return Received packet type
* @see nmeaPACKTYPE
*/
int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr)
{
int retval = GPNON;
nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
//NMEA_ASSERT(parser && parser->buffer);
if(node)
{
*pack_ptr = node->pack;
retval = node->packType;
}
return retval;
}
/**
* \brief Delete top packet from parser
* @return Deleted packet type
* @see nmeaPACKTYPE
*/
int nmea_parser_drop(nmeaPARSER *parser)
{
int retval = GPNON;
nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
//NMEA_ASSERT(parser && parser->buffer);
if(node)
{
if(node->pack)
free(node->pack);
retval = node->packType;
parser->top_node = node->next_node;
if(!parser->top_node)
parser->end_node = 0;
free(node);
}
return retval;
}
/**
* \brief Clear cache of parser
* @return true (1) - success
*/
int nmea_parser_buff_clear(nmeaPARSER *parser)
{
//NMEA_ASSERT(parser && parser->buffer);
parser->buff_use = 0;
return 1;
}
/**
* \brief Clear packets queue into parser
* @return true (1) - success
*/
int nmea_parser_queue_clear(nmeaPARSER *parser)
{
//NMEA_ASSERT(parser);
while(parser->top_node)
nmea_parser_drop(parser);
return 1;
}
apps/gps/nmealib/sentence.c
-54
View File
@@ -1,54 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: sentence.c 17 2008-03-11 11:56:11Z xtimor $
*
*/
#include "nmea/sentence.h"
#include <string.h>
void nmea_zero_GPGGA(nmeaGPGGA *pack)
{
memset(pack, 0, sizeof(nmeaGPGGA));
nmea_time_now(&pack->utc);
pack->ns = 'N';
pack->ew = 'E';
pack->elv_units = 'M';
pack->diff_units = 'M';
}
void nmea_zero_GPGSA(nmeaGPGSA *pack)
{
memset(pack, 0, sizeof(nmeaGPGSA));
pack->fix_mode = 'A';
pack->fix_type = NMEA_FIX_BAD;
}
void nmea_zero_GPGSV(nmeaGPGSV *pack)
{
memset(pack, 0, sizeof(nmeaGPGSV));
}
void nmea_zero_GPRMC(nmeaGPRMC *pack)
{
memset(pack, 0, sizeof(nmeaGPRMC));
nmea_time_now(&pack->utc);
pack->status = 'V';
pack->ns = 'N';
pack->ew = 'E';
pack->declin_ew = 'E';
}
void nmea_zero_GPVTG(nmeaGPVTG *pack)
{
memset(pack, 0, sizeof(nmeaGPVTG));
pack->dir_t = 'T';
pack->dec_m = 'M';
pack->spn_n = 'N';
pack->spk_k = 'K';
}
apps/gps/nmealib/time.c
-63
View File
@@ -1,63 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: time.c 4 2007-08-27 13:11:03Z xtimor $
*
*/
/*! \file time.h */
#include "nmea/time.h"
#ifdef NMEA_WIN
# pragma warning(disable: 4201)
# pragma warning(disable: 4214)
# pragma warning(disable: 4115)
# include <windows.h>
# pragma warning(default: 4201)
# pragma warning(default: 4214)
# pragma warning(default: 4115)
#else
# include <time.h>
#endif
#ifdef NMEA_WIN
void nmea_time_now(nmeaTIME *stm)
{
SYSTEMTIME st;
GetSystemTime(&st);
stm->year = st.wYear - 1900;
stm->mon = st.wMonth - 1;
stm->day = st.wDay;
stm->hour = st.wHour;
stm->min = st.wMinute;
stm->sec = st.wSecond;
stm->hsec = st.wMilliseconds / 10;
}
#else /* NMEA_WIN */
void nmea_time_now(nmeaTIME *stm)
{
time_t lt;
struct tm *tt;
time(&lt);
tt = gmtime(&lt);
stm->year = tt->tm_year;
stm->mon = tt->tm_mon;
stm->day = tt->tm_mday;
stm->hour = tt->tm_hour;
stm->min = tt->tm_min;
stm->sec = tt->tm_sec;
stm->hsec = 0;
}
#endif
apps/gps/nmealib/tok.c
-267
View File
@@ -1,267 +0,0 @@
/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: tok.c 17 2008-03-11 11:56:11Z xtimor $
*
*/
/*! \file tok.h */
#include "nmea/tok.h"
#include <stdarg.h>
#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>
#include <string.h>
#include <limits.h>
#define NMEA_TOKS_COMPARE (1)
#define NMEA_TOKS_PERCENT (2)
#define NMEA_TOKS_WIDTH (3)
#define NMEA_TOKS_TYPE (4)
//memchr replacement
void *
memchr(s, c, n)
const void *s;
unsigned char c;
size_t n;
{
if (n != 0) {
const unsigned char *p = s;
do {
if (*p++ == c)
return ((void *)(p - 1));
} while (--n != 0);
}
return (NULL);
}
/**
* \brief Calculate control sum of binary buffer
*/
int nmea_calc_crc(const char *buff, int buff_sz)
{
int chsum = 0,
it;
for(it = 0; it < buff_sz; ++it)
chsum ^= (int)buff[it];
return chsum;
}
/**
* \brief Convert string to number
*/
int nmea_atoi(const char *str, int str_sz, int radix)
{
char *tmp_ptr;
char buff[NMEA_CONVSTR_BUF];
int res = 0;
if(str_sz < NMEA_CONVSTR_BUF)
{
memcpy(&buff[0], str, str_sz);
buff[str_sz] = '\0';
res = strtol(&buff[0], &tmp_ptr, radix);
}
return res;
}
/**
* \brief Convert string to fraction number
*/
float nmea_atof(const char *str, int str_sz)
{
char *tmp_ptr;
char buff[NMEA_CONVSTR_BUF];
float res = 0;
if(str_sz < NMEA_CONVSTR_BUF)
{
memcpy(&buff[0], str, str_sz);
buff[str_sz] = '\0';
res = (float)strtod(&buff[0], &tmp_ptr);
}
return res;
}
/**
* \brief Formating string (like standart printf) with CRC tail (*CRC)
*/
int nmea_printf(char *buff, int buff_sz, const char *format, ...)
{
int retval, add = 0;
va_list arg_ptr;
if(buff_sz <= 0)
return 0;
va_start(arg_ptr, format);
retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr);
if(retval > 0)
{
add = NMEA_POSIX(snprintf)(
buff + retval, buff_sz - retval, "*%02x\r\n",
nmea_calc_crc(buff + 1, retval - 1));
}
retval += add;
if(retval < 0 || retval > buff_sz)
{
memset(buff, ' ', buff_sz);
retval = buff_sz;
}
va_end(arg_ptr);
return retval;
}
/**
* \brief Analyse string (specificate for NMEA sentences)
*/
int nmea_scanf(const char *buff, int buff_sz, const char *format, ...)
{
const char *beg_tok;
const char *end_buf = buff + buff_sz;
va_list arg_ptr;
int tok_type = NMEA_TOKS_COMPARE;
int width = 0;
const char *beg_fmt = 0;
int snum = 0, unum = 0;
int tok_count = 0;
void *parg_target;
va_start(arg_ptr, format);
for(; *format && buff < end_buf; ++format)
{
switch(tok_type)
{
case NMEA_TOKS_COMPARE:
if('%' == *format)
tok_type = NMEA_TOKS_PERCENT;
else if(*buff++ != *format)
goto fail;
break;
case NMEA_TOKS_PERCENT:
width = 0;
beg_fmt = format;
tok_type = NMEA_TOKS_WIDTH;
case NMEA_TOKS_WIDTH:
if(isdigit(*format))
break;
{
tok_type = NMEA_TOKS_TYPE;
if(format > beg_fmt)
width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10);
}
case NMEA_TOKS_TYPE:
beg_tok = buff;
if(!width && ('c' == *format || 'C' == *format) && *buff != format[1])
width = 1;
if(width)
{
if(buff + width <= end_buf)
buff += width;
else
goto fail;
}
else
{
if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff))))
buff = end_buf;
}
if(buff > end_buf)
goto fail;
tok_type = NMEA_TOKS_COMPARE;
tok_count++;
parg_target = 0; width = (int)(buff - beg_tok);
switch(*format)
{
case 'c':
case 'C':
parg_target = (void *)va_arg(arg_ptr, char *);
if(width && 0 != (parg_target))
*((char *)parg_target) = *beg_tok;
break;
case 's':
case 'S':
parg_target = (void *)va_arg(arg_ptr, char *);
if(width && 0 != (parg_target))
{
memcpy(parg_target, beg_tok, width);
((char *)parg_target)[width] = '\0';
}
break;
case 'f':
case 'g':
case 'G':
case 'e':
case 'E':
parg_target = (void *)va_arg(arg_ptr, float *);
if(width && 0 != (parg_target))
*((float *)parg_target) = nmea_atof(beg_tok, width);
break;
};
if(parg_target)
break;
if(0 == (parg_target = (void *)va_arg(arg_ptr, int *)))
break;
if(!width)
break;
switch(*format)
{
case 'd':
case 'i':
snum = nmea_atoi(beg_tok, width, 10);
memcpy(parg_target, &snum, sizeof(int));
break;
case 'u':
unum = nmea_atoi(beg_tok, width, 10);
memcpy(parg_target, &unum, sizeof(unsigned int));
break;
case 'x':
case 'X':
unum = nmea_atoi(beg_tok, width, 16);
memcpy(parg_target, &unum, sizeof(unsigned int));
break;
case 'o':
unum = nmea_atoi(beg_tok, width, 8);
memcpy(parg_target, &unum, sizeof(unsigned int));
break;
default:
goto fail;
};
break;
};
}
fail:
va_end(arg_ptr);
return tok_count;
}
apps/gps/ubx.c
-1022
View File
File diff suppressed because it is too large Load Diff
apps/gps/ubx.h
-415
View File
@@ -1,415 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* @file U-Blox protocol definitions */
#ifndef UBX_H_
#define UBX_H_
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stdbool.h>
#include <unistd.h>
#include <pthread.h>
//internal definitions (not depending on the ubx protocol
#define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */
#define UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS 3000000 /**< Allow 3 seconds maximum inter-message time */
#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 2000000 /**< Check for current state every two seconds */
#define UBX_CONFIG_TIMEOUT 1000000
#define APPNAME "gps: ubx"
#define UBX_SYNC_1 0xB5
#define UBX_SYNC_2 0x62
//UBX Protocoll definitions (this is the subset of the messages that are parsed)
#define UBX_CLASS_NAV 0x01
#define UBX_CLASS_RXM 0x02
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
#define UBX_MESSAGE_NAV_POSLLH 0x02
#define UBX_MESSAGE_NAV_SOL 0x06
#define UBX_MESSAGE_NAV_TIMEUTC 0x21
#define UBX_MESSAGE_NAV_DOP 0x04
#define UBX_MESSAGE_NAV_SVINFO 0x30
#define UBX_MESSAGE_NAV_VELNED 0x12
#define UBX_MESSAGE_RXM_SVSI 0x20
#define UBX_MESSAGE_ACK_ACK 0x01
#define UBX_MESSAGE_ACK_NAK 0x00
#define UBX_MESSAGE_CFG_PRT 0x00
#define UBX_MESSAGE_CFG_NAV5 0x24
#define UBX_MESSAGE_CFG_MSG 0x01
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< ubx in */
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< ubx out */
#define UBX_CFG_NAV5_LENGTH 36
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */
#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
#define UBX_CFG_MSG_LENGTH 8
#define UBX_CFG_MSG_PAYLOAD_RATE {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} /**< UART1 chosen */
// ************
/** the structures of the binary packets */
#pragma pack(push, 1)
typedef struct {
uint32_t time_milliseconds; // GPS Millisecond Time of Week
int32_t lon; // Longitude * 1e-7, deg
int32_t lat; // Latitude * 1e-7, deg
int32_t height; // Height above Ellipsoid, mm
int32_t height_msl; // Height above mean sea level, mm
uint32_t hAcc; // Horizontal Accuracy Estimate, mm
uint32_t vAcc; // Vertical Accuracy Estimate, mm
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_nav_posllh_packet;
typedef type_gps_bin_nav_posllh_packet gps_bin_nav_posllh_packet_t;
typedef struct {
uint32_t time_milliseconds; // GPS Millisecond Time of Week
int32_t time_nanoseconds; // Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000
int16_t week; // GPS week (GPS time)
uint8_t gpsFix; //GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix
uint8_t flags;
int32_t ecefX;
int32_t ecefY;
int32_t ecefZ;
uint32_t pAcc;
int32_t ecefVX;
int32_t ecefVY;
int32_t ecefVZ;
uint32_t sAcc;
uint16_t pDOP;
uint8_t reserved1;
uint8_t numSV;
uint32_t reserved2;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_nav_sol_packet;
typedef type_gps_bin_nav_sol_packet gps_bin_nav_sol_packet_t;
typedef struct {
uint32_t time_milliseconds; // GPS Millisecond Time of Week
uint32_t time_accuracy; //Time Accuracy Estimate, ns
int32_t time_nanoseconds; //Nanoseconds of second, range -1e9 .. 1e9 (UTC)
uint16_t year; //Year, range 1999..2099 (UTC)
uint8_t month; //Month, range 1..12 (UTC)
uint8_t day; //Day of Month, range 1..31 (UTC)
uint8_t hour; //Hour of Day, range 0..23 (UTC)
uint8_t min; //Minute of Hour, range 0..59 (UTC)
uint8_t sec; //Seconds of Minute, range 0..59 (UTC)
uint8_t valid_flag; //Validity Flags (see ubx documentation)
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_nav_timeutc_packet;
typedef type_gps_bin_nav_timeutc_packet gps_bin_nav_timeutc_packet_t;
typedef struct {
uint32_t time_milliseconds; // GPS Millisecond Time of Week
uint16_t gDOP; //Geometric DOP (scaling 0.01)
uint16_t pDOP; //Position DOP (scaling 0.01)
uint16_t tDOP; //Time DOP (scaling 0.01)
uint16_t vDOP; //Vertical DOP (scaling 0.01)
uint16_t hDOP; //Horizontal DOP (scaling 0.01)
uint16_t nDOP; //Northing DOP (scaling 0.01)
uint16_t eDOP; //Easting DOP (scaling 0.01)
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_nav_dop_packet;
typedef type_gps_bin_nav_dop_packet gps_bin_nav_dop_packet_t;
typedef struct {
uint32_t time_milliseconds; // GPS Millisecond Time of Week
uint8_t numCh; //Number of channels
uint8_t globalFlags;
uint16_t reserved2;
} type_gps_bin_nav_svinfo_part1_packet;
typedef type_gps_bin_nav_svinfo_part1_packet gps_bin_nav_svinfo_part1_packet_t;
typedef struct {
uint8_t chn; //Channel number, 255 for SVs not assigned to a channel
uint8_t svid; //Satellite ID
uint8_t flags;
uint8_t quality;
uint8_t cno; //Carrier to Noise Ratio (Signal Strength), dbHz
int8_t elev; //Elevation in integer degrees
int16_t azim; //Azimuth in integer degrees
int32_t prRes; //Pseudo range residual in centimetres
} type_gps_bin_nav_svinfo_part2_packet;
typedef type_gps_bin_nav_svinfo_part2_packet gps_bin_nav_svinfo_part2_packet_t;
typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_nav_svinfo_part3_packet;
typedef type_gps_bin_nav_svinfo_part3_packet gps_bin_nav_svinfo_part3_packet_t;
typedef struct {
uint32_t time_milliseconds; // GPS Millisecond Time of Week
int32_t velN; //NED north velocity, cm/s
int32_t velE; //NED east velocity, cm/s
int32_t velD; //NED down velocity, cm/s
uint32_t speed; //Speed (3-D), cm/s
uint32_t gSpeed; //Ground Speed (2-D), cm/s
int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
uint32_t sAcc; //Speed Accuracy Estimate, cm/s
uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_nav_velned_packet;
typedef type_gps_bin_nav_velned_packet gps_bin_nav_velned_packet_t;
typedef struct {
int32_t time_milliseconds; // Measurement integer millisecond GPS time of week
int16_t week; //Measurement GPS week number
uint8_t numVis; //Number of visible satellites
//... rest of package is not used in this implementation
} type_gps_bin_rxm_svsi_packet;
typedef type_gps_bin_rxm_svsi_packet gps_bin_rxm_svsi_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_ack_ack_packet;
typedef type_gps_bin_ack_ack_packet gps_bin_ack_ack_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_ack_nak_packet;
typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint8_t portID;
uint8_t res0;
uint16_t res1;
uint32_t mode;
uint32_t baudRate;
uint16_t inProtoMask;
uint16_t outProtoMask;
uint16_t flags;
uint16_t pad;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_prt_packet;
typedef type_gps_bin_cfg_prt_packet type_gps_bin_cfg_prt_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint32_t reserved2;
uint32_t reserved3;
uint32_t reserved4;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_nav5_packet;
typedef type_gps_bin_cfg_nav5_packet type_gps_bin_cfg_nav5_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint8_t msgClass_payload;
uint8_t msgID_payload;
uint8_t rate[6];
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_msg_packet;
typedef type_gps_bin_cfg_msg_packet type_gps_bin_cfg_msg_packet_t;
// END the structures of the binary packets
// ************
enum UBX_CONFIG_STATE {
UBX_CONFIG_STATE_NONE = 0,
UBX_CONFIG_STATE_PRT = 1,
UBX_CONFIG_STATE_NAV5 = 2,
UBX_CONFIG_STATE_MSG_NAV_POSLLH = 3,
UBX_CONFIG_STATE_MSG_NAV_TIMEUTC = 4,
UBX_CONFIG_STATE_MSG_NAV_DOP = 5,
UBX_CONFIG_STATE_MSG_NAV_SVINFO = 6,
UBX_CONFIG_STATE_MSG_NAV_SOL = 7,
UBX_CONFIG_STATE_MSG_NAV_VELNED = 8,
UBX_CONFIG_STATE_MSG_RXM_SVSI = 9,
UBX_CONFIG_STATE_CONFIGURED = 10
};
enum UBX_MESSAGE_CLASSES {
CLASS_UNKNOWN = 0,
NAV = 1,
RXM = 2,
ACK = 3,
CFG = 4
};
enum UBX_MESSAGE_IDS {
//these numbers do NOT correspond to the message id numbers of the ubx protocol
ID_UNKNOWN = 0,
NAV_POSLLH = 1,
NAV_SOL = 2,
NAV_TIMEUTC = 3,
NAV_DOP = 4,
NAV_SVINFO = 5,
NAV_VELNED = 6,
RXM_SVSI = 7,
CFG_NAV5 = 8,
ACK_ACK = 9,
ACK_NAK = 10
};
enum UBX_DECODE_STATES {
UBX_DECODE_UNINIT = 0,
UBX_DECODE_GOT_SYNC1 = 1,
UBX_DECODE_GOT_SYNC2 = 2,
UBX_DECODE_GOT_CLASS = 3,
UBX_DECODE_GOT_MESSAGEID = 4,
UBX_DECODE_GOT_LENGTH1 = 5,
UBX_DECODE_GOT_LENGTH2 = 6
};
typedef struct {
union {
uint16_t ck;
struct {
uint8_t ck_a;
uint8_t ck_b;
};
};
enum UBX_DECODE_STATES decode_state;
bool print_errors;
int16_t rx_count;
uint16_t payload_size;
enum UBX_MESSAGE_CLASSES message_class;
enum UBX_MESSAGE_IDS message_id;
uint64_t last_message_timestamps[UBX_NO_OF_MESSAGES];
} type_gps_bin_ubx_state;
typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
#pragma pack(pop)
extern pthread_mutex_t *ubx_mutex;
extern gps_bin_ubx_state_t *ubx_state;
void ubx_decode_init(void);
void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b);
int ubx_parse(uint8_t b, char *gps_rx_buffer);
int configure_gps_ubx(int *fd);
int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size);
int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd);
void calculate_ubx_checksum(uint8_t *message, uint8_t length);
void *ubx_watchdog_loop(void *args);
void *ubx_loop(void *args);
#endif /* UBX_H_ */