forked from Archive/PX4-Autopilot
gps: make the driver compile and run on QURT
This commit is contained in:
Julian Oes
Lorenz Meier
parent
bc9e24102d
commit
1cebfde840
|
|
@ -12,9 +12,13 @@
|
|||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/satellite_info.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 <math.h>
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <nuttx/clock.h>
|
||||
#include <nuttx/arch.h>
|
||||
#include <fcntl.h>
|
||||
#endif
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <px4_config.h>
|
||||
#include <nuttx/arch.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <termios.h>
|
||||
#include <poll.h>
|
||||
#else
|
||||
#include <sys/ioctl.h>
|
||||
#include <dev_fs_lib_serial.h>
|
||||
#endif
|
||||
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "gps_helper.h"
|
||||
|
||||
/**
|
||||
* @file gps_helper.cpp
|
||||
*
|
||||
* @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
|
||||
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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
// 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;
|
||||
|
|
|
|||
|
|
@ -38,6 +38,7 @@
|
|||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <px4_defines.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#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;
|
||||
|
|
|
|||
|
|
@ -50,7 +50,6 @@
|
|||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
|
@ -61,12 +60,12 @@
|
|||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/satellite_info.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
#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;
|
||||
|
|
|
|||
|
|
@ -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 <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 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;
|
||||
|
|
|
|||
|
|
@ -33,7 +33,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#ifndef __PX4_QURT
|
||||
#include <nuttx/sched.h>
|
||||
#endif
|
||||
|
||||
/* SCHED_PRIORITY_MAX */
|
||||
#define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX
|
||||
|
|
|
|||
Loading…
Reference in New Issue