Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

gps: make the driver compile and run on QURT

Browse Source
This commit is contained in:
Julian Oes
2016-03-03 12:12:11 +01:00
committed by Lorenz Meier
parent bc9e24102d
commit 1cebfde840
9 changed files with 293 additions and 255 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/ashtech.cpp
+23 -23
View File
@@ -12,9 +12,13 @@
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h> #include <uORB/topics/satellite_info.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <px4_time.h>
#include <px4_defines.h>
#ifdef __PX4_QURT
#include <dspal_time.h>
#endif
#include <fcntl.h> #include <fcntl.h>
#include <math.h>
ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info):
_fd(fd), _fd(fd),
@@ -99,6 +103,9 @@ int ASHTECH::handle_message(int len)
timeinfo.tm_hour = ashtech_hour; timeinfo.tm_hour = ashtech_hour;
timeinfo.tm_min = ashtech_minute; timeinfo.tm_min = ashtech_minute;
timeinfo.tm_sec = int(ashtech_sec); timeinfo.tm_sec = int(ashtech_sec);
// TODO: this functionality is not available on the Snapdragon yet
#ifndef __PX4_QURT
time_t epoch = mktime(&timeinfo); time_t epoch = mktime(&timeinfo);
if (epoch > GPS_EPOCH_SECS) { if (epoch > GPS_EPOCH_SECS) {
@@ -112,7 +119,7 @@ int ASHTECH::handle_message(int len)
ts.tv_sec = epoch; ts.tv_sec = epoch;
ts.tv_nsec = usecs * 1000; ts.tv_nsec = usecs * 1000;
if (clock_settime(CLOCK_REALTIME, &ts)) { if (px4_clock_settime(CLOCK_REALTIME, &ts)) {
warn("failed setting clock"); warn("failed setting clock");
} }
@@ -123,6 +130,10 @@ int ASHTECH::handle_message(int len)
_gps_position->time_utc_usec = 0; _gps_position->time_utc_usec = 0;
} }
#else
_gps_position->time_utc_usec = 0;
#endif
_gps_position->timestamp_time = hrt_absolute_time(); _gps_position->timestamp_time = hrt_absolute_time();
} }
@@ -514,10 +525,6 @@ int ASHTECH::handle_message(int len)
int ASHTECH::receive(unsigned timeout) int ASHTECH::receive(unsigned timeout)
{ {
{ {
/* poll descriptor */
pollfd fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[32]; 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++; j++;
} }
/* everything is read */ /* everything is read */
j = bytes_count = 0; j = bytes_count = 0;
/* then poll for new data */ /* then poll or read for new data */
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2); int ret = poll_or_read(_fd, buf, sizeof(buf), timeout * 2);
if (ret < 0) { if (ret < 0) {
/* something went wrong when polling */ /* something went wrong when polling */
return -1; return -1;
} else if (ret == 0) { } else if (ret == 0) {
/* Timeout */ /* Timeout while polling or just nothing read if reading, let's
return -1; * stay here, and use timeout below. */
} else if (ret > 0) { } else if (ret > 0) {
/* if we have new data from GPS, go handle it */ /* if we have new data from GPS, go handle it */
if (fds[0].revents & POLLIN) { bytes_count = ret;
/* }
* We are here because poll says there is some data, so this
* won't block even on a blocking device. If more bytes are /* in case we get crap from GPS or time out */
* available, we'll go back to poll() again... if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) {
*/ return -1;
bytes_count = ::read(_fd, buf, sizeof(buf));
}
} }
} }
} }
src/drivers/gps/gps.cpp
+82 -131
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -36,22 +36,24 @@
* Driver for the GPS on a serial port * Driver for the GPS on a serial port
*/ */
#ifndef __PX4_QURT
#include <nuttx/clock.h> #include <nuttx/clock.h>
#include <nuttx/arch.h>
#include <fcntl.h>
#endif
#include <sys/types.h> #include <sys/types.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <stdbool.h> #include <stdbool.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#include <fcntl.h>
#include <poll.h> #include <poll.h>
#include <errno.h> #include <errno.h>
#include <stdio.h> #include <stdio.h>
#include <math.h> #include <math.h>
#include <unistd.h> #include <unistd.h>
#include <fcntl.h>
#include <px4_config.h> #include <px4_config.h>
#include <nuttx/arch.h>
#include <arch/board/board.h> #include <arch/board/board.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/device/i2c.h> #include <drivers/device/i2c.h>
@@ -80,6 +82,7 @@
#endif #endif
static const int ERROR = -1; static const int ERROR = -1;
/* class for dynamic allocation of satellite info data */ /* class for dynamic allocation of satellite info data */
class GPS_Sat_Info class GPS_Sat_Info
{ {
@@ -88,7 +91,7 @@ public:
}; };
class GPS : public device::CDev class GPS
{ {
public: public:
GPS(const char *uart_path, bool fake_gps, bool enable_sat_info); GPS(const char *uart_path, bool fake_gps, bool enable_sat_info);
@@ -96,8 +99,6 @@ public:
virtual int init(); virtual int init();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/** /**
* Diagnostics - print some basic information about the driver. * 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) : GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
CDev("gps", GPS0_DEVICE_PATH),
_task_should_exit(false), _task_should_exit(false),
_healthy(false), _healthy(false),
_mode_changed(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; _p_report_sat_info = &_Sat_Info->_data;
memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
} }
_debug_enabled = true;
} }
GPS::~GPS() GPS::~GPS()
@@ -212,7 +210,7 @@ GPS::~GPS()
/* well, kill it anyway, though this will probably crash */ /* well, kill it anyway, though this will probably crash */
if (_task != -1) { if (_task != -1) {
task_delete(_task); px4_task_delete(_task);
} }
g_dev = nullptr; g_dev = nullptr;
@@ -222,48 +220,17 @@ GPS::~GPS()
int int
GPS::init() GPS::init()
{ {
int ret = ERROR;
/* do regular cdev init */
if (CDev::init() != OK) {
goto out;
}
/* start the GPS driver worker task */ /* start the GPS driver worker task */
_task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, _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) { if (_task < 0) {
warnx("task start failed: %d", errno); PX4_WARN("task start failed: %d", errno);
return -errno; return -errno;
} }
ret = OK; return 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;
} }
void void
@@ -275,17 +242,26 @@ GPS::task_main_trampoline(void *arg)
void void
GPS::task_main() GPS::task_main()
{ {
/* open the serial port */ /* open the serial port */
_serial_fd = ::open(_port, O_RDWR); _serial_fd = ::open(_port, O_RDWR);
if (_serial_fd < 0) { 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 */ /* tell the dtor that we are exiting, set error code */
_task = -1; _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(); uint64_t last_rate_measurement = hrt_absolute_time();
unsigned last_rate_count = 0; unsigned last_rate_count = 0;
@@ -315,13 +291,11 @@ GPS::task_main()
/* no time and satellite information simulated */ /* no time and satellite information simulated */
if (!(_pub_blocked)) { if (_report_gps_pos_pub != nullptr) {
if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else { } else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
} }
usleep(2e5); usleep(2e5);
@@ -351,14 +325,12 @@ GPS::task_main()
break; break;
} }
unlock();
/* the Ashtech driver lies about successful configuration and the /* the Ashtech driver lies about successful configuration and the
* MTK driver is not well tested, so we really only trust the UBX * MTK driver is not well tested, so we really only trust the UBX
* driver for an advance publication * driver for an advance publication
*/ */
if (_Helper->configure(_baudrate) == 0) { if (_Helper->configure(_baudrate) == 0) {
unlock();
/* reset report */ /* reset report */
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); 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.epv = 10000.0f;
_report_gps_pos.fix_type = 0; _report_gps_pos.fix_type = 0;
if (!(_pub_blocked)) { if (_report_gps_pos_pub != nullptr) {
if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else { } else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
} }
/* GPS is obviously detected successfully, reset statistics */ /* GPS is obviously detected successfully, reset statistics */
@@ -400,23 +370,21 @@ GPS::task_main()
// lock(); // lock();
/* opportunistic publishing - else invalid data would end up on the bus */ /* opportunistic publishing - else invalid data would end up on the bus */
if (!(_pub_blocked)) { if (helper_ret & 1) {
if (helper_ret & 1) { if (_report_gps_pos_pub != nullptr) {
if (_report_gps_pos_pub != nullptr) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else { } else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
} }
}
if (_p_report_sat_info && (helper_ret & 2)) { if (_p_report_sat_info && (helper_ret & 2)) {
if (_report_sat_info_pub != nullptr) { if (_report_sat_info_pub != nullptr) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
} else { } else {
_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
}
} }
} }
@@ -453,22 +421,20 @@ GPS::task_main()
break; break;
} }
warnx("module found: %s", mode_str); PX4_WARN("module found: %s", mode_str);
_healthy = true; _healthy = true;
} }
} }
PX4_INFO("returned after no success");
if (_healthy) { if (_healthy) {
warnx("module lost"); PX4_WARN("module lost");
_healthy = false; _healthy = false;
_rate = 0.0f; _rate = 0.0f;
} }
lock();
} }
lock();
/* select next mode */ /* select next mode */
switch (_mode) { switch (_mode) {
case GPS_DRIVER_MODE_UBX: case GPS_DRIVER_MODE_UBX:
@@ -490,13 +456,13 @@ GPS::task_main()
} }
warnx("exiting"); PX4_WARN("exiting");
::close(_serial_fd); ::close(_serial_fd);
/* tell the dtor that we are exiting */ /* tell the dtor that we are exiting */
_task = -1; _task = -1;
_exit(0); px4_task_exit(0);
} }
@@ -505,12 +471,12 @@ void
GPS::cmd_reset() GPS::cmd_reset()
{ {
#ifdef GPIO_GPS_NRESET #ifdef GPIO_GPS_NRESET
warnx("Toggling GPS reset pin"); PX4_WARN("Toggling GPS reset pin");
stm32_configgpio(GPIO_GPS_NRESET); stm32_configgpio(GPIO_GPS_NRESET);
stm32_gpiowrite(GPIO_GPS_NRESET, 0); stm32_gpiowrite(GPIO_GPS_NRESET, 0);
usleep(100); usleep(100);
stm32_gpiowrite(GPIO_GPS_NRESET, 1); stm32_gpiowrite(GPIO_GPS_NRESET, 1);
warnx("Toggled GPS reset pin"); PX4_WARN("Toggled GPS reset pin");
#endif #endif
} }
@@ -519,21 +485,21 @@ GPS::print_info()
{ {
//GPS Mode //GPS Mode
if (_fake_gps) { if (_fake_gps) {
warnx("protocol: SIMULATED"); PX4_WARN("protocol: SIMULATED");
} }
else { else {
switch (_mode) { switch (_mode) {
case GPS_DRIVER_MODE_UBX: case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX"); PX4_WARN("protocol: UBX");
break; break;
case GPS_DRIVER_MODE_MTK: case GPS_DRIVER_MODE_MTK:
warnx("protocol: MTK"); PX4_WARN("protocol: MTK");
break; break;
case GPS_DRIVER_MODE_ASHTECH: case GPS_DRIVER_MODE_ASHTECH:
warnx("protocol: ASHTECH"); PX4_WARN("protocol: ASHTECH");
break; break;
default: default:
@@ -541,23 +507,23 @@ GPS::print_info()
} }
} }
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); PX4_WARN("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
warnx("sat info: %s, noise: %d, jamming detected: %s", PX4_WARN("sat info: %s, noise: %d, jamming detected: %s",
(_p_report_sat_info != nullptr) ? "enabled" : "disabled", (_p_report_sat_info != nullptr) ? "enabled" : "disabled",
_report_gps_pos.noise_per_ms, _report_gps_pos.noise_per_ms,
_report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
if (_report_gps_pos.timestamp_position != 0) { if (_report_gps_pos.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, 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); _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); PX4_WARN("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, 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); (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); PX4_WARN("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); PX4_WARN("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()); PX4_WARN("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()); PX4_WARN("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate); PX4_WARN("rate publication:\t%6.2f Hz", (double)_rate);
} }
@@ -584,8 +550,6 @@ void info();
void void
start(const char *uart_path, bool fake_gps, bool enable_sat_info) start(const char *uart_path, bool fake_gps, bool enable_sat_info)
{ {
int fd;
if (g_dev != nullptr) { if (g_dev != nullptr) {
errx(1, "already started"); errx(1, "already started");
} }
@@ -601,15 +565,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info)
goto fail; goto fail;
} }
/* set the poll rate to default, starts automatic data collection */ return;
fd = open(GPS0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
errx(1, "open: %s\n", GPS0_DEVICE_PATH);
goto fail;
}
exit(0);
fail: fail:
@@ -618,7 +574,8 @@ fail:
g_dev = nullptr; g_dev = nullptr;
} }
errx(1, "start failed"); PX4_ERR("start failed");
return;
} }
/** /**
@@ -630,7 +587,7 @@ stop()
delete g_dev; delete g_dev;
g_dev = nullptr; g_dev = nullptr;
exit(0); px4_task_exit(0);
} }
/** /**
@@ -651,17 +608,8 @@ test()
void void
reset() reset()
{ {
int fd = open(GPS0_DEVICE_PATH, O_RDONLY); PX4_ERR("GPS reset not supported");
return;
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "reset failed");
}
exit(0);
} }
/** /**
@@ -676,7 +624,7 @@ info()
g_dev->print_info(); g_dev->print_info();
exit(0); return;
} }
} // namespace } // namespace
@@ -685,7 +633,6 @@ info()
int int
gps_main(int argc, char *argv[]) gps_main(int argc, char *argv[])
{ {
/* set to default */ /* set to default */
const char *device_name = GPS_DEFAULT_UART_PORT; const char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false; bool fake_gps = false;
@@ -701,6 +648,7 @@ gps_main(int argc, char *argv[])
device_name = argv[3]; device_name = argv[3];
} else { } else {
PX4_ERR("DID NOT GET -d");
goto out; goto out;
} }
} }
@@ -747,6 +695,9 @@ gps_main(int argc, char *argv[])
gps::info(); gps::info();
} }
return 0;
out: 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;
} }
src/drivers/gps/gps_helper.cpp
+88 -3
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -31,19 +31,31 @@
* *
****************************************************************************/ ****************************************************************************/
#ifndef __PX4_QURT
#include <termios.h> #include <termios.h>
#include <poll.h>
#else
#include <sys/ioctl.h>
#include <dev_fs_lib_serial.h>
#endif
#include <unistd.h>
#include <errno.h> #include <errno.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include "gps_helper.h" #include "gps_helper.h"
/** /**
* @file gps_helper.cpp * @file gps_helper.cpp
* *
* @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch> * @author Julian Oes <julian@oes.ch>
*/ */
#define GPS_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls
float float
GPS_Helper::get_position_update_rate() GPS_Helper::get_position_update_rate()
{ {
@@ -74,6 +86,35 @@ GPS_Helper::store_update_rates()
int int
GPS_Helper::set_baudrate(const int &fd, unsigned baud) 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 */ /* process baud rate */
int speed; int speed;
@@ -89,7 +130,7 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
case 115200: speed = B115200; break; case 115200: speed = B115200; break;
default: default:
warnx("ERR: baudrate: %d\n", baud); PX4_ERR("ERR: unknown baudrate: %d", baud);
return -EINVAL; return -EINVAL;
} }
@@ -121,5 +162,49 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
return -1; return -1;
} }
#endif
return 0; 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
}
src/drivers/gps/gps_helper.h
+15
View File
@@ -43,6 +43,7 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
// TODO: this number seems wrong
#define GPS_EPOCH_SECS 1234567890ULL #define GPS_EPOCH_SECS 1234567890ULL
class GPS_Helper class GPS_Helper
@@ -60,6 +61,20 @@ public:
void reset_update_rates(); void reset_update_rates();
void store_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: protected:
uint8_t _rate_count_lat_lon; uint8_t _rate_count_lat_lon;
uint8_t _rate_count_vel; uint8_t _rate_count_vel;
src/drivers/gps/mtk.cpp
+29 -41
View File
@@ -38,6 +38,7 @@
* @author Julian Oes <joes@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch>
*/ */
#include <px4_defines.h>
#include <unistd.h> #include <unistd.h>
#include <stdio.h> #include <stdio.h>
#include <poll.h> #include <poll.h>
@@ -111,11 +112,6 @@ errout:
int int
MTK::receive(unsigned timeout) MTK::receive(unsigned timeout)
{ {
/* poll descriptor */
pollfd fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[32]; uint8_t buf[32];
gps_mtk_packet_t packet; gps_mtk_packet_t packet;
@@ -123,52 +119,36 @@ MTK::receive(unsigned timeout)
uint64_t time_started = hrt_absolute_time(); uint64_t time_started = hrt_absolute_time();
int j = 0; int j = 0;
ssize_t count = 0;
while (true) { while (true) {
/* first read whatever is left */ int ret = poll_or_read(_fd, buf, sizeof(buf), timeout);
if (j < count) {
/* pass received bytes to the packet decoder */ if (ret > 0) {
while (j < count) { /* first read whatever is left */
if (parse_char(buf[j], packet) > 0) { if (j < ret) {
handle_message(packet); /* pass received bytes to the packet decoder */
return 1; 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 */ /* everything is read */
if (time_started + timeout * 1000 < hrt_absolute_time()) { j = 0;
return -1;
}
j++;
} }
/* everything is read */ } else {
j = count = 0; PX4_INFO("waiting");
usleep(20000);
} }
/* then poll for new data */ /* in case we keep trying but only get crap from GPS */
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); if (time_started + timeout * 1000 < hrt_absolute_time()) {
if (ret < 0) {
/* something went wrong when polling */
return -1; 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_conversion_temp -= timeinfo.tm_min * 100000;
timeinfo.tm_sec = timeinfo_conversion_temp / 1000; timeinfo.tm_sec = timeinfo_conversion_temp / 1000;
timeinfo_conversion_temp -= timeinfo.tm_sec * 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); time_t epoch = mktime(&timeinfo);
if (epoch > GPS_EPOCH_SECS) { if (epoch > GPS_EPOCH_SECS) {
@@ -304,6 +288,10 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->time_utc_usec = 0; _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(); _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
// Position and velocity update always at the same time // Position and velocity update always at the same time
src/drivers/gps/mtk.h
+2 -3
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -35,7 +35,7 @@
* @file mtk.cpp * @file mtk.cpp
* *
* @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch> * @author Julian Oes <julian@oes.ch>
*/ */
#ifndef MTK_H_ #ifndef MTK_H_
@@ -118,7 +118,6 @@ private:
struct vehicle_gps_position_s *_gps_position; struct vehicle_gps_position_s *_gps_position;
mtk_decode_state_t _decode_state; mtk_decode_state_t _decode_state;
uint8_t _mtk_revision; uint8_t _mtk_revision;
uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE];
unsigned _rx_count; unsigned _rx_count;
uint8_t _rx_ck_a; uint8_t _rx_ck_a;
uint8_t _rx_ck_b; uint8_t _rx_ck_b;
src/drivers/gps/ubx.cpp
+45 -51
View File
@@ -50,7 +50,6 @@
#include <assert.h> #include <assert.h>
#include <math.h> #include <math.h>
#include <poll.h>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <time.h> #include <time.h>
@@ -61,12 +60,12 @@
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h> #include <uORB/topics/satellite_info.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <px4_defines.h>
#include "ubx.h" #include "ubx.h"
#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK #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_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 DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval
#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) #define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
@@ -77,13 +76,13 @@
/**** Trace macros, disable for production builds */ /**** Trace macros, disable for production builds */
#define UBX_TRACE_PARSER(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* decoding progress in parse_char() */ #define UBX_TRACE_PARSER(s, ...) {/*PX4_INFO(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_RXMSG(s, ...) {/*PX4_INFO(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_SVINFO(s, ...) {/*PX4_INFO(s, ## __VA_ARGS__);*/} /* NAV-SVINFO processing (debug use only, will cause rx buffer overflows) */
/**** Warning macros, disable to save memory */ /**** Warning macros, disable to save memory */
#define UBX_WARN(s, ...) {warnx(s, ## __VA_ARGS__);} #define UBX_WARN(s, ...) {PX4_WARN(s, ## __VA_ARGS__);}
#define UBX_DEBUG(s, ...) {/*warnx(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) : UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) :
_fd(fd), _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 int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
UBX::receive(const unsigned timeout) UBX::receive(const unsigned timeout)
{ {
/* poll descriptor */
pollfd fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[128]; uint8_t buf[128];
/* timeout additional to poll */ /* timeout additional to poll */
uint64_t time_started = hrt_absolute_time(); uint64_t time_started = hrt_absolute_time();
ssize_t count = 0;
int handled = 0; int handled = 0;
while (true) { while (true) {
bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled; 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 */ /* Wait for only UBX_PACKET_TIMEOUT if something already received. */
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); int ret = poll_or_read(_fd, buf, sizeof(buf), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
if (ret < 0) { if (ret < 0) {
/* something went wrong when polling */ /* something went wrong when polling or reading */
UBX_WARN("ubx poll() err"); UBX_WARN("ubx poll_or_read err");
return -1; return -1;
} else if (ret == 0) { } 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) { if (ready_to_return) {
_got_posllh = false; _got_posllh = false;
_got_velned = false; _got_velned = false;
return handled; return handled;
} else {
return -1;
} }
} else if (ret > 0) { } else {
/* if we have new data from GPS, go handle it */ UBX_DEBUG("read %d bytes", ret);
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));
/* pass received bytes to the packet decoder */ /* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++) { for (int i = 0; i < ret; i++) {
handled |= parse_char(buf[i]); handled |= parse_char(buf[i]);
} UBX_DEBUG("parsed %d: 0x%x", i, buf[i]);
} }
} }
/* abort after timeout if no useful packets received */ /* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < hrt_absolute_time()) { if (time_started + timeout * 1000 < hrt_absolute_time()) {
UBX_DEBUG("timed out, returning");
return -1; return -1;
} }
} }
@@ -374,7 +355,7 @@ UBX::parse_char(const uint8_t b)
/* Expecting Sync1 */ /* Expecting Sync1 */
case UBX_DECODE_SYNC1: case UBX_DECODE_SYNC1:
if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2 if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2
UBX_TRACE_PARSER("\nA"); UBX_TRACE_PARSER("A");
_decode_state = UBX_DECODE_SYNC2; _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)) { if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
// Part 1 complete: decode Part 1 buffer // 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); _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); (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->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->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); _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)sat_index + 1,
(unsigned)_satellite_info->used[sat_index], (unsigned)_satellite_info->used[sat_index],
(unsigned)_satellite_info->snr[sat_index], (unsigned)_satellite_info->snr[sat_index],
@@ -803,7 +784,7 @@ UBX::payload_rx_done(void)
switch (_rx_msg) { switch (_rx_msg) {
case UBX_MSG_NAV_PVT: 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 //Check if position fix flag is good
if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) { 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_hour = _buf.payload_rx_nav_pvt.hour;
timeinfo.tm_min = _buf.payload_rx_nav_pvt.min; timeinfo.tm_min = _buf.payload_rx_nav_pvt.min;
timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; 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); time_t epoch = mktime(&timeinfo);
if (epoch > GPS_EPOCH_SECS) { if (epoch > GPS_EPOCH_SECS) {
@@ -867,6 +851,10 @@ UBX::payload_rx_done(void)
} else { } else {
_gps_position->time_utc_usec = 0; _gps_position->time_utc_usec = 0;
} }
#else
_gps_position->time_utc_usec = 0;
#endif
} }
_gps_position->timestamp_time = hrt_absolute_time(); _gps_position->timestamp_time = hrt_absolute_time();
@@ -884,7 +872,7 @@ UBX::payload_rx_done(void)
break; break;
case UBX_MSG_NAV_POSLLH: 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->lat = _buf.payload_rx_nav_posllh.lat;
_gps_position->lon = _buf.payload_rx_nav_posllh.lon; _gps_position->lon = _buf.payload_rx_nav_posllh.lon;
@@ -902,7 +890,7 @@ UBX::payload_rx_done(void)
break; break;
case UBX_MSG_NAV_SOL: 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->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 _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; break;
case UBX_MSG_NAV_DOP: 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->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 _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; break;
case UBX_MSG_NAV_TIMEUTC: 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) { if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) {
// convert to unix timestamp // convert to unix timestamp
@@ -936,6 +924,8 @@ UBX::payload_rx_done(void)
timeinfo.tm_hour = _buf.payload_rx_nav_timeutc.hour; timeinfo.tm_hour = _buf.payload_rx_nav_timeutc.hour;
timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min; timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min;
timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; 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); time_t epoch = mktime(&timeinfo);
// only set the time if it makes sense // only set the time if it makes sense
@@ -959,6 +949,10 @@ UBX::payload_rx_done(void)
} else { } else {
_gps_position->time_utc_usec = 0; _gps_position->time_utc_usec = 0;
} }
#else
_gps_position->time_utc_usec = 0;
#endif
} }
_gps_position->timestamp_time = hrt_absolute_time(); _gps_position->timestamp_time = hrt_absolute_time();
@@ -967,7 +961,7 @@ UBX::payload_rx_done(void)
break; break;
case UBX_MSG_NAV_SVINFO: 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 already populated by payload_rx_add_svinfo(), just add a timestamp
_satellite_info->timestamp = hrt_absolute_time(); _satellite_info->timestamp = hrt_absolute_time();
@@ -976,7 +970,7 @@ UBX::payload_rx_done(void)
break; break;
case UBX_MSG_NAV_VELNED: 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_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 */ _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; break;
case UBX_MSG_MON_VER: case UBX_MSG_MON_VER:
UBX_TRACE_RXMSG("Rx MON-VER\n"); UBX_TRACE_RXMSG("Rx MON-VER");
ret = 1; ret = 1;
break; break;
case UBX_MSG_MON_HW: case UBX_MSG_MON_HW:
UBX_TRACE_RXMSG("Rx MON-HW\n"); UBX_TRACE_RXMSG("Rx MON-HW");
switch (_rx_payload_length) { switch (_rx_payload_length) {
@@ -1027,7 +1021,7 @@ UBX::payload_rx_done(void)
break; break;
case UBX_MSG_ACK_ACK: 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)) { if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
_ack_state = UBX_ACK_GOT_ACK; _ack_state = UBX_ACK_GOT_ACK;
@@ -1037,7 +1031,7 @@ UBX::payload_rx_done(void)
break; break;
case UBX_MSG_ACK_NAK: 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)) { if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
_ack_state = UBX_ACK_GOT_NAK; _ack_state = UBX_ACK_GOT_NAK;
src/drivers/gps/ubx.h
+7 -3
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -38,7 +38,7 @@
* including Prototol Specification. * including Prototol Specification.
* *
* @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch> * @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com> * @author Anton Babushkin <anton.babushkin@me.com>
* *
* @author Hannes Delago * @author Hannes Delago
@@ -446,7 +446,12 @@ typedef union {
ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5; ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5;
ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas; ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas;
ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg; 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[]; uint8_t raw[];
#endif
} ubx_buf_t; } ubx_buf_t;
#pragma pack(pop) #pragma pack(pop)
@@ -552,7 +557,6 @@ private:
int _fd; int _fd;
struct vehicle_gps_position_s *_gps_position; struct vehicle_gps_position_s *_gps_position;
struct satellite_info_s *_satellite_info; struct satellite_info_s *_satellite_info;
bool _enable_sat_info;
bool _configured; bool _configured;
ubx_ack_state_t _ack_state; ubx_ack_state_t _ack_state;
bool _got_posllh; bool _got_posllh;
src/modules/systemlib/scheduling_priorities.h
+2
View File
@@ -33,7 +33,9 @@
#pragma once #pragma once
#ifndef __PX4_QURT
#include <nuttx/sched.h> #include <nuttx/sched.h>
#endif
/* SCHED_PRIORITY_MAX */ /* SCHED_PRIORITY_MAX */
#define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX #define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX