From c815aff842c127488c3885d6cb88ebfaf3dcd3bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Sep 2012 17:22:24 +0200 Subject: Deamonized GPS app, fixed GPS issues, reworking RC input --- apps/gps/gps.c | 81 ++++++++++++++++++++++++++++++++++++++++++++++---- apps/gps/mtk.c | 6 ++-- apps/gps/nmea_helper.c | 4 +-- 3 files changed, 81 insertions(+), 10 deletions(-) (limited to 'apps/gps') diff --git a/apps/gps/gps.c b/apps/gps/gps.c index 6d3ff0d6b..309b9a17a 100644 --- a/apps/gps/gps.c +++ b/apps/gps/gps.c @@ -58,6 +58,10 @@ #include #include +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 */ + /** * GPS module readout and publishing. * @@ -68,6 +72,26 @@ */ __EXPORT int gps_main(int argc, char *argv[]); +/** + * Mainloop of deamon. + */ +int gps_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +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 ****************************************************************************/ @@ -137,11 +161,56 @@ void setup_port(char *device, int speed, int *fd) } -/* - * Main function of gps app. +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). */ int gps_main(int argc, char *argv[]) { + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("gps already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_create("gps", SCHED_PRIORITY_DEFAULT, 4096, gps_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + thread_running = true; + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\gps is running\n"); + } else { + printf("\tgps not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/* + * Main function of gps app. + */ +int gps_thread_main(int argc, char *argv[]) { + /* welcome message */ printf("[gps] Initialized. Searching for GPS receiver..\n"); @@ -163,7 +232,7 @@ int gps_main(int argc, char *argv[]) /* read arguments */ int i; - for (i = 1; i < argc; i++) { + for (i = 0; i < argc; i++) { if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set printf(commandline_usage, argv[0]); return 0; @@ -294,7 +363,7 @@ int gps_main(int argc, char *argv[]) } - while (true) { + while (!thread_should_exit) { /* Infinite retries or break if retry == false */ /* Loop over all configurations of baud rate and protocol */ @@ -482,7 +551,9 @@ int gps_main(int argc, char *argv[]) close(mavlink_fd); - return ERROR; + printf("[gps] exiting.\n"); + + return 0; } int open_port(char *port) diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c index 0333c7100..290fa6129 100644 --- a/apps/gps/mtk.c +++ b/apps/gps/mtk.c @@ -121,7 +121,7 @@ int mtk_parse(uint8_t b, char *gps_rx_buffer) 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->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 @@ -311,14 +311,14 @@ void *mtk_loop(void *arg) /* 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); + orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), mtk_gps); while (1) { /* 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); + orb_publish(ORB_ID(vehicle_gps_position), gps_handle, mtk_gps); } else { break; diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c index 54912b6d3..fccc7414a 100644 --- a/apps/gps/nmea_helper.c +++ b/apps/gps/nmea_helper.c @@ -202,8 +202,8 @@ void *nmea_loop(void *arg) 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->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 -- cgit v1.2.3