You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
335 lines
9.4 KiB
335 lines
9.4 KiB
/**************************************************************************** |
|
* |
|
* 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 "nmea_helper.h" |
|
#include <sys/prctl.h> |
|
#include <poll.h> |
|
#include <uORB/uORB.h> |
|
#include <uORB/topics/vehicle_gps_position.h> |
|
#include <mavlink/mavlink_log.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 *arg) |
|
{ |
|
/* Set thread name */ |
|
prctl(PR_SET_NAME, "gps nmea read", getpid()); |
|
|
|
/* Retrieve file descriptor */ |
|
int fd = *((int *)arg); |
|
|
|
/* 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 = {0}; |
|
nmea_gps = &nmea_gps_d; |
|
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps); |
|
|
|
while (1) { |
|
/* 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 = epoch * 1e6 + info->utc.hsec * 1e4; |
|
nmea_gps->fix_type = (uint8_t)info->fix; |
|
nmea_gps->lat = (int32_t)(ndeg2degree(info->lat) * 1e7); |
|
nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7); |
|
nmea_gps->alt = (int32_t)(info->elv * 1e3); |
|
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 = 65535; |
|
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); |
|
|
|
return NULL; |
|
|
|
} |
|
|
|
void *nmea_watchdog_loop(void *arg) |
|
{ |
|
/* 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; |
|
|
|
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); |
|
|
|
while (1) { |
|
// 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); |
|
} |
|
|
|
close(mavlink_fd); |
|
|
|
return NULL; |
|
}
|
|
|