diff --git a/apps/gps/gps.c b/apps/gps/gps.c index 6d3ff0d6bf..309b9a17a8 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 0333c71008..290fa61292 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 54912b6d37..fccc7414ad 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 diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index b65e701db7..36d444209d 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1704,7 +1704,7 @@ int mavlink_main(int argc, char *argv[]) } thread_should_exit = false; - mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4400, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c index 0bd8f09f1e..bbb0564ee6 100644 --- a/apps/px4/ground_estimator/ground_estimator.c +++ b/apps/px4/ground_estimator/ground_estimator.c @@ -41,6 +41,9 @@ #include #include +#include +#include + static bool thread_should_exit = false; /**< ground_estimator exit flag */ static bool thread_running = false; /**< ground_estimator status flag */ static int ground_estimator_task; /**< Handle of ground_estimator task / thread */ @@ -64,9 +67,49 @@ int ground_estimator_thread_main(int argc, char *argv[]) { printf("[ground_estimator] starting\n"); + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + bool publishing = false; + + /* advertise attitude */ + struct debug_key_value_s dbg = { .key = "velx ", .value = 0.0f }; + orb_advert_t pub_att = orb_advertise(ORB_ID(debug_key_value), &dbg); + + float position[3] = {}; + float velocity[3] = {}; + + uint64_t last_time = 0; + while (!thread_should_exit) { - printf("Hello ground_estimator!\n"); - sleep(10); + + /* wait for sensor update */ + int ret = poll(fds, 1, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); + } else { + struct sensor_combined_s s; + orb_copy(ORB_ID(sensor_combined), sub_raw, &s); + + uint64_t dt = s.timestamp - last_time; + + /* Integration */ + position[0] += velocity[0] * dt; + position[1] += velocity[1] * dt; + position[2] += velocity[2] * dt; + + velocity[0] += velocity[0] * 0.01f + 0.99f * s.acceleration_m_s2[0] * dt; + velocity[1] += velocity[1] * 0.01f + 0.99f * s.acceleration_m_s2[1] * dt; + velocity[2] += velocity[2] * 0.01f + 0.99f * s.acceleration_m_s2[2] * dt; + + dbg.value = position[0]; + + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + } + } printf("[ground_estimator] exiting.\n");