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.cpp137
1 files changed, 90 insertions, 47 deletions
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]");
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-03 17:01:18 +0000