forked from Archive/PX4-Autopilot
Lorenz Meier
commit
781845587c
|
|
@ -1678,12 +1678,13 @@ int commander_thread_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
|
||||
|
||||
/* check for first, long-term and valid GPS lock -> set home position */
|
||||
float hdop_m = gps_position.eph / 100.0f;
|
||||
float vdop_m = gps_position.epv / 100.0f;
|
||||
float hdop_m = gps_position.eph_m;
|
||||
float vdop_m = gps_position.epv_m;
|
||||
|
||||
/* check if gps fix is ok */
|
||||
// XXX magic number
|
||||
float dop_threshold_m = 2.0f;
|
||||
float hdop_threshold_m = 4.0f;
|
||||
float vdop_threshold_m = 8.0f;
|
||||
|
||||
/*
|
||||
* If horizontal dilution of precision (hdop / eph)
|
||||
|
|
@ -1694,10 +1695,12 @@ int commander_thread_main(int argc, char *argv[])
|
|||
* the system is currently not armed, set home
|
||||
* position to the current position.
|
||||
*/
|
||||
if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m)
|
||||
&& (vdop_m < dop_threshold_m)
|
||||
|
||||
if (gps_position.fix_type == GPS_FIX_TYPE_3D
|
||||
&& (hdop_m < hdop_threshold_m)
|
||||
&& (vdop_m < vdop_threshold_m)
|
||||
&& !home_position_set
|
||||
&& (hrt_absolute_time() - gps_position.timestamp < 2000000)
|
||||
&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
|
||||
&& !current_status.flag_system_armed) {
|
||||
warnx("setting home position");
|
||||
|
||||
|
|
@ -1706,11 +1709,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
home.lon = gps_position.lon;
|
||||
home.alt = gps_position.alt;
|
||||
|
||||
home.eph = gps_position.eph;
|
||||
home.epv = gps_position.epv;
|
||||
home.eph_m = gps_position.eph_m;
|
||||
home.epv_m = gps_position.epv_m;
|
||||
|
||||
home.s_variance = gps_position.s_variance;
|
||||
home.p_variance = gps_position.p_variance;
|
||||
home.s_variance_m_s = gps_position.s_variance_m_s;
|
||||
home.p_variance_m = gps_position.p_variance_m;
|
||||
|
||||
/* announce new home position */
|
||||
if (home_pub > 0) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,70 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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_DEFAULT_UART_PORT "/dev/ttyS3"
|
||||
|
||||
#define GPS_DEVICE_PATH "/dev/gps"
|
||||
|
||||
typedef enum {
|
||||
GPS_DRIVER_MODE_NONE = 0,
|
||||
GPS_DRIVER_MODE_UBX,
|
||||
GPS_DRIVER_MODE_MTK,
|
||||
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))
|
||||
|
||||
#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,536 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 a serial port
|
||||
*/
|
||||
|
||||
#include <nuttx/clock.h>
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.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 <fcntl.h>
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/arch.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/scheduling_priorities.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_gps.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
#include "ubx.h"
|
||||
#include "mtk.h"
|
||||
|
||||
|
||||
#define TIMEOUT_5HZ 400
|
||||
#define RATE_MEASUREMENT_PERIOD 5000000
|
||||
|
||||
/* 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(const char* uart_path);
|
||||
~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:
|
||||
|
||||
bool _task_should_exit; ///< flag to make the main worker task exit
|
||||
int _serial_fd; ///< serial interface to GPS
|
||||
unsigned _baudrate; ///< current baudrate
|
||||
char _port[20]; ///< device / serial port path
|
||||
volatile int _task; //< worker task
|
||||
bool _healthy; ///< flag to signal if the GPS is ok
|
||||
bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed
|
||||
bool _mode_changed; ///< flag that the GPS mode has changed
|
||||
gps_driver_mode_t _mode; ///< current mode
|
||||
GPS_Helper *_Helper; ///< instance of GPS parser
|
||||
struct vehicle_gps_position_s _report; ///< uORB topic for gps position
|
||||
orb_advert_t _report_pub; ///< uORB pub for gps position
|
||||
float _rate; ///< position update rate
|
||||
|
||||
|
||||
/**
|
||||
* Try to configure the GPS, handle outgoing communication to the GPS
|
||||
*/
|
||||
void config();
|
||||
|
||||
/**
|
||||
* Trampoline to the worker task
|
||||
*/
|
||||
static void task_main_trampoline(void *arg);
|
||||
|
||||
|
||||
/**
|
||||
* Worker task: main GPS thread that configures the GPS and parses incoming data, always running
|
||||
*/
|
||||
void task_main(void);
|
||||
|
||||
/**
|
||||
* Set the baudrate of the UART to the GPS
|
||||
*/
|
||||
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(const char* uart_path) :
|
||||
CDev("gps", GPS_DEVICE_PATH),
|
||||
_task_should_exit(false),
|
||||
_healthy(false),
|
||||
_mode_changed(false),
|
||||
_mode(GPS_DRIVER_MODE_UBX),
|
||||
_Helper(nullptr),
|
||||
_report_pub(-1),
|
||||
_rate(0.0f)
|
||||
{
|
||||
/* store port name */
|
||||
strncpy(_port, uart_path, sizeof(_port));
|
||||
/* enforce null termination */
|
||||
_port[sizeof(_port) - 1] = '\0';
|
||||
|
||||
/* we need this potentially before it could be set in task_main */
|
||||
g_dev = this;
|
||||
memset(&_report, 0, sizeof(_report));
|
||||
|
||||
_debug_enabled = true;
|
||||
}
|
||||
|
||||
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 GPS driver worker task */
|
||||
_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
|
||||
|
||||
if (_task < 0) {
|
||||
warnx("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;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
GPS::task_main_trampoline(void *arg)
|
||||
{
|
||||
g_dev->task_main();
|
||||
}
|
||||
|
||||
void
|
||||
GPS::task_main()
|
||||
{
|
||||
log("starting");
|
||||
|
||||
/* open the serial port */
|
||||
_serial_fd = ::open(_port, O_RDWR);
|
||||
|
||||
if (_serial_fd < 0) {
|
||||
log("failed to open serial port: %s err: %d", _port, errno);
|
||||
/* tell the dtor that we are exiting, set error code */
|
||||
_task = -1;
|
||||
_exit(1);
|
||||
}
|
||||
|
||||
uint64_t last_rate_measurement = hrt_absolute_time();
|
||||
unsigned last_rate_count = 0;
|
||||
|
||||
/* loop handling received serial bytes and also configuring in between */
|
||||
while (!_task_should_exit) {
|
||||
|
||||
if (_Helper != nullptr) {
|
||||
delete(_Helper);
|
||||
/* set to zero to ensure parser is not used while not instantiated */
|
||||
_Helper = nullptr;
|
||||
}
|
||||
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_Helper = new UBX(_serial_fd, &_report);
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_Helper = new MTK(_serial_fd, &_report);
|
||||
break;
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
//_Helper = new NMEA(); //TODO: add NMEA
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
unlock();
|
||||
if (_Helper->configure(_baudrate) == 0) {
|
||||
unlock();
|
||||
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
|
||||
// lock();
|
||||
/* opportunistic publishing - else invalid data would end up on the bus */
|
||||
if (_report_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
|
||||
} else {
|
||||
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
|
||||
}
|
||||
|
||||
last_rate_count++;
|
||||
|
||||
/* measure update rate every 5 seconds */
|
||||
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
|
||||
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
|
||||
last_rate_measurement = hrt_absolute_time();
|
||||
last_rate_count = 0;
|
||||
}
|
||||
|
||||
if (!_healthy) {
|
||||
warnx("module found");
|
||||
_healthy = true;
|
||||
}
|
||||
}
|
||||
if (_healthy) {
|
||||
warnx("module lost");
|
||||
_healthy = false;
|
||||
_rate = 0.0f;
|
||||
}
|
||||
|
||||
lock();
|
||||
}
|
||||
lock();
|
||||
|
||||
/* select next mode */
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_mode = GPS_DRIVER_MODE_MTK;
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
break;
|
||||
// case GPS_DRIVER_MODE_NMEA:
|
||||
// _mode = GPS_DRIVER_MODE_UBX;
|
||||
// break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
debug("exiting");
|
||||
|
||||
::close(_serial_fd);
|
||||
|
||||
/* tell the dtor that we are exiting */
|
||||
_task = -1;
|
||||
_exit(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void
|
||||
GPS::cmd_reset()
|
||||
{
|
||||
//XXX add reset?
|
||||
}
|
||||
|
||||
void
|
||||
GPS::print_info()
|
||||
{
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
warnx("protocol: UBX");
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
warnx("protocol: MTK");
|
||||
break;
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
warnx("protocol: NMEA");
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
|
||||
if (_report.timestamp_position != 0) {
|
||||
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
|
||||
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
|
||||
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
|
||||
warnx("update rate: %6.2f Hz", (double)_rate);
|
||||
}
|
||||
|
||||
usleep(100000);
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace gps
|
||||
{
|
||||
|
||||
GPS *g_dev;
|
||||
|
||||
void start(const char *uart_path);
|
||||
void stop();
|
||||
void test();
|
||||
void reset();
|
||||
void info();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*/
|
||||
void
|
||||
start(const char *uart_path)
|
||||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
errx(1, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new GPS(uart_path);
|
||||
|
||||
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) {
|
||||
errx(1, "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");
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the driver.
|
||||
*/
|
||||
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");
|
||||
|
||||
g_dev->print_info();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
|
||||
int
|
||||
gps_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
/* set to default */
|
||||
char* device_name = GPS_DEFAULT_UART_PORT;
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
/* work around getopt unreliability */
|
||||
if (argc > 3) {
|
||||
if (!strcmp(argv[2], "-d")) {
|
||||
device_name = argv[3];
|
||||
} else {
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
gps::start(device_name);
|
||||
}
|
||||
|
||||
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();
|
||||
|
||||
out:
|
||||
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
|
||||
}
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <systemlib/err.h>
|
||||
#include "gps_helper.h"
|
||||
|
||||
/* @file gps_helper.cpp */
|
||||
|
||||
int
|
||||
GPS_Helper::set_baudrate(const int &fd, 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;
|
||||
|
||||
warnx("try baudrate: %d\n", speed);
|
||||
|
||||
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(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 ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
|
||||
return -1;
|
||||
}
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
|
||||
return -1;
|
||||
}
|
||||
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
|
||||
warnx("ERROR setting baudrate (tcsetattr)\n");
|
||||
return -1;
|
||||
}
|
||||
/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 gps_helper.h */
|
||||
|
||||
#ifndef GPS_HELPER_H
|
||||
#define GPS_HELPER_H
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
class GPS_Helper
|
||||
{
|
||||
public:
|
||||
virtual int configure(unsigned &baud) = 0;
|
||||
virtual int receive(unsigned timeout) = 0;
|
||||
int set_baudrate(const int &fd, unsigned baud);
|
||||
};
|
||||
|
||||
#endif /* GPS_HELPER_H */
|
||||
|
|
@ -0,0 +1,274 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 mkt.cpp */
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "mtk.h"
|
||||
|
||||
|
||||
MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
||||
_fd(fd),
|
||||
_gps_position(gps_position),
|
||||
_mtk_revision(0)
|
||||
{
|
||||
decode_init();
|
||||
}
|
||||
|
||||
MTK::~MTK()
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
MTK::configure(unsigned &baudrate)
|
||||
{
|
||||
/* set baudrate first */
|
||||
if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
|
||||
return -1;
|
||||
|
||||
baudrate = MTK_BAUDRATE;
|
||||
|
||||
/* Write config messages, don't wait for an answer */
|
||||
if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
/* timeout additional to poll */
|
||||
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;
|
||||
}
|
||||
/* 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 = count = 0;
|
||||
}
|
||||
|
||||
/* then poll for new data */
|
||||
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
|
||||
|
||||
if (ret < 0) {
|
||||
/* something went wrong when polling */
|
||||
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));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MTK::decode_init(void)
|
||||
{
|
||||
_rx_ck_a = 0;
|
||||
_rx_ck_b = 0;
|
||||
_rx_count = 0;
|
||||
_decode_state = MTK_DECODE_UNINIT;
|
||||
}
|
||||
int
|
||||
MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
if (_decode_state == MTK_DECODE_UNINIT) {
|
||||
|
||||
if (b == MTK_SYNC1_V16) {
|
||||
_decode_state = MTK_DECODE_GOT_CK_A;
|
||||
_mtk_revision = 16;
|
||||
} else if (b == MTK_SYNC1_V19) {
|
||||
_decode_state = MTK_DECODE_GOT_CK_A;
|
||||
_mtk_revision = 19;
|
||||
}
|
||||
|
||||
} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
|
||||
if (b == MTK_SYNC2) {
|
||||
_decode_state = MTK_DECODE_GOT_CK_B;
|
||||
|
||||
} else {
|
||||
// Second start symbol was wrong, reset state machine
|
||||
decode_init();
|
||||
}
|
||||
|
||||
} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
|
||||
// Add to checksum
|
||||
if (_rx_count < 33)
|
||||
add_byte_to_checksum(b);
|
||||
|
||||
// Fill packet buffer
|
||||
((uint8_t*)(&packet))[_rx_count] = b;
|
||||
_rx_count++;
|
||||
|
||||
/* Packet size minus checksum, XXX ? */
|
||||
if (_rx_count >= sizeof(packet)) {
|
||||
/* Compare checksum */
|
||||
if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
|
||||
ret = 1;
|
||||
} else {
|
||||
warnx("MTK Checksum invalid");
|
||||
ret = -1;
|
||||
}
|
||||
// Reset state machine to decode next packet
|
||||
decode_init();
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
MTK::handle_message(gps_mtk_packet_t &packet)
|
||||
{
|
||||
if (_mtk_revision == 16) {
|
||||
_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
|
||||
_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
|
||||
} else if (_mtk_revision == 19) {
|
||||
_gps_position->lat = packet.latitude; // both degrees*1e7
|
||||
_gps_position->lon = packet.longitude; // both degrees*1e7
|
||||
} else {
|
||||
warnx("mtk: unknown revision");
|
||||
_gps_position->lat = 0;
|
||||
_gps_position->lon = 0;
|
||||
}
|
||||
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
|
||||
_gps_position->fix_type = packet.fix_type;
|
||||
_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
|
||||
_gps_position->epv_m = 0.0; //unknown in mtk custom mode
|
||||
_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
|
||||
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
|
||||
_gps_position->satellites_visible = packet.satellites;
|
||||
|
||||
/* convert time and date information to unix timestamp */
|
||||
struct tm timeinfo; //TODO: test this conversion
|
||||
uint32_t timeinfo_conversion_temp;
|
||||
|
||||
timeinfo.tm_mday = packet.date * 1e-4;
|
||||
timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4;
|
||||
timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1;
|
||||
timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100;
|
||||
|
||||
timeinfo.tm_hour = packet.utc_time * 1e-7;
|
||||
timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7;
|
||||
timeinfo.tm_min = timeinfo_conversion_temp * 1e-5;
|
||||
timeinfo_conversion_temp -= timeinfo.tm_min * 1e5;
|
||||
timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3;
|
||||
timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
|
||||
time_t epoch = mktime(&timeinfo);
|
||||
|
||||
_gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
|
||||
_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
|
||||
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void
|
||||
MTK::add_byte_to_checksum(uint8_t b)
|
||||
{
|
||||
_rx_ck_a = _rx_ck_a + b;
|
||||
_rx_ck_b = _rx_ck_b + _rx_ck_a;
|
||||
}
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 mtk.h */
|
||||
|
||||
#ifndef MTK_H_
|
||||
#define MTK_H_
|
||||
|
||||
#include "gps_helper.h"
|
||||
|
||||
#define MTK_SYNC1_V16 0xd0
|
||||
#define MTK_SYNC1_V19 0xd1
|
||||
#define MTK_SYNC2 0xdd
|
||||
|
||||
#define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n"
|
||||
#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n"
|
||||
#define SBAS_ON "$PMTK313,1*2E\r\n"
|
||||
#define WAAS_ON "$PMTK301,2*2E\r\n"
|
||||
#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n"
|
||||
|
||||
#define MTK_TIMEOUT_5HZ 400
|
||||
#define MTK_BAUDRATE 38400
|
||||
|
||||
typedef enum {
|
||||
MTK_DECODE_UNINIT = 0,
|
||||
MTK_DECODE_GOT_CK_A = 1,
|
||||
MTK_DECODE_GOT_CK_B = 2
|
||||
} mtk_decode_state_t;
|
||||
|
||||
/** the structures of the binary packets */
|
||||
#pragma pack(push, 1)
|
||||
|
||||
typedef struct {
|
||||
uint8_t payload; ///< Number of payload bytes
|
||||
int32_t latitude; ///< Latitude in degrees * 10^7
|
||||
int32_t longitude; ///< Longitude in degrees * 10^7
|
||||
uint32_t msl_altitude; ///< MSL altitude in meters * 10^2
|
||||
uint32_t ground_speed; ///< velocity in m/s
|
||||
int32_t heading; ///< heading in degrees * 10^2
|
||||
uint8_t satellites; ///< number of sattelites used
|
||||
uint8_t fix_type; ///< fix type: XXX correct for that
|
||||
uint32_t date;
|
||||
uint32_t utc_time;
|
||||
uint16_t hdop; ///< horizontal dilution of position (without unit)
|
||||
uint8_t ck_a;
|
||||
uint8_t ck_b;
|
||||
} gps_mtk_packet_t;
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
#define MTK_RECV_BUFFER_SIZE 40
|
||||
|
||||
class MTK : public GPS_Helper
|
||||
{
|
||||
public:
|
||||
MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
|
||||
~MTK();
|
||||
int receive(unsigned timeout);
|
||||
int configure(unsigned &baudrate);
|
||||
|
||||
private:
|
||||
/**
|
||||
* Parse the binary MTK packet
|
||||
*/
|
||||
int parse_char(uint8_t b, gps_mtk_packet_t &packet);
|
||||
|
||||
/**
|
||||
* Handle the package once it has arrived
|
||||
*/
|
||||
void handle_message(gps_mtk_packet_t &packet);
|
||||
|
||||
/**
|
||||
* Reset the parse state machine for a fresh start
|
||||
*/
|
||||
void decode_init(void);
|
||||
|
||||
/**
|
||||
* While parsing add every byte (except the sync bytes) to the checksum
|
||||
*/
|
||||
void add_byte_to_checksum(uint8_t);
|
||||
|
||||
int _fd;
|
||||
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;
|
||||
};
|
||||
|
||||
#endif /* MTK_H_ */
|
||||
|
|
@ -0,0 +1,745 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <poll.h>
|
||||
#include <math.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"
|
||||
|
||||
#define UBX_CONFIG_TIMEOUT 100
|
||||
|
||||
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
||||
_fd(fd),
|
||||
_gps_position(gps_position),
|
||||
_waiting_for_ack(false)
|
||||
{
|
||||
decode_init();
|
||||
}
|
||||
|
||||
UBX::~UBX()
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
UBX::configure(unsigned &baudrate)
|
||||
{
|
||||
_waiting_for_ack = true;
|
||||
|
||||
/* try different baudrates */
|
||||
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
|
||||
|
||||
for (int baud_i = 0; baud_i < 5; baud_i++) {
|
||||
baudrate = baudrates_to_try[baud_i];
|
||||
set_baudrate(_fd, baudrate);
|
||||
|
||||
/* Send a CFG-PRT message to set the UBX protocol for in and out
|
||||
* and leave the baudrate as it is, we just want an ACK-ACK from this
|
||||
*/
|
||||
type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
|
||||
/* Set everything else of the packet to 0, otherwise the module wont accept it */
|
||||
memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
|
||||
|
||||
_clsID_needed = UBX_CLASS_CFG;
|
||||
_msgID_needed = UBX_MESSAGE_CFG_PRT;
|
||||
|
||||
/* Define the package contents, don't change the baudrate */
|
||||
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;
|
||||
|
||||
send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
|
||||
|
||||
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
/* try next baudrate */
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Send a CFG-PRT message again, this time change the baudrate */
|
||||
|
||||
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;
|
||||
|
||||
send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
|
||||
if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
|
||||
set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
|
||||
baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
|
||||
}
|
||||
|
||||
/* no ack is ecpected here, keep going configuring */
|
||||
|
||||
/* 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));
|
||||
|
||||
_clsID_needed = UBX_CLASS_CFG;
|
||||
_msgID_needed = UBX_MESSAGE_CFG_RATE;
|
||||
|
||||
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;
|
||||
|
||||
send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
|
||||
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
/* try next baudrate */
|
||||
continue;
|
||||
}
|
||||
|
||||
/* send a NAV5 message to set the options for the internal filter */
|
||||
type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
|
||||
memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
|
||||
|
||||
_clsID_needed = UBX_CLASS_CFG;
|
||||
_msgID_needed = UBX_MESSAGE_CFG_NAV5;
|
||||
|
||||
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;
|
||||
|
||||
send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
|
||||
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
/* try next baudrate */
|
||||
continue;
|
||||
}
|
||||
|
||||
type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
|
||||
memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
|
||||
|
||||
_clsID_needed = UBX_CLASS_CFG;
|
||||
_msgID_needed = UBX_MESSAGE_CFG_MSG;
|
||||
|
||||
cfg_msg_packet.clsID = UBX_CLASS_CFG;
|
||||
cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG;
|
||||
cfg_msg_packet.length = UBX_CFG_MSG_LENGTH;
|
||||
/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
|
||||
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
|
||||
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
|
||||
|
||||
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
|
||||
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
/* try next baudrate */
|
||||
continue;
|
||||
}
|
||||
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
|
||||
|
||||
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
|
||||
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
/* try next baudrate */
|
||||
continue;
|
||||
}
|
||||
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
|
||||
/* For satelites info 1Hz is enough */
|
||||
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
|
||||
|
||||
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
|
||||
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
/* try next baudrate */
|
||||
continue;
|
||||
}
|
||||
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
|
||||
|
||||
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
|
||||
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
/* try next baudrate */
|
||||
continue;
|
||||
}
|
||||
|
||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
|
||||
|
||||
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
|
||||
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
/* try next baudrate */
|
||||
continue;
|
||||
}
|
||||
// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
||||
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
|
||||
|
||||
// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
|
||||
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
|
||||
|
||||
_waiting_for_ack = false;
|
||||
return 0;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int
|
||||
UBX::receive(unsigned timeout)
|
||||
{
|
||||
/* poll descriptor */
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _fd;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
uint8_t buf[32];
|
||||
|
||||
/* timeout additional to poll */
|
||||
uint64_t time_started = hrt_absolute_time();
|
||||
|
||||
int j = 0;
|
||||
ssize_t count = 0;
|
||||
|
||||
while (true) {
|
||||
|
||||
/* pass received bytes to the packet decoder */
|
||||
while (j < count) {
|
||||
if (parse_char(buf[j]) > 0) {
|
||||
/* return to configure during configuration or to the gps driver during normal work
|
||||
* if a packet has arrived */
|
||||
if (handle_message() > 0)
|
||||
return 1;
|
||||
}
|
||||
/* 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 = count = 0;
|
||||
|
||||
/* then poll for new data */
|
||||
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
|
||||
|
||||
if (ret < 0) {
|
||||
/* something went wrong when polling */
|
||||
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));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
UBX::parse_char(uint8_t 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 */
|
||||
decode_init();
|
||||
}
|
||||
break;
|
||||
/* Now look for class */
|
||||
case UBX_DECODE_GOT_SYNC2:
|
||||
/* everything except sync1 and sync2 needs to be added to the checksum */
|
||||
add_byte_to_checksum(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
|
||||
decode_init();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case UBX_DECODE_GOT_CLASS:
|
||||
add_byte_to_checksum(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
|
||||
decode_init();
|
||||
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
|
||||
// decode_init();
|
||||
// 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
|
||||
decode_init();
|
||||
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
|
||||
decode_init();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
default: //should not happen because we set the class
|
||||
warnx("UBX Error, we set a class that we don't know");
|
||||
decode_init();
|
||||
// config_needed = true;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case UBX_DECODE_GOT_MESSAGEID:
|
||||
add_byte_to_checksum(b);
|
||||
_payload_size = b; //this is the first length byte
|
||||
_decode_state = UBX_DECODE_GOT_LENGTH1;
|
||||
break;
|
||||
case UBX_DECODE_GOT_LENGTH1:
|
||||
add_byte_to_checksum(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)
|
||||
add_byte_to_checksum(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
|
||||
|
||||
/* compare checksum */
|
||||
if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) {
|
||||
return 1;
|
||||
} else {
|
||||
decode_init();
|
||||
return -1;
|
||||
warnx("ubx: Checksum wrong");
|
||||
}
|
||||
|
||||
return 1;
|
||||
} else if (_rx_count < RECV_BUFFER_SIZE) {
|
||||
_rx_count++;
|
||||
} else {
|
||||
warnx("ubx: buffer full");
|
||||
decode_init();
|
||||
return -1;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return 0; //XXX ?
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
UBX::handle_message()
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
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");
|
||||
if (!_waiting_for_ack) {
|
||||
gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
|
||||
|
||||
_gps_position->lat = packet->lat;
|
||||
_gps_position->lon = packet->lon;
|
||||
_gps_position->alt = packet->height_msl;
|
||||
|
||||
_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
|
||||
_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
|
||||
|
||||
/* Add timestamp to finish the report */
|
||||
_gps_position->timestamp_position = hrt_absolute_time();
|
||||
/* only return 1 when new position is available */
|
||||
ret = 1;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case NAV_SOL: {
|
||||
// printf("GOT NAV_SOL MESSAGE\n");
|
||||
if (!_waiting_for_ack) {
|
||||
gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
|
||||
|
||||
_gps_position->fix_type = packet->gpsFix;
|
||||
_gps_position->s_variance_m_s = packet->sAcc;
|
||||
_gps_position->p_variance_m = packet->pAcc;
|
||||
|
||||
_gps_position->timestamp_variance = hrt_absolute_time();
|
||||
}
|
||||
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;
|
||||
//
|
||||
// _gps_position->eph_m = packet->hDOP;
|
||||
// _gps_position->epv = packet->vDOP;
|
||||
//
|
||||
// _gps_position->timestamp_posdilution = hrt_absolute_time();
|
||||
//
|
||||
// _new_nav_dop = true;
|
||||
//
|
||||
// break;
|
||||
// }
|
||||
|
||||
case NAV_TIMEUTC: {
|
||||
// printf("GOT NAV_TIMEUTC MESSAGE\n");
|
||||
|
||||
if (!_waiting_for_ack) {
|
||||
gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
|
||||
|
||||
//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_time = hrt_absolute_time();
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case NAV_SVINFO: {
|
||||
// printf("GOT NAV_SVINFO MESSAGE\n");
|
||||
|
||||
if (!_waiting_for_ack) {
|
||||
//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);
|
||||
|
||||
//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;
|
||||
}
|
||||
_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
|
||||
|
||||
/* set timestamp if any sat info is available */
|
||||
if (packet_part1->numCh > 0) {
|
||||
_gps_position->satellite_info_available = true;
|
||||
} else {
|
||||
_gps_position->satellite_info_available = false;
|
||||
}
|
||||
_gps_position->timestamp_satellites = hrt_absolute_time();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case NAV_VELNED: {
|
||||
// printf("GOT NAV_VELNED MESSAGE\n");
|
||||
|
||||
if (!_waiting_for_ack) {
|
||||
gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
|
||||
|
||||
_gps_position->vel_m_s = (float)packet->speed * 1e-2f;
|
||||
_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f;
|
||||
_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f;
|
||||
_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f;
|
||||
_gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
|
||||
_gps_position->vel_ned_valid = true;
|
||||
_gps_position->timestamp_velocity = hrt_absolute_time();
|
||||
}
|
||||
|
||||
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;
|
||||
//
|
||||
// _gps_position->satellites_visible = packet->numVis;
|
||||
// _gps_position->counter++;
|
||||
// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
|
||||
//
|
||||
// break;
|
||||
// }
|
||||
case ACK_ACK: {
|
||||
// printf("GOT ACK_ACK\n");
|
||||
gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
|
||||
|
||||
if (_waiting_for_ack) {
|
||||
if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) {
|
||||
ret = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ACK_NAK: {
|
||||
// printf("GOT ACK_NAK\n");
|
||||
warnx("UBX: Received: Not Acknowledged");
|
||||
/* configuration obviously not successful */
|
||||
ret = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
default: //we don't know the message
|
||||
warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
|
||||
ret = -1;
|
||||
break;
|
||||
}
|
||||
// end if _rx_count high enough
|
||||
decode_init();
|
||||
return ret; //XXX?
|
||||
}
|
||||
|
||||
void
|
||||
UBX::decode_init(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::add_byte_to_checksum(uint8_t b)
|
||||
{
|
||||
_rx_ck_a = _rx_ck_a + b;
|
||||
_rx_ck_b = _rx_ck_b + _rx_ck_a;
|
||||
}
|
||||
|
||||
void
|
||||
UBX::add_checksum_to_message(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;
|
||||
}
|
||||
/* The checksum is written to the last to bytes of a message */
|
||||
message[length-2] = ck_a;
|
||||
message[length-1] = ck_b;
|
||||
}
|
||||
|
||||
void
|
||||
UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
|
||||
{
|
||||
ssize_t ret = 0;
|
||||
|
||||
/* Calculate the checksum now */
|
||||
add_checksum_to_message(packet, length);
|
||||
|
||||
const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
|
||||
|
||||
/* Start with the two sync bytes */
|
||||
ret += write(fd, sync_bytes, sizeof(sync_bytes));
|
||||
ret += write(fd, packet, length);
|
||||
|
||||
if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
|
||||
warnx("ubx: config write fail");
|
||||
}
|
||||
|
|
@ -0,0 +1,395 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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_SYNC1 0xB5
|
||||
#define UBX_SYNC2 0x62
|
||||
|
||||
/* ClassIDs (the ones that are used) */
|
||||
#define UBX_CLASS_NAV 0x01
|
||||
//#define UBX_CLASS_RXM 0x02
|
||||
#define UBX_CLASS_ACK 0x05
|
||||
#define UBX_CLASS_CFG 0x06
|
||||
|
||||
/* MessageIDs (the ones that are used) */
|
||||
#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 /**< UART1 */
|
||||
#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_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
|
||||
#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
|
||||
|
||||
#define UBX_MAX_PAYLOAD_LENGTH 500
|
||||
|
||||
// ************
|
||||
/** 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(const int &fd, struct vehicle_gps_position_s *gps_position);
|
||||
~UBX();
|
||||
int receive(unsigned timeout);
|
||||
int configure(unsigned &baudrate);
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Parse the binary MTK packet
|
||||
*/
|
||||
int parse_char(uint8_t b);
|
||||
|
||||
/**
|
||||
* Handle the package once it has arrived
|
||||
*/
|
||||
int handle_message(void);
|
||||
|
||||
/**
|
||||
* Reset the parse state machine for a fresh start
|
||||
*/
|
||||
void decode_init(void);
|
||||
|
||||
/**
|
||||
* While parsing add every byte (except the sync bytes) to the checksum
|
||||
*/
|
||||
void add_byte_to_checksum(uint8_t);
|
||||
|
||||
/**
|
||||
* Add the two checksum bytes to an outgoing message
|
||||
*/
|
||||
void add_checksum_to_message(uint8_t* message, const unsigned length);
|
||||
|
||||
/**
|
||||
* Helper to send a config packet
|
||||
*/
|
||||
void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
|
||||
|
||||
int _fd;
|
||||
struct vehicle_gps_position_s *_gps_position;
|
||||
ubx_config_state_t _config_state;
|
||||
bool _waiting_for_ack;
|
||||
uint8_t _clsID_needed;
|
||||
uint8_t _msgID_needed;
|
||||
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;
|
||||
};
|
||||
|
||||
#endif /* UBX_H_ */
|
||||
|
|
@ -190,11 +190,12 @@ void KalmanNav::update()
|
|||
if (!_positionInitialized &&
|
||||
_attitudeInitialized && // wait for attitude first
|
||||
gpsUpdate &&
|
||||
_gps.fix_type > 2 &&
|
||||
_gps.counter_pos_valid > 10) {
|
||||
vN = _gps.vel_n;
|
||||
vE = _gps.vel_e;
|
||||
vD = _gps.vel_d;
|
||||
_gps.fix_type > 2
|
||||
//&& _gps.counter_pos_valid > 10
|
||||
) {
|
||||
vN = _gps.vel_n_m_s;
|
||||
vE = _gps.vel_e_m_s;
|
||||
vD = _gps.vel_d_m_s;
|
||||
setLatDegE7(_gps.lat);
|
||||
setLonDegE7(_gps.lon);
|
||||
setAltE3(_gps.alt);
|
||||
|
|
@ -259,7 +260,7 @@ void KalmanNav::updatePublications()
|
|||
|
||||
// position publication
|
||||
_pos.timestamp = _pubTimeStamp;
|
||||
_pos.time_gps_usec = _gps.timestamp;
|
||||
_pos.time_gps_usec = _gps.timestamp_position;
|
||||
_pos.valid = true;
|
||||
_pos.lat = getLatDegE7();
|
||||
_pos.lon = getLonDegE7();
|
||||
|
|
@ -630,8 +631,8 @@ int KalmanNav::correctPos()
|
|||
|
||||
// residual
|
||||
Vector y(5);
|
||||
y(0) = _gps.vel_n - vN;
|
||||
y(1) = _gps.vel_e - vE;
|
||||
y(0) = _gps.vel_n_m_s - vN;
|
||||
y(1) = _gps.vel_e_m_s - vE;
|
||||
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
|
||||
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
|
||||
y(4) = double(_gps.alt) / 1.0e3 - alt;
|
||||
|
|
@ -650,9 +651,9 @@ int KalmanNav::correctPos()
|
|||
// abort correction and return
|
||||
printf("[kalman_demo] numerical failure in gps correction\n");
|
||||
// fallback to GPS
|
||||
vN = _gps.vel_n;
|
||||
vE = _gps.vel_e;
|
||||
vD = _gps.vel_d;
|
||||
vN = _gps.vel_n_m_s;
|
||||
vE = _gps.vel_e_m_s;
|
||||
vD = _gps.vel_d_m_s;
|
||||
setLatDegE7(_gps.lat);
|
||||
setLonDegE7(_gps.lon);
|
||||
setAltE3(_gps.alt);
|
||||
|
|
|
|||
|
|
@ -625,7 +625,9 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
/* 20 Hz / 50 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
|
||||
/* 2 Hz */
|
||||
/* 10 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
|
||||
/* 10 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
|
||||
|
||||
} else if (baudrate >= 115200) {
|
||||
|
|
@ -634,8 +636,10 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
|
||||
/* 5 Hz / 100 ms */
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
|
||||
|
|
@ -651,6 +655,8 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
|
||||
|
||||
} else {
|
||||
/* very low baud rate, limit to 1 Hz / 1000 ms */
|
||||
|
|
|
|||
|
|
@ -387,22 +387,22 @@ handle_message(mavlink_message_t *msg)
|
|||
static uint64_t old_timestamp = 0;
|
||||
|
||||
/* gps */
|
||||
hil_gps.timestamp = gps.time_usec;
|
||||
hil_gps.counter = hil_counter++;
|
||||
hil_gps.timestamp_position = gps.time_usec;
|
||||
// hil_gps.counter = hil_counter++;
|
||||
hil_gps.time_gps_usec = gps.time_usec;
|
||||
hil_gps.lat = gps.lat;
|
||||
hil_gps.lon = gps.lon;
|
||||
hil_gps.alt = gps.alt;
|
||||
hil_gps.counter_pos_valid = hil_counter++;
|
||||
hil_gps.eph = gps.eph;
|
||||
hil_gps.epv = gps.epv;
|
||||
hil_gps.s_variance = 100;
|
||||
hil_gps.p_variance = 100;
|
||||
hil_gps.vel = gps.vel;
|
||||
hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
|
||||
hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
|
||||
hil_gps.vel_d = 0.0f;
|
||||
hil_gps.cog = gps.cog;
|
||||
// hil_gps.counter_pos_valid = hil_counter++;
|
||||
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
|
||||
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
|
||||
hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance?
|
||||
hil_gps.p_variance_m = 100; // XXX 100 m variance?
|
||||
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
|
||||
hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
|
||||
hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
|
||||
hil_gps.vel_d_m_s = 0.0f;
|
||||
hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad
|
||||
hil_gps.fix_type = gps.fix_type;
|
||||
hil_gps.satellites_visible = gps.satellites_visible;
|
||||
|
||||
|
|
|
|||
|
|
@ -231,15 +231,15 @@ l_vehicle_gps_position(struct listener *l)
|
|||
|
||||
/* GPS position */
|
||||
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
|
||||
gps.timestamp,
|
||||
gps.timestamp_position,
|
||||
gps.fix_type,
|
||||
gps.lat,
|
||||
gps.lon,
|
||||
gps.alt,
|
||||
gps.eph,
|
||||
gps.epv,
|
||||
gps.vel,
|
||||
gps.cog,
|
||||
(uint16_t)(gps.eph_m * 1e2f), // from m to cm
|
||||
(uint16_t)(gps.epv_m * 1e2f), // from m to cm
|
||||
(uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s
|
||||
(uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100
|
||||
gps.satellites_visible);
|
||||
|
||||
if (gps.satellite_info_available && (gps_counter % 4 == 0)) {
|
||||
|
|
@ -698,7 +698,7 @@ uorb_receive_start(void)
|
|||
|
||||
/* --- GPS VALUE --- */
|
||||
mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */
|
||||
orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */
|
||||
|
||||
/* --- HOME POSITION --- */
|
||||
mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
|
||||
|
|
|
|||
|
|
@ -61,10 +61,10 @@ struct home_position_s
|
|||
int32_t lat; /**< Latitude in 1E7 degrees */
|
||||
int32_t lon; /**< Longitude in 1E7 degrees */
|
||||
int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */
|
||||
uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
|
||||
uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
|
||||
float s_variance; /**< speed accuracy estimate cm/s */
|
||||
float p_variance; /**< position accuracy estimate cm */
|
||||
float eph_m; /**< GPS HDOP horizontal dilution of position in m */
|
||||
float epv_m; /**< GPS VDOP horizontal dilution of position in m */
|
||||
float s_variance_m_s; /**< speed accuracy estimate m/s */
|
||||
float p_variance_m; /**< position accuracy estimate m */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -55,35 +55,38 @@
|
|||
*/
|
||||
struct vehicle_gps_position_s
|
||||
{
|
||||
uint64_t timestamp; /**< Timestamp (microseconds since system boot) */
|
||||
uint32_t counter; /**< Count of GPS messages */
|
||||
uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
|
||||
uint64_t timestamp_position; /**< Timestamp for position information */
|
||||
int32_t lat; /**< Latitude in 1E7 degrees */
|
||||
int32_t lon; /**< Longitude in 1E7 degrees */
|
||||
int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */
|
||||
|
||||
int32_t lat; /**< Latitude in 1E7 degrees //LOGME */
|
||||
int32_t lon; /**< Longitude in 1E7 degrees //LOGME */
|
||||
int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL //LOGME */
|
||||
uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */
|
||||
uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */
|
||||
uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
|
||||
float s_variance; /**< speed accuracy estimate cm/s */
|
||||
float p_variance; /**< position accuracy estimate cm */
|
||||
uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */
|
||||
float vel_n; /**< GPS ground speed in m/s */
|
||||
float vel_e; /**< GPS ground speed in m/s */
|
||||
float vel_d; /**< GPS ground speed in m/s */
|
||||
uint16_t cog; /**< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */
|
||||
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
|
||||
uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */
|
||||
uint64_t timestamp_variance;
|
||||
float s_variance_m_s; /**< speed accuracy estimate m/s */
|
||||
float p_variance_m; /**< position accuracy estimate m */
|
||||
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
|
||||
|
||||
uint8_t satellite_prn[20]; /**< Global satellite ID */
|
||||
uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */
|
||||
uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
|
||||
uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
|
||||
uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */
|
||||
uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */
|
||||
float eph_m; /**< GPS HDOP horizontal dilution of position in m */
|
||||
float epv_m; /**< GPS VDOP horizontal dilution of position in m */
|
||||
|
||||
/* flags */
|
||||
float vel_ned_valid; /**< Flag to indicate if NED speed is valid */
|
||||
uint64_t timestamp_velocity; /**< Timestamp for velocity informations */
|
||||
float vel_m_s; /**< GPS ground speed (m/s) */
|
||||
float vel_n_m_s; /**< GPS ground speed in m/s */
|
||||
float vel_e_m_s; /**< GPS ground speed in m/s */
|
||||
float vel_d_m_s; /**< GPS ground speed in m/s */
|
||||
float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad */
|
||||
bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */
|
||||
|
||||
uint64_t timestamp_time; /**< Timestamp for time information */
|
||||
uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
|
||||
|
||||
uint64_t timestamp_satellites; /**< Timestamp for sattelite information */
|
||||
uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */
|
||||
uint8_t satellite_prn[20]; /**< Global satellite ID */
|
||||
uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */
|
||||
uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
|
||||
uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
|
||||
uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */
|
||||
bool satellite_info_available; /**< 0 for no info, 1 for info available */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -80,7 +80,6 @@ CONFIGURED_APPS += uORB
|
|||
|
||||
CONFIGURED_APPS += mavlink
|
||||
CONFIGURED_APPS += mavlink_onboard
|
||||
CONFIGURED_APPS += gps
|
||||
CONFIGURED_APPS += commander
|
||||
CONFIGURED_APPS += sdlog
|
||||
CONFIGURED_APPS += sensors
|
||||
|
|
@ -114,6 +113,7 @@ CONFIGURED_APPS += drivers/stm32/tone_alarm
|
|||
CONFIGURED_APPS += drivers/stm32/adc
|
||||
CONFIGURED_APPS += drivers/px4fmu
|
||||
CONFIGURED_APPS += drivers/hil
|
||||
CONFIGURED_APPS += drivers/gps
|
||||
|
||||
# Testing stuff
|
||||
CONFIGURED_APPS += px4/sensors_bringup
|
||||
|
|
|
|||
Loading…
Reference in New Issue