diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-02-04 16:27:01 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-02-04 16:27:01 -0800 |
commit | 039d394c207830a2c9c3a22e03302c867bd57af6 (patch) | |
tree | 67101a08b1d9c47b661e81d9ccf35e4b74d0fec9 /apps/drivers/gps/gps.cpp | |
parent | cb0fd834ae15c45cf6993f8c9eea9c99b2b153dd (diff) | |
download | px4-firmware-039d394c207830a2c9c3a22e03302c867bd57af6.tar.gz px4-firmware-039d394c207830a2c9c3a22e03302c867bd57af6.tar.bz2 px4-firmware-039d394c207830a2c9c3a22e03302c867bd57af6.zip |
Merged with newer, cleaned up code, fixed the checksum error
Diffstat (limited to 'apps/drivers/gps/gps.cpp')
-rw-r--r-- | apps/drivers/gps/gps.cpp | 130 |
1 files changed, 72 insertions, 58 deletions
diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 49b94fc2e..450b3091b 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -33,7 +33,7 @@ /** * @file gps.cpp - * Driver for the GPS on UART6 + * Driver for the GPS on a serial port */ #include <nuttx/config.h> @@ -107,46 +107,41 @@ public: private: - int _task_should_exit; - int _serial_fd; ///< serial interface to GPS - unsigned _baudrate; - unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; - uint8_t _send_buffer[SEND_BUFFER_LENGTH]; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - volatile int _task; ///< worker task + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to + volatile int _task; ///< worker task + bool _config_needed; ///< flag to signal that configuration of GPS is needed + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _mode_changed; ///< flag that the GPS mode has changed + gps_driver_mode_t _mode; ///< current mode + GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + struct vehicle_gps_position_s _report; ///< uORB topic for gps position + orb_advert_t _report_pub; ///< uORB pub for gps position + float _rate; ///< position update rate - bool _response_received; - bool _config_needed; - bool _baudrate_changed; - bool _mode_changed; - bool _healthy; - gps_driver_mode_t _mode; - unsigned _messages_received; - float _rate; ///< position update rate - - GPS_Helper *_Helper; ///< class for either UBX, MTK or NMEA helper - - struct vehicle_gps_position_s _report; ///< the current GPS report - orb_advert_t _report_pub; ///< the publication topic, only valid on first valid GPS module message - - void recv(); + /** + * Try to configure the GPS, handle outgoing communication to the GPS + */ void config(); + /** + * Trampoline to the worker task + */ static void task_main_trampoline(void *arg); /** - * Worker task. + * Worker task: main GPS thread that configures the GPS and parses incoming data, always running */ void task_main(void); /** - * Set the module baud rate + * Set the baudrate of the UART to the GPS */ - int set_baudrate(unsigned baud); + int set_baudrate(unsigned baud); /** * Send a reset command to the GPS @@ -176,12 +171,10 @@ GPS::GPS() : _config_needed(true), _baudrate_changed(false), _mode_changed(true), - _healthy(false), _mode(GPS_DRIVER_MODE_UBX), - _messages_received(0), _Helper(nullptr), - _rate(0.0f), - _report_pub(-1) + _report_pub(-1), + _rate(0.0f) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -216,7 +209,7 @@ GPS::init() if (CDev::init() != OK) goto out; - /* start the IO interface task */ + /* start the GPS driver worker task */ _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { @@ -275,17 +268,22 @@ void GPS::config() { int length = 0; + uint8_t send_buffer[SEND_BUFFER_LENGTH]; - _Helper->configure(_config_needed, _baudrate_changed, _baudrate, _send_buffer, length, SEND_BUFFER_LENGTH); + _Helper->configure(_config_needed, _baudrate_changed, _baudrate, send_buffer, length, SEND_BUFFER_LENGTH); - /* the message needs to be sent at the old baudrate */ + /* The config message is sent sent at the old baudrate */ if (length > 0) { - if (length != ::write(_serial_fd, _send_buffer, length)) { + + if (length != ::write(_serial_fd, send_buffer, length)) { debug("write config failed"); return; } } + /* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed + * from 9600 to 38400 + */ if (_baudrate_changed) { set_baudrate(_baudrate); _baudrate_changed = false; @@ -304,11 +302,10 @@ GPS::task_main() log("starting"); /* open the serial port */ - _serial_fd = ::open("/dev/ttyS3", O_RDWR); + _serial_fd = ::open("/dev/ttyS3", O_RDWR); //TODO make the device dynamic depending on startup parameters - uint8_t buf[256]; - int baud_i = 0; - uint64_t time_before_configuration; + /* buffer to read from the serial port */ + uint8_t buf[32]; if (_serial_fd < 0) { log("failed to open serial port: %d", errno); @@ -326,16 +323,16 @@ GPS::task_main() /* lock against the ioctl handler */ lock(); - + unsigned baud_i = 0; _baudrate = _baudrates_to_try[baud_i]; set_baudrate(_baudrate); - time_before_configuration = hrt_absolute_time(); + uint64_t time_before_configuration = hrt_absolute_time(); uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; - /* loop handling received serial bytes */ + /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_mode_changed) { @@ -364,14 +361,17 @@ GPS::task_main() _mode_changed = false; } + /* If a configuration does not finish in the config timeout, change the baudrate */ if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES; _baudrate = _baudrates_to_try[baud_i]; set_baudrate(_baudrate); + _Helper->reset(); time_before_configuration = hrt_absolute_time(); } - + /* during configuration, the timeout should be small, so that we can send config messages in between parsing, + * but during normal operation, it should never timeout because updates should arrive with 5Hz */ int poll_timeout; if (_config_needed) { poll_timeout = 50; @@ -409,11 +409,21 @@ GPS::task_main() /* pass received bytes to the packet decoder */ int j; + int ret_parse = 0; for (j = 0; j < count; j++) { - _messages_received += _Helper->parse(buf[j], &_report); + ret_parse += _Helper->parse(buf[j], &_report); } - if (_messages_received > 0) { - if (_config_needed == true) { + + if (ret_parse < 0) { + /* This means something went wrong in the parser, let's reconfigure */ + if (!_config_needed) { + _config_needed = true; + debug("Lost GPS module"); + } + config(); + } else if (ret_parse > 0) { + /* Looks like we got a valid position update, stop configuring and publish it */ + if (_config_needed) { _config_needed = false; debug("Found GPS module"); } @@ -424,8 +434,6 @@ GPS::task_main() } else { _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } - - _messages_received = 0; last_rate_count++; /* measure update rate every 5 seconds */ @@ -435,10 +443,10 @@ GPS::task_main() last_rate_count = 0; } } + /* else if ret_parse == 0: just keep parsing */ } } } -out: debug("exiting"); ::close(_serial_fd); @@ -469,7 +477,6 @@ GPS::set_baudrate(unsigned baud) warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; } - struct termios uart_config; int termios_state; @@ -482,24 +489,26 @@ GPS::set_baudrate(unsigned baud) uart_config.c_cflag &= ~(CSTOPB | PARENB); /* set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting config: %d (cfsetispeed, cfsetospeed)\n", termios_state); + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + return -1; + } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); return -1; } - - if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate (tcsetattr)\n"); return -1; } - /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ + return 0; } void GPS::cmd_reset() { - _healthy = false; + _config_needed = true; } void @@ -524,10 +533,12 @@ GPS::print_info() warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); if (_report.timestamp != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - ((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); + (double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("update rate: %6.2f Hz", (double)_rate); } + + usleep(100000); } /** @@ -583,6 +594,9 @@ fail: errx(1, "driver start failed"); } +/** + * Stop the driver. + */ void stop() { |