mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-06 16:49:51 +08:00
Merge pull request #30 from julianoes/gps_fix
some gps fixes (only tested without gps attached)
This commit is contained in:
Lorenz Meier
+109
-102
@@ -38,7 +38,6 @@
|
||||
*/
|
||||
|
||||
#include "gps.h"
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
@@ -58,9 +57,9 @@
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
static bool thread_should_exit; /**< Deamon status flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/**
|
||||
* GPS module readout and publishing.
|
||||
@@ -82,16 +81,6 @@ int gps_thread_main(int argc, char *argv[]);
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
fprintf(stderr, "\tusage: %s -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Definitions
|
||||
****************************************************************************/
|
||||
@@ -115,7 +104,6 @@ enum GPS_MODES {
|
||||
};
|
||||
|
||||
|
||||
|
||||
#define AUTO_DETECTION_COUNT 8
|
||||
const int autodetection_baudrates[] = {B9600, B38400, B38400, B9600, B9600, B38400, B9600, B38400};
|
||||
const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea
|
||||
@@ -125,40 +113,9 @@ const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS
|
||||
****************************************************************************/
|
||||
int open_port(char *port);
|
||||
|
||||
void close_port(int fd);
|
||||
void close_port(int *fd);
|
||||
|
||||
void setup_port(char *device, int speed, int *fd)
|
||||
{
|
||||
|
||||
/* open port (baud rate is set in defconfig file) */
|
||||
*fd = open_port(device);
|
||||
|
||||
if (*fd != -1) {
|
||||
if (gps_verbose) printf("[gps] Port opened: %s at %d speed\r\n", device, speed);
|
||||
|
||||
} else {
|
||||
fprintf(stderr, "[gps] Could not open port, exiting gps app!\r\n");
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
/* Try to set baud rate */
|
||||
struct termios uart_config;
|
||||
int termios_state;
|
||||
|
||||
if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) {
|
||||
fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\r\n", device, termios_state);
|
||||
close(*fd);
|
||||
}
|
||||
|
||||
/* Set baud rate */
|
||||
cfsetispeed(&uart_config, speed);
|
||||
cfsetospeed(&uart_config, speed);
|
||||
|
||||
if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) {
|
||||
fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", device);
|
||||
close(*fd);
|
||||
}
|
||||
}
|
||||
void setup_port(char *device, int speed, int *fd);
|
||||
|
||||
|
||||
/**
|
||||
@@ -195,8 +152,8 @@ int gps_main(int argc, char *argv[])
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\gps is running\n");
|
||||
} else {
|
||||
printf("\tgps is running\n");
|
||||
} else {
|
||||
printf("\tgps not started\n");
|
||||
}
|
||||
exit(0);
|
||||
@@ -215,7 +172,7 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
printf("[gps] Initialized. Searching for GPS receiver..\n");
|
||||
|
||||
/* default values */
|
||||
const char *commandline_usage = "\tusage: %s -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n";
|
||||
const char *commandline_usage = "\tusage: %s {start|stop|status} -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n";
|
||||
char *device = "/dev/ttyS3";
|
||||
char mode[10];
|
||||
strcpy(mode, "all");
|
||||
@@ -235,6 +192,7 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
for (i = 0; i < argc; i++) {
|
||||
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set
|
||||
printf(commandline_usage, argv[0]);
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -244,6 +202,7 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
|
||||
} else {
|
||||
printf(commandline_usage, argv[0]);
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
@@ -254,6 +213,7 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
|
||||
} else {
|
||||
printf(commandline_usage, argv[0]);
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
@@ -264,6 +224,7 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
|
||||
} else {
|
||||
printf(commandline_usage, argv[0]);
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
@@ -274,6 +235,7 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
|
||||
} else {
|
||||
printf(commandline_usage, argv[0]);
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
@@ -294,45 +256,25 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
case -1: gps_baud_try_all = true; break;
|
||||
|
||||
case 0: current_gps_speed = B0; break;
|
||||
|
||||
case 50: current_gps_speed = B50; break;
|
||||
|
||||
case 75: current_gps_speed = B75; break;
|
||||
|
||||
case 110: current_gps_speed = B110; break;
|
||||
|
||||
case 134: current_gps_speed = B134; break;
|
||||
|
||||
case 150: current_gps_speed = B150; break;
|
||||
|
||||
case 200: current_gps_speed = B200; break;
|
||||
|
||||
case 300: current_gps_speed = B300; break;
|
||||
|
||||
case 600: current_gps_speed = B600; break;
|
||||
|
||||
case 1200: current_gps_speed = B1200; break;
|
||||
|
||||
case 1800: current_gps_speed = B1800; break;
|
||||
|
||||
case 2400: current_gps_speed = B2400; break;
|
||||
|
||||
case 4800: current_gps_speed = B4800; break;
|
||||
|
||||
case 9600: current_gps_speed = B9600; break;
|
||||
|
||||
case 19200: current_gps_speed = B19200; break;
|
||||
|
||||
case 38400: current_gps_speed = B38400; break;
|
||||
|
||||
case 57600: current_gps_speed = B57600; break;
|
||||
|
||||
case 115200: current_gps_speed = B115200; break;
|
||||
|
||||
case 230400: current_gps_speed = B230400; break;
|
||||
|
||||
case 460800: current_gps_speed = B460800; break;
|
||||
|
||||
case 921600: current_gps_speed = B921600; break;
|
||||
|
||||
default:
|
||||
@@ -359,9 +301,12 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
} else {
|
||||
fprintf(stderr, "\t[gps] Invalid mode argument\n");
|
||||
printf(commandline_usage);
|
||||
thread_running = false;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* Declare file descriptor for gps here, open port later in setup_port */
|
||||
int fd;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
/* Infinite retries or break if retry == false */
|
||||
@@ -388,15 +333,14 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
* if the gps was once running the wtachdog thread will not return but instead try to reconfigure the gps (depending on the mode/protocol)
|
||||
*/
|
||||
|
||||
if (current_gps_mode == GPS_MODE_UBX) { //TODO: make a small enum with all modes to avoid all the strcpy
|
||||
if (current_gps_mode == GPS_MODE_UBX) {
|
||||
|
||||
if (gps_verbose) printf("[gps] Trying UBX mode at %d baud\n", current_gps_speed);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "GPS: trying to connect to a ubx module");
|
||||
|
||||
int fd;
|
||||
mavlink_log_info(mavlink_fd, "[gps] trying to connect to a ubx module");
|
||||
|
||||
setup_port(device, current_gps_speed, &fd);
|
||||
|
||||
|
||||
/* start ubx thread and watchdog */
|
||||
pthread_t ubx_thread;
|
||||
pthread_t ubx_watchdog_thread;
|
||||
@@ -411,13 +355,18 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
pthread_attr_t ubx_loop_attr;
|
||||
pthread_attr_init(&ubx_loop_attr);
|
||||
pthread_attr_setstacksize(&ubx_loop_attr, 3000);
|
||||
pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&fd);
|
||||
sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver
|
||||
|
||||
struct arg_struct args;
|
||||
args.fd_ptr = &fd;
|
||||
args.thread_should_exit_ptr = &thread_should_exit;
|
||||
|
||||
pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&args);
|
||||
sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver
|
||||
|
||||
pthread_attr_t ubx_wd_attr;
|
||||
pthread_attr_init(&ubx_wd_attr);
|
||||
pthread_attr_setstacksize(&ubx_wd_attr, 1400);
|
||||
int pthread_create_res = pthread_create(&ubx_watchdog_thread, &ubx_wd_attr, ubx_watchdog_loop, (void *)&fd);
|
||||
int pthread_create_res = pthread_create(&ubx_watchdog_thread, &ubx_wd_attr, ubx_watchdog_loop, (void *)&args);
|
||||
|
||||
if (pthread_create_res != 0) fprintf(stderr, "[gps] ERROR: could not create ubx watchdog thread, pthread_create =%d\n", pthread_create_res);
|
||||
|
||||
@@ -432,18 +381,23 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
|
||||
gps_mode_success = true;
|
||||
terminate_gps_thread = false;
|
||||
|
||||
/* maybe the watchdog was stopped through the thread_should_exit flag */
|
||||
} else if (thread_should_exit) {
|
||||
pthread_join(ubx_thread, NULL);
|
||||
if (gps_verbose) printf("[gps] ubx watchdog and ubx loop threads have been terminated, exiting.");
|
||||
close(mavlink_fd);
|
||||
close_port(&fd);
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
close_port(fd);
|
||||
}
|
||||
|
||||
if (current_gps_mode == GPS_MODE_MTK) {
|
||||
close_port(&fd);
|
||||
} else if (current_gps_mode == GPS_MODE_MTK) {
|
||||
if (gps_verbose) printf("[gps] trying MTK binary mode at %d baud\n", current_gps_speed);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[gps] trying to connect to a MTK module");
|
||||
|
||||
|
||||
int fd;
|
||||
setup_port(device, current_gps_speed, &fd);
|
||||
|
||||
/* start mtk thread and watchdog */
|
||||
@@ -463,9 +417,14 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
pthread_attr_t mtk_loop_attr;
|
||||
pthread_attr_init(&mtk_loop_attr);
|
||||
pthread_attr_setstacksize(&mtk_loop_attr, 2048);
|
||||
pthread_create(&mtk_thread, &mtk_loop_attr, mtk_loop, (void *)&fd);
|
||||
|
||||
struct arg_struct args;
|
||||
args.fd_ptr = &fd;
|
||||
args.thread_should_exit_ptr = &thread_should_exit;
|
||||
|
||||
pthread_create(&mtk_thread, &mtk_loop_attr, mtk_loop, (void *)&args);
|
||||
sleep(2);
|
||||
pthread_create(&mtk_watchdog_thread, NULL, mtk_watchdog_loop, (void *)&fd);
|
||||
pthread_create(&mtk_watchdog_thread, NULL, mtk_watchdog_loop, (void *)&args);
|
||||
|
||||
/* wait for threads to complete */
|
||||
pthread_join(mtk_watchdog_thread, (void *)&fd);
|
||||
@@ -476,23 +435,21 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
terminate_gps_thread = true;
|
||||
pthread_join(mtk_thread, NULL);
|
||||
|
||||
// if(true == gps_mode_try_all)
|
||||
// strcpy(mode, "nmea");
|
||||
//if(true == gps_mode_try_all)
|
||||
//strcpy(mode, "nmea");
|
||||
|
||||
gps_mode_success = true;
|
||||
terminate_gps_thread = false;
|
||||
}
|
||||
|
||||
close_port(fd);
|
||||
close_port(&fd);
|
||||
|
||||
}
|
||||
|
||||
if (current_gps_mode == GPS_MODE_NMEA) {
|
||||
} else if (current_gps_mode == GPS_MODE_NMEA) {
|
||||
if (gps_verbose) printf("[gps] Trying NMEA mode at %d baud\n", current_gps_speed);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[gps] trying to connect to a NMEA module");
|
||||
|
||||
int fd;
|
||||
|
||||
setup_port(device, current_gps_speed, &fd);
|
||||
|
||||
/* start nmea thread and watchdog */
|
||||
@@ -509,9 +466,14 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
pthread_attr_t nmea_loop_attr;
|
||||
pthread_attr_init(&nmea_loop_attr);
|
||||
pthread_attr_setstacksize(&nmea_loop_attr, 4096);
|
||||
pthread_create(&nmea_thread, &nmea_loop_attr, nmea_loop, (void *)&fd);
|
||||
|
||||
struct arg_struct args;
|
||||
args.fd_ptr = &fd;
|
||||
args.thread_should_exit_ptr = &thread_should_exit;
|
||||
|
||||
pthread_create(&nmea_thread, &nmea_loop_attr, nmea_loop, (void *)&args);
|
||||
sleep(2);
|
||||
pthread_create(&nmea_watchdog_thread, NULL, nmea_watchdog_loop, (void *)&fd);
|
||||
pthread_create(&nmea_watchdog_thread, NULL, nmea_watchdog_loop, (void *)&args);
|
||||
|
||||
/* wait for threads to complete */
|
||||
pthread_join(nmea_watchdog_thread, (void *)&fd);
|
||||
@@ -526,14 +488,23 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
terminate_gps_thread = false;
|
||||
}
|
||||
|
||||
close_port(fd);
|
||||
close_port(&fd);
|
||||
}
|
||||
|
||||
/* exit quickly if stop command has been received */
|
||||
if (thread_should_exit) {
|
||||
printf("[gps] stopped, exiting.\n");
|
||||
close(mavlink_fd);
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* if both, mode and baud is set by argument, we only need one loop*/
|
||||
if (gps_mode_try_all == false && gps_baud_try_all == false)
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
if (retry) {
|
||||
printf("[gps] No configuration was successful, retrying in %d seconds \n", RETRY_INTERVAL_SECONDS);
|
||||
mavlink_log_info(mavlink_fd, "[gps] No configuration was successful, retrying...");
|
||||
@@ -549,13 +520,21 @@ int gps_thread_main(int argc, char *argv[]) {
|
||||
sleep(RETRY_INTERVAL_SECONDS);
|
||||
}
|
||||
|
||||
close(mavlink_fd);
|
||||
|
||||
printf("[gps] exiting.\n");
|
||||
|
||||
close(mavlink_fd);
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static void usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
fprintf(stderr, "\tusage: gps {start|status|stop} -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int open_port(char *port)
|
||||
{
|
||||
int fd; /**< File descriptor for the gps port */
|
||||
@@ -566,11 +545,39 @@ int open_port(char *port)
|
||||
}
|
||||
|
||||
|
||||
void close_port(int fd)
|
||||
void close_port(int *fd)
|
||||
{
|
||||
/* Close serial port */
|
||||
close(fd);
|
||||
close(*fd);
|
||||
}
|
||||
|
||||
void setup_port(char *device, int speed, int *fd)
|
||||
{
|
||||
/* open port (baud rate is set in defconfig file) */
|
||||
*fd = open_port(device);
|
||||
|
||||
if (*fd != -1) {
|
||||
if (gps_verbose) printf("[gps] Port opened: %s at %d speed\r\n", device, speed);
|
||||
|
||||
} else {
|
||||
fprintf(stderr, "[gps] Could not open port, exiting gps app!\r\n");
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
/* Try to set baud rate */
|
||||
struct termios uart_config;
|
||||
int termios_state;
|
||||
|
||||
if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) {
|
||||
fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\r\n", device, termios_state);
|
||||
close(*fd);
|
||||
}
|
||||
if (gps_verbose) printf("[gps] Try to set baud rate %d now\n",speed);
|
||||
/* Set baud rate */
|
||||
cfsetispeed(&uart_config, speed);
|
||||
cfsetospeed(&uart_config, speed);
|
||||
if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) {
|
||||
fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", device);
|
||||
close(*fd);
|
||||
}
|
||||
}
|
||||
|
||||
+6
-8
@@ -6,15 +6,13 @@
|
||||
*/
|
||||
|
||||
#ifndef GPS_H_
|
||||
#define GPS_H_
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
int gps_fd;
|
||||
|
||||
//extern gps_bin_ubx_state_t * ubx_state;
|
||||
#define GPS_H
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
struct arg_struct {
|
||||
int *fd_ptr;
|
||||
bool *thread_should_exit_ptr;
|
||||
};
|
||||
|
||||
#endif /* GPS_H_ */
|
||||
|
||||
+25
-16
@@ -35,12 +35,14 @@
|
||||
|
||||
/* @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 <arch/board/up_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
@@ -180,7 +182,7 @@ int mtk_parse(uint8_t b, char *gps_rx_buffer)
|
||||
|
||||
}
|
||||
|
||||
int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if the thread should terminate
|
||||
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;
|
||||
@@ -191,7 +193,7 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if
|
||||
int gpsRxOverflow = 0;
|
||||
|
||||
struct pollfd fds;
|
||||
fds.fd = fd;
|
||||
fds.fd = *fd;
|
||||
fds.events = POLLIN;
|
||||
|
||||
// This blocks the task until there is something on the buffer
|
||||
@@ -206,7 +208,7 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if
|
||||
}
|
||||
|
||||
if (poll(&fds, 1, 1000) > 0) {
|
||||
if (read(fd, &c, 1) > 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.
|
||||
@@ -243,11 +245,11 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if
|
||||
return ret;
|
||||
}
|
||||
|
||||
int configure_gps_mtk(int fd)
|
||||
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));
|
||||
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");
|
||||
@@ -258,7 +260,7 @@ int configure_gps_mtk(int fd)
|
||||
}
|
||||
|
||||
//set custom mode
|
||||
result_write = write(fd, MEDIATEK_CUSTOM_BINARY_MODE, strlen(MEDIATEK_CUSTOM_BINARY_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);
|
||||
@@ -273,7 +275,7 @@ int configure_gps_mtk(int fd)
|
||||
return success;
|
||||
}
|
||||
|
||||
void *mtk_loop(void *arg)
|
||||
void *mtk_loop(void *args)
|
||||
{
|
||||
// int oldstate;
|
||||
// pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, oldstate);
|
||||
@@ -282,8 +284,10 @@ void *mtk_loop(void *arg)
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "gps mtk read", getpid());
|
||||
|
||||
/* Retrieve file descriptor */
|
||||
int fd = *((int *)arg);
|
||||
/* 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;
|
||||
@@ -313,7 +317,7 @@ void *mtk_loop(void *arg)
|
||||
mtk_gps = &mtk_gps_d;
|
||||
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), mtk_gps);
|
||||
|
||||
while (1) {
|
||||
while (!(*thread_should_exit)) {
|
||||
/* Parse a message from the gps receiver */
|
||||
if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) {
|
||||
|
||||
@@ -321,15 +325,19 @@ void *mtk_loop(void *arg)
|
||||
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 *arg)
|
||||
void *mtk_watchdog_loop(void *args)
|
||||
{
|
||||
// printf("in mtk watchdog loop\n");
|
||||
fflush(stdout);
|
||||
@@ -337,8 +345,10 @@ void *mtk_watchdog_loop(void *arg)
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "gps mtk watchdog", getpid());
|
||||
|
||||
/* Retrieve file descriptor */
|
||||
int fd = *((int *)arg);
|
||||
/* 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;
|
||||
|
||||
@@ -349,7 +359,7 @@ void *mtk_watchdog_loop(void *arg)
|
||||
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
|
||||
while (1) {
|
||||
while (!(*thread_should_exit)) {
|
||||
fflush(stdout);
|
||||
|
||||
/* if we have no update for a long time reconfigure gps */
|
||||
@@ -416,8 +426,7 @@ void *mtk_watchdog_loop(void *arg)
|
||||
|
||||
usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);
|
||||
}
|
||||
|
||||
if(gps_verbose) printf("[gps] mtk watchdog is going to terminate\n");
|
||||
close(mavlink_fd);
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
+4
-4
@@ -87,12 +87,12 @@ 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 read_gps_mtk(int *fd, char *gps_rx_buffer, int buffer_size);
|
||||
|
||||
int configure_gps_mtk(int fd);
|
||||
int configure_gps_mtk(int *fd);
|
||||
|
||||
void *mtk_loop(void *arg);
|
||||
void *mtk_loop(void *args);
|
||||
|
||||
void *mtk_watchdog_loop(void *arg);
|
||||
void *mtk_watchdog_loop(void *args);
|
||||
|
||||
#endif /* MTK_H_ */
|
||||
|
||||
+28
-18
@@ -34,13 +34,17 @@
|
||||
****************************************************************************/
|
||||
|
||||
/* @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 <arch/board/up_hrt.h>
|
||||
|
||||
#define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2
|
||||
#define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2
|
||||
@@ -59,7 +63,7 @@ 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 read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser)
|
||||
{
|
||||
int ret = 1;
|
||||
char c;
|
||||
@@ -69,7 +73,7 @@ int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info,
|
||||
int gpsRxOverflow = 0;
|
||||
|
||||
struct pollfd fds;
|
||||
fds.fd = fd;
|
||||
fds.fd = *fd;
|
||||
fds.events = POLLIN;
|
||||
|
||||
// NMEA or SINGLE-SENTENCE GPS mode
|
||||
@@ -86,7 +90,7 @@ int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info,
|
||||
}
|
||||
|
||||
if (poll(&fds, 1, 1000) > 0) {
|
||||
if (read(fd, &c, 1) > 0) {
|
||||
if (read(*fd, &c, 1) > 0) {
|
||||
// detect start while acquiring stream
|
||||
// printf("Char = %c\n", c);
|
||||
if (!start_flag && (c == '$')) {
|
||||
@@ -155,13 +159,15 @@ float ndeg2degree(float val)
|
||||
return val;
|
||||
}
|
||||
|
||||
void *nmea_loop(void *arg)
|
||||
void *nmea_loop(void *args)
|
||||
{
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "gps nmea read", getpid());
|
||||
|
||||
/* Retrieve file descriptor */
|
||||
int fd = *((int *)arg);
|
||||
/* 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;
|
||||
@@ -174,11 +180,11 @@ void *nmea_loop(void *arg)
|
||||
nmea_zero_INFO(info);
|
||||
|
||||
/* advertise GPS topic */
|
||||
struct vehicle_gps_position_s nmea_gps_d = {0};
|
||||
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 (1) {
|
||||
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);
|
||||
|
||||
@@ -200,11 +206,11 @@ void *nmea_loop(void *arg)
|
||||
// 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->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) * 1e7);
|
||||
nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7);
|
||||
nmea_gps->alt = (int32_t)(info->elv * 1e3);
|
||||
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
|
||||
@@ -251,12 +257,12 @@ void *nmea_loop(void *arg)
|
||||
|
||||
//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 *arg)
|
||||
void *nmea_watchdog_loop(void *args)
|
||||
{
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "gps nmea watchdog", getpid());
|
||||
@@ -267,9 +273,14 @@ void *nmea_watchdog_loop(void *arg)
|
||||
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 (1) {
|
||||
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);
|
||||
@@ -328,8 +339,7 @@ void *nmea_watchdog_loop(void *arg)
|
||||
|
||||
usleep(NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS);
|
||||
}
|
||||
|
||||
if(gps_verbose) printf("[gps] nmea watchdog loop is going to terminate\n");
|
||||
close(mavlink_fd);
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
@@ -30,7 +30,7 @@ extern pthread_mutex_t *nmea_mutex;
|
||||
|
||||
|
||||
|
||||
int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser);
|
||||
int read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser);
|
||||
|
||||
void *nmea_loop(void *arg);
|
||||
|
||||
|
||||
+51
-43
@@ -35,12 +35,16 @@
|
||||
|
||||
/* @file U-Blox protocol implementation */
|
||||
|
||||
|
||||
#include "ubx.h"
|
||||
#include "gps.h"
|
||||
#include <sys/prctl.h>
|
||||
#include <poll.h>
|
||||
#include <arch/board/up_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
@@ -340,8 +344,8 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
|
||||
|
||||
|
||||
|
||||
ubx_gps->time_gps_usec = (uint64_t)epoch * 1e6; //TODO: test this
|
||||
ubx_gps->time_gps_usec += packet->time_nanoseconds * 1e-3;
|
||||
ubx_gps->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
|
||||
ubx_gps->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
|
||||
|
||||
ubx_gps->timestamp = hrt_absolute_time();
|
||||
ubx_gps->counter++;
|
||||
@@ -474,7 +478,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
|
||||
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
|
||||
|
||||
ubx_gps->vel = (uint16_t)packet->speed;
|
||||
ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3);
|
||||
ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3f);
|
||||
|
||||
ubx_gps->timestamp = hrt_absolute_time();
|
||||
ubx_gps->counter++;
|
||||
@@ -570,44 +574,44 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length)
|
||||
// printf("[%x,%x]\n", message[length-2], message[length-1]);
|
||||
}
|
||||
|
||||
int configure_gps_ubx(int fd)
|
||||
int configure_gps_ubx(int *fd)
|
||||
{
|
||||
fflush((FILE *)fd);
|
||||
//fflush(((FILE *)fd));
|
||||
|
||||
//TODO: write this in a loop once it is tested
|
||||
//UBX_CFG_PRT_PART:
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , fd);
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , *fd);
|
||||
|
||||
usleep(100000);
|
||||
|
||||
//NAV_POSLLH:
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , fd);
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , *fd);
|
||||
usleep(100000);
|
||||
|
||||
//NAV_TIMEUTC:
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , fd);
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , *fd);
|
||||
usleep(100000);
|
||||
|
||||
//NAV_DOP:
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , fd);
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , *fd);
|
||||
usleep(100000);
|
||||
|
||||
//NAV_SOL:
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , fd);
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , *fd);
|
||||
usleep(100000);
|
||||
|
||||
|
||||
//NAV_SVINFO:
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , fd);
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , *fd);
|
||||
usleep(100000);
|
||||
|
||||
//NAV_VELNED:
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , fd);
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , *fd);
|
||||
usleep(100000);
|
||||
|
||||
|
||||
//RXM_SVSI:
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , fd);
|
||||
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd);
|
||||
usleep(100000);
|
||||
|
||||
return 0;
|
||||
@@ -615,16 +619,15 @@ int configure_gps_ubx(int fd)
|
||||
|
||||
|
||||
|
||||
int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size)
|
||||
int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
|
||||
{
|
||||
|
||||
uint8_t ret = 0;
|
||||
uint8_t c;
|
||||
int rx_count = 0;
|
||||
int gpsRxOverflow = 0;
|
||||
|
||||
struct pollfd fds;
|
||||
fds.fd = fd;
|
||||
fds.fd = *fd;
|
||||
fds.events = POLLIN;
|
||||
|
||||
// UBX GPS mode
|
||||
@@ -641,7 +644,7 @@ int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size)
|
||||
}
|
||||
|
||||
if (poll(&fds, 1, 1000) > 0) {
|
||||
if (read(fd, &c, 1) > 0) {
|
||||
if (read(*fd, &c, 1) > 0) {
|
||||
|
||||
// printf("Read %x\n",c);
|
||||
if (rx_count >= buffer_size) {
|
||||
@@ -685,7 +688,7 @@ int write_config_message_ubx(uint8_t *message, size_t length, int fd)
|
||||
uint8_t ck_a = 0;
|
||||
uint8_t ck_b = 0;
|
||||
|
||||
int i;
|
||||
unsigned int i;
|
||||
|
||||
for (i = 2; i < length; i++) {
|
||||
ck_a = ck_a + message[i];
|
||||
@@ -702,14 +705,16 @@ int write_config_message_ubx(uint8_t *message, size_t length, int fd)
|
||||
|
||||
}
|
||||
|
||||
void *ubx_watchdog_loop(void *arg)
|
||||
void *ubx_watchdog_loop(void *args)
|
||||
{
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "gps ubx watchdog", getpid());
|
||||
|
||||
|
||||
/* Retrieve file descriptor */
|
||||
int fd = *((int *)arg);
|
||||
/* 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;
|
||||
|
||||
/* GPS watchdog error message skip counter */
|
||||
|
||||
@@ -721,7 +726,9 @@ void *ubx_watchdog_loop(void *arg)
|
||||
|
||||
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
while (1) {
|
||||
//int err_skip_counter = 0;
|
||||
|
||||
while (!(*thread_should_exit)) {
|
||||
/* if some values are to old reconfigure gps */
|
||||
int i;
|
||||
pthread_mutex_lock(ubx_mutex);
|
||||
@@ -732,7 +739,7 @@ void *ubx_watchdog_loop(void *arg)
|
||||
// printf("timestamp_now=%llu\n", timestamp_now);
|
||||
// printf("last_message_timestamps=%llu\n", ubx_state->last_message_timestamps[i]);
|
||||
if (timestamp_now - ubx_state->last_message_timestamps[i] > UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS) {
|
||||
// printf("Warning: GPS ubx message %d not received for a long time\n", i);
|
||||
//printf("Warning: GPS ubx message %d not received for a long time\n", i);
|
||||
all_okay = false;
|
||||
}
|
||||
}
|
||||
@@ -748,7 +755,7 @@ void *ubx_watchdog_loop(void *arg)
|
||||
// err_skip_counter = 20;
|
||||
// }
|
||||
// err_skip_counter--;
|
||||
// printf("gps_mode_try_all =%u, ubx_fail_count=%u, ubx_healthy=%u, once_ok=%u\n", gps_mode_try_all, ubx_fail_count, ubx_healthy, once_ok);
|
||||
//printf("gps_mode_try_all =%u, ubx_fail_count=%u, ubx_healthy=%u, once_ok=%u\n", gps_mode_try_all, ubx_fail_count, ubx_healthy, once_ok);
|
||||
|
||||
|
||||
/* If we have too many failures and another mode or baud should be tried, exit... */
|
||||
@@ -766,7 +773,6 @@ void *ubx_watchdog_loop(void *arg)
|
||||
ubx_healthy = false;
|
||||
ubx_success_count = 0;
|
||||
}
|
||||
|
||||
/* trying to reconfigure the gps configuration */
|
||||
configure_gps_ubx(fd);
|
||||
fflush(stdout);
|
||||
@@ -777,7 +783,7 @@ void *ubx_watchdog_loop(void *arg)
|
||||
ubx_success_count++;
|
||||
|
||||
if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
|
||||
printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
|
||||
//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
|
||||
// global_data_send_subsystem_info(&ubx_present_enabled_healthy);
|
||||
mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n");
|
||||
ubx_healthy = true;
|
||||
@@ -790,18 +796,20 @@ void *ubx_watchdog_loop(void *arg)
|
||||
usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);
|
||||
}
|
||||
|
||||
if(gps_verbose) printf("[gps] ubx loop is going to terminate\n");
|
||||
close(mavlink_fd);
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void *ubx_loop(void *arg)
|
||||
void *ubx_loop(void *args)
|
||||
{
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "gps ubx read", getpid());
|
||||
|
||||
/* Retrieve file descriptor */
|
||||
int fd = *((int *)arg);
|
||||
|
||||
/* 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 */
|
||||
char gps_rx_buffer[UBX_BUFFER_SIZE];
|
||||
@@ -809,14 +817,13 @@ void *ubx_loop(void *arg)
|
||||
|
||||
if (gps_verbose) printf("[gps] UBX protocol driver starting..\r\n");
|
||||
|
||||
//set parameters for ubx
|
||||
//set parameters for ubx_state
|
||||
|
||||
|
||||
// //ubx state
|
||||
// gps_bin_ubx_state_t * ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
|
||||
// printf("gps: ubx_state created\n");
|
||||
// ubx_decode_init();
|
||||
// ubx_state->print_errors = false;
|
||||
//ubx state
|
||||
ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
|
||||
//printf("gps: ubx_state created\n");
|
||||
ubx_decode_init();
|
||||
ubx_state->print_errors = false;
|
||||
|
||||
|
||||
/* set parameters for ubx */
|
||||
@@ -825,7 +832,7 @@ void *ubx_loop(void *arg)
|
||||
printf("[gps] Configuration of gps module to ubx failed\r\n");
|
||||
|
||||
/* Write shared variable sys_status */
|
||||
|
||||
// TODO enable this again
|
||||
//global_data_send_subsystem_info(&ubx_present);
|
||||
|
||||
} else {
|
||||
@@ -834,17 +841,17 @@ void *ubx_loop(void *arg)
|
||||
// XXX Shouldn't the system status only change if the module is known to work ok?
|
||||
|
||||
/* Write shared variable sys_status */
|
||||
|
||||
// TODO enable this again
|
||||
//global_data_send_subsystem_info(&ubx_present_enabled);
|
||||
}
|
||||
|
||||
struct vehicle_gps_position_s ubx_gps_d = {0};
|
||||
struct vehicle_gps_position_s ubx_gps_d = {.counter = 0};
|
||||
|
||||
ubx_gps = &ubx_gps_d;
|
||||
|
||||
orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
|
||||
|
||||
while (1) {
|
||||
while (!(*thread_should_exit)) {
|
||||
/* Parse a message from the gps receiver */
|
||||
if (0 == read_gps_ubx(fd, gps_rx_buffer, UBX_BUFFER_SIZE)) {
|
||||
/* publish new GPS position */
|
||||
@@ -857,7 +864,8 @@ void *ubx_loop(void *arg)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if(gps_verbose) printf("[gps] ubx read is going to terminate\n");
|
||||
close(gps_pub);
|
||||
return NULL;
|
||||
|
||||
}
|
||||
|
||||
+4
-4
@@ -298,17 +298,17 @@ 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 configure_gps_ubx(int *fd);
|
||||
|
||||
int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size);
|
||||
int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size);
|
||||
|
||||
int write_config_message_ubx(uint8_t *message, size_t length, int fd);
|
||||
|
||||
void calculate_ubx_checksum(uint8_t *message, uint8_t length);
|
||||
|
||||
void *ubx_watchdog_loop(void *arg);
|
||||
void *ubx_watchdog_loop(void *args);
|
||||
|
||||
void *ubx_loop(void *arg);
|
||||
void *ubx_loop(void *args);
|
||||
|
||||
|
||||
#endif /* UBX_H_ */
|
||||
|
||||
+5
-3
@@ -234,9 +234,11 @@
|
||||
#endif
|
||||
|
||||
/* This is the maximum number of arguments that will be accepted for a command */
|
||||
|
||||
#define NSH_MAX_ARGUMENTS 6
|
||||
|
||||
#ifdef CONFIG_NSH_MAX_ARGUMENTS
|
||||
# define NSH_MAX_ARGUMENTS CONFIG_NSH_MAX_ARGUMENTS
|
||||
#else
|
||||
# define NSH_MAX_ARGUMENTS 10
|
||||
#endif
|
||||
/* strerror() produces much nicer output but is, however, quite large and
|
||||
* will only be used if CONFIG_NSH_STRERROR is defined. Note that the strerror
|
||||
* interface must also have been enabled with CONFIG_LIBC_STRERROR.
|
||||
|
||||
@@ -941,6 +941,7 @@ CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6"
|
||||
# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
|
||||
# CONFIG_NSH_STRERROR - Use strerror(errno)
|
||||
# CONFIG_NSH_LINELEN - Maximum length of one command line
|
||||
# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line
|
||||
# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
|
||||
# CONFIG_NSH_DISABLESCRIPT - Disable scripting support
|
||||
# CONFIG_NSH_DISABLEBG - Disable background commands
|
||||
@@ -972,6 +973,7 @@ CONFIG_NSH_BUILTIN_APPS=y
|
||||
CONFIG_NSH_FILEIOSIZE=512
|
||||
CONFIG_NSH_STRERROR=y
|
||||
CONFIG_NSH_LINELEN=128
|
||||
CONFIG_NSH_MAX_ARGUMENTS=12
|
||||
CONFIG_NSH_NESTDEPTH=8
|
||||
CONFIG_NSH_DISABLESCRIPT=n
|
||||
CONFIG_NSH_DISABLEBG=n
|
||||
|
||||
Reference in New Issue
Block a user