aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/blinkm/blinkm.cpp8
-rw-r--r--src/drivers/gps/gps.cpp137
-rw-r--r--src/drivers/gps/gps_helper.h4
-rw-r--r--src/drivers/gps/mtk.cpp2
-rw-r--r--src/drivers/gps/ubx.cpp1020
-rw-r--r--src/drivers/gps/ubx.h701
-rw-r--r--src/drivers/hott/messages.cpp2
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp2
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp3
-rw-r--r--src/modules/sdlog2/sdlog2.c50
-rw-r--r--src/modules/uORB/Subscription.cpp2
-rw-r--r--src/modules/uORB/objects_common.cpp3
-rw-r--r--src/modules/uORB/topics/satellite_info.h89
-rw-r--r--src/modules/uORB/topics/vehicle_gps_position.h9
14 files changed, 1219 insertions, 813 deletions
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 98c491ce6..92e089217 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -559,13 +559,7 @@ BlinkM::led()
}
/* get number of used satellites in navigation */
- num_of_used_sats = 0;
-
- for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
- if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
- num_of_used_sats++;
- }
- }
+ num_of_used_sats = vehicle_gps_position_raw.satellites_used;
if (new_data_vehicle_status || no_data_vehicle_status < 3) {
if (num_of_cells == 0) {
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index dd505abdb..1d8ef1b99 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -63,6 +63,7 @@
#include <drivers/drv_gps.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
#include <board_config.h>
@@ -79,10 +80,18 @@
#endif
static const int ERROR = -1;
+/* class for dynamic allocation of satellite info data */
+class GPS_Sat_Info
+{
+public:
+ struct satellite_info_s _data;
+};
+
+
class GPS : public device::CDev
{
public:
- GPS(const char *uart_path, bool fake_gps);
+ GPS(const char *uart_path, bool fake_gps, bool enable_sat_info);
virtual ~GPS();
virtual int init();
@@ -102,12 +111,15 @@ private:
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 _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
+ GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object
+ struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position
+ orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position
+ struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info
+ orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info
float _rate; ///< position update rate
bool _fake_gps; ///< fake gps output
@@ -154,14 +166,17 @@ GPS *g_dev;
}
-GPS::GPS(const char *uart_path, bool fake_gps) :
+GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
CDev("gps", GPS_DEVICE_PATH),
_task_should_exit(false),
_healthy(false),
_mode_changed(false),
_mode(GPS_DRIVER_MODE_UBX),
_Helper(nullptr),
- _report_pub(-1),
+ _Sat_Info(nullptr),
+ _report_gps_pos_pub(-1),
+ _p_report_sat_info(nullptr),
+ _report_sat_info_pub(-1),
_rate(0.0f),
_fake_gps(fake_gps)
{
@@ -172,7 +187,14 @@ GPS::GPS(const char *uart_path, bool fake_gps) :
/* we need this potentially before it could be set in task_main */
g_dev = this;
- memset(&_report, 0, sizeof(_report));
+ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
+
+ /* create satellite info data object if requested */
+ if (enable_sat_info) {
+ _Sat_Info = new(GPS_Sat_Info);
+ _p_report_sat_info = &_Sat_Info->_data;
+ memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
+ }
_debug_enabled = true;
}
@@ -207,7 +229,7 @@ GPS::init()
/* start the GPS driver worker task */
_task = task_spawn_cmd("gps", SCHED_DEFAULT,
- SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr);
+ SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) {
warnx("task start failed: %d", errno);
@@ -271,33 +293,33 @@ GPS::task_main()
if (_fake_gps) {
- _report.timestamp_position = hrt_absolute_time();
- _report.lat = (int32_t)47.378301e7f;
- _report.lon = (int32_t)8.538777e7f;
- _report.alt = (int32_t)1200e3f;
- _report.timestamp_variance = hrt_absolute_time();
- _report.s_variance_m_s = 10.0f;
- _report.p_variance_m = 10.0f;
- _report.c_variance_rad = 0.1f;
- _report.fix_type = 3;
- _report.eph = 0.9f;
- _report.epv = 1.8f;
- _report.timestamp_velocity = hrt_absolute_time();
- _report.vel_n_m_s = 0.0f;
- _report.vel_e_m_s = 0.0f;
- _report.vel_d_m_s = 0.0f;
- _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
- _report.cog_rad = 0.0f;
- _report.vel_ned_valid = true;
+ _report_gps_pos.timestamp_position = hrt_absolute_time();
+ _report_gps_pos.lat = (int32_t)47.378301e7f;
+ _report_gps_pos.lon = (int32_t)8.538777e7f;
+ _report_gps_pos.alt = (int32_t)1200e3f;
+ _report_gps_pos.timestamp_variance = hrt_absolute_time();
+ _report_gps_pos.s_variance_m_s = 10.0f;
+ _report_gps_pos.p_variance_m = 10.0f;
+ _report_gps_pos.c_variance_rad = 0.1f;
+ _report_gps_pos.fix_type = 3;
+ _report_gps_pos.eph = 0.9f;
+ _report_gps_pos.epv = 1.8f;
+ _report_gps_pos.timestamp_velocity = hrt_absolute_time();
+ _report_gps_pos.vel_n_m_s = 0.0f;
+ _report_gps_pos.vel_e_m_s = 0.0f;
+ _report_gps_pos.vel_d_m_s = 0.0f;
+ _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
+ _report_gps_pos.cog_rad = 0.0f;
+ _report_gps_pos.vel_ned_valid = true;
//no time and satellite information simulated
if (!(_pub_blocked)) {
- if (_report_pub > 0) {
- orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+ if (_report_gps_pos_pub > 0) {
+ orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
- _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+ _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
@@ -313,11 +335,11 @@ GPS::task_main()
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
- _Helper = new UBX(_serial_fd, &_report);
+ _Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info);
break;
case GPS_DRIVER_MODE_MTK:
- _Helper = new MTK(_serial_fd, &_report);
+ _Helper = new MTK(_serial_fd, &_report_gps_pos);
break;
default:
@@ -332,20 +354,33 @@ GPS::task_main()
// GPS is obviously detected successfully, reset statistics
_Helper->reset_update_rates();
- while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
+ int helper_ret;
+ while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (!(_pub_blocked)) {
- if (_report_pub > 0) {
- orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+ if (helper_ret & 1) {
+ if (_report_gps_pos_pub > 0) {
+ orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
- } else {
- _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+ } else {
+ _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
+ }
+ }
+ if (_p_report_sat_info && (helper_ret & 2)) {
+ if (_report_sat_info_pub > 0) {
+ orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
+
+ } else {
+ _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
+ }
}
}
- last_rate_count++;
+ if (helper_ret & 1) { // consider only pos info updates for rate calculation */
+ last_rate_count++;
+ }
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
@@ -446,12 +481,13 @@ GPS::print_info()
}
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+ warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
- if (_report.timestamp_position != 0) {
- warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
- _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0d);
- warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
- warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
+ if (_report_gps_pos.timestamp_position != 0) {
+ warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
+ _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
+ warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
+ warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate);
@@ -469,7 +505,7 @@ namespace gps
GPS *g_dev;
-void start(const char *uart_path, bool fake_gps);
+void start(const char *uart_path, bool fake_gps, bool enable_sat_info);
void stop();
void test();
void reset();
@@ -479,7 +515,7 @@ void info();
* Start the driver.
*/
void
-start(const char *uart_path, bool fake_gps)
+start(const char *uart_path, bool fake_gps, bool enable_sat_info)
{
int fd;
@@ -487,7 +523,7 @@ start(const char *uart_path, bool fake_gps)
errx(1, "already started");
/* create the driver */
- g_dev = new GPS(uart_path, fake_gps);
+ g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
if (g_dev == nullptr)
goto fail;
@@ -580,6 +616,7 @@ gps_main(int argc, char *argv[])
/* set to default */
const char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false;
+ bool enable_sat_info = false;
/*
* Start/load the driver.
@@ -601,7 +638,13 @@ gps_main(int argc, char *argv[])
fake_gps = true;
}
- gps::start(device_name, fake_gps);
+ /* Detect sat info option */
+ for (int i = 2; i < argc; i++) {
+ if (!strcmp(argv[i], "-s"))
+ enable_sat_info = true;
+ }
+
+ gps::start(device_name, fake_gps, enable_sat_info);
}
if (!strcmp(argv[1], "stop"))
@@ -626,5 +669,5 @@ gps_main(int argc, char *argv[])
gps::info();
out:
- errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
+ errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
}
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index d14a95afe..3623397b2 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -62,8 +62,8 @@ protected:
uint8_t _rate_count_lat_lon;
uint8_t _rate_count_vel;
- float _rate_lat_lon;
- float _rate_vel;
+ float _rate_lat_lon = 0.0f;
+ float _rate_vel = 0.0f;
uint64_t _interval_rate_start;
};
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 41716cd97..c4f4f7bec 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -263,7 +263,7 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
_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;
+ _gps_position->satellites_used = packet.satellites;
/* convert time and date information to unix timestamp */
struct tm timeinfo; //TODO: test this conversion
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 404607571..44434a1df 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013, 2014 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
@@ -34,14 +34,18 @@
/**
* @file ubx.cpp
*
- * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
+ * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description
* including Prototol Specification.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
+ * @author Hannes Delago
+ * (rework, add ubx7+ compatibility)
+ *
* @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_%28UBX-13003221%29.pdf
*/
#include <assert.h>
@@ -55,24 +59,43 @@
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
#include <drivers/drv_hrt.h>
#include "ubx.h"
-#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK
-#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received
+#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK
+#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received
#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls
-#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval
+#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval
+
+#define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
+#define SWAP16(X) ((((X) >> 8) & 0x00ff) | (((X) << 8) & 0xff00))
+
+#define FNV1_32_INIT ((uint32_t)0x811c9dc5) // init value for FNV1 hash algorithm
+#define FNV1_32_PRIME ((uint32_t)0x01000193) // magic prime for FNV1 hash algorithm
+
+
+/**** Trace macros, disable for production builds */
+#define UBX_TRACE_PARSER(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* decoding progress in parse_char() */
+#define UBX_TRACE_RXMSG(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* Rx msgs in payload_rx_done() */
+#define UBX_TRACE_SVINFO(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* NAV-SVINFO processing (debug use only, will cause rx buffer overflows) */
+
+/**** Warning macros, disable to save memory */
+#define UBX_WARN(s, ...) {warnx(s, ## __VA_ARGS__);}
+
-UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
+UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) :
_fd(fd),
_gps_position(gps_position),
+ _satellite_info(satellite_info),
_configured(false),
- _waiting_for_ack(false),
+ _ack_state(UBX_ACK_IDLE),
_got_posllh(false),
_got_velned(false),
- _got_timeutc(false),
- _disable_cmd_last(0)
+ _disable_cmd_last(0),
+ _ack_waiting_msg(0),
+ _ubx_version(0)
{
decode_init();
}
@@ -86,182 +109,160 @@ UBX::configure(unsigned &baudrate)
{
_configured = false;
/* try different baudrates */
- const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+ const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200};
- int baud_i;
+ unsigned baud_i;
- for (baud_i = 0; baud_i < 5; baud_i++) {
- baudrate = baudrates_to_try[baud_i];
+ for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) {
+ baudrate = baudrates[baud_i];
set_baudrate(_fd, baudrate);
+ /* flush input and wait for at least 20 ms silence */
+ decode_init();
+ receive(20);
+ decode_init();
+
/* 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));
-
- _message_class_needed = UBX_CLASS_CFG;
- _message_id_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 (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+ * and leave the baudrate as it is, we just want an ACK-ACK for this */
+ memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt));
+ _buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID;
+ _buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE;
+ _buf.payload_tx_cfg_prt.baudRate = baudrate;
+ _buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK;
+ _buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK;
+
+ send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt));
+
+ if (wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false) < 0) {
/* try next baudrate */
continue;
}
/* Send a CFG-PRT message again, this time change the baudrate */
+ memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt));
+ _buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID;
+ _buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE;
+ _buf.payload_tx_cfg_prt.baudRate = UBX_TX_CFG_PRT_BAUDRATE;
+ _buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK;
+ _buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK;
- 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));
+ send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt));
/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
- wait_for_ack(UBX_CONFIG_TIMEOUT);
+ wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
- if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
- set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
- baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+ if (UBX_TX_CFG_PRT_BAUDRATE != baudrate) {
+ set_baudrate(_fd, UBX_TX_CFG_PRT_BAUDRATE);
+ baudrate = UBX_TX_CFG_PRT_BAUDRATE;
}
/* at this point we have correct baudrate on both ends */
break;
}
- if (baud_i >= 5) {
- return 1;
+ if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
+ return 1; // connection and/or baudrate detection failed
}
- /* send a CFG-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));
-
- _message_class_needed = UBX_CLASS_CFG;
- _message_id_needed = UBX_MESSAGE_CFG_RATE;
+ /* Send a CFG-RATE message to define update rate */
+ memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate));
+ _buf.payload_tx_cfg_rate.measRate = UBX_TX_CFG_RATE_MEASINTERVAL;
+ _buf.payload_tx_cfg_rate.navRate = UBX_TX_CFG_RATE_NAVRATE;
+ _buf.payload_tx_cfg_rate.timeRef = UBX_TX_CFG_RATE_TIMEREF;
- 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_MEASINTERVAL;
- cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
- cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
+ send_message(UBX_MSG_CFG_RATE, _buf.raw, sizeof(_buf.payload_tx_cfg_rate));
- send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
-
- if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("CFG FAIL: RATE");
+ if (wait_for_ack(UBX_MSG_CFG_RATE, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
/* 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));
-
- _message_class_needed = UBX_CLASS_CFG;
- _message_id_needed = UBX_MESSAGE_CFG_NAV5;
+ memset(&_buf.payload_tx_cfg_nav5, 0, sizeof(_buf.payload_tx_cfg_nav5));
+ _buf.payload_tx_cfg_nav5.mask = UBX_TX_CFG_NAV5_MASK;
+ _buf.payload_tx_cfg_nav5.dynModel = UBX_TX_CFG_NAV5_DYNMODEL;
+ _buf.payload_tx_cfg_nav5.fixMode = UBX_TX_CFG_NAV5_FIXMODE;
- 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_message(UBX_MSG_CFG_NAV5, _buf.raw, sizeof(_buf.payload_tx_cfg_nav5));
- send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
-
- if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("CFG FAIL: NAV5");
+ if (wait_for_ack(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
/* configure message rates */
/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
- configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
+ configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
- if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("MSG CFG FAIL: NAV POSLLH");
+ if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
- configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
+ configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
- if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("MSG CFG FAIL: NAV TIMEUTC");
+ if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
- configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
+ configure_message_rate(UBX_MSG_NAV_SOL, 1);
- if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("MSG CFG FAIL: NAV SOL");
+ if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
- configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
+ configure_message_rate(UBX_MSG_NAV_VELNED, 1);
- if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("MSG CFG FAIL: NAV VELNED");
+ if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
- configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
+ configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0);
- if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("MSG CFG FAIL: NAV SVINFO");
+ if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
- configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
+ configure_message_rate(UBX_MSG_MON_HW, 1);
- if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("MSG CFG FAIL: MON HW");
+ if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
+ /* request module version information by sending an empty MON-VER message */
+ send_message(UBX_MSG_MON_VER, nullptr, 0);
+
_configured = true;
return 0;
}
-int
-UBX::wait_for_ack(unsigned timeout)
+int // -1 = NAK, error or timeout, 0 = ACK
+UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report)
{
- _waiting_for_ack = true;
- uint64_t time_started = hrt_absolute_time();
+ int ret = -1;
- while (hrt_absolute_time() < time_started + timeout * 1000) {
- if (receive(timeout) > 0) {
- if (!_waiting_for_ack) {
- return 1;
- }
+ _ack_state = UBX_ACK_WAITING;
+ _ack_waiting_msg = msg; // memorize sent msg class&ID for ACK check
+ hrt_abstime time_started = hrt_absolute_time();
+
+ while ((_ack_state == UBX_ACK_WAITING) && (hrt_absolute_time() < time_started + timeout * 1000)) {
+ receive(timeout);
+ }
+
+ if (_ack_state == UBX_ACK_GOT_ACK) {
+ ret = 0; // ACK received ok
+ } else if (report) {
+ if (_ack_state == UBX_ACK_GOT_NAK) {
+ UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
} else {
- return -1; // timeout or error receiving, or NAK
+ UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
}
}
- return -1; // timeout
+ _ack_state = UBX_ACK_IDLE;
+ return ret;
}
-int
-UBX::receive(unsigned timeout)
+int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
+UBX::receive(const unsigned timeout)
{
/* poll descriptor */
pollfd fds[1];
@@ -275,17 +276,17 @@ UBX::receive(unsigned timeout)
ssize_t count = 0;
- bool handled = false;
+ int handled = 0;
while (true) {
- bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled;
+ bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled;
/* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
if (ret < 0) {
/* something went wrong when polling */
- warnx("poll error");
+ UBX_WARN("ubx poll() err");
return -1;
} else if (ret == 0) {
@@ -293,8 +294,7 @@ UBX::receive(unsigned timeout)
if (ready_to_return) {
_got_posllh = false;
_got_velned = false;
- _got_timeutc = false;
- return 1;
+ return handled;
} else {
return -1;
@@ -314,440 +314,596 @@ UBX::receive(unsigned timeout)
/* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++) {
- if (parse_char(buf[i]) > 0) {
- if (handle_message() > 0)
- handled = true;
- }
+ handled |= parse_char(buf[i]);
}
}
}
/* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < hrt_absolute_time()) {
- warnx("timeout - no useful messages");
return -1;
}
}
}
-int
-UBX::parse_char(uint8_t b)
+int // 0 = decoding, 1 = message handled, 2 = sat info message handled
+UBX::parse_char(const uint8_t b)
{
+ int ret = 0;
+
switch (_decode_state) {
- /* First, look for sync1 */
- case UBX_DECODE_UNINIT:
- if (b == UBX_SYNC1) {
- _decode_state = UBX_DECODE_GOT_SYNC1;
- }
+ /* Expecting Sync1 */
+ case UBX_DECODE_SYNC1:
+ if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2
+ UBX_TRACE_PARSER("\nA");
+ _decode_state = UBX_DECODE_SYNC2;
+ }
break;
- /* Second, look for sync2 */
- case UBX_DECODE_GOT_SYNC1:
- if (b == UBX_SYNC2) {
- _decode_state = UBX_DECODE_GOT_SYNC2;
+ /* Expecting Sync2 */
+ case UBX_DECODE_SYNC2:
+ if (b == UBX_SYNC2) { // Sync2 found --> expecting Class
+ UBX_TRACE_PARSER("B");
+ _decode_state = UBX_DECODE_CLASS;
- } else {
- /* Second start symbol was wrong, reset state machine */
+ } else { // Sync1 not followed by Sync2: reset parser
decode_init();
- /* don't return error, it can be just false sync1 */
}
+ break;
+ /* Expecting Class */
+ case UBX_DECODE_CLASS:
+ UBX_TRACE_PARSER("C");
+ add_byte_to_checksum(b); // checksum is calculated for everything except Sync and Checksum bytes
+ _rx_msg = b;
+ _decode_state = UBX_DECODE_ID;
break;
- /* Now look for class */
- case UBX_DECODE_GOT_SYNC2:
- /* everything except sync1 and sync2 needs to be added to the checksum */
+ /* Expecting ID */
+ case UBX_DECODE_ID:
+ UBX_TRACE_PARSER("D");
add_byte_to_checksum(b);
- _message_class = b;
- _decode_state = UBX_DECODE_GOT_CLASS;
+ _rx_msg |= b << 8;
+ _decode_state = UBX_DECODE_LENGTH1;
break;
- case UBX_DECODE_GOT_CLASS:
+ /* Expecting first length byte */
+ case UBX_DECODE_LENGTH1:
+ UBX_TRACE_PARSER("E");
add_byte_to_checksum(b);
- _message_id = b;
- _decode_state = UBX_DECODE_GOT_MESSAGEID;
+ _rx_payload_length = b;
+ _decode_state = UBX_DECODE_LENGTH2;
break;
- case UBX_DECODE_GOT_MESSAGEID:
+ /* Expecting second length byte */
+ case UBX_DECODE_LENGTH2:
+ UBX_TRACE_PARSER("F");
add_byte_to_checksum(b);
- _payload_size = b; //this is the first length byte
- _decode_state = UBX_DECODE_GOT_LENGTH1;
+ _rx_payload_length |= b << 8; // calculate payload size
+ if (payload_rx_init() != 0) { // start payload reception
+ // payload will not be handled, discard message
+ decode_init();
+ } else {
+ _decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1;
+ }
break;
- case UBX_DECODE_GOT_LENGTH1:
+ /* Expecting payload */
+ case UBX_DECODE_PAYLOAD:
+ UBX_TRACE_PARSER(".");
add_byte_to_checksum(b);
- _payload_size += b << 8; // here comes the second byte of length
- _decode_state = UBX_DECODE_GOT_LENGTH2;
+ switch (_rx_msg) {
+ case UBX_MSG_NAV_SVINFO:
+ ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte
+ break;
+ case UBX_MSG_MON_VER:
+ ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte
+ break;
+ default:
+ ret = payload_rx_add(b); // add a payload byte
+ break;
+ }
+ if (ret < 0) {
+ // payload not handled, discard message
+ decode_init();
+ } else if (ret > 0) {
+ // payload complete, expecting checksum
+ _decode_state = UBX_DECODE_CHKSUM1;
+ } else {
+ // expecting more payload, stay in state UBX_DECODE_PAYLOAD
+ }
+ ret = 0;
+ break;
+
+ /* Expecting first checksum byte */
+ case UBX_DECODE_CHKSUM1:
+ if (_rx_ck_a != b) {
+ UBX_WARN("ubx checksum err");
+ decode_init();
+ } else {
+ _decode_state = UBX_DECODE_CHKSUM2;
+ }
+ break;
+
+ /* Expecting second checksum byte */
+ case UBX_DECODE_CHKSUM2:
+ if (_rx_ck_b != b) {
+ UBX_WARN("ubx checksum err");
+ } else {
+ ret = payload_rx_done(); // finish payload processing
+ }
+ decode_init();
break;
- case UBX_DECODE_GOT_LENGTH2:
+ default:
+ break;
+ }
- /* Add to checksum if not yet at checksum byte */
- if (_rx_count < _payload_size)
- add_byte_to_checksum(b);
+ return ret;
+}
- _rx_buffer[_rx_count] = b;
+/**
+ * Start payload rx
+ */
+int // -1 = abort, 0 = continue
+UBX::payload_rx_init()
+{
+ int ret = 0;
- /* 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]) {
- decode_init();
- return 1; // message received successfully
+ _rx_state = UBX_RXMSG_HANDLE; // handle by default
- } else {
- warnx("checksum wrong");
- decode_init();
- return -1;
- }
+ switch (_rx_msg) {
+ case UBX_MSG_NAV_POSLLH:
+ if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t))
+ _rx_state = UBX_RXMSG_ERROR_LENGTH;
+ else if (!_configured)
+ _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+ break;
- } else if (_rx_count < RECV_BUFFER_SIZE) {
- _rx_count++;
+ case UBX_MSG_NAV_SOL:
+ if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t))
+ _rx_state = UBX_RXMSG_ERROR_LENGTH;
+ else if (!_configured)
+ _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+ break;
- } else {
- warnx("buffer full");
- decode_init();
- return -1;
- }
+ case UBX_MSG_NAV_TIMEUTC:
+ if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t))
+ _rx_state = UBX_RXMSG_ERROR_LENGTH;
+ else if (!_configured)
+ _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+ break;
+
+ case UBX_MSG_NAV_SVINFO:
+ if (_satellite_info == nullptr)
+ _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested
+ else if (!_configured)
+ _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+ else
+ memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info
+ break;
+
+ case UBX_MSG_NAV_VELNED:
+ if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t))
+ _rx_state = UBX_RXMSG_ERROR_LENGTH;
+ else if (!_configured)
+ _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+ break;
+
+ case UBX_MSG_MON_VER:
+ break; // unconditionally handle this message
+ case UBX_MSG_MON_HW:
+ if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */
+ && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */
+ _rx_state = UBX_RXMSG_ERROR_LENGTH;
+ else if (!_configured)
+ _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+ break;
+
+ case UBX_MSG_ACK_ACK:
+ if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t))
+ _rx_state = UBX_RXMSG_ERROR_LENGTH;
+ else if (_configured)
+ _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
+ break;
+
+ case UBX_MSG_ACK_NAK:
+ if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t))
+ _rx_state = UBX_RXMSG_ERROR_LENGTH;
+ else if (_configured)
+ _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
break;
default:
+ _rx_state = UBX_RXMSG_DISABLE; // disable all other messages
+ break;
+ }
+
+ switch (_rx_state) {
+ case UBX_RXMSG_HANDLE: // handle message
+ case UBX_RXMSG_IGNORE: // ignore message but don't report error
+ ret = 0;
+ break;
+
+ case UBX_RXMSG_DISABLE: // disable unexpected messages
+ UBX_WARN("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
+
+ {
+ hrt_abstime t = hrt_absolute_time();
+
+ if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
+ /* don't attempt for every message to disable, some might not be disabled */
+ _disable_cmd_last = t;
+ UBX_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
+ configure_message_rate(_rx_msg, 0);
+ }
+ }
+
+ ret = -1; // return error, abort handling this message
+ break;
+
+ case UBX_RXMSG_ERROR_LENGTH: // error: invalid length
+ UBX_WARN("ubx msg 0x%04x invalid len %u", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
+ ret = -1; // return error, abort handling this message
+ break;
+
+ default: // invalid message state
+ UBX_WARN("ubx internal err1");
+ ret = -1; // return error, abort handling this message
break;
}
- return 0; // message decoding in progress
+ return ret;
}
+/**
+ * Add payload rx byte
+ */
+int // -1 = error, 0 = ok, 1 = payload completed
+UBX::payload_rx_add(const uint8_t b)
+{
+ int ret = 0;
+ _buf.raw[_rx_payload_index] = b;
-int
-UBX::handle_message()
+ if (++_rx_payload_index >= _rx_payload_length) {
+ ret = 1; // payload received completely
+ }
+
+ return ret;
+}
+
+/**
+ * Add NAV-SVINFO payload rx byte
+ */
+int // -1 = error, 0 = ok, 1 = payload completed
+UBX::payload_rx_add_nav_svinfo(const uint8_t b)
{
int ret = 0;
- if (_configured) {
- /* handle only info messages when configured */
- switch (_message_class) {
- case UBX_CLASS_NAV:
- switch (_message_id) {
- case UBX_MESSAGE_NAV_POSLLH: {
- // printf("GOT NAV_POSLLH\n");
- 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 = (float)packet->hAcc * 1e-3f; // from mm to m
- _gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m
- _gps_position->timestamp_position = hrt_absolute_time();
-
- _rate_count_lat_lon++;
-
- _got_posllh = true;
- ret = 1;
- break;
- }
+ if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
+ // Fill Part 1 buffer
+ _buf.raw[_rx_payload_index] = b;
+ } else {
+ if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
+ // Part 1 complete: decode Part 1 buffer
+ _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, SAT_INFO_MAX_SATELLITES);
+ UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh);
+ }
+ if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) {
+ // Still room in _satellite_info: fill Part 2 buffer
+ unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t);
+ _buf.raw[buf_index] = b;
+ if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) {
+ // Part 2 complete: decode Part 2 buffer
+ unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t);
+ _satellite_info->used[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01);
+ _satellite_info->snr[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno);
+ _satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev);
+ _satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f);
+ _satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid);
+ UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u\n",
+ (unsigned)sat_index + 1,
+ (unsigned)_satellite_info->used[sat_index],
+ (unsigned)_satellite_info->snr[sat_index],
+ (unsigned)_satellite_info->elevation[sat_index],
+ (unsigned)_satellite_info->azimuth[sat_index],
+ (unsigned)_satellite_info->svid[sat_index]
+ );
+ }
+ }
+ }
- case UBX_MESSAGE_NAV_SOL: {
- // printf("GOT NAV_SOL\n");
- gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
+ if (++_rx_payload_index >= _rx_payload_length) {
+ ret = 1; // payload received completely
+ }
- _gps_position->fix_type = packet->gpsFix;
- _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s
- _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m
- _gps_position->timestamp_variance = hrt_absolute_time();
+ return ret;
+}
- ret = 1;
- break;
- }
+/**
+ * Add MON-VER payload rx byte
+ */
+int // -1 = error, 0 = ok, 1 = payload completed
+UBX::payload_rx_add_mon_ver(const uint8_t b)
+{
+ int ret = 0;
+
+ if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) {
+ // Fill Part 1 buffer
+ _buf.raw[_rx_payload_index] = b;
+ } else {
+ if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) {
+ // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings
+ _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT);
+ _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version);
+ UBX_WARN("VER hash 0x%08x", _ubx_version);
+ UBX_WARN("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
+ UBX_WARN("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
+ }
+ // fill Part 2 buffer
+ unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t);
+ _buf.raw[buf_index] = b;
+ if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
+ // Part 2 complete: decode Part 2 buffer
+ UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
+ }
+ }
+
+ if (++_rx_payload_index >= _rx_payload_length) {
+ ret = 1; // payload received completely
+ }
+
+ return ret;
+}
+
+/**
+ * Finish payload rx
+ */
+int // 0 = no message handled, 1 = message handled, 2 = sat info message handled
+UBX::payload_rx_done(void)
+{
+ int ret = 0;
+
+ // return if no message handled
+ if (_rx_state != UBX_RXMSG_HANDLE) {
+ return ret;
+ }
+
+ // handle message
+ switch (_rx_msg) {
+
+ case UBX_MSG_NAV_POSLLH:
+ UBX_TRACE_RXMSG("Rx NAV-POSLLH\n");
+
+ _gps_position->lat = _buf.payload_rx_nav_posllh.lat;
+ _gps_position->lon = _buf.payload_rx_nav_posllh.lon;
+ _gps_position->alt = _buf.payload_rx_nav_posllh.hMSL;
+ _gps_position->eph = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m
+ _gps_position->epv = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m
+
+ _gps_position->timestamp_position = hrt_absolute_time();
+
+ _rate_count_lat_lon++;
+ _got_posllh = true;
+
+ ret = 1;
+ break;
+
+ case UBX_MSG_NAV_SOL:
+ UBX_TRACE_RXMSG("Rx NAV-SOL\n");
+
+ _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix;
+ _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m
+ _gps_position->p_variance_m = (float)_buf.payload_rx_nav_sol.pAcc * 1e-2f; // from cm to m
+ _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV;
+
+ _gps_position->timestamp_variance = hrt_absolute_time();
+
+ ret = 1;
+ break;
- case UBX_MESSAGE_NAV_TIMEUTC: {
- // printf("GOT NAV_TIMEUTC\n");
- gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
+ case UBX_MSG_NAV_TIMEUTC:
+ UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n");
- /* 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);
+ {
+ /* convert to unix timestamp */
+ struct tm timeinfo;
+ timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900;
+ timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1;
+ timeinfo.tm_mday = _buf.payload_rx_nav_timeutc.day;
+ timeinfo.tm_hour = _buf.payload_rx_nav_timeutc.hour;
+ timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min;
+ timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec;
+ time_t epoch = mktime(&timeinfo);
#ifndef CONFIG_RTC
- //Since we lack a hardware RTC, set the system time clock based on GPS UTC
- //TODO generalize this by moving into gps.cpp?
- timespec ts;
- ts.tv_sec = epoch;
- ts.tv_nsec = packet->time_nanoseconds;
- clock_settime(CLOCK_REALTIME, &ts);
+ //Since we lack a hardware RTC, set the system time clock based on GPS UTC
+ //TODO generalize this by moving into gps.cpp?
+ timespec ts;
+ ts.tv_sec = epoch;
+ ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
+ clock_settime(CLOCK_REALTIME, &ts);
#endif
- _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();
+ _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+ _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f);
+ }
- _got_timeutc = true;
- ret = 1;
- break;
- }
+ _gps_position->timestamp_time = hrt_absolute_time();
- case UBX_MESSAGE_NAV_SVINFO: {
- //printf("GOT NAV_SVINFO\n");
- const int length_part1 = 8;
- gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
- const int length_part2 = 12;
- gps_bin_nav_svinfo_part2_packet_t *packet_part2;
-
- uint8_t satellites_used = 0;
- int i;
-
- //printf("Number of Channels: %d\n", packet_part1->numCh);
- for (i = 0; i < packet_part1->numCh; i++) {
- /* set pointer to sattelite_i information */
- packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
-
- /* write satellite information to global storage */
- uint8_t sv_used = packet_part2->flags & 0x01;
-
- if (sv_used) {
- /* count SVs used for NAV */
- satellites_used++;
- }
-
- /* record info for all channels, whether or not the SV is used for NAV */
- _gps_position->satellite_used[i] = sv_used;
- _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);
- _gps_position->satellite_prn[i] = packet_part2->svid;
- //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
- }
-
- for (i = packet_part1->numCh; i < 20; i++) {
- /* 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
-
- 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();
-
- ret = 1;
- break;
- }
+ ret = 1;
+ break;
- case UBX_MESSAGE_NAV_VELNED: {
- // printf("GOT NAV_VELNED\n");
- gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
+ case UBX_MSG_NAV_SVINFO:
+ UBX_TRACE_RXMSG("Rx NAV-SVINFO\n");
- _gps_position->vel_m_s = (float)packet->speed * 1e-2f;
- _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */
- _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
- _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
- _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
- _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
- _gps_position->vel_ned_valid = true;
- _gps_position->timestamp_velocity = hrt_absolute_time();
+ // _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp
+ _satellite_info->timestamp = hrt_absolute_time();
- _rate_count_vel++;
+ ret = 2;
+ break;
- _got_velned = true;
- ret = 1;
- break;
- }
+ case UBX_MSG_NAV_VELNED:
+ UBX_TRACE_RXMSG("Rx NAV-VELNED\n");
- default:
- break;
- }
+ _gps_position->vel_m_s = (float)_buf.payload_rx_nav_velned.speed * 1e-2f;
+ _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_velned.velN * 1e-2f; /* NED NORTH velocity */
+ _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_velned.velE * 1e-2f; /* NED EAST velocity */
+ _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_velned.velD * 1e-2f; /* NED DOWN velocity */
+ _gps_position->cog_rad = (float)_buf.payload_rx_nav_velned.heading * M_DEG_TO_RAD_F * 1e-5f;
+ _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_velned.cAcc * M_DEG_TO_RAD_F * 1e-5f;
+ _gps_position->vel_ned_valid = true;
- break;
+ _gps_position->timestamp_velocity = hrt_absolute_time();
- case UBX_CLASS_ACK: {
- /* ignore ACK when already configured */
- ret = 1;
- break;
- }
+ _rate_count_vel++;
+ _got_velned = true;
- case UBX_CLASS_MON: {
- switch (_message_id) {
- case UBX_MESSAGE_MON_HW: {
+ ret = 1;
+ break;
- struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
+ case UBX_MSG_MON_VER:
+ UBX_TRACE_RXMSG("Rx MON-VER\n");
- _gps_position->noise_per_ms = p->noisePerMS;
- _gps_position->jamming_indicator = p->jamInd;
+ ret = 1;
+ break;
- ret = 1;
- break;
- }
+ case UBX_MSG_MON_HW:
+ UBX_TRACE_RXMSG("Rx MON-HW\n");
- default:
- break;
- }
- }
+ switch (_rx_payload_length) {
- default:
+ case sizeof(ubx_payload_rx_mon_hw_ubx6_t): /* u-blox 6 msg format */
+ _gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx6.noisePerMS;
+ _gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx6.jamInd;
+
+ ret = 1;
break;
- }
- if (ret == 0) {
- /* message not handled */
- warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+ case sizeof(ubx_payload_rx_mon_hw_ubx7_t): /* u-blox 7+ msg format */
+ _gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx7.noisePerMS;
+ _gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx7.jamInd;
- hrt_abstime t = hrt_absolute_time();
+ ret = 1;
+ break;
- if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
- /* don't attempt for every message to disable, some might not be disabled */
- _disable_cmd_last = t;
- warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
- configure_message_rate(_message_class, _message_id, 0);
- }
+ default: // unexpected payload size:
+ ret = 0; // don't handle message
+ break;
}
+ break;
- } else {
- /* handle only ACK while configuring */
- if (_message_class == UBX_CLASS_ACK) {
- switch (_message_id) {
- case UBX_MESSAGE_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 == _message_class_needed && packet->msgID == _message_id_needed) {
- _waiting_for_ack = false;
- ret = 1;
- }
- }
-
- break;
- }
+ case UBX_MSG_ACK_ACK:
+ UBX_TRACE_RXMSG("Rx ACK-ACK\n");
- case UBX_MESSAGE_ACK_NAK: {
- // printf("GOT ACK_NAK\n");
- warnx("ubx: not acknowledged");
- /* configuration obviously not successful */
- _waiting_for_ack = false;
- ret = -1;
- break;
- }
+ if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
+ _ack_state = UBX_ACK_GOT_ACK;
+ }
- default:
- break;
- }
+ ret = 1;
+ break;
+
+ case UBX_MSG_ACK_NAK:
+ UBX_TRACE_RXMSG("Rx ACK-NAK\n");
+
+ if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
+ _ack_state = UBX_ACK_GOT_NAK;
}
+
+ ret = 1;
+ break;
+
+ default:
+ break;
}
- decode_init();
return ret;
}
void
UBX::decode_init(void)
{
+ _decode_state = UBX_DECODE_SYNC1;
_rx_ck_a = 0;
_rx_ck_b = 0;
- _rx_count = 0;
- _decode_state = UBX_DECODE_UNINIT;
- _payload_size = 0;
- /* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */
+ _rx_payload_length = 0;
+ _rx_payload_index = 0;
}
void
-UBX::add_byte_to_checksum(uint8_t b)
+UBX::add_byte_to_checksum(const 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)
+UBX::calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum)
{
- 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;
+ for (uint16_t i = 0; i < length; i++) {
+ checksum->ck_a = checksum->ck_a + buffer[i];
+ checksum->ck_b = checksum->ck_b + checksum->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::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
+UBX::configure_message_rate(const uint16_t msg, const uint8_t rate)
{
- for (unsigned i = 0; i < length; i++) {
- ck_a = ck_a + message[i];
- ck_b = ck_b + ck_a;
- }
+ ubx_payload_tx_cfg_msg_t cfg_msg; // don't use _buf (allow interleaved operation)
+
+ cfg_msg.msg = msg;
+ cfg_msg.rate = rate;
+
+ send_message(UBX_MSG_CFG_MSG, (uint8_t *)&cfg_msg, sizeof(cfg_msg));
}
void
-UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
+UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length)
{
- struct ubx_cfg_msg_rate msg;
- msg.msg_class = msg_class;
- msg.msg_id = msg_id;
- msg.rate = rate;
- send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+ ubx_header_t header = {UBX_SYNC1, UBX_SYNC2};
+ ubx_checksum_t checksum = {0, 0};
+
+ // Populate header
+ header.msg = msg;
+ header.length = length;
+
+ // Calculate checksum
+ calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes
+ if (payload != nullptr)
+ calc_checksum(payload, length, &checksum);
+
+ // Send message
+ write(_fd, (const void *)&header, sizeof(header));
+ if (payload != nullptr)
+ write(_fd, (const void *)payload, length);
+ write(_fd, (const void *)&checksum, sizeof(checksum));
}
-void
-UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
+uint32_t
+UBX::fnv1_32_str(uint8_t *str, uint32_t hval)
{
- ssize_t ret = 0;
-
- /* calculate the checksum now */
- add_checksum_to_message(packet, length);
-
- const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
+ uint8_t *s = str;
+
+ /*
+ * FNV-1 hash each octet in the buffer
+ */
+ while (*s) {
+
+ /* multiply by the 32 bit FNV magic prime mod 2^32 */
+#if defined(NO_FNV_GCC_OPTIMIZATION)
+ hval *= FNV1_32_PRIME;
+#else
+ hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24);
+#endif
- /* start with the two sync bytes */
- ret += write(fd, sync_bytes, sizeof(sync_bytes));
- ret += write(fd, packet, length);
+ /* xor the bottom with the current octet */
+ hval ^= (uint32_t)*s++;
+ }
- if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
- warnx("ubx: configuration write fail");
+ /* return our new hash value */
+ return hval;
}
-void
-UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
-{
- struct ubx_header header;
- uint8_t ck_a = 0, ck_b = 0;
- header.sync1 = UBX_SYNC1;
- header.sync2 = UBX_SYNC2;
- header.msg_class = msg_class;
- header.msg_id = msg_id;
- header.length = size;
-
- add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
- add_checksum((uint8_t *)msg, size, ck_a, ck_b);
-
- /* configure ACK check */
- _message_class_needed = msg_class;
- _message_id_needed = msg_id;
-
- write(_fd, (const char *)&header, sizeof(header));
- write(_fd, (const char *)msg, size);
- write(_fd, (const char *)&ck_a, 1);
- write(_fd, (const char *)&ck_b, 1);
-}
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 43d688893..1a9e7cb0c 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013, 2014 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
@@ -34,13 +34,16 @@
/**
* @file ubx.h
*
- * U-Blox protocol definition. Following u-blox 6/7 Receiver Description
+ * U-Blox protocol definition. Following u-blox 6/7/8 Receiver Description
* including Prototol Specification.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
+ * @author Hannes Delago
+ * (rework, add ubx7+ compatibility)
+ *
*/
#ifndef UBX_H_
@@ -51,319 +54,423 @@
#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
-#define UBX_CLASS_MON 0x0A
-
-/* MessageIDs (the ones that are used) */
-#define UBX_MESSAGE_NAV_POSLLH 0x02
-//#define UBX_MESSAGE_NAV_DOP 0x04
-#define UBX_MESSAGE_NAV_SOL 0x06
-#define UBX_MESSAGE_NAV_VELNED 0x12
-//#define UBX_MESSAGE_RXM_SVSI 0x20
-#define UBX_MESSAGE_NAV_TIMEUTC 0x21
-#define UBX_MESSAGE_NAV_SVINFO 0x30
-#define UBX_MESSAGE_ACK_NAK 0x00
-#define UBX_MESSAGE_ACK_ACK 0x01
-#define UBX_MESSAGE_CFG_PRT 0x00
-#define UBX_MESSAGE_CFG_MSG 0x01
-#define UBX_MESSAGE_CFG_RATE 0x08
-#define UBX_MESSAGE_CFG_NAV5 0x24
-
-#define UBX_MESSAGE_MON_HW 0x09
-
-#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 /**< 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_MEASINTERVAL 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 0x0005 /**< XXX 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_CFG_MSG_PAYLOAD_RATE1_05HZ 10
-
-#define UBX_MAX_PAYLOAD_LENGTH 500
-
-// ************
-/** the structures of the binary packets */
+/* Message Classes */
+#define UBX_CLASS_NAV 0x01
+#define UBX_CLASS_ACK 0x05
+#define UBX_CLASS_CFG 0x06
+#define UBX_CLASS_MON 0x0A
+
+/* Message IDs */
+#define UBX_ID_NAV_POSLLH 0x02
+#define UBX_ID_NAV_SOL 0x06
+#define UBX_ID_NAV_PVT 0x07
+#define UBX_ID_NAV_VELNED 0x12
+#define UBX_ID_NAV_TIMEUTC 0x21
+#define UBX_ID_NAV_SVINFO 0x30
+#define UBX_ID_ACK_NAK 0x00
+#define UBX_ID_ACK_ACK 0x01
+#define UBX_ID_CFG_PRT 0x00
+#define UBX_ID_CFG_MSG 0x01
+#define UBX_ID_CFG_RATE 0x08
+#define UBX_ID_CFG_NAV5 0x24
+#define UBX_ID_MON_VER 0x04
+#define UBX_ID_MON_HW 0x09
+
+/* Message Classes & IDs */
+#define UBX_MSG_NAV_POSLLH ((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8)
+#define UBX_MSG_NAV_SOL ((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8)
+#define UBX_MSG_NAV_PVT ((UBX_CLASS_NAV) | UBX_ID_NAV_PVT << 8)
+#define UBX_MSG_NAV_VELNED ((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8)
+#define UBX_MSG_NAV_TIMEUTC ((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8)
+#define UBX_MSG_NAV_SVINFO ((UBX_CLASS_NAV) | UBX_ID_NAV_SVINFO << 8)
+#define UBX_MSG_ACK_NAK ((UBX_CLASS_ACK) | UBX_ID_ACK_NAK << 8)
+#define UBX_MSG_ACK_ACK ((UBX_CLASS_ACK) | UBX_ID_ACK_ACK << 8)
+#define UBX_MSG_CFG_PRT ((UBX_CLASS_CFG) | UBX_ID_CFG_PRT << 8)
+#define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
+#define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
+#define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
+#define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
+#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
+
+/* RX NAV-PVT message content details */
+/* Bitfield "valid" masks */
+#define UBX_RX_NAV_PVT_VALID_VALIDDATE 0x01 /**< validDate (Valid UTC Date) */
+#define UBX_RX_NAV_PVT_VALID_VALIDTIME 0x02 /**< validTime (Valid UTC Time) */
+#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED 0x04 /**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */
+
+/* Bitfield "flags" masks */
+#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK 0x01 /**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */
+#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN 0x02 /**< diffSoln (1 if differential corrections were applied) */
+#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE 0x1C /**< psmState (Power Save Mode state (see Power Management)) */
+#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID 0x20 /**< headVehValid (Heading of vehicle is valid) */
+
+/* TX CFG-PRT message contents */
+#define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */
+#define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
+#define UBX_TX_CFG_PRT_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */
+#define UBX_TX_CFG_PRT_INPROTOMASK 0x01 /**< UBX in */
+#define UBX_TX_CFG_PRT_OUTPROTOMASK 0x01 /**< UBX out */
+
+/* TX CFG-RATE message contents */
+#define UBX_TX_CFG_RATE_MEASINTERVAL 200 /**< 200ms for 5Hz */
+#define UBX_TX_CFG_RATE_NAVRATE 1 /**< cannot be changed */
+#define UBX_TX_CFG_RATE_TIMEREF 0 /**< 0: UTC, 1: GPS time */
+
+/* TX CFG-NAV5 message contents */
+#define UBX_TX_CFG_NAV5_MASK 0x0005 /**< Only update dynamic model and fix mode */
+#define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
+#define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
+
+/* TX CFG-MSG message contents */
+#define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_TX_CFG_MSG_RATE1_05HZ 10
+
+
+/*** u-blox protocol binary message and payload definitions ***/
#pragma pack(push, 1)
-struct ubx_header {
- uint8_t sync1;
- uint8_t sync2;
- uint8_t msg_class;
- uint8_t msg_id;
- uint16_t length;
-};
-
+/* General: Header */
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;
+ uint8_t sync1;
+ uint8_t sync2;
+ uint16_t msg;
+ uint16_t length;
+} ubx_header_t;
+/* General: Checksum */
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;
+ uint8_t ck_a;
+ uint8_t ck_b;
+} ubx_checksum_t ;
+/* Rx NAV-POSLLH */
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;
-
+ uint32_t iTOW; /**< GPS Time of Week [ms] */
+ int32_t lon; /**< Longitude [1e-7 deg] */
+ int32_t lat; /**< Latitude [1e-7 deg] */
+ int32_t height; /**< Height above ellipsoid [mm] */
+ int32_t hMSL; /**< Height above mean sea level [mm] */
+ uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */
+ uint32_t vAcc; /**< Vertical accuracy estimate [mm] */
+} ubx_payload_rx_nav_posllh_t;
+
+/* Rx NAV-SOL */
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;
-
+ uint32_t iTOW; /**< GPS Time of Week [ms] */
+ int32_t fTOW; /**< Fractional part of iTOW (range: +/-500000) [ns] */
+ int16_t week; /**< GPS week */
+ uint8_t gpsFix; /**< GPSfix type: 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; /**< Number of SVs used in Nav Solution */
+ uint32_t reserved2;
+} ubx_payload_rx_nav_sol_t;
+
+/* Rx NAV-PVT */
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;
-
+ uint32_t iTOW; /**< GPS Time of Week [ms] */
+ uint16_t year; /**< Year (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..60 (UTC) */
+ uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
+ uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */
+ int32_t nano; /**< Fraction of second (UTC) [-1e9...1e9 ns] */
+ uint8_t fixType; /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */
+ uint8_t flags; /**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */
+ uint8_t reserved1;
+ uint8_t numSV; /**< Number of SVs used in Nav Solution */
+ int32_t lon; /**< Longitude [1e-7 deg] */
+ int32_t lat; /**< Latitude [1e-7 deg] */
+ int32_t height; /**< Height above ellipsoid [mm] */
+ int32_t hMSL; /**< Height above mean sea level [mm] */
+ uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */
+ uint32_t vAcc; /**< Vertical accuracy estimate [mm] */
+ int32_t velN; /**< NED north velocity [mm/s]*/
+ int32_t velE; /**< NED east velocity [mm/s]*/
+ int32_t velD; /**< NED down velocity [mm/s]*/
+ int32_t gSpeed; /**< Ground Speed (2-D) [mm/s] */
+ int32_t headMot; /**< Heading of motion (2-D) [1e-5 deg] */
+ uint32_t sAcc; /**< Speed accuracy estimate [mm/s] */
+ uint32_t headAcc; /**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */
+ uint16_t pDOP; /**< Position DOP [0.01] */
+ uint16_t reserved2;
+ uint32_t reserved3;
+ int32_t headVeh; /**< Heading of vehicle (2-D) [1e-5 deg] */
+ uint32_t reserved4;
+} ubx_payload_rx_nav_pvt_t;
+
+/* Rx NAV-TIMEUTC */
typedef struct {
- uint8_t ck_a;
- uint8_t ck_b;
-} gps_bin_nav_svinfo_part3_packet_t;
-
+ uint32_t iTOW; /**< GPS Time of Week [ms] */
+ uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */
+ int32_t nano; /**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */
+ 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..60 (UTC) */
+ uint8_t valid; /**< Validity Flags (see ubx documentation) */
+} ubx_payload_rx_nav_timeutc_t;
+
+/* Rx NAV-SVINFO Part 1 */
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;
-
-struct gps_bin_mon_hw_packet {
- uint32_t pinSel;
- uint32_t pinBank;
- uint32_t pinDir;
- uint32_t pinVal;
- uint16_t noisePerMS;
- uint16_t agcCnt;
- uint8_t aStatus;
- uint8_t aPower;
- uint8_t flags;
- uint8_t __reserved1;
- uint32_t usedMask;
- uint8_t VP[25];
- uint8_t jamInd;
- uint16_t __reserved3;
- uint32_t pinIrq;
- uint32_t pulLH;
- uint32_t pullL;
-};
-
-
-//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;
+ uint32_t iTOW; /**< GPS Time of Week [ms] */
+ uint8_t numCh; /**< Number of channels */
+ uint8_t globalFlags;
+ uint16_t reserved2;
+} ubx_payload_rx_nav_svinfo_part1_t;
+/* Rx NAV-SVINFO Part 2 (repeated) */
typedef struct {
- uint8_t clsID;
- uint8_t msgID;
- uint8_t ck_a;
- uint8_t ck_b;
-} gps_bin_ack_ack_packet_t;
-
+ 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 [deg] */
+ int16_t azim; /**< Azimuth [deg] */
+ int32_t prRes; /**< Pseudo range residual [cm] */
+} ubx_payload_rx_nav_svinfo_part2_t;
+
+/* Rx NAV-VELNED */
typedef struct {
- uint8_t clsID;
- uint8_t msgID;
- uint8_t ck_a;
- uint8_t ck_b;
-} gps_bin_ack_nak_packet_t;
-
+ uint32_t iTOW; /**< GPS Time of Week [ms] */
+ int32_t velN; /**< North velocity component [cm/s]*/
+ int32_t velE; /**< East velocity component [cm/s]*/
+ int32_t velD; /**< Down velocity component [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 [1e-5 deg] */
+ uint32_t sAcc; /**< Speed accuracy estimate [cm/s] */
+ uint32_t cAcc; /**< Course / Heading accuracy estimate [1e-5 deg] */
+} ubx_payload_rx_nav_velned_t;
+
+/* Rx MON-HW (ubx6) */
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;
-
+ uint32_t pinSel;
+ uint32_t pinBank;
+ uint32_t pinDir;
+ uint32_t pinVal;
+ uint16_t noisePerMS;
+ uint16_t agcCnt;
+ uint8_t aStatus;
+ uint8_t aPower;
+ uint8_t flags;
+ uint8_t reserved1;
+ uint32_t usedMask;
+ uint8_t VP[25];
+ uint8_t jamInd;
+ uint16_t reserved3;
+ uint32_t pinIrq;
+ uint32_t pullH;
+ uint32_t pullL;
+} ubx_payload_rx_mon_hw_ubx6_t;
+
+/* Rx MON-HW (ubx7+) */
+typedef struct {
+ uint32_t pinSel;
+ uint32_t pinBank;
+ uint32_t pinDir;
+ uint32_t pinVal;
+ uint16_t noisePerMS;
+ uint16_t agcCnt;
+ uint8_t aStatus;
+ uint8_t aPower;
+ uint8_t flags;
+ uint8_t reserved1;
+ uint32_t usedMask;
+ uint8_t VP[17];
+ uint8_t jamInd;
+ uint16_t reserved3;
+ uint32_t pinIrq;
+ uint32_t pullH;
+ uint32_t pullL;
+} ubx_payload_rx_mon_hw_ubx7_t;
+
+/* Rx MON-VER Part 1 */
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;
+ uint8_t swVersion[30];
+ uint8_t hwVersion[10];
+} ubx_payload_rx_mon_ver_part1_t;
+/* Rx MON-VER Part 2 (repeated) */
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;
+ uint8_t extension[30];
+} ubx_payload_rx_mon_ver_part2_t;
+
+/* Rx ACK-ACK */
+typedef union {
+ uint16_t msg;
+ struct {
+ uint8_t clsID;
+ uint8_t msgID;
+ };
+} ubx_payload_rx_ack_ack_t;
+
+/* Rx ACK-NAK */
+typedef union {
+ uint16_t msg;
+ struct {
+ uint8_t clsID;
+ uint8_t msgID;
+ };
+} ubx_payload_rx_ack_nak_t;
+
+/* Tx CFG-PRT */
+typedef struct {
+ uint8_t portID;
+ uint8_t reserved0;
+ uint16_t txReady;
+ uint32_t mode;
+ uint32_t baudRate;
+ uint16_t inProtoMask;
+ uint16_t outProtoMask;
+ uint16_t flags;
+ uint16_t reserved5;
+} ubx_payload_tx_cfg_prt_t;
+
+/* Tx CFG-RATE */
+typedef struct {
+ uint16_t measRate; /**< Measurement Rate, GPS measurements are taken every measRate milliseconds */
+ uint16_t navRate; /**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */
+ uint16_t timeRef; /**< Alignment to reference time: 0 = UTC time, 1 = GPS time */
+} ubx_payload_tx_cfg_rate_t;
+/* Tx CFG-NAV5 */
+typedef struct {
+ uint16_t mask;
+ uint8_t dynModel; /**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
+ uint8_t fixMode; /**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */
+ 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;
+ uint8_t cnoThreshNumSVs; /**< (ubx7+ only, else 0) */
+ uint8_t cnoThresh; /**< (ubx7+ only, else 0) */
+ uint16_t reserved;
+ uint16_t staticHoldMaxDist; /**< (ubx8+ only, else 0) */
+ uint8_t utcStandard; /**< (ubx8+ only, else 0) */
+ uint8_t reserved3;
+ uint32_t reserved4;
+} ubx_payload_tx_cfg_nav5_t;
+
+/* Tx CFG-MSG */
typedef struct {
- uint8_t clsID;
- uint8_t msgID;
- uint16_t length;
- uint8_t msgClass_payload;
- uint8_t msgID_payload;
+ union {
+ uint16_t msg;
+ struct {
+ uint8_t msgClass;
+ uint8_t msgID;
+ };
+ };
uint8_t rate;
- uint8_t ck_a;
- uint8_t ck_b;
-} type_gps_bin_cfg_msg_packet_t;
+} ubx_payload_tx_cfg_msg_t;
+
+/* General message and payload buffer union */
+typedef union {
+ ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh;
+ ubx_payload_rx_nav_sol_t payload_rx_nav_sol;
+ ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc;
+ ubx_payload_rx_nav_svinfo_part1_t payload_rx_nav_svinfo_part1;
+ ubx_payload_rx_nav_svinfo_part2_t payload_rx_nav_svinfo_part2;
+ ubx_payload_rx_nav_velned_t payload_rx_nav_velned;
+ ubx_payload_rx_mon_hw_ubx6_t payload_rx_mon_hw_ubx6;
+ ubx_payload_rx_mon_hw_ubx7_t payload_rx_mon_hw_ubx7;
+ ubx_payload_rx_mon_ver_part1_t payload_rx_mon_ver_part1;
+ ubx_payload_rx_mon_ver_part2_t payload_rx_mon_ver_part2;
+ ubx_payload_rx_ack_ack_t payload_rx_ack_ack;
+ ubx_payload_rx_ack_nak_t payload_rx_ack_nak;
+ ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt;
+ ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate;
+ ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5;
+ ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg;
+ uint8_t raw[];
+} ubx_buf_t;
-struct ubx_cfg_msg_rate {
- uint8_t msg_class;
- uint8_t msg_id;
- uint8_t rate;
-};
-
-
-// END the structures of the binary packets
-// ************
+#pragma pack(pop)
+/*** END OF u-blox protocol binary message and payload definitions ***/
+/* Decoder state */
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_SYNC1 = 0,
+ UBX_DECODE_SYNC2,
+ UBX_DECODE_CLASS,
+ UBX_DECODE_ID,
+ UBX_DECODE_LENGTH1,
+ UBX_DECODE_LENGTH2,
+ UBX_DECODE_PAYLOAD,
+ UBX_DECODE_CHKSUM1,
+ UBX_DECODE_CHKSUM2
} ubx_decode_state_t;
-//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
-#pragma pack(pop)
+/* Rx message state */
+typedef enum {
+ UBX_RXMSG_IGNORE = 0,
+ UBX_RXMSG_HANDLE,
+ UBX_RXMSG_DISABLE,
+ UBX_RXMSG_ERROR_LENGTH
+} ubx_rxmsg_state_t;
+
+/* ACK state */
+typedef enum {
+ UBX_ACK_IDLE = 0,
+ UBX_ACK_WAITING,
+ UBX_ACK_GOT_ACK,
+ UBX_ACK_GOT_NAK
+} ubx_ack_state_t;
-#define RECV_BUFFER_SIZE 300 //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(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
~UBX();
- int receive(unsigned timeout);
+ int receive(const unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
- * Parse the binary MTK packet
+ * Parse the binary UBX packet
+ */
+ int parse_char(const uint8_t b);
+
+ /**
+ * Start payload rx
*/
- int parse_char(uint8_t b);
+ int payload_rx_init(void);
/**
- * Handle the package once it has arrived
+ * Add payload rx byte
*/
- int handle_message(void);
+ int payload_rx_add(const uint8_t b);
+ int payload_rx_add_nav_svinfo(const uint8_t b);
+ int payload_rx_add_mon_ver(const uint8_t b);
+
+ /**
+ * Finish payload rx
+ */
+ int payload_rx_done(void);
/**
* Reset the parse state machine for a fresh start
@@ -373,44 +480,52 @@ private:
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
- void add_byte_to_checksum(uint8_t);
+ void add_byte_to_checksum(const uint8_t);
/**
- * Add the two checksum bytes to an outgoing message
+ * Send a message
*/
- void add_checksum_to_message(uint8_t *message, const unsigned length);
+ void send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length);
/**
- * Helper to send a config packet
+ * Configure message rate
*/
- void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
+ void configure_message_rate(const uint16_t msg, const uint8_t rate);
- void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
-
- void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
+ /**
+ * Calculate & add checksum for given buffer
+ */
+ void calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum);
- void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+ /**
+ * Wait for message acknowledge
+ */
+ int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report);
- int wait_for_ack(unsigned timeout);
+ /**
+ * Calculate FNV1 hash
+ */
+ uint32_t fnv1_32_str(uint8_t *str, uint32_t hval);
int _fd;
struct vehicle_gps_position_s *_gps_position;
+ struct satellite_info_s *_satellite_info;
+ bool _enable_sat_info;
bool _configured;
- bool _waiting_for_ack;
+ ubx_ack_state_t _ack_state;
bool _got_posllh;
bool _got_velned;
- bool _got_timeutc;
- uint8_t _message_class_needed;
- uint8_t _message_id_needed;
ubx_decode_state_t _decode_state;
- uint8_t _rx_buffer[RECV_BUFFER_SIZE];
- unsigned _rx_count;
+ uint16_t _rx_msg;
+ ubx_rxmsg_state_t _rx_state;
+ uint16_t _rx_payload_length;
+ uint16_t _rx_payload_index;
uint8_t _rx_ck_a;
uint8_t _rx_ck_b;
- uint8_t _message_class;
- uint8_t _message_id;
- unsigned _payload_size;
hrt_abstime _disable_cmd_last;
+ uint16_t _ack_waiting_msg;
+ ubx_buf_t _buf;
+ uint32_t _ubx_version;
};
#endif /* UBX_H_ */
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 1e779e8dc..086132573 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -226,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.sensor_id = GPS_SENSOR_ID;
msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
- msg.gps_num_sat = gps.satellites_visible;
+ msg.gps_num_sat = gps.satellites_used;
/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index e91b73d1b..667be87b7 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -677,7 +677,7 @@ protected:
cm_uint16_from_m_float(gps.epv),
gps.vel_m_s * 100.0f,
_wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
- gps.satellites_visible);
+ gps.satellites_used);
}
}
};
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index bb977d277..53a1638a3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -751,9 +751,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.vel_ned_valid = true;
hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
- hil_gps.timestamp_satellites = timestamp;
hil_gps.fix_type = gps.fix_type;
- hil_gps.satellites_visible = gps.satellites_visible;
+ hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used?
if (_gps_pub < 0) {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 9b071ff99..39e5b6c41 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -74,6 +74,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/optical_flow.h>
@@ -141,6 +142,8 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
fds[fdsc_count].events = POLLIN; \
fdsc_count++;
+#define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
+
static bool main_thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
@@ -944,6 +947,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct tecs_status_s tecs_status;
struct system_power_s system_power;
struct servorail_status_s servorail_status;
+ struct satellite_info_s sat_info;
struct wind_estimate_s wind_estimate;
} buf;
@@ -1007,6 +1011,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int global_pos_sub;
int triplet_sub;
int gps_pos_sub;
+ int sat_info_sub;
int vicon_pos_sub;
int flow_sub;
int rc_sub;
@@ -1026,6 +1031,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+ subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -1066,11 +1072,14 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
+ /* initialize calculated mean SNR */
+ float snr_mean = 0.0f;
+
/* enable logging on start if needed */
if (log_on_start) {
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
- if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
+ if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
gps_time = buf_gps_pos.time_gps_usec;
}
}
@@ -1128,14 +1137,6 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GPS POSITION - UNIT #1 --- */
if (gps_pos_updated) {
- float snr_mean = 0.0f;
-
- for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) {
- snr_mean += buf_gps_pos.satellite_snr[i];
- }
-
- snr_mean /= buf_gps_pos.satellites_visible;
-
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
@@ -1148,44 +1149,55 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
- log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible;
+ log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used;
log_msg.body.log_GPS.snr_mean = snr_mean;
log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms;
log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator;
LOGBUFFER_WRITE_AND_COUNT(GPS);
+ }
+
+ /* --- SATELLITE INFO - UNIT #1 --- */
+ if (_extended_logging) {
+
+ if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) {
- if (_extended_logging) {
/* log the SNR of each satellite for a detailed view of signal quality */
- unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
+ unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
log_msg.msg_type = LOG_GS0A_MSG;
memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
- /* fill set A */
- for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+ snr_mean = 0.0f;
- int satindex = buf_gps_pos.satellite_prn[i] - 1;
+ /* fill set A and calculate mean SNR */
+ for (unsigned i = 0; i < sat_info_count; i++) {
+
+ snr_mean += buf.sat_info.snr[i];
+
+ int satindex = buf.sat_info.svid[i] - 1;
/* handles index exceeding and wraps to to arithmetic errors */
if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
/* map satellites by their ID so that logs from two receivers can be compared */
- log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+ log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i];
}
}
LOGBUFFER_WRITE_AND_COUNT(GS0A);
+ snr_mean /= sat_info_count;
log_msg.msg_type = LOG_GS0B_MSG;
memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
+
/* fill set B */
- for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+ for (unsigned i = 0; i < sat_info_count; i++) {
/* get second bank of satellites, thus deduct bank size from index */
- int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr;
+ int satindex = buf.sat_info.svid[i] - 1 - log_max_snr;
/* handles index exceeding and wraps to to arithmetic errors */
if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
/* map satellites by their ID so that logs from two receivers can be compared */
- log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+ log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i];
}
}
LOGBUFFER_WRITE_AND_COUNT(GS0B);
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index c1d1a938f..44b6debc7 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -40,6 +40,7 @@
#include "topics/parameter_update.h"
#include "topics/actuator_controls.h"
#include "topics/vehicle_gps_position.h"
+#include "topics/satellite_info.h"
#include "topics/sensor_combined.h"
#include "topics/vehicle_attitude.h"
#include "topics/vehicle_global_position.h"
@@ -88,6 +89,7 @@ T Subscription<T>::getData() {
template class __EXPORT Subscription<parameter_update_s>;
template class __EXPORT Subscription<actuator_controls_s>;
template class __EXPORT Subscription<vehicle_gps_position_s>;
+template class __EXPORT Subscription<satellite_info_s>;
template class __EXPORT Subscription<sensor_combined_s>;
template class __EXPORT Subscription<vehicle_attitude_s>;
template class __EXPORT Subscription<vehicle_global_position_s>;
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 7c3bb0009..687fc1d4a 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -75,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s);
#include "topics/vehicle_gps_position.h"
ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
+#include "topics/satellite_info.h"
+ORB_DEFINE(satellite_info, struct satellite_info_s);
+
#include "topics/home_position.h"
ORB_DEFINE(home_position, struct home_position_s);
diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h
new file mode 100644
index 000000000..37c2faa96
--- /dev/null
+++ b/src/modules/uORB/topics/satellite_info.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.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 satellite_info.h
+ * Definition of the GNSS satellite info uORB topic.
+ */
+
+#ifndef TOPIC_SAT_INFO_H_
+#define TOPIC_SAT_INFO_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * GNSS Satellite Info.
+ */
+
+#define SAT_INFO_MAX_SATELLITES 20
+
+struct satellite_info_s {
+ uint64_t timestamp; /**< Timestamp of satellite info */
+ uint8_t count; /**< Number of satellites in satellite info */
+ uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */
+ uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */
+ uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+ uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+ uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
+};
+
+/**
+ * NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs
+ * u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf
+ *
+ * GPS 1-32
+ * SBAS 120-158
+ * Galileo 211-246
+ * BeiDou 159-163, 33-64
+ * QZSS 193-197
+ * GLONASS 65-96, 255
+ *
+ */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(satellite_info);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index bbacb733a..dbd59f4f3 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -82,14 +82,7 @@ struct vehicle_gps_position_s {
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]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
- bool satellite_info_available; /**< 0 for no info, 1 for info available */
+ uint8_t satellites_used; /**< Number of satellites used */
};
/**
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-06 05:38:33 +0000