Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 03:36:07 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

gps starting and stopping should be working correctly now, ubx not continuing whith configuring should be fixed

Browse Source
This commit is contained in:
Julian Oes
2012-09-20 11:56:30 +02:00
parent 71b37a859c
commit e7241fb37f
8 changed files with 148 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/gps/gps.c
+42 -13
View File
@@ -38,7 +38,6 @@
*/ */
#include "gps.h" #include "gps.h"
#include <nuttx/config.h> #include <nuttx/config.h>
#include <unistd.h> #include <unistd.h>
#include <stdlib.h> #include <stdlib.h>
@@ -58,8 +57,9 @@
#include <v1.0/common/mavlink.h> #include <v1.0/common/mavlink.h>
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
static bool thread_should_exit; /**< Deamon status flag */
static bool thread_running = false; /**< Deamon status flag */ static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */ static int deamon_task; /**< Handle of deamon task / thread */
/** /**
* GPS module readout and publishing. * GPS module readout and publishing.
@@ -104,7 +104,6 @@ enum GPS_MODES {
}; };
#define AUTO_DETECTION_COUNT 8 #define AUTO_DETECTION_COUNT 8
const int autodetection_baudrates[] = {B9600, B38400, B38400, B9600, B9600, B38400, B9600, B38400}; 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 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
@@ -140,14 +139,14 @@ int gps_main(int argc, char *argv[])
exit(0); exit(0);
} }
gps_thread_should_exit = false; thread_should_exit = false;
deamon_task = task_create("gps", SCHED_PRIORITY_DEFAULT, 4096, gps_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); deamon_task = task_create("gps", SCHED_PRIORITY_DEFAULT, 4096, gps_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true; thread_running = true;
exit(0); exit(0);
} }
if (!strcmp(argv[1], "stop")) { if (!strcmp(argv[1], "stop")) {
gps_thread_should_exit = true; thread_should_exit = true;
exit(0); exit(0);
} }
@@ -193,6 +192,7 @@ int gps_thread_main(int argc, char *argv[]) {
for (i = 0; i < argc; i++) { for (i = 0; i < argc; i++) {
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set
printf(commandline_usage, argv[0]); printf(commandline_usage, argv[0]);
thread_running = false;
return 0; return 0;
} }
@@ -202,6 +202,7 @@ int gps_thread_main(int argc, char *argv[]) {
} else { } else {
printf(commandline_usage, argv[0]); printf(commandline_usage, argv[0]);
thread_running = false;
return 0; return 0;
} }
} }
@@ -212,6 +213,7 @@ int gps_thread_main(int argc, char *argv[]) {
} else { } else {
printf(commandline_usage, argv[0]); printf(commandline_usage, argv[0]);
thread_running = false;
return 0; return 0;
} }
} }
@@ -222,6 +224,7 @@ int gps_thread_main(int argc, char *argv[]) {
} else { } else {
printf(commandline_usage, argv[0]); printf(commandline_usage, argv[0]);
thread_running = false;
return 0; return 0;
} }
} }
@@ -232,6 +235,7 @@ int gps_thread_main(int argc, char *argv[]) {
} else { } else {
printf(commandline_usage, argv[0]); printf(commandline_usage, argv[0]);
thread_running = false;
return 0; return 0;
} }
} }
@@ -297,13 +301,14 @@ int gps_thread_main(int argc, char *argv[]) {
} else { } else {
fprintf(stderr, "\t[gps] Invalid mode argument\n"); fprintf(stderr, "\t[gps] Invalid mode argument\n");
printf(commandline_usage); printf(commandline_usage);
thread_running = false;
return ERROR; return ERROR;
} }
/* Declare file descriptor for gps here, open port later in setup_port */ /* Declare file descriptor for gps here, open port later in setup_port */
int fd; int fd;
while (!gps_thread_should_exit) { while (!thread_should_exit) {
/* Infinite retries or break if retry == false */ /* Infinite retries or break if retry == false */
/* Loop over all configurations of baud rate and protocol */ /* Loop over all configurations of baud rate and protocol */
@@ -351,13 +356,17 @@ int gps_thread_main(int argc, char *argv[]) {
pthread_attr_init(&ubx_loop_attr); pthread_attr_init(&ubx_loop_attr);
pthread_attr_setstacksize(&ubx_loop_attr, 3000); pthread_attr_setstacksize(&ubx_loop_attr, 3000);
pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&fd); 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 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_t ubx_wd_attr;
pthread_attr_init(&ubx_wd_attr); pthread_attr_init(&ubx_wd_attr);
pthread_attr_setstacksize(&ubx_wd_attr, 1400); 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); if (pthread_create_res != 0) fprintf(stderr, "[gps] ERROR: could not create ubx watchdog thread, pthread_create =%d\n", pthread_create_res);
@@ -372,7 +381,17 @@ int gps_thread_main(int argc, char *argv[]) {
gps_mode_success = true; gps_mode_success = true;
terminate_gps_thread = false; 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); close_port(&fd);
} else if (current_gps_mode == GPS_MODE_MTK) { } else if (current_gps_mode == GPS_MODE_MTK) {
if (gps_verbose) printf("[gps] trying MTK binary mode at %d baud\n", current_gps_speed); if (gps_verbose) printf("[gps] trying MTK binary mode at %d baud\n", current_gps_speed);
@@ -398,9 +417,14 @@ int gps_thread_main(int argc, char *argv[]) {
pthread_attr_t mtk_loop_attr; pthread_attr_t mtk_loop_attr;
pthread_attr_init(&mtk_loop_attr); pthread_attr_init(&mtk_loop_attr);
pthread_attr_setstacksize(&mtk_loop_attr, 2048); 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); 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 */ /* wait for threads to complete */
pthread_join(mtk_watchdog_thread, (void *)&fd); pthread_join(mtk_watchdog_thread, (void *)&fd);
@@ -442,9 +466,14 @@ int gps_thread_main(int argc, char *argv[]) {
pthread_attr_t nmea_loop_attr; pthread_attr_t nmea_loop_attr;
pthread_attr_init(&nmea_loop_attr); pthread_attr_init(&nmea_loop_attr);
pthread_attr_setstacksize(&nmea_loop_attr, 4096); 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); 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 */ /* wait for threads to complete */
pthread_join(nmea_watchdog_thread, (void *)&fd); pthread_join(nmea_watchdog_thread, (void *)&fd);
@@ -463,7 +492,7 @@ int gps_thread_main(int argc, char *argv[]) {
} }
/* exit quickly if stop command has been received */ /* exit quickly if stop command has been received */
if (gps_thread_should_exit) { if (thread_should_exit) {
printf("[gps] stopped, exiting.\n"); printf("[gps] stopped, exiting.\n");
close(mavlink_fd); close(mavlink_fd);
thread_running = false; thread_running = false;
apps/gps/gps.h
+4 -1
View File
@@ -10,6 +10,9 @@
#include <stdbool.h> #include <stdbool.h>
static bool gps_thread_should_exit; /**< Deamon status flag */ struct arg_struct {
int *fd_ptr;
bool *thread_should_exit_ptr;
};
#endif /* GPS_H_ */ #endif /* GPS_H_ */
apps/gps/mtk.c
+24 -16
View File
@@ -42,6 +42,7 @@
#include <sys/prctl.h> #include <sys/prctl.h>
#include <pthread.h> #include <pthread.h>
#include <poll.h> #include <poll.h>
#include <fcntl.h>
#include <arch/board/up_hrt.h> #include <arch/board/up_hrt.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
@@ -181,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"); // printf("in read_gps_mtk\n");
uint8_t ret = 0; uint8_t ret = 0;
@@ -192,7 +193,7 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if
int gpsRxOverflow = 0; int gpsRxOverflow = 0;
struct pollfd fds; struct pollfd fds;
fds.fd = fd; fds.fd = *fd;
fds.events = POLLIN; fds.events = POLLIN;
// This blocks the task until there is something on the buffer // This blocks the task until there is something on the buffer
@@ -207,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 (poll(&fds, 1, 1000) > 0) {
if (read(fd, &c, 1) > 0) { if (read(*fd, &c, 1) > 0) {
// printf("Read %x\n",c); // printf("Read %x\n",c);
if (rx_count >= buffer_size) { if (rx_count >= buffer_size) {
// The buffer is already full and we haven't found a valid NMEA sentence. // The buffer is already full and we haven't found a valid NMEA sentence.
@@ -244,11 +245,11 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if
return ret; return ret;
} }
int configure_gps_mtk(int fd) int configure_gps_mtk(int *fd)
{ {
int success = 0; int success = 0;
size_t result_write; 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)) { if (result_write != strlen(MEDIATEK_REFRESH_RATE_10HZ)) {
printf("[gps] Set update speed to 10 Hz failed\r\n"); printf("[gps] Set update speed to 10 Hz failed\r\n");
@@ -259,7 +260,7 @@ int configure_gps_mtk(int fd)
} }
//set custom mode //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)) { if (result_write != strlen(MEDIATEK_CUSTOM_BINARY_MODE)) {
//global_data_send_subsystem_info(&mtk_present); //global_data_send_subsystem_info(&mtk_present);
@@ -274,7 +275,7 @@ int configure_gps_mtk(int fd)
return success; return success;
} }
void *mtk_loop(void *arg) void *mtk_loop(void *args)
{ {
// int oldstate; // int oldstate;
// pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, oldstate); // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, oldstate);
@@ -283,8 +284,10 @@ void *mtk_loop(void *arg)
/* Set thread name */ /* Set thread name */
prctl(PR_SET_NAME, "gps mtk read", getpid()); prctl(PR_SET_NAME, "gps mtk read", getpid());
/* Retrieve file descriptor */ /* Retrieve file descriptor and thread flag */
int fd = *((int *)arg); 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 */ /* Initialize gps stuff */
// int buffer_size = 1000; // int buffer_size = 1000;
@@ -314,7 +317,7 @@ void *mtk_loop(void *arg)
mtk_gps = &mtk_gps_d; mtk_gps = &mtk_gps_d;
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), mtk_gps); orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), mtk_gps);
while (!gps_thread_should_exit) { while (!(*thread_should_exit)) {
/* Parse a message from the gps receiver */ /* Parse a message from the gps receiver */
if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) { if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) {
@@ -322,15 +325,19 @@ void *mtk_loop(void *arg)
orb_publish(ORB_ID(vehicle_gps_position), gps_handle, mtk_gps); orb_publish(ORB_ID(vehicle_gps_position), gps_handle, mtk_gps);
} else { } else {
/* de-advertise */
close(gps_handle);
break; break;
} }
} }
close(gps_handle);
if(gps_verbose) printf("[gps] mtk loop is going to terminate\n");
return NULL; return NULL;
} }
void *mtk_watchdog_loop(void *arg) void *mtk_watchdog_loop(void *args)
{ {
// printf("in mtk watchdog loop\n"); // printf("in mtk watchdog loop\n");
fflush(stdout); fflush(stdout);
@@ -338,8 +345,10 @@ void *mtk_watchdog_loop(void *arg)
/* Set thread name */ /* Set thread name */
prctl(PR_SET_NAME, "gps mtk watchdog", getpid()); prctl(PR_SET_NAME, "gps mtk watchdog", getpid());
/* Retrieve file descriptor */ /* Retrieve file descriptor and thread flag */
int fd = *((int *)arg); 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; bool mtk_healthy = false;
@@ -350,7 +359,7 @@ void *mtk_watchdog_loop(void *arg)
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
while (!gps_thread_should_exit) { while (!(*thread_should_exit)) {
fflush(stdout); fflush(stdout);
/* if we have no update for a long time reconfigure gps */ /* if we have no update for a long time reconfigure gps */
@@ -417,8 +426,7 @@ void *mtk_watchdog_loop(void *arg)
usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS); usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);
} }
if(gps_verbose) printf("[gps] mtk watchdog is going to terminate\n");
close(mavlink_fd); close(mavlink_fd);
return NULL; return NULL;
} }
apps/gps/mtk.h
+4 -4
View File
@@ -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 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_ */ #endif /* MTK_H_ */
apps/gps/nmea_helper.c
+28 -18
View File
@@ -37,10 +37,14 @@
#include "gps.h" #include "gps.h"
#include "nmea_helper.h" #include "nmea_helper.h"
#include <sys/prctl.h> #include <sys/prctl.h>
#include <unistd.h>
#include <poll.h> #include <poll.h>
#include <fcntl.h>
#include <unistd.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
#include <arch/board/up_hrt.h>
#define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2 #define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2
#define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2 #define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2
@@ -59,7 +63,7 @@ extern bool gps_verbose;
extern int current_gps_speed; 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; int ret = 1;
char c; char c;
@@ -69,13 +73,13 @@ int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info,
int gpsRxOverflow = 0; int gpsRxOverflow = 0;
struct pollfd fds; struct pollfd fds;
fds.fd = fd; fds.fd = *fd;
fds.events = POLLIN; fds.events = POLLIN;
// NMEA or SINGLE-SENTENCE GPS mode // NMEA or SINGLE-SENTENCE GPS mode
while (!gps_thread_should_exit) { while (1) {
//check if the thread should terminate //check if the thread should terminate
if (terminate_gps_thread == true) { if (terminate_gps_thread == true) {
// printf("terminate_gps_thread=%u ", terminate_gps_thread); // printf("terminate_gps_thread=%u ", terminate_gps_thread);
@@ -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 (poll(&fds, 1, 1000) > 0) {
if (read(fd, &c, 1) > 0) { if (read(*fd, &c, 1) > 0) {
// detect start while acquiring stream // detect start while acquiring stream
// printf("Char = %c\n", c); // printf("Char = %c\n", c);
if (!start_flag && (c == '$')) { if (!start_flag && (c == '$')) {
@@ -155,13 +159,15 @@ float ndeg2degree(float val)
return val; return val;
} }
void *nmea_loop(void *arg) void *nmea_loop(void *args)
{ {
/* Set thread name */ /* Set thread name */
prctl(PR_SET_NAME, "gps nmea read", getpid()); prctl(PR_SET_NAME, "gps nmea read", getpid());
/* Retrieve file descriptor */ /* Retrieve file descriptor and thread flag */
int fd = *((int *)arg); 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 */ /* Initialize gps stuff */
nmeaINFO info_d; nmeaINFO info_d;
@@ -174,11 +180,11 @@ void *nmea_loop(void *arg)
nmea_zero_INFO(info); nmea_zero_INFO(info);
/* advertise GPS topic */ /* 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; nmea_gps = &nmea_gps_d;
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps); orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps);
while (!gps_thread_should_exit) { while (!(*thread_should_exit)) {
/* Parse a message from the gps receiver */ /* Parse a message from the gps receiver */
uint8_t read_res = read_gps_nmea(fd, gps_rx_buffer, NMEA_BUFFER_SIZE, info, &parser); 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); // 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->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->fix_type = (uint8_t)info->fix;
nmea_gps->lat = (int32_t)(ndeg2degree(info->lat) * 1e7); nmea_gps->lat = (int32_t)(ndeg2degree(info->lat) * 1e7f);
nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7); nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7f);
nmea_gps->alt = (int32_t)(info->elv * 1e3); nmea_gps->alt = (int32_t)(info->elv * 1000.0f);
nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling
nmea_gps->epv = (uint16_t)(info->VDOP * 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->vel = (uint16_t)(info->speed * 1000 / 36); //*1000/3600*100
@@ -251,12 +257,12 @@ void *nmea_loop(void *arg)
//destroy gps parser //destroy gps parser
nmea_parser_destroy(&parser); nmea_parser_destroy(&parser);
if(gps_verbose) printf("[gps] nmea loop is going to terminate\n");
return NULL; return NULL;
} }
void *nmea_watchdog_loop(void *arg) void *nmea_watchdog_loop(void *args)
{ {
/* Set thread name */ /* Set thread name */
prctl(PR_SET_NAME, "gps nmea watchdog", getpid()); prctl(PR_SET_NAME, "gps nmea watchdog", getpid());
@@ -267,9 +273,14 @@ void *nmea_watchdog_loop(void *arg)
uint8_t nmea_success_count = 0; uint8_t nmea_success_count = 0;
bool once_ok = false; 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); int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
while (!gps_thread_should_exit) { while (!(*thread_should_exit)) {
// printf("nmea_watchdog_loop : while "); // printf("nmea_watchdog_loop : while ");
/* if we have no update for a long time print warning (in nmea mode there is no reconfigure) */ /* if we have no update for a long time print warning (in nmea mode there is no reconfigure) */
pthread_mutex_lock(nmea_mutex); pthread_mutex_lock(nmea_mutex);
@@ -328,8 +339,7 @@ void *nmea_watchdog_loop(void *arg)
usleep(NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS); usleep(NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS);
} }
if(gps_verbose) printf("[gps] nmea watchdog loop is going to terminate\n");
close(mavlink_fd); close(mavlink_fd);
return NULL; return NULL;
} }
apps/gps/nmea_helper.h
+1 -1
View File
@@ -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); void *nmea_loop(void *arg);
apps/gps/ubx.c
+42 -34
View File
@@ -35,13 +35,16 @@
/* @file U-Blox protocol implementation */ /* @file U-Blox protocol implementation */
#include "gps.h"
#include "ubx.h" #include "ubx.h"
#include "gps.h"
#include <sys/prctl.h> #include <sys/prctl.h>
#include <poll.h> #include <poll.h>
#include <arch/board/up_hrt.h> #include <arch/board/up_hrt.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <string.h> #include <string.h>
#include <stdbool.h>
#include <fcntl.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
@@ -341,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 = (uint64_t)epoch * 1000000; //TODO: test this
ubx_gps->time_gps_usec += packet->time_nanoseconds * 1e-3; ubx_gps->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
ubx_gps->timestamp = hrt_absolute_time(); ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++; ubx_gps->counter++;
@@ -475,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) { if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
ubx_gps->vel = (uint16_t)packet->speed; 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->timestamp = hrt_absolute_time();
ubx_gps->counter++; ubx_gps->counter++;
@@ -573,7 +576,7 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length)
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 //TODO: write this in a loop once it is tested
//UBX_CFG_PRT_PART: //UBX_CFG_PRT_PART:
@@ -616,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 ret = 0;
uint8_t c; uint8_t c;
int rx_count = 0; int rx_count = 0;
int gpsRxOverflow = 0; int gpsRxOverflow = 0;
struct pollfd fds; struct pollfd fds;
fds.fd = fd; fds.fd = *fd;
fds.events = POLLIN; fds.events = POLLIN;
// UBX GPS mode // UBX GPS mode
@@ -642,7 +644,7 @@ int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size)
} }
if (poll(&fds, 1, 1000) > 0) { if (poll(&fds, 1, 1000) > 0) {
if (read(fd, &c, 1) > 0) { if (read(*fd, &c, 1) > 0) {
// printf("Read %x\n",c); // printf("Read %x\n",c);
if (rx_count >= buffer_size) { if (rx_count >= buffer_size) {
@@ -686,7 +688,7 @@ int write_config_message_ubx(uint8_t *message, size_t length, int fd)
uint8_t ck_a = 0; uint8_t ck_a = 0;
uint8_t ck_b = 0; uint8_t ck_b = 0;
int i; unsigned int i;
for (i = 2; i < length; i++) { for (i = 2; i < length; i++) {
ck_a = ck_a + message[i]; ck_a = ck_a + message[i];
@@ -703,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 */ /* Set thread name */
prctl(PR_SET_NAME, "gps ubx watchdog", getpid()); prctl(PR_SET_NAME, "gps ubx watchdog", getpid());
/* Retrieve file descriptor */ /* Retrieve file descriptor and thread flag */
int fd = *((int *)arg); 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 */ /* GPS watchdog error message skip counter */
@@ -722,7 +726,9 @@ void *ubx_watchdog_loop(void *arg)
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
while (!gps_thread_should_exit) { int err_skip_counter = 0;
while (!(*thread_should_exit)) {
/* if some values are to old reconfigure gps */ /* if some values are to old reconfigure gps */
int i; int i;
pthread_mutex_lock(ubx_mutex); pthread_mutex_lock(ubx_mutex);
@@ -733,7 +739,7 @@ void *ubx_watchdog_loop(void *arg)
// printf("timestamp_now=%llu\n", timestamp_now); // printf("timestamp_now=%llu\n", timestamp_now);
// printf("last_message_timestamps=%llu\n", ubx_state->last_message_timestamps[i]); // 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) { 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; all_okay = false;
} }
} }
@@ -743,13 +749,13 @@ void *ubx_watchdog_loop(void *arg)
if (!all_okay) { if (!all_okay) {
/* gps error */ /* gps error */
ubx_fail_count++; ubx_fail_count++;
// if (err_skip_counter == 0) if (err_skip_counter == 0)
// { {
// printf("GPS Watchdog detected gps not running or having problems\n"); printf("GPS Watchdog detected gps not running or having problems\n");
// err_skip_counter = 20; err_skip_counter = 20;
// } }
// err_skip_counter--; 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... */ /* If we have too many failures and another mode or baud should be tried, exit... */
@@ -767,9 +773,8 @@ void *ubx_watchdog_loop(void *arg)
ubx_healthy = false; ubx_healthy = false;
ubx_success_count = 0; ubx_success_count = 0;
} }
/* trying to reconfigure the gps configuration */ /* trying to reconfigure the gps configuration */
configure_gps_ubx(&fd); configure_gps_ubx(fd);
fflush(stdout); fflush(stdout);
sleep(1); sleep(1);
@@ -778,7 +783,7 @@ void *ubx_watchdog_loop(void *arg)
ubx_success_count++; ubx_success_count++;
if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) { 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); // global_data_send_subsystem_info(&ubx_present_enabled_healthy);
mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n"); mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n");
ubx_healthy = true; ubx_healthy = true;
@@ -791,18 +796,20 @@ void *ubx_watchdog_loop(void *arg)
usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS); usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);
} }
if(gps_verbose) printf("[gps] ubx loop is going to terminate\n");
close(mavlink_fd); close(mavlink_fd);
return NULL; return NULL;
} }
void *ubx_loop(void *arg) void *ubx_loop(void *args)
{ {
/* Set thread name */ /* Set thread name */
prctl(PR_SET_NAME, "gps ubx read", getpid()); prctl(PR_SET_NAME, "gps ubx read", getpid());
/* Retrieve file descriptor */ /* Retrieve file descriptor and thread flag */
int fd = *((int *)arg); 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 */ /* Initialize gps stuff */
char gps_rx_buffer[UBX_BUFFER_SIZE]; char gps_rx_buffer[UBX_BUFFER_SIZE];
@@ -813,7 +820,7 @@ void *ubx_loop(void *arg)
//set parameters for ubx_state //set parameters for ubx_state
//ubx state //ubx state
gps_bin_ubx_state_t * ubx_state = malloc(sizeof(gps_bin_ubx_state_t)); ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
//printf("gps: ubx_state created\n"); //printf("gps: ubx_state created\n");
ubx_decode_init(); ubx_decode_init();
ubx_state->print_errors = false; ubx_state->print_errors = false;
@@ -821,7 +828,7 @@ void *ubx_loop(void *arg)
/* set parameters for ubx */ /* set parameters for ubx */
if (configure_gps_ubx(&fd) != 0) { if (configure_gps_ubx(fd) != 0) {
printf("[gps] Configuration of gps module to ubx failed\r\n"); printf("[gps] Configuration of gps module to ubx failed\r\n");
/* Write shared variable sys_status */ /* Write shared variable sys_status */
@@ -838,13 +845,13 @@ void *ubx_loop(void *arg)
//global_data_send_subsystem_info(&ubx_present_enabled); //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; ubx_gps = &ubx_gps_d;
orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps); orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
while (!gps_thread_should_exit) { while (!(*thread_should_exit)) {
/* Parse a message from the gps receiver */ /* Parse a message from the gps receiver */
if (0 == read_gps_ubx(fd, gps_rx_buffer, UBX_BUFFER_SIZE)) { if (0 == read_gps_ubx(fd, gps_rx_buffer, UBX_BUFFER_SIZE)) {
/* publish new GPS position */ /* 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; return NULL;
} }
apps/gps/ubx.h
+3 -3
View File
@@ -300,15 +300,15 @@ 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); int write_config_message_ubx(uint8_t *message, size_t length, int fd);
void calculate_ubx_checksum(uint8_t *message, uint8_t length); 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_ */ #endif /* UBX_H_ */