aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/gps/gps.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/gps/gps.cpp')
-rw-r--r--src/drivers/gps/gps.cpp98
1 files changed, 58 insertions, 40 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;
/*
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-26 18:14:10 +0000