diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index e4a371e382..59a3e9b050 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -12,9 +12,13 @@ #include #include #include +#include +#include +#ifdef __PX4_QURT +#include +#endif #include -#include ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): _fd(fd), @@ -99,6 +103,9 @@ int ASHTECH::handle_message(int len) timeinfo.tm_hour = ashtech_hour; timeinfo.tm_min = ashtech_minute; timeinfo.tm_sec = int(ashtech_sec); + + // TODO: this functionality is not available on the Snapdragon yet +#ifndef __PX4_QURT time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { @@ -112,7 +119,7 @@ int ASHTECH::handle_message(int len) ts.tv_sec = epoch; ts.tv_nsec = usecs * 1000; - if (clock_settime(CLOCK_REALTIME, &ts)) { + if (px4_clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } @@ -123,6 +130,10 @@ int ASHTECH::handle_message(int len) _gps_position->time_utc_usec = 0; } +#else + _gps_position->time_utc_usec = 0; +#endif + _gps_position->timestamp_time = hrt_absolute_time(); } @@ -514,10 +525,6 @@ int ASHTECH::handle_message(int len) int ASHTECH::receive(unsigned timeout) { { - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _fd; - fds[0].events = POLLIN; uint8_t buf[32]; @@ -541,38 +548,31 @@ int ASHTECH::receive(unsigned timeout) } } - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) { - return -1; - } - j++; } /* everything is read */ j = bytes_count = 0; - /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2); + /* then poll or read for new data */ + int ret = poll_or_read(_fd, buf, sizeof(buf), timeout * 2); if (ret < 0) { /* something went wrong when polling */ return -1; } else if (ret == 0) { - /* Timeout */ - return -1; + /* Timeout while polling or just nothing read if reading, let's + * stay here, and use timeout below. */ } else if (ret > 0) { /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - bytes_count = ::read(_fd, buf, sizeof(buf)); - } + bytes_count = ret; + } + + /* in case we get crap from GPS or time out */ + if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) { + return -1; } } } diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index e2ddab5678..35c264652a 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,22 +36,24 @@ * Driver for the GPS on a serial port */ +#ifndef __PX4_QURT #include +#include +#include +#endif + #include #include #include #include #include #include -#include #include #include #include #include #include -#include #include -#include #include #include #include @@ -80,6 +82,7 @@ #endif static const int ERROR = -1; + /* class for dynamic allocation of satellite info data */ class GPS_Sat_Info { @@ -88,7 +91,7 @@ public: }; -class GPS : public device::CDev +class GPS { public: GPS(const char *uart_path, bool fake_gps, bool enable_sat_info); @@ -96,8 +99,6 @@ public: virtual int init(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - /** * Diagnostics - print some basic information about the driver. */ @@ -167,7 +168,6 @@ GPS *g_dev = nullptr; GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : - CDev("gps", GPS0_DEVICE_PATH), _task_should_exit(false), _healthy(false), _mode_changed(false), @@ -195,8 +195,6 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : _p_report_sat_info = &_Sat_Info->_data; memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); } - - _debug_enabled = true; } GPS::~GPS() @@ -212,7 +210,7 @@ GPS::~GPS() /* well, kill it anyway, though this will probably crash */ if (_task != -1) { - task_delete(_task); + px4_task_delete(_task); } g_dev = nullptr; @@ -222,48 +220,17 @@ GPS::~GPS() int GPS::init() { - int ret = ERROR; - - /* do regular cdev init */ - if (CDev::init() != OK) { - goto out; - } /* start the GPS driver worker task */ _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr); + SCHED_PRIORITY_SLOW_DRIVER, 1200, (px4_main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { - warnx("task start failed: %d", errno); + PX4_WARN("task start failed: %d", errno); return -errno; } - ret = OK; -out: - return ret; -} - -int -GPS::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - lock(); - - int ret = OK; - - switch (cmd) { - case SENSORIOCRESET: - cmd_reset(); - break; - - default: - /* give it to parent if no one wants it */ - ret = CDev::ioctl(filp, cmd, arg); - break; - } - - unlock(); - - return ret; + return OK; } void @@ -275,17 +242,26 @@ GPS::task_main_trampoline(void *arg) void GPS::task_main() { - /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { - DEVICE_LOG("failed to open serial port: %s err: %d", _port, errno); + while (true) { + PX4_WARN("failed to open serial port: %s err: %d", _port, errno); + } + /* tell the dtor that we are exiting, set error code */ _task = -1; - _exit(1); + exit(1); } +#ifndef __PX4_QURT + // TODO: this call is not supported on Snapdragon just yet. + // However it seems to be nonblocking anyway and working. + int flags = fcntl(_serial_fd, F_GETFL, 0); + fcntl(_serial_fd, F_SETFL, flags | O_NONBLOCK); +#endif + uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; @@ -315,13 +291,11 @@ GPS::task_main() /* no time and satellite information simulated */ - if (!(_pub_blocked)) { - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } usleep(2e5); @@ -351,14 +325,12 @@ GPS::task_main() break; } - unlock(); /* the Ashtech driver lies about successful configuration and the * MTK driver is not well tested, so we really only trust the UBX * driver for an advance publication */ if (_Helper->configure(_baudrate) == 0) { - unlock(); /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); @@ -381,13 +353,11 @@ GPS::task_main() _report_gps_pos.epv = 10000.0f; _report_gps_pos.fix_type = 0; - if (!(_pub_blocked)) { - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } /* GPS is obviously detected successfully, reset statistics */ @@ -400,23 +370,21 @@ GPS::task_main() // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ - if (!(_pub_blocked)) { - if (helper_ret & 1) { - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (helper_ret & 1) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } + } - if (_p_report_sat_info && (helper_ret & 2)) { - if (_report_sat_info_pub != nullptr) { - orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); + if (_p_report_sat_info && (helper_ret & 2)) { + if (_report_sat_info_pub != nullptr) { + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); - } else { - _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); - } + } else { + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); } } @@ -453,22 +421,20 @@ GPS::task_main() break; } - warnx("module found: %s", mode_str); + PX4_WARN("module found: %s", mode_str); _healthy = true; } } + PX4_INFO("returned after no success"); + if (_healthy) { - warnx("module lost"); + PX4_WARN("module lost"); _healthy = false; _rate = 0.0f; } - - lock(); } - lock(); - /* select next mode */ switch (_mode) { case GPS_DRIVER_MODE_UBX: @@ -490,13 +456,13 @@ GPS::task_main() } - warnx("exiting"); + PX4_WARN("exiting"); ::close(_serial_fd); /* tell the dtor that we are exiting */ _task = -1; - _exit(0); + px4_task_exit(0); } @@ -505,12 +471,12 @@ void GPS::cmd_reset() { #ifdef GPIO_GPS_NRESET - warnx("Toggling GPS reset pin"); + PX4_WARN("Toggling GPS reset pin"); stm32_configgpio(GPIO_GPS_NRESET); stm32_gpiowrite(GPIO_GPS_NRESET, 0); usleep(100); stm32_gpiowrite(GPIO_GPS_NRESET, 1); - warnx("Toggled GPS reset pin"); + PX4_WARN("Toggled GPS reset pin"); #endif } @@ -519,21 +485,21 @@ GPS::print_info() { //GPS Mode if (_fake_gps) { - warnx("protocol: SIMULATED"); + PX4_WARN("protocol: SIMULATED"); } else { switch (_mode) { case GPS_DRIVER_MODE_UBX: - warnx("protocol: UBX"); + PX4_WARN("protocol: UBX"); break; case GPS_DRIVER_MODE_MTK: - warnx("protocol: MTK"); + PX4_WARN("protocol: MTK"); break; case GPS_DRIVER_MODE_ASHTECH: - warnx("protocol: ASHTECH"); + PX4_WARN("protocol: ASHTECH"); break; default: @@ -541,23 +507,23 @@ GPS::print_info() } } - warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s, noise: %d, jamming detected: %s", - (_p_report_sat_info != nullptr) ? "enabled" : "disabled", - _report_gps_pos.noise_per_ms, - _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + PX4_WARN("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + PX4_WARN("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp_position != 0) { - warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, - _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); - warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); - warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, - (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); - warnx("hdop: %.2f, vdop: %.2f", (double)_report_gps_pos.hdop, (double)_report_gps_pos.vdop); - warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); - warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); - warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); - warnx("rate publication:\t%6.2f Hz", (double)_rate); + PX4_WARN("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + PX4_WARN("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + PX4_WARN("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); + PX4_WARN("hdop: %.2f, vdop: %.2f", (double)_report_gps_pos.hdop, (double)_report_gps_pos.vdop); + PX4_WARN("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); + PX4_WARN("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); + PX4_WARN("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); + PX4_WARN("rate publication:\t%6.2f Hz", (double)_rate); } @@ -584,8 +550,6 @@ void info(); void start(const char *uart_path, bool fake_gps, bool enable_sat_info) { - int fd; - if (g_dev != nullptr) { errx(1, "already started"); } @@ -601,15 +565,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info) goto fail; } - /* set the poll rate to default, starts automatic data collection */ - fd = open(GPS0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - errx(1, "open: %s\n", GPS0_DEVICE_PATH); - goto fail; - } - - exit(0); + return; fail: @@ -618,7 +574,8 @@ fail: g_dev = nullptr; } - errx(1, "start failed"); + PX4_ERR("start failed"); + return; } /** @@ -630,7 +587,7 @@ stop() delete g_dev; g_dev = nullptr; - exit(0); + px4_task_exit(0); } /** @@ -651,17 +608,8 @@ test() void reset() { - int fd = open(GPS0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - err(1, "failed "); - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - err(1, "reset failed"); - } - - exit(0); + PX4_ERR("GPS reset not supported"); + return; } /** @@ -676,7 +624,7 @@ info() g_dev->print_info(); - exit(0); + return; } } // namespace @@ -685,7 +633,6 @@ info() int gps_main(int argc, char *argv[]) { - /* set to default */ const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; @@ -701,6 +648,7 @@ gps_main(int argc, char *argv[]) device_name = argv[3]; } else { + PX4_ERR("DID NOT GET -d"); goto out; } } @@ -747,6 +695,9 @@ gps_main(int argc, char *argv[]) gps::info(); } + return 0; + out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); + PX4_ERR("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); + return 1; } diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 8157bc7e68..7ac6dceda5 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,19 +31,31 @@ * ****************************************************************************/ +#ifndef __PX4_QURT #include +#include +#else +#include +#include +#endif + +#include #include #include #include + #include "gps_helper.h" /** * @file gps_helper.cpp * * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes */ +#define GPS_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls + + float GPS_Helper::get_position_update_rate() { @@ -74,6 +86,35 @@ GPS_Helper::store_update_rates() int GPS_Helper::set_baudrate(const int &fd, unsigned baud) { + +#if __PX4_QURT + // TODO: currently QURT does not support configuration with termios. + dspal_serial_ioctl_data_rate data_rate; + + switch (baud) { + case 9600: data_rate.bit_rate = DSPAL_SIO_BITRATE_9600; break; + + case 19200: data_rate.bit_rate = DSPAL_SIO_BITRATE_19200; break; + + case 38400: data_rate.bit_rate = DSPAL_SIO_BITRATE_38400; break; + + case 57600: data_rate.bit_rate = DSPAL_SIO_BITRATE_57600; break; + + case 115200: data_rate.bit_rate = DSPAL_SIO_BITRATE_115200; break; + + default: + PX4_ERR("ERR: unknown baudrate: %d", baud); + return -EINVAL; + } + + int ret = ::ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&data_rate); + + if (ret != 0) { + + return ret; + } + +#else /* process baud rate */ int speed; @@ -89,7 +130,7 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; default: - warnx("ERR: baudrate: %d\n", baud); + PX4_ERR("ERR: unknown baudrate: %d", baud); return -EINVAL; } @@ -121,5 +162,49 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) return -1; } +#endif return 0; } + +int +GPS_Helper::poll_or_read(int fd, uint8_t *buf, size_t buf_length, uint64_t timeout) +{ + +#ifndef __PX4_QURT + + /* For non QURT, use the usual polling. */ + + pollfd fds[1]; + fds[0].fd = fd; + fds[0].events = POLLIN; + + /* Poll for new data, */ + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + + if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. But don't read immediately + * by 1-2 bytes, wait for some more data to save expensive read() calls. + * If more bytes are available, we'll go back to poll() again. + */ + usleep(GPS_WAIT_BEFORE_READ * 1000); + return ::read(fd, buf, buf_length); + + } else { + return -1; + } + + } else { + return ret; + } + +#else + /* For QURT, just use read for now, since this doesn't block, we need to slow it down + * just a bit. */ + usleep(10000); + return ::read(fd, buf, buf_length); +#endif +} diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 0ce05fcb50..e30313a893 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -43,6 +43,7 @@ #include #include +// TODO: this number seems wrong #define GPS_EPOCH_SECS 1234567890ULL class GPS_Helper @@ -60,6 +61,20 @@ public: void reset_update_rates(); void store_update_rates(); + /* This is an abstraction for the poll on serial used. The + * implementation is different for QURT than for POSIX and + * NuttX. + * + * @param fd: serial file descriptor + * @param buf: pointer to read buffer + * @param buf_length: size of read buffer + * @param timeout: timeout time in us + * @return: 0 for nothing read, or poll timed out + * < 0 for error + * > 0 number of bytes read + */ + int poll_or_read(int fd, uint8_t *buf, size_t buf_length, uint64_t timeout); + protected: uint8_t _rate_count_lat_lon; uint8_t _rate_count_vel; diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 7117566255..73a5185307 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -38,6 +38,7 @@ * @author Julian Oes */ +#include #include #include #include @@ -111,11 +112,6 @@ errout: int MTK::receive(unsigned timeout) { - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _fd; - fds[0].events = POLLIN; - uint8_t buf[32]; gps_mtk_packet_t packet; @@ -123,52 +119,36 @@ MTK::receive(unsigned timeout) uint64_t time_started = hrt_absolute_time(); int j = 0; - ssize_t count = 0; while (true) { - /* first read whatever is left */ - if (j < count) { - /* pass received bytes to the packet decoder */ - while (j < count) { - if (parse_char(buf[j], packet) > 0) { - handle_message(packet); - return 1; + int ret = poll_or_read(_fd, buf, sizeof(buf), timeout); + + if (ret > 0) { + /* first read whatever is left */ + if (j < ret) { + /* pass received bytes to the packet decoder */ + while (j < ret) { + if (parse_char(buf[j], packet) > 0) { + handle_message(packet); + return 1; + } + + j++; } - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout * 1000 < hrt_absolute_time()) { - return -1; - } - - j++; + /* everything is read */ + j = 0; } - /* everything is read */ - j = count = 0; + } else { + PX4_INFO("waiting"); + usleep(20000); } - /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); - - if (ret < 0) { - /* something went wrong when polling */ + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout * 1000 < hrt_absolute_time()) { return -1; - - } else if (ret == 0) { - /* Timeout */ - return -1; - - } else if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - count = ::read(_fd, buf, sizeof(buf)); - } } } } @@ -282,6 +262,10 @@ MTK::handle_message(gps_mtk_packet_t &packet) timeinfo_conversion_temp -= timeinfo.tm_min * 100000; timeinfo.tm_sec = timeinfo_conversion_temp / 1000; timeinfo_conversion_temp -= timeinfo.tm_sec * 1000; + + // TODO: this functionality is not available on the Snapdragon yet +#ifndef __PX4_QURT + time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { @@ -304,6 +288,10 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->time_utc_usec = 0; } +#else + _gps_position->time_utc_usec = 0; +#endif + _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); // Position and velocity update always at the same time diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h index a913d3d89f..9ff894e69b 100644 --- a/src/drivers/gps/mtk.h +++ b/src/drivers/gps/mtk.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,7 @@ * @file mtk.cpp * * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes */ #ifndef MTK_H_ @@ -118,7 +118,6 @@ private: struct vehicle_gps_position_s *_gps_position; mtk_decode_state_t _decode_state; uint8_t _mtk_revision; - uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; unsigned _rx_count; uint8_t _rx_ck_a; uint8_t _rx_ck_b; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index ef5a221600..9ce05f59d4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -50,7 +50,6 @@ #include #include -#include #include #include #include @@ -61,12 +60,12 @@ #include #include #include +#include #include "ubx.h" #define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK #define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received -#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls #define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval #define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) @@ -77,13 +76,13 @@ /**** Trace macros, disable for production builds */ -#define UBX_TRACE_PARSER(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* decoding progress in parse_char() */ -#define UBX_TRACE_RXMSG(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* Rx msgs in payload_rx_done() */ -#define UBX_TRACE_SVINFO(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* NAV-SVINFO processing (debug use only, will cause rx buffer overflows) */ +#define UBX_TRACE_PARSER(s, ...) {/*PX4_INFO(s, ## __VA_ARGS__);*/} /* decoding progress in parse_char() */ +#define UBX_TRACE_RXMSG(s, ...) {/*PX4_INFO(s, ## __VA_ARGS__);*/} /* Rx msgs in payload_rx_done() */ +#define UBX_TRACE_SVINFO(s, ...) {/*PX4_INFO(s, ## __VA_ARGS__);*/} /* NAV-SVINFO processing (debug use only, will cause rx buffer overflows) */ /**** Warning macros, disable to save memory */ -#define UBX_WARN(s, ...) {warnx(s, ## __VA_ARGS__);} -#define UBX_DEBUG(s, ...) {/*warnx(s, ## __VA_ARGS__);*/} +#define UBX_WARN(s, ...) {PX4_WARN(s, ## __VA_ARGS__);} +#define UBX_DEBUG(s, ...) {/*PX4_WARN(s, ## __VA_ARGS__);*/} UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) : _fd(fd), @@ -302,63 +301,45 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled UBX::receive(const unsigned timeout) { - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _fd; - fds[0].events = POLLIN; - uint8_t buf[128]; /* timeout additional to poll */ uint64_t time_started = hrt_absolute_time(); - ssize_t count = 0; - int handled = 0; while (true) { bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled; - /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); + /* Wait for only UBX_PACKET_TIMEOUT if something already received. */ + int ret = poll_or_read(_fd, buf, sizeof(buf), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { - /* something went wrong when polling */ - UBX_WARN("ubx poll() err"); + /* something went wrong when polling or reading */ + UBX_WARN("ubx poll_or_read err"); return -1; } else if (ret == 0) { - /* return success after short delay after receiving a packet or timeout after long delay */ + /* return success if ready */ if (ready_to_return) { _got_posllh = false; _got_velned = false; return handled; - - } else { - return -1; } - } else if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. But don't read immediately - * by 1-2 bytes, wait for some more data to save expensive read() calls. - * If more bytes are available, we'll go back to poll() again. - */ - usleep(UBX_WAIT_BEFORE_READ * 1000); - count = read(_fd, buf, sizeof(buf)); + } else { + UBX_DEBUG("read %d bytes", ret); - /* pass received bytes to the packet decoder */ - for (int i = 0; i < count; i++) { - handled |= parse_char(buf[i]); - } + /* pass received bytes to the packet decoder */ + for (int i = 0; i < ret; i++) { + handled |= parse_char(buf[i]); + UBX_DEBUG("parsed %d: 0x%x", i, buf[i]); } } /* abort after timeout if no useful packets received */ if (time_started + timeout * 1000 < hrt_absolute_time()) { + UBX_DEBUG("timed out, returning"); return -1; } } @@ -374,7 +355,7 @@ UBX::parse_char(const uint8_t b) /* Expecting Sync1 */ case UBX_DECODE_SYNC1: if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2 - UBX_TRACE_PARSER("\nA"); + UBX_TRACE_PARSER("A"); _decode_state = UBX_DECODE_SYNC2; } @@ -707,7 +688,7 @@ UBX::payload_rx_add_nav_svinfo(const uint8_t b) if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { // Part 1 complete: decode Part 1 buffer _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, satellite_info_s::SAT_INFO_MAX_SATELLITES); - UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, + UBX_TRACE_SVINFO("SVINFO len %u numCh %u", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); } @@ -727,7 +708,7 @@ UBX::payload_rx_add_nav_svinfo(const uint8_t b) _satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev); _satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f); _satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid); - UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u\n", + UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u", (unsigned)sat_index + 1, (unsigned)_satellite_info->used[sat_index], (unsigned)_satellite_info->snr[sat_index], @@ -803,7 +784,7 @@ UBX::payload_rx_done(void) switch (_rx_msg) { case UBX_MSG_NAV_PVT: - UBX_TRACE_RXMSG("Rx NAV-PVT\n"); + UBX_TRACE_RXMSG("Rx NAV-PVT"); //Check if position fix flag is good if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) { @@ -846,6 +827,9 @@ UBX::payload_rx_done(void) timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour; timeinfo.tm_min = _buf.payload_rx_nav_pvt.min; timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; + + // TODO: this functionality is not available on the Snapdragon yet +#ifndef __PX4_QURT time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { @@ -867,6 +851,10 @@ UBX::payload_rx_done(void) } else { _gps_position->time_utc_usec = 0; } + +#else + _gps_position->time_utc_usec = 0; +#endif } _gps_position->timestamp_time = hrt_absolute_time(); @@ -884,7 +872,7 @@ UBX::payload_rx_done(void) break; case UBX_MSG_NAV_POSLLH: - UBX_TRACE_RXMSG("Rx NAV-POSLLH\n"); + UBX_TRACE_RXMSG("Rx NAV-POSLLH"); _gps_position->lat = _buf.payload_rx_nav_posllh.lat; _gps_position->lon = _buf.payload_rx_nav_posllh.lon; @@ -902,7 +890,7 @@ UBX::payload_rx_done(void) break; case UBX_MSG_NAV_SOL: - UBX_TRACE_RXMSG("Rx NAV-SOL\n"); + UBX_TRACE_RXMSG("Rx NAV-SOL"); _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m @@ -914,7 +902,7 @@ UBX::payload_rx_done(void) break; case UBX_MSG_NAV_DOP: - UBX_TRACE_RXMSG("Rx NAV-DOP\n"); + UBX_TRACE_RXMSG("Rx NAV-DOP"); _gps_position->hdop = _buf.payload_rx_nav_dop.hDOP * 0.01f; // from cm to m _gps_position->vdop = _buf.payload_rx_nav_dop.vDOP * 0.01f; // from cm to m @@ -925,7 +913,7 @@ UBX::payload_rx_done(void) break; case UBX_MSG_NAV_TIMEUTC: - UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); + UBX_TRACE_RXMSG("Rx NAV-TIMEUTC"); if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) { // convert to unix timestamp @@ -936,6 +924,8 @@ UBX::payload_rx_done(void) timeinfo.tm_hour = _buf.payload_rx_nav_timeutc.hour; timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min; timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; + // TODO: this functionality is not available on the Snapdragon yet +#ifndef __PX4_QURT time_t epoch = mktime(&timeinfo); // only set the time if it makes sense @@ -959,6 +949,10 @@ UBX::payload_rx_done(void) } else { _gps_position->time_utc_usec = 0; } + +#else + _gps_position->time_utc_usec = 0; +#endif } _gps_position->timestamp_time = hrt_absolute_time(); @@ -967,7 +961,7 @@ UBX::payload_rx_done(void) break; case UBX_MSG_NAV_SVINFO: - UBX_TRACE_RXMSG("Rx NAV-SVINFO\n"); + UBX_TRACE_RXMSG("Rx NAV-SVINFO"); // _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp _satellite_info->timestamp = hrt_absolute_time(); @@ -976,7 +970,7 @@ UBX::payload_rx_done(void) break; case UBX_MSG_NAV_VELNED: - UBX_TRACE_RXMSG("Rx NAV-VELNED\n"); + UBX_TRACE_RXMSG("Rx NAV-VELNED"); _gps_position->vel_m_s = (float)_buf.payload_rx_nav_velned.speed * 1e-2f; _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_velned.velN * 1e-2f; /* NED NORTH velocity */ @@ -995,13 +989,13 @@ UBX::payload_rx_done(void) break; case UBX_MSG_MON_VER: - UBX_TRACE_RXMSG("Rx MON-VER\n"); + UBX_TRACE_RXMSG("Rx MON-VER"); ret = 1; break; case UBX_MSG_MON_HW: - UBX_TRACE_RXMSG("Rx MON-HW\n"); + UBX_TRACE_RXMSG("Rx MON-HW"); switch (_rx_payload_length) { @@ -1027,7 +1021,7 @@ UBX::payload_rx_done(void) break; case UBX_MSG_ACK_ACK: - UBX_TRACE_RXMSG("Rx ACK-ACK\n"); + UBX_TRACE_RXMSG("Rx ACK-ACK"); if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { _ack_state = UBX_ACK_GOT_ACK; @@ -1037,7 +1031,7 @@ UBX::payload_rx_done(void) break; case UBX_MSG_ACK_NAK: - UBX_TRACE_RXMSG("Rx ACK-NAK\n"); + UBX_TRACE_RXMSG("Rx ACK-NAK"); if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { _ack_state = UBX_ACK_GOT_NAK; diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 9670362f4f..423a898896 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,7 +38,7 @@ * including Prototol Specification. * * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes * @author Anton Babushkin * * @author Hannes Delago @@ -446,7 +446,12 @@ typedef union { ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5; ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas; ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg; +#ifdef __PX4_QURT + // TODO: determine length needed here + uint8_t raw[256]; +#else uint8_t raw[]; +#endif } ubx_buf_t; #pragma pack(pop) @@ -552,7 +557,6 @@ private: int _fd; struct vehicle_gps_position_s *_gps_position; struct satellite_info_s *_satellite_info; - bool _enable_sat_info; bool _configured; ubx_ack_state_t _ack_state; bool _got_posllh; diff --git a/src/modules/systemlib/scheduling_priorities.h b/src/modules/systemlib/scheduling_priorities.h index be1dbfcd88..82ad27052b 100644 --- a/src/modules/systemlib/scheduling_priorities.h +++ b/src/modules/systemlib/scheduling_priorities.h @@ -33,7 +33,9 @@ #pragma once +#ifndef __PX4_QURT #include +#endif /* SCHED_PRIORITY_MAX */ #define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX