aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/gps/gps.cpp98
-rw-r--r--src/drivers/gps/ubx.cpp119
-rw-r--r--src/drivers/gps/ubx.h10
3 files changed, 116 insertions, 111 deletions
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index d2ffc30ba..d8e2bd797 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>
@@ -106,8 +107,10 @@ private:
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
+ 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 _report_sat_info; ///< 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
@@ -161,7 +164,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) :
_mode_changed(false),
_mode(GPS_DRIVER_MODE_UBX),
_Helper(nullptr),
- _report_pub(-1),
+ _report_gps_pos_pub(-1),
+ _report_sat_info_pub(-1),
_rate(0.0f),
_fake_gps(fake_gps)
{
@@ -172,7 +176,8 @@ 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));
+ memset(&_report_sat_info, 0, sizeof(_report_sat_info));
_debug_enabled = true;
}
@@ -271,33 +276,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_m = 0.9f;
- _report.epv_m = 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_m = 0.9f;
+ _report_gps_pos.epv_m = 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 +318,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, &_report_sat_info, UBX_ENABLE_NAV_SVINFO);
break;
case GPS_DRIVER_MODE_MTK:
- _Helper = new MTK(_serial_fd, &_report);
+ _Helper = new MTK(_serial_fd, &_report_gps_pos);
break;
default:
@@ -332,20 +337,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 (helper_ret & 2) {
+ if (_report_sat_info_pub > 0) {
+ orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, &_report_sat_info);
+
+ } else {
+ _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), &_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) {
@@ -357,7 +375,7 @@ GPS::task_main()
}
if (!_healthy) {
- char *mode_str = "unknown";
+ const char *mode_str = "unknown";
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
@@ -447,11 +465,11 @@ GPS::print_info()
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
- if (_report.timestamp_position != 0) {
- warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
- _report.satellites_used, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
- warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
- warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m);
+ 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_m, (double)_report_gps_pos.epv_m);
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);
@@ -578,7 +596,7 @@ gps_main(int argc, char *argv[])
{
/* set to default */
- char *device_name = GPS_DEFAULT_UART_PORT;
+ const char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false;
/*
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 462875b6c..dd47572b4 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -65,9 +65,13 @@
#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
-UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
+#define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
+
+UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info) :
_fd(fd),
_gps_position(gps_position),
+ _satellite_info(satellite_info),
+ _enable_sat_info(enable_sat_info),
_configured(false),
_waiting_for_ack(false),
_disable_cmd_last(0)
@@ -84,14 +88,19 @@ 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();
+ wait_for_ack(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
*/
@@ -107,7 +116,7 @@ UBX::configure(unsigned &baudrate)
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.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;
@@ -125,7 +134,7 @@ UBX::configure(unsigned &baudrate)
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.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;
@@ -144,7 +153,7 @@ UBX::configure(unsigned &baudrate)
break;
}
- if (baud_i >= 5) {
+ if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
return 1;
}
@@ -220,14 +229,14 @@ UBX::configure(unsigned &baudrate)
return 1;
}
-#ifdef UBX_ENABLE_NAV_SVINFO
- configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
+ if (_enable_sat_info) {
+ configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
- if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("MSG CFG FAIL: NAV SVINFO");
- return 1;
+ if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+ warnx("MSG CFG FAIL: NAV SVINFO");
+ return 1;
+ }
}
-#endif
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
@@ -244,14 +253,13 @@ int
UBX::wait_for_ack(unsigned timeout)
{
_waiting_for_ack = true;
- uint64_t time_started = hrt_absolute_time();
+ uint64_t time_end = hrt_absolute_time() + timeout * 1000;
- while (hrt_absolute_time() < time_started + timeout * 1000) {
+ while ((timeout = (time_end - hrt_absolute_time()) / 1000) > 0) {
if (receive(timeout) > 0) {
if (!_waiting_for_ack) {
return 1;
}
-
} else {
return -1; // timeout or error receiving, or NAK
}
@@ -275,7 +283,7 @@ UBX::receive(unsigned timeout)
ssize_t count = 0;
- bool handled = false;
+ int handled = 0;
while (true) {
@@ -290,7 +298,7 @@ UBX::receive(unsigned timeout)
} else if (ret == 0) {
/* return success after short delay after receiving a packet or timeout after long delay */
if (handled) {
- return 1;
+ return handled;
} else {
return -1;
@@ -311,8 +319,7 @@ 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 |= handle_message();
}
}
}
@@ -334,6 +341,7 @@ UBX::parse_char(uint8_t b)
case UBX_DECODE_UNINIT:
if (b == UBX_SYNC1) {
_decode_state = UBX_DECODE_GOT_SYNC1;
+ //### printf("\nA");
}
break;
@@ -342,11 +350,13 @@ UBX::parse_char(uint8_t b)
case UBX_DECODE_GOT_SYNC1:
if (b == UBX_SYNC2) {
_decode_state = UBX_DECODE_GOT_SYNC2;
+ //### printf("B");
} else {
/* Second start symbol was wrong, reset state machine */
decode_init();
/* don't return error, it can be just false sync1 */
+ //### printf("-");
}
break;
@@ -357,24 +367,28 @@ UBX::parse_char(uint8_t b)
add_byte_to_checksum(b);
_message_class = b;
_decode_state = UBX_DECODE_GOT_CLASS;
+ //### printf("C");
break;
case UBX_DECODE_GOT_CLASS:
add_byte_to_checksum(b);
_message_id = b;
_decode_state = UBX_DECODE_GOT_MESSAGEID;
+ //### printf("D");
break;
case UBX_DECODE_GOT_MESSAGEID:
add_byte_to_checksum(b);
_payload_size = b; //this is the first length byte
_decode_state = UBX_DECODE_GOT_LENGTH1;
+ //### printf("E");
break;
case UBX_DECODE_GOT_LENGTH1:
add_byte_to_checksum(b);
_payload_size += b << 8; // here comes the second byte of length
_decode_state = UBX_DECODE_GOT_LENGTH2;
+ //### printf("F");
break;
case UBX_DECODE_GOT_LENGTH2:
@@ -389,6 +403,7 @@ UBX::parse_char(uint8_t b)
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]) {
+ //### printf("O\n");
return 1; // message received successfully
} else {
@@ -399,6 +414,7 @@ UBX::parse_char(uint8_t b)
} else if (_rx_count < RECV_BUFFER_SIZE) {
_rx_count++;
+ //### printf(".");
} else {
warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size);
@@ -489,65 +505,37 @@ UBX::handle_message()
break;
}
-#ifdef UBX_ENABLE_NAV_SVINFO
case UBX_MESSAGE_NAV_SVINFO: {
//printf("GOT NAV_SVINFO\n");
+ if (!_enable_sat_info) { // if satellite info processing not enabled:
+ break; // ignore and disable NAV-SVINFO message
+ }
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]);
+ for (_satellite_info->count = 0;
+ _satellite_info->count < MIN(packet_part1->numCh, sizeof(_satellite_info->snr) / sizeof(_satellite_info->snr[0]));
+ _satellite_info->count++) {
+ /* set pointer to satellite_i information */
+ packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + _satellite_info->count* 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;
+ _satellite_info->used[_satellite_info->count] = packet_part2->flags & 0x01;
+ _satellite_info->snr[_satellite_info->count] = packet_part2->cno;
+ _satellite_info->elevation[_satellite_info->count] = (uint8_t)(packet_part2->elev);
+ _satellite_info->azimuth[_satellite_info->count] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
+ _satellite_info->svid[_satellite_info->count] = packet_part2->svid;
+ //printf("SAT %d: %d %d %d %d\n", _satellite_info->count, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
}
- /* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */
- /* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones
- /* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */
+ _satellite_info->timestamp = hrt_absolute_time();
- 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;
+ ret = 2;
break;
}
-#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */
case UBX_MESSAGE_NAV_VELNED: {
// printf("GOT NAV_VELNED\n");
@@ -650,7 +638,6 @@ UBX::handle_message()
warnx("ubx: not acknowledged");
/* configuration obviously not successful */
_waiting_for_ack = false;
- ret = -1;
break;
}
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index b844c315e..90aeffedc 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -48,9 +48,7 @@
#include "gps_helper.h"
-/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */
-/* TODO: make this a command line option, allocate buffer dynamically */
-#undef UBX_ENABLE_NAV_SVINFO
+#define UBX_ENABLE_NAV_SVINFO 1 /* TODO: make this a command line option, allocate buffer(s) dynamically */
#define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62
@@ -378,7 +376,7 @@ typedef enum {
/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */
/* TODO: make this a command line option, allocate buffer dynamically */
#define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */
-#ifdef UBX_ENABLE_NAV_SVINFO
+#if (UBX_ENABLE_NAV_SVINFO != 0)
#define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD)
#else
#define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD)
@@ -387,7 +385,7 @@ typedef enum {
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, const bool &enable_sat_info);
~UBX();
int receive(unsigned timeout);
int configure(unsigned &baudrate);
@@ -434,6 +432,8 @@ private:
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;
uint8_t _message_class_needed;
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-10 07:29:08 +0000