forked from Archive/PX4-Autopilot
Reduced, but functional u-blox series driver
This commit is contained in:
Lorenz Meier
parent
3e5cd26777
commit
50b736333f
|
|
@ -0,0 +1,80 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file GPS driver interface.
|
||||
*/
|
||||
|
||||
#ifndef _DRV_GPS_H
|
||||
#define _DRV_GPS_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "drv_sensor.h"
|
||||
#include "drv_orb_dev.h"
|
||||
|
||||
#define GPS_DEVICE_PATH "/dev/gps"
|
||||
|
||||
typedef enum {
|
||||
GPS_DRIVER_MODE_UBX = 0,
|
||||
GPS_DRIVER_MODE_MTK19,
|
||||
GPS_DRIVER_MODE_MTK16,
|
||||
GPS_DRIVER_MODE_NMEA,
|
||||
} gps_driver_mode_t;
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* ObjDev tag for GPS data.
|
||||
*/
|
||||
ORB_DECLARE(gps);
|
||||
|
||||
/*
|
||||
* ioctl() definitions
|
||||
*/
|
||||
#define _GPSIOCBASE (0x2800) //TODO: arbitrary choice...
|
||||
#define _GPSIOC(_n) (_IOC(_GPSIOCBASE, _n))
|
||||
|
||||
/** configure ubx mode */
|
||||
#define GPS_CONFIGURE_UBX _GPSIOC(0)
|
||||
|
||||
/** configure mtk mode */
|
||||
#define GPS_CONFIGURE_MTK19 _GPSIOC(1)
|
||||
#define GPS_CONFIGURE_MTK16 _GPSIOC(2)
|
||||
|
||||
/** configure mtk mode */
|
||||
#define GPS_CONFIGURE_NMEA _GPSIOC(3)
|
||||
|
||||
#endif /* _DRV_GPS_H */
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2012 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
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# GPS driver
|
||||
#
|
||||
|
||||
APPNAME = gps
|
||||
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||
STACKSIZE = 2048
|
||||
|
||||
include $(APPDIR)/mk/app.mk
|
||||
|
|
@ -0,0 +1,627 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file gps.cpp
|
||||
* Driver for the GPS on UART6
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <semaphore.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <termios.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/clock.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/scheduling_priorities.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include <drivers/drv_gps.h>
|
||||
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
#include "ubx.h"
|
||||
|
||||
#define SEND_BUFFER_LENGTH 100
|
||||
#define TIMEOUT 1000000 //1s
|
||||
|
||||
#define NUMBER_OF_BAUDRATES 4
|
||||
#define CONFIG_TIMEOUT 2000000
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
#ifndef CONFIG_SCHED_WORKQUEUE
|
||||
# error This requires CONFIG_SCHED_WORKQUEUE.
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
class GPS : public device::CDev
|
||||
{
|
||||
public:
|
||||
GPS();
|
||||
~GPS();
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
void print_info();
|
||||
|
||||
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 _response_received;
|
||||
bool _config_needed;
|
||||
bool _baudrate_changed;
|
||||
bool _mode_changed;
|
||||
bool _healthy;
|
||||
gps_driver_mode_t _mode;
|
||||
unsigned _messages_received;
|
||||
|
||||
GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper
|
||||
|
||||
struct vehicle_gps_position_s _report;
|
||||
orb_advert_t _report_pub;
|
||||
|
||||
orb_advert_t _gps_topic;
|
||||
|
||||
void recv();
|
||||
|
||||
void config();
|
||||
|
||||
static void task_main_trampoline(void *arg);
|
||||
|
||||
|
||||
/**
|
||||
* worker task
|
||||
*/
|
||||
void task_main(void);
|
||||
|
||||
int set_baudrate(unsigned baud);
|
||||
|
||||
/**
|
||||
* Send a reset command to the GPS
|
||||
*/
|
||||
void cmd_reset();
|
||||
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
* Driver 'main' command.
|
||||
*/
|
||||
extern "C" __EXPORT int gps_main(int argc, char *argv[]);
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
GPS *g_dev;
|
||||
|
||||
}
|
||||
|
||||
|
||||
GPS::GPS() :
|
||||
CDev("gps", GPS_DEVICE_PATH),
|
||||
_task_should_exit(false),
|
||||
_baudrates_to_try({9600, 38400, 57600, 115200}),
|
||||
_config_needed(true),
|
||||
_baudrate_changed(false),
|
||||
_mode_changed(true),
|
||||
_healthy(false),
|
||||
_mode(GPS_DRIVER_MODE_UBX),
|
||||
_messages_received(0),
|
||||
_Helper(nullptr)
|
||||
{
|
||||
/* we need this potentially before it could be set in task_main */
|
||||
g_dev = this;
|
||||
|
||||
_debug_enabled = true;
|
||||
debug("[gps driver] instantiated");
|
||||
}
|
||||
|
||||
GPS::~GPS()
|
||||
{
|
||||
/* tell the task we want it to go away */
|
||||
_task_should_exit = true;
|
||||
|
||||
/* spin waiting for the task to stop */
|
||||
for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
|
||||
/* give it another 100ms */
|
||||
usleep(100000);
|
||||
}
|
||||
|
||||
/* well, kill it anyway, though this will probably crash */
|
||||
if (_task != -1)
|
||||
task_delete(_task);
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
int
|
||||
GPS::init()
|
||||
{
|
||||
int ret = ERROR;
|
||||
|
||||
/* do regular cdev init */
|
||||
if (CDev::init() != OK)
|
||||
goto out;
|
||||
|
||||
/* start the IO interface task */
|
||||
_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 4096, (main_t)&GPS::task_main_trampoline, nullptr);
|
||||
|
||||
if (_task < 0) {
|
||||
debug("task start failed: %d", errno);
|
||||
return -errno;
|
||||
}
|
||||
|
||||
// if (_gps_topic < 0)
|
||||
// debug("failed to create GPS object");
|
||||
|
||||
ret = OK;
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
switch (cmd) {
|
||||
case GPS_CONFIGURE_UBX:
|
||||
if (_mode != GPS_DRIVER_MODE_UBX) {
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
_mode_changed = true;
|
||||
}
|
||||
break;
|
||||
case GPS_CONFIGURE_MTK19:
|
||||
if (_mode != GPS_DRIVER_MODE_MTK19) {
|
||||
_mode = GPS_DRIVER_MODE_MTK19;
|
||||
_mode_changed = true;
|
||||
}
|
||||
break;
|
||||
case GPS_CONFIGURE_MTK16:
|
||||
if (_mode != GPS_DRIVER_MODE_MTK16) {
|
||||
_mode = GPS_DRIVER_MODE_MTK16;
|
||||
_mode_changed = true;
|
||||
}
|
||||
break;
|
||||
case GPS_CONFIGURE_NMEA:
|
||||
if (_mode != GPS_DRIVER_MODE_NMEA) {
|
||||
_mode = GPS_DRIVER_MODE_NMEA;
|
||||
_mode_changed = true;
|
||||
}
|
||||
break;
|
||||
case SENSORIOCRESET:
|
||||
cmd_reset();
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
GPS::config()
|
||||
{
|
||||
int length = 0;
|
||||
|
||||
_Helper->configure(_config_needed, _baudrate_changed, _baudrate, _send_buffer, length, SEND_BUFFER_LENGTH);
|
||||
|
||||
/* the message needs to be sent at the old baudrate */
|
||||
if (length > 0) {
|
||||
if (length != ::write(_serial_fd, _send_buffer, length)) {
|
||||
debug("write config failed");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (_baudrate_changed) {
|
||||
set_baudrate(_baudrate);
|
||||
_baudrate_changed = false;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
GPS::task_main_trampoline(void *arg)
|
||||
{
|
||||
g_dev->task_main();
|
||||
}
|
||||
|
||||
void
|
||||
GPS::task_main()
|
||||
{
|
||||
log("starting");
|
||||
|
||||
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
|
||||
|
||||
memset(&_report, 0, sizeof(_report));
|
||||
|
||||
/* open the serial port */
|
||||
_serial_fd = ::open("/dev/ttyS3", O_RDWR);
|
||||
|
||||
uint8_t buf[256];
|
||||
int baud_i = 0;
|
||||
uint64_t time_before_configuration;
|
||||
|
||||
if (_serial_fd < 0) {
|
||||
log("failed to open serial port: %d", errno);
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* poll descriptor */
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _serial_fd;
|
||||
fds[0].events = POLLIN;
|
||||
debug("ready");
|
||||
|
||||
/* lock against the ioctl handler */
|
||||
lock();
|
||||
|
||||
|
||||
_baudrate = _baudrates_to_try[baud_i];
|
||||
set_baudrate(_baudrate);
|
||||
|
||||
time_before_configuration = hrt_absolute_time();
|
||||
|
||||
/* loop handling received serial bytes */
|
||||
while (!_task_should_exit) {
|
||||
|
||||
if (_mode_changed) {
|
||||
if (_Helper != nullptr) {
|
||||
delete(_Helper);
|
||||
_Helper = nullptr; // XXX is this needed?
|
||||
}
|
||||
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_Helper = new UBX();
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK19:
|
||||
//_Helper = new MTK19();
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK16:
|
||||
//_Helper = new MTK16();
|
||||
break;
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
//_Helper = new NMEA();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
_mode_changed = false;
|
||||
}
|
||||
|
||||
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);
|
||||
time_before_configuration = hrt_absolute_time();
|
||||
}
|
||||
|
||||
|
||||
int poll_timeout;
|
||||
if (_config_needed) {
|
||||
poll_timeout = 50;
|
||||
} else {
|
||||
poll_timeout = 250;
|
||||
}
|
||||
/* sleep waiting for data, but no more than 1000ms */
|
||||
unlock();
|
||||
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout);
|
||||
lock();
|
||||
|
||||
|
||||
|
||||
|
||||
/* this would be bad... */
|
||||
if (ret < 0) {
|
||||
log("poll error %d", errno);
|
||||
} else if (ret == 0) {
|
||||
config();
|
||||
if (_config_needed == false) {
|
||||
_config_needed = true;
|
||||
debug("Lost GPS module");
|
||||
}
|
||||
} else if (ret > 0) {
|
||||
/* if we have new data from GPS, go handle it */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
int count;
|
||||
|
||||
/*
|
||||
* 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(_serial_fd, buf, sizeof(buf));
|
||||
|
||||
/* pass received bytes to the packet decoder */
|
||||
int j;
|
||||
for (j = 0; j < count; j++) {
|
||||
_messages_received += _Helper->parse(buf[j], &_report);
|
||||
}
|
||||
if (_messages_received > 0) {
|
||||
if (_config_needed == true) {
|
||||
_config_needed = false;
|
||||
debug("Found GPS module");
|
||||
}
|
||||
|
||||
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
|
||||
_messages_received = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
out:
|
||||
debug("exiting");
|
||||
|
||||
::close(_serial_fd);
|
||||
|
||||
/* tell the dtor that we are exiting */
|
||||
_task = -1;
|
||||
_exit(0);
|
||||
}
|
||||
|
||||
int
|
||||
GPS::set_baudrate(unsigned baud)
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
|
||||
switch (baud) {
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
default:
|
||||
warnx("ERROR: Unsupported baudrate: %d\n", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
tcgetattr(_serial_fd, &uart_config);
|
||||
|
||||
/* clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
/* no parity, one stop bit */
|
||||
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);
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
|
||||
warnx("ERROR setting baudrate (tcsetattr)\n");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
GPS::cmd_reset()
|
||||
{
|
||||
_healthy = false;
|
||||
}
|
||||
|
||||
void
|
||||
GPS::print_info()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace gps
|
||||
{
|
||||
|
||||
GPS *g_dev;
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
void test();
|
||||
void reset();
|
||||
void info();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*/
|
||||
void
|
||||
start()
|
||||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
errx(1, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new GPS;
|
||||
|
||||
if (g_dev == nullptr)
|
||||
goto fail;
|
||||
|
||||
if (OK != g_dev->init())
|
||||
goto fail;
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(GPS_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
printf("Could not open device path: %s\n", GPS_DEVICE_PATH);
|
||||
goto fail;
|
||||
}
|
||||
exit(0);
|
||||
|
||||
fail:
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
errx(1, "driver start failed");
|
||||
}
|
||||
|
||||
void
|
||||
stop()
|
||||
{
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform some basic functional tests on the driver;
|
||||
* make sure we can collect data from the sensor in polled
|
||||
* and automatic modes.
|
||||
*/
|
||||
void
|
||||
test()
|
||||
{
|
||||
|
||||
errx(0, "PASS");
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the driver.
|
||||
*/
|
||||
void
|
||||
reset()
|
||||
{
|
||||
int fd = open(GPS_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "failed ");
|
||||
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||
err(1, "driver reset failed");
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Print the status of the driver.
|
||||
*/
|
||||
void
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr)
|
||||
errx(1, "driver not running");
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
|
||||
int
|
||||
gps_main(int argc, char *argv[])
|
||||
{
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "start"))
|
||||
gps::start();
|
||||
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
gps::stop();
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[1], "test"))
|
||||
gps::test();
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "reset"))
|
||||
gps::reset();
|
||||
|
||||
/*
|
||||
* Print driver status.
|
||||
*/
|
||||
if (!strcmp(argv[1], "status"))
|
||||
gps::info();
|
||||
|
||||
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'");
|
||||
}
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* @file U-Blox protocol definitions */
|
||||
|
||||
#ifndef GPS_HELPER_H
|
||||
|
||||
class GPS_Helper
|
||||
{
|
||||
public:
|
||||
virtual void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned) = 0;
|
||||
virtual int parse(uint8_t, struct vehicle_gps_position_s*) = 0;
|
||||
};
|
||||
|
||||
#endif /* GPS_HELPER_H */
|
||||
|
|
@ -0,0 +1,818 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* @file U-Blox protocol implementation */
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "ubx.h"
|
||||
|
||||
|
||||
UBX::UBX() :
|
||||
_waited(0),
|
||||
_config_state(UBX_CONFIG_STATE_PRT),
|
||||
_new_nav_posllh(false),
|
||||
_new_nav_timeutc(false),
|
||||
_new_nav_dop(false),
|
||||
_new_nav_sol(false),
|
||||
_new_nav_velned(false)
|
||||
{
|
||||
decodeInit();
|
||||
_waiting_for_ack = false;
|
||||
}
|
||||
|
||||
UBX::~UBX()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length)
|
||||
{
|
||||
/* make sure the buffer to write the message is long enough */
|
||||
assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length);
|
||||
|
||||
/* try again after 10 times */
|
||||
if (_waited > 10) {
|
||||
_waiting_for_ack = false;
|
||||
}
|
||||
|
||||
if (!_waiting_for_ack) {
|
||||
_waiting_for_ack = true;
|
||||
_waited = 0;
|
||||
if (_config_state == UBX_CONFIG_STATE_CONFIGURED) {
|
||||
config_needed = false;
|
||||
length = 0;
|
||||
_config_state = UBX_CONFIG_STATE_PRT; /* set the state for next time */
|
||||
_waiting_for_ack = false;
|
||||
return;
|
||||
} else if (_config_state == UBX_CONFIG_STATE_PRT) {
|
||||
/* send a CFT-PRT message to define set ubx protocol and leave the baudrate as is, we just want an ACK-ACK from this */
|
||||
type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
|
||||
memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
|
||||
|
||||
cfg_prt_packet.clsID = UBX_CLASS_CFG;
|
||||
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
|
||||
cfg_prt_packet.length = UBX_CFG_PRT_LENGTH;
|
||||
cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID;
|
||||
cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE;
|
||||
cfg_prt_packet.baudRate = baudrate;
|
||||
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
|
||||
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
|
||||
|
||||
addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
|
||||
|
||||
buffer[0] = UBX_SYNC1;
|
||||
buffer[1] = UBX_SYNC2;
|
||||
memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet));
|
||||
length = sizeof(cfg_prt_packet)+2;
|
||||
|
||||
} else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) {
|
||||
|
||||
// printf("Send change of baudrate now\n");
|
||||
|
||||
/* send a CFT-PRT message to define set ubx protocol and and baudrate, now let's try to switch the baudrate */
|
||||
type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
|
||||
memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
|
||||
|
||||
cfg_prt_packet.clsID = UBX_CLASS_CFG;
|
||||
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
|
||||
cfg_prt_packet.length = UBX_CFG_PRT_LENGTH;
|
||||
cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID;
|
||||
cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE;
|
||||
cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
|
||||
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
|
||||
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
|
||||
|
||||
addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
|
||||
|
||||
buffer[0] = UBX_SYNC1;
|
||||
buffer[1] = UBX_SYNC2;
|
||||
memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet));
|
||||
length = sizeof(cfg_prt_packet)+2;
|
||||
|
||||
/* detection when the baudrate has been changed in the first step */
|
||||
if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
|
||||
/* set a flag and exit */
|
||||
baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE;
|
||||
baudrate_changed = true;
|
||||
_config_state = UBX_CONFIG_STATE_RATE;
|
||||
_waiting_for_ack = false;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
} else if (_config_state == UBX_CONFIG_STATE_RATE) {
|
||||
|
||||
/* send a CFT-RATE message to define update rate */
|
||||
type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
|
||||
memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
|
||||
|
||||
cfg_rate_packet.clsID = UBX_CLASS_CFG;
|
||||
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
|
||||
cfg_rate_packet.length = UBX_CFG_RATE_LENGTH;
|
||||
cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE;
|
||||
cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
|
||||
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
|
||||
|
||||
addChecksumToMessage((uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
|
||||
|
||||
buffer[0] = UBX_SYNC1;
|
||||
buffer[1] = UBX_SYNC2;
|
||||
memcpy(&(buffer[2]), &cfg_rate_packet, sizeof(cfg_rate_packet));
|
||||
length = sizeof(cfg_rate_packet)+2;
|
||||
|
||||
} else if (_config_state == UBX_CONFIG_STATE_NAV5) {
|
||||
/* send a NAV5 message to set the options for the internal estimator */
|
||||
type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
|
||||
memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); //set everything to 0
|
||||
|
||||
cfg_nav5_packet.clsID = UBX_CLASS_CFG;
|
||||
cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5;
|
||||
cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH;
|
||||
cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK;
|
||||
cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
|
||||
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
|
||||
|
||||
addChecksumToMessage((uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
|
||||
|
||||
buffer[0] = UBX_SYNC1;
|
||||
buffer[1] = UBX_SYNC2;
|
||||
memcpy(&(buffer[2]), &cfg_nav5_packet, sizeof(cfg_nav5_packet));
|
||||
length = sizeof(cfg_nav5_packet)+2;
|
||||
|
||||
} else {
|
||||
/* catch the remaining config states here */
|
||||
|
||||
type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
|
||||
memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); //set everything to 0
|
||||
|
||||
cfg_msg_packet.clsID = UBX_CLASS_CFG;
|
||||
cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG;
|
||||
cfg_msg_packet.length = UBX_CFG_MSG_LENGTH;
|
||||
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1;
|
||||
|
||||
switch (_config_state) {
|
||||
case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_DOP:
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
|
||||
cfg_msg_packet.rate[1] = 5;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_SOL:
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_VELNED:
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
|
||||
break;
|
||||
// case UBX_CONFIG_STATE_MSG_RXM_SVSI:
|
||||
// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
|
||||
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
|
||||
// break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
addChecksumToMessage((uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
|
||||
|
||||
buffer[0] = UBX_SYNC1;
|
||||
buffer[1] = UBX_SYNC2;
|
||||
memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet));
|
||||
length = sizeof(cfg_msg_packet)+2;
|
||||
}
|
||||
} else {
|
||||
_waited++;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position)
|
||||
{
|
||||
int ret = 0;
|
||||
//printf("Received char: %c\n", b);
|
||||
|
||||
switch (_decode_state) {
|
||||
/* First, look for sync1 */
|
||||
case UBX_DECODE_UNINIT:
|
||||
if (b == UBX_SYNC1) {
|
||||
_decode_state = UBX_DECODE_GOT_SYNC1;
|
||||
}
|
||||
break;
|
||||
/* Second, look for sync2 */
|
||||
case UBX_DECODE_GOT_SYNC1:
|
||||
if (b == UBX_SYNC2) {
|
||||
_decode_state = UBX_DECODE_GOT_SYNC2;
|
||||
} else {
|
||||
/* Second start symbol was wrong, reset state machine */
|
||||
decodeInit();
|
||||
}
|
||||
break;
|
||||
/* Now look for class */
|
||||
case UBX_DECODE_GOT_SYNC2:
|
||||
/* everything except sync1 and sync2 needs to be added to the checksum */
|
||||
addByteToChecksum(b);
|
||||
/* check for known class */
|
||||
switch (b) {
|
||||
case UBX_CLASS_ACK:
|
||||
_decode_state = UBX_DECODE_GOT_CLASS;
|
||||
_message_class = ACK;
|
||||
break;
|
||||
|
||||
case UBX_CLASS_NAV:
|
||||
_decode_state = UBX_DECODE_GOT_CLASS;
|
||||
_message_class = NAV;
|
||||
break;
|
||||
|
||||
// case UBX_CLASS_RXM:
|
||||
// _decode_state = UBX_DECODE_GOT_CLASS;
|
||||
// _message_class = RXM;
|
||||
// break;
|
||||
|
||||
case UBX_CLASS_CFG:
|
||||
_decode_state = UBX_DECODE_GOT_CLASS;
|
||||
_message_class = CFG;
|
||||
break;
|
||||
default: //unknown class: reset state machine
|
||||
decodeInit();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case UBX_DECODE_GOT_CLASS:
|
||||
addByteToChecksum(b);
|
||||
switch (_message_class) {
|
||||
case NAV:
|
||||
switch (b) {
|
||||
case UBX_MESSAGE_NAV_POSLLH:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
_message_id = NAV_POSLLH;
|
||||
break;
|
||||
|
||||
case UBX_MESSAGE_NAV_SOL:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
_message_id = NAV_SOL;
|
||||
break;
|
||||
|
||||
case UBX_MESSAGE_NAV_TIMEUTC:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
_message_id = NAV_TIMEUTC;
|
||||
break;
|
||||
|
||||
case UBX_MESSAGE_NAV_DOP:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
_message_id = NAV_DOP;
|
||||
break;
|
||||
|
||||
case UBX_MESSAGE_NAV_SVINFO:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
_message_id = NAV_SVINFO;
|
||||
break;
|
||||
|
||||
case UBX_MESSAGE_NAV_VELNED:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
_message_id = NAV_VELNED;
|
||||
break;
|
||||
|
||||
default: //unknown class: reset state machine, should not happen
|
||||
decodeInit();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
// case RXM:
|
||||
// switch (b) {
|
||||
// case UBX_MESSAGE_RXM_SVSI:
|
||||
// _decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
// _message_id = RXM_SVSI;
|
||||
// break;
|
||||
//
|
||||
// default: //unknown class: reset state machine, should not happen
|
||||
// decodeInit();
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
|
||||
case CFG:
|
||||
switch (b) {
|
||||
case UBX_MESSAGE_CFG_NAV5:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
_message_id = CFG_NAV5;
|
||||
break;
|
||||
|
||||
default: //unknown class: reset state machine, should not happen
|
||||
decodeInit();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case ACK:
|
||||
switch (b) {
|
||||
case UBX_MESSAGE_ACK_ACK:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
_message_id = ACK_ACK;
|
||||
break;
|
||||
case UBX_MESSAGE_ACK_NAK:
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
_message_id = ACK_NAK;
|
||||
break;
|
||||
default: //unknown class: reset state machine, should not happen
|
||||
decodeInit();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
default: //should not happen because we set the class
|
||||
printf("UBX Error, we set a class that we don't know\n");
|
||||
decodeInit();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case UBX_DECODE_GOT_MESSAGEID:
|
||||
addByteToChecksum(b);
|
||||
_payload_size = b; //this is the first length byte
|
||||
_decode_state = UBX_DECODE_GOT_LENGTH1;
|
||||
break;
|
||||
case UBX_DECODE_GOT_LENGTH1:
|
||||
addByteToChecksum(b);
|
||||
_payload_size += b << 8; // here comes the second byte of length
|
||||
_decode_state = UBX_DECODE_GOT_LENGTH2;
|
||||
break;
|
||||
case UBX_DECODE_GOT_LENGTH2:
|
||||
/* Add to checksum if not yet at checksum byte */
|
||||
if (_rx_count < _payload_size)
|
||||
addByteToChecksum(b);
|
||||
_rx_buffer[_rx_count] = b;
|
||||
/* once the payload has arrived, we can process the information */
|
||||
if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
|
||||
switch (_message_id) { //this enum is unique for all ids --> no need to check the class
|
||||
case NAV_POSLLH: {
|
||||
// printf("GOT NAV_POSLLH MESSAGE\n");
|
||||
gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
|
||||
|
||||
//Check if checksum is valid and the store the gps information
|
||||
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
gps_position->lat = packet->lat;
|
||||
gps_position->lon = packet->lon;
|
||||
gps_position->alt = packet->height_msl;
|
||||
|
||||
gps_position->counter_pos_valid++;
|
||||
gps_position->counter++;
|
||||
|
||||
_new_nav_posllh = true;
|
||||
|
||||
} else {
|
||||
printf("[gps] NAV_POSLLH: checksum invalid\n");
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decodeInit();
|
||||
return ret;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case NAV_SOL: {
|
||||
// printf("GOT NAV_SOL MESSAGE\n");
|
||||
gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
|
||||
|
||||
//Check if checksum is valid and the store the gps information
|
||||
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
|
||||
gps_position->fix_type = packet->gpsFix;
|
||||
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
gps_position->counter++;
|
||||
gps_position->s_variance = packet->sAcc;
|
||||
gps_position->p_variance = packet->pAcc;
|
||||
|
||||
_new_nav_sol = true;
|
||||
|
||||
} else {
|
||||
printf("[gps] NAV_SOL: checksum invalid\n");
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decodeInit();
|
||||
return ret;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case NAV_DOP: {
|
||||
// printf("GOT NAV_DOP MESSAGE\n");
|
||||
gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
|
||||
|
||||
//Check if checksum is valid and the store the gps information
|
||||
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
|
||||
gps_position->eph = packet->hDOP;
|
||||
gps_position->epv = packet->vDOP;
|
||||
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
gps_position->counter++;
|
||||
|
||||
_new_nav_dop = true;
|
||||
|
||||
} else {
|
||||
printf("[gps] NAV_DOP: checksum invalid\n");
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decodeInit();
|
||||
return ret;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case NAV_TIMEUTC: {
|
||||
// printf("GOT NAV_TIMEUTC MESSAGE\n");
|
||||
gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
|
||||
|
||||
//Check if checksum is valid and the store the gps information
|
||||
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
//convert to unix timestamp
|
||||
struct tm timeinfo;
|
||||
timeinfo.tm_year = packet->year - 1900;
|
||||
timeinfo.tm_mon = packet->month - 1;
|
||||
timeinfo.tm_mday = packet->day;
|
||||
timeinfo.tm_hour = packet->hour;
|
||||
timeinfo.tm_min = packet->min;
|
||||
timeinfo.tm_sec = packet->sec;
|
||||
|
||||
time_t epoch = mktime(&timeinfo);
|
||||
|
||||
gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
|
||||
gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
|
||||
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
gps_position->counter++;
|
||||
|
||||
_new_nav_timeutc = true;
|
||||
|
||||
} else {
|
||||
printf("\t[gps] NAV_TIMEUTC: checksum invalid\n");
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decodeInit();
|
||||
return ret;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case NAV_SVINFO: {
|
||||
// printf("GOT NAV_SVINFO MESSAGE\n");
|
||||
|
||||
//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
|
||||
const int length_part1 = 8;
|
||||
char _rx_buffer_part1[length_part1];
|
||||
memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
|
||||
gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1;
|
||||
|
||||
//read checksum
|
||||
const int length_part3 = 2;
|
||||
char _rx_buffer_part3[length_part3];
|
||||
memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
|
||||
gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) _rx_buffer_part3;
|
||||
|
||||
//Check if checksum is valid and then store the gps information
|
||||
if (_rx_ck_a == packet_part3->ck_a && _rx_ck_b == packet_part3->ck_b) {
|
||||
//definitions needed to read numCh elements from the buffer:
|
||||
const int length_part2 = 12;
|
||||
gps_bin_nav_svinfo_part2_packet_t *packet_part2;
|
||||
char _rx_buffer_part2[length_part2]; //for temporal storage
|
||||
|
||||
uint8_t satellites_used = 0;
|
||||
int i;
|
||||
|
||||
for (i = 0; i < packet_part1->numCh; i++) { //for each channel
|
||||
|
||||
/* Get satellite information from the buffer */
|
||||
memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
|
||||
packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
|
||||
|
||||
|
||||
/* Write satellite information in the global storage */
|
||||
gps_position->satellite_prn[i] = packet_part2->svid;
|
||||
|
||||
//if satellite information is healthy store the data
|
||||
uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
|
||||
|
||||
if (!unhealthy) {
|
||||
if ((packet_part2->flags) & 1) { //flags is a bitfield
|
||||
gps_position->satellite_used[i] = 1;
|
||||
satellites_used++;
|
||||
|
||||
} else {
|
||||
gps_position->satellite_used[i] = 0;
|
||||
}
|
||||
|
||||
gps_position->satellite_snr[i] = packet_part2->cno;
|
||||
gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
|
||||
gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
|
||||
|
||||
} else {
|
||||
gps_position->satellite_used[i] = 0;
|
||||
gps_position->satellite_snr[i] = 0;
|
||||
gps_position->satellite_elevation[i] = 0;
|
||||
gps_position->satellite_azimuth[i] = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
|
||||
/* Unused channels have to be set to zero for e.g. MAVLink */
|
||||
gps_position->satellite_prn[i] = 0;
|
||||
gps_position->satellite_used[i] = 0;
|
||||
gps_position->satellite_snr[i] = 0;
|
||||
gps_position->satellite_elevation[i] = 0;
|
||||
gps_position->satellite_azimuth[i] = 0;
|
||||
}
|
||||
|
||||
/* set flag if any sat info is available */
|
||||
if (!packet_part1->numCh > 0) {
|
||||
gps_position->satellite_info_available = 1;
|
||||
|
||||
} else {
|
||||
gps_position->satellite_info_available = 0;
|
||||
}
|
||||
|
||||
gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
gps_position->counter++;
|
||||
|
||||
// as this message arrives only with 1Hz and is not essential, we don't take it into account for the report
|
||||
|
||||
} else {
|
||||
printf("\t[gps] NAV_SVINFO: checksum invalid\n");
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decodeInit();
|
||||
return ret;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case NAV_VELNED: {
|
||||
// printf("GOT NAV_VELNED MESSAGE\n");
|
||||
gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
|
||||
|
||||
//Check if checksum is valid and the store the gps information
|
||||
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
|
||||
gps_position->vel = (uint16_t)packet->speed;
|
||||
gps_position->vel_n = packet->velN / 100.0f;
|
||||
gps_position->vel_e = packet->velE / 100.0f;
|
||||
gps_position->vel_d = packet->velD / 100.0f;
|
||||
gps_position->vel_ned_valid = true;
|
||||
gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f);
|
||||
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
gps_position->counter++;
|
||||
|
||||
|
||||
_new_nav_velned = true;
|
||||
|
||||
} else {
|
||||
printf("[gps] NAV_VELNED: checksum invalid\n");
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decodeInit();
|
||||
return ret;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
// case RXM_SVSI: {
|
||||
// printf("GOT RXM_SVSI MESSAGE\n");
|
||||
// const int length_part1 = 7;
|
||||
// char _rx_buffer_part1[length_part1];
|
||||
// memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
|
||||
// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1;
|
||||
//
|
||||
// //Check if checksum is valid and the store the gps information
|
||||
// if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) {
|
||||
//
|
||||
// gps_position->satellites_visible = packet->numVis;
|
||||
// gps_position->timestamp = hrt_absolute_time();
|
||||
// gps_position->counter++;
|
||||
// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
|
||||
// ret = 1;
|
||||
//
|
||||
// } else {
|
||||
// printf("[gps] RXM_SVSI: checksum invalid\n");
|
||||
// }
|
||||
//
|
||||
// // Reset state machine to decode next packet
|
||||
// decodeInit();
|
||||
// return ret;
|
||||
//
|
||||
// break;
|
||||
// }
|
||||
|
||||
case ACK_ACK: {
|
||||
// printf("GOT ACK_ACK\n");
|
||||
gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
|
||||
|
||||
//Check if checksum is valid
|
||||
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
|
||||
_waiting_for_ack = false;
|
||||
|
||||
switch (_config_state) {
|
||||
case UBX_CONFIG_STATE_PRT:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT)
|
||||
_config_state = UBX_CONFIG_STATE_PRT_NEW_BAUDRATE;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_PRT_NEW_BAUDRATE:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT)
|
||||
_config_state = UBX_CONFIG_STATE_RATE;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_RATE:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_RATE)
|
||||
_config_state = UBX_CONFIG_STATE_NAV5;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_NAV5:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5)
|
||||
_config_state = UBX_CONFIG_STATE_MSG_NAV_POSLLH;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
_config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
_config_state = UBX_CONFIG_STATE_MSG_NAV_DOP;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_DOP:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
_config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
_config_state = UBX_CONFIG_STATE_MSG_NAV_SOL;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_SOL:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
_config_state = UBX_CONFIG_STATE_MSG_NAV_VELNED;
|
||||
break;
|
||||
case UBX_CONFIG_STATE_MSG_NAV_VELNED:
|
||||
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
_config_state = UBX_CONFIG_STATE_CONFIGURED;
|
||||
break;
|
||||
// case UBX_CONFIG_STATE_MSG_RXM_SVSI:
|
||||
// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
|
||||
// _config_state = UBX_CONFIG_STATE_CONFIGURED;
|
||||
// break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
printf("[gps] ACK_ACK: checksum invalid\n");
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decodeInit();
|
||||
return ret;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case ACK_NAK: {
|
||||
// printf("GOT ACK_NAK\n");
|
||||
gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer;
|
||||
|
||||
//Check if checksum is valid
|
||||
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
|
||||
|
||||
printf("[gps] the ubx gps returned: not acknowledged\n");
|
||||
ret = 1;
|
||||
|
||||
} else {
|
||||
printf("[gps] ACK_NAK: checksum invalid\n");
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decodeInit();
|
||||
return ret;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default: //we don't know the message
|
||||
printf("Unknown message received: %d-%d\n",_message_class,_message_id);
|
||||
decodeInit();
|
||||
|
||||
break;
|
||||
}
|
||||
} // end if _rx_count high enough
|
||||
if (_rx_count < RECV_BUFFER_SIZE) {
|
||||
_rx_count++;
|
||||
} else {
|
||||
printf("Buffer full");
|
||||
decodeInit();
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
/* return 1 when all needed messages have arrived */
|
||||
if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) {
|
||||
|
||||
gps_position->timestamp = hrt_absolute_time();
|
||||
ret = 1;
|
||||
_new_nav_posllh = false;
|
||||
_new_nav_timeutc = false;
|
||||
_new_nav_dop = false;
|
||||
_new_nav_sol = false;
|
||||
_new_nav_velned = false;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
UBX::decodeInit(void)
|
||||
{
|
||||
_rx_ck_a = 0;
|
||||
_rx_ck_b = 0;
|
||||
_rx_count = 0;
|
||||
_decode_state = UBX_DECODE_UNINIT;
|
||||
_message_class = CLASS_UNKNOWN;
|
||||
_message_id = ID_UNKNOWN;
|
||||
_payload_size = 0;
|
||||
}
|
||||
|
||||
void
|
||||
UBX::addByteToChecksum(uint8_t b)
|
||||
{
|
||||
_rx_ck_a = _rx_ck_a + b;
|
||||
_rx_ck_b = _rx_ck_b + _rx_ck_a;
|
||||
}
|
||||
|
||||
void
|
||||
UBX::addChecksumToMessage(uint8_t* message, const unsigned length)
|
||||
{
|
||||
uint8_t ck_a = 0;
|
||||
uint8_t ck_b = 0;
|
||||
unsigned i;
|
||||
|
||||
for (i = 0; i < length-2; i++) {
|
||||
ck_a = ck_a + message[i];
|
||||
ck_b = ck_b + ck_a;
|
||||
}
|
||||
message[length-2] = ck_a;
|
||||
message[length-1] = ck_b;
|
||||
}
|
||||
|
|
@ -0,0 +1,369 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* @file U-Blox protocol definitions */
|
||||
|
||||
#ifndef UBX_H_
|
||||
#define UBX_H_
|
||||
|
||||
#include "gps_helper.h"
|
||||
|
||||
#define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */
|
||||
|
||||
#define UBX_SYNC1 0xB5
|
||||
#define UBX_SYNC2 0x62
|
||||
|
||||
//UBX Protocol definitions (this is the subset of the messages that are parsed)
|
||||
#define UBX_CLASS_NAV 0x01
|
||||
//#define UBX_CLASS_RXM 0x02
|
||||
#define UBX_CLASS_ACK 0x05
|
||||
#define UBX_CLASS_CFG 0x06
|
||||
#define UBX_MESSAGE_NAV_POSLLH 0x02
|
||||
#define UBX_MESSAGE_NAV_SOL 0x06
|
||||
#define UBX_MESSAGE_NAV_TIMEUTC 0x21
|
||||
#define UBX_MESSAGE_NAV_DOP 0x04
|
||||
#define UBX_MESSAGE_NAV_SVINFO 0x30
|
||||
#define UBX_MESSAGE_NAV_VELNED 0x12
|
||||
//#define UBX_MESSAGE_RXM_SVSI 0x20
|
||||
#define UBX_MESSAGE_ACK_ACK 0x01
|
||||
#define UBX_MESSAGE_ACK_NAK 0x00
|
||||
#define UBX_MESSAGE_CFG_PRT 0x00
|
||||
#define UBX_MESSAGE_CFG_NAV5 0x24
|
||||
#define UBX_MESSAGE_CFG_MSG 0x01
|
||||
#define UBX_MESSAGE_CFG_RATE 0x08
|
||||
|
||||
#define UBX_CFG_PRT_LENGTH 20
|
||||
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */
|
||||
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
|
||||
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
|
||||
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< ubx in */
|
||||
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< ubx out */
|
||||
|
||||
#define UBX_CFG_RATE_LENGTH 6
|
||||
#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */
|
||||
#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */
|
||||
#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */
|
||||
|
||||
|
||||
#define UBX_CFG_NAV5_LENGTH 36
|
||||
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */
|
||||
#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
|
||||
#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
|
||||
|
||||
#define UBX_CFG_MSG_LENGTH 8
|
||||
#define UBX_CFG_MSG_PAYLOAD_RATE1 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} UART1 chosen */
|
||||
|
||||
|
||||
// ************
|
||||
/** the structures of the binary packets */
|
||||
#pragma pack(push, 1)
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; // GPS Millisecond Time of Week
|
||||
int32_t lon; // Longitude * 1e-7, deg
|
||||
int32_t lat; // Latitude * 1e-7, deg
|
||||
int32_t height; // Height above Ellipsoid, mm
|
||||
int32_t height_msl; // Height above mean sea level, mm
|
||||
uint32_t hAcc; // Horizontal Accuracy Estimate, mm
|
||||
uint32_t vAcc; // Vertical Accuracy Estimate, mm
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_posllh_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; // GPS Millisecond Time of Week
|
||||
int32_t time_nanoseconds; // Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000
|
||||
int16_t week; // GPS week (GPS time)
|
||||
uint8_t gpsFix; //GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix
|
||||
uint8_t flags;
|
||||
int32_t ecefX;
|
||||
int32_t ecefY;
|
||||
int32_t ecefZ;
|
||||
uint32_t pAcc;
|
||||
int32_t ecefVX;
|
||||
int32_t ecefVY;
|
||||
int32_t ecefVZ;
|
||||
uint32_t sAcc;
|
||||
uint16_t pDOP;
|
||||
uint8_t reserved1;
|
||||
uint8_t numSV;
|
||||
uint32_t reserved2;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_sol_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; // GPS Millisecond Time of Week
|
||||
uint32_t time_accuracy; //Time Accuracy Estimate, ns
|
||||
int32_t time_nanoseconds; //Nanoseconds of second, range -1e9 .. 1e9 (UTC)
|
||||
uint16_t year; //Year, range 1999..2099 (UTC)
|
||||
uint8_t month; //Month, range 1..12 (UTC)
|
||||
uint8_t day; //Day of Month, range 1..31 (UTC)
|
||||
uint8_t hour; //Hour of Day, range 0..23 (UTC)
|
||||
uint8_t min; //Minute of Hour, range 0..59 (UTC)
|
||||
uint8_t sec; //Seconds of Minute, range 0..59 (UTC)
|
||||
uint8_t valid_flag; //Validity Flags (see ubx documentation)
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_timeutc_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; // GPS Millisecond Time of Week
|
||||
uint16_t gDOP; //Geometric DOP (scaling 0.01)
|
||||
uint16_t pDOP; //Position DOP (scaling 0.01)
|
||||
uint16_t tDOP; //Time DOP (scaling 0.01)
|
||||
uint16_t vDOP; //Vertical DOP (scaling 0.01)
|
||||
uint16_t hDOP; //Horizontal DOP (scaling 0.01)
|
||||
uint16_t nDOP; //Northing DOP (scaling 0.01)
|
||||
uint16_t eDOP; //Easting DOP (scaling 0.01)
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_dop_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; // GPS Millisecond Time of Week
|
||||
uint8_t numCh; //Number of channels
|
||||
uint8_t globalFlags;
|
||||
uint16_t reserved2;
|
||||
|
||||
} gps_bin_nav_svinfo_part1_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t chn; //Channel number, 255 for SVs not assigned to a channel
|
||||
uint8_t svid; //Satellite ID
|
||||
uint8_t flags;
|
||||
uint8_t quality;
|
||||
uint8_t cno; //Carrier to Noise Ratio (Signal Strength), dbHz
|
||||
int8_t elev; //Elevation in integer degrees
|
||||
int16_t azim; //Azimuth in integer degrees
|
||||
int32_t prRes; //Pseudo range residual in centimetres
|
||||
|
||||
} gps_bin_nav_svinfo_part2_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_svinfo_part3_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time_milliseconds; // GPS Millisecond Time of Week
|
||||
int32_t velN; //NED north velocity, cm/s
|
||||
int32_t velE; //NED east velocity, cm/s
|
||||
int32_t velD; //NED down velocity, cm/s
|
||||
uint32_t speed; //Speed (3-D), cm/s
|
||||
uint32_t gSpeed; //Ground Speed (2-D), cm/s
|
||||
int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
|
||||
uint32_t sAcc; //Speed Accuracy Estimate, cm/s
|
||||
uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_nav_velned_packet_t;
|
||||
|
||||
//typedef struct {
|
||||
// int32_t time_milliseconds; // Measurement integer millisecond GPS time of week
|
||||
// int16_t week; //Measurement GPS week number
|
||||
// uint8_t numVis; //Number of visible satellites
|
||||
//
|
||||
// //... rest of package is not used in this implementation
|
||||
//
|
||||
//} gps_bin_rxm_svsi_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_ack_ack_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_bin_ack_nak_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint16_t length;
|
||||
uint8_t portID;
|
||||
uint8_t res0;
|
||||
uint16_t res1;
|
||||
uint32_t mode;
|
||||
uint32_t baudRate;
|
||||
uint16_t inProtoMask;
|
||||
uint16_t outProtoMask;
|
||||
uint16_t flags;
|
||||
uint16_t pad;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} type_gps_bin_cfg_prt_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint16_t length;
|
||||
uint16_t measRate;
|
||||
uint16_t navRate;
|
||||
uint16_t timeRef;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} type_gps_bin_cfg_rate_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint16_t length;
|
||||
uint16_t mask;
|
||||
uint8_t dynModel;
|
||||
uint8_t fixMode;
|
||||
int32_t fixedAlt;
|
||||
uint32_t fixedAltVar;
|
||||
int8_t minElev;
|
||||
uint8_t drLimit;
|
||||
uint16_t pDop;
|
||||
uint16_t tDop;
|
||||
uint16_t pAcc;
|
||||
uint16_t tAcc;
|
||||
uint8_t staticHoldThresh;
|
||||
uint8_t dgpsTimeOut;
|
||||
uint32_t reserved2;
|
||||
uint32_t reserved3;
|
||||
uint32_t reserved4;
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} type_gps_bin_cfg_nav5_packet_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t clsID;
|
||||
uint8_t msgID;
|
||||
uint16_t length;
|
||||
uint8_t msgClass_payload;
|
||||
uint8_t msgID_payload;
|
||||
uint8_t rate[6];
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} type_gps_bin_cfg_msg_packet_t;
|
||||
|
||||
|
||||
// END the structures of the binary packets
|
||||
// ************
|
||||
|
||||
typedef enum {
|
||||
UBX_CONFIG_STATE_PRT = 0,
|
||||
UBX_CONFIG_STATE_PRT_NEW_BAUDRATE,
|
||||
UBX_CONFIG_STATE_RATE,
|
||||
UBX_CONFIG_STATE_NAV5,
|
||||
UBX_CONFIG_STATE_MSG_NAV_POSLLH,
|
||||
UBX_CONFIG_STATE_MSG_NAV_TIMEUTC,
|
||||
UBX_CONFIG_STATE_MSG_NAV_DOP,
|
||||
UBX_CONFIG_STATE_MSG_NAV_SVINFO,
|
||||
UBX_CONFIG_STATE_MSG_NAV_SOL,
|
||||
UBX_CONFIG_STATE_MSG_NAV_VELNED,
|
||||
// UBX_CONFIG_STATE_MSG_RXM_SVSI,
|
||||
UBX_CONFIG_STATE_CONFIGURED
|
||||
} ubx_config_state_t;
|
||||
|
||||
typedef enum {
|
||||
CLASS_UNKNOWN = 0,
|
||||
NAV = 1,
|
||||
RXM = 2,
|
||||
ACK = 3,
|
||||
CFG = 4
|
||||
} ubx_message_class_t;
|
||||
|
||||
typedef enum {
|
||||
//these numbers do NOT correspond to the message id numbers of the ubx protocol
|
||||
ID_UNKNOWN = 0,
|
||||
NAV_POSLLH,
|
||||
NAV_SOL,
|
||||
NAV_TIMEUTC,
|
||||
NAV_DOP,
|
||||
NAV_SVINFO,
|
||||
NAV_VELNED,
|
||||
// RXM_SVSI,
|
||||
CFG_NAV5,
|
||||
ACK_ACK,
|
||||
ACK_NAK,
|
||||
} ubx_message_id_t;
|
||||
|
||||
typedef enum {
|
||||
UBX_DECODE_UNINIT = 0,
|
||||
UBX_DECODE_GOT_SYNC1,
|
||||
UBX_DECODE_GOT_SYNC2,
|
||||
UBX_DECODE_GOT_CLASS,
|
||||
UBX_DECODE_GOT_MESSAGEID,
|
||||
UBX_DECODE_GOT_LENGTH1,
|
||||
UBX_DECODE_GOT_LENGTH2
|
||||
} ubx_decode_state_t;
|
||||
|
||||
//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
|
||||
#pragma pack(pop)
|
||||
|
||||
#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
|
||||
|
||||
class UBX : public GPS_Helper
|
||||
{
|
||||
public:
|
||||
UBX();
|
||||
~UBX();
|
||||
|
||||
void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned);
|
||||
int parse(uint8_t, struct vehicle_gps_position_s*);
|
||||
|
||||
private:
|
||||
void decodeInit(void);
|
||||
void addByteToChecksum(uint8_t);
|
||||
void addChecksumToMessage(uint8_t*, const unsigned);
|
||||
unsigned _waited;
|
||||
bool _waiting_for_ack;
|
||||
ubx_config_state_t _config_state;
|
||||
ubx_decode_state_t _decode_state;
|
||||
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
|
||||
unsigned _rx_count;
|
||||
uint8_t _rx_ck_a;
|
||||
uint8_t _rx_ck_b;
|
||||
ubx_message_class_t _message_class;
|
||||
ubx_message_id_t _message_id;
|
||||
unsigned _payload_size;
|
||||
bool _new_nav_posllh;
|
||||
bool _new_nav_timeutc;
|
||||
bool _new_nav_dop;
|
||||
bool _new_nav_sol;
|
||||
bool _new_nav_velned;
|
||||
};
|
||||
|
||||
#endif /* UBX_H_ */
|
||||
Loading…
Reference in New Issue