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.cpp170
1 files changed, 112 insertions, 58 deletions
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a736cbdf6..34dd63086 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 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
@@ -56,12 +56,16 @@
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/i2c.h>
+#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/err.h>
#include <drivers/drv_gps.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
+
+#include <board_config.h>
#include "ubx.h"
#include "mtk.h"
@@ -76,16 +80,18 @@
#endif
static const int ERROR = -1;
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
+/* 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();
@@ -103,14 +109,17 @@ private:
int _serial_fd; ///< serial interface to GPS
unsigned _baudrate; ///< current baudrate
char _port[20]; ///< device / serial port path
- volatile int _task; //< worker task
+ 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
@@ -118,7 +127,7 @@ private:
/**
* Try to configure the GPS, handle outgoing communication to the GPS
*/
- void config();
+ void config();
/**
* Trampoline to the worker task
@@ -157,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)
{
@@ -175,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;
}
@@ -209,7 +228,8 @@ GPS::init()
goto out;
/* start the GPS driver worker task */
- _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+ _task = task_spawn_cmd("gps", SCHED_DEFAULT,
+ SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) {
warnx("task start failed: %d", errno);
@@ -232,6 +252,11 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCRESET:
cmd_reset();
break;
+
+ default:
+ /* give it to parent if no one wants it */
+ ret = CDev::ioctl(filp, cmd, arg);
+ break;
}
unlock();
@@ -268,33 +293,32 @@ 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)400e3f;
- _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 = 10.0f;
- _report.epv_m = 10.0f;
- _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.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);
}
}
@@ -310,11 +334,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:
@@ -329,20 +353,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_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_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+ } 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) {
@@ -354,7 +391,7 @@ GPS::task_main()
}
if (!_healthy) {
- char *mode_str = "unknown";
+ const char *mode_str = "unknown";
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
@@ -416,7 +453,14 @@ GPS::task_main()
void
GPS::cmd_reset()
{
- //XXX add reset?
+#ifdef GPIO_GPS_NRESET
+ warnx("Toggling GPS reset pin");
+ stm32_configgpio(GPIO_GPS_NRESET);
+ stm32_gpiowrite(GPIO_GPS_NRESET, 0);
+ usleep(100);
+ stm32_gpiowrite(GPIO_GPS_NRESET, 1);
+ warnx("Toggled GPS reset pin");
+#endif
}
void
@@ -436,12 +480,15 @@ 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: %fms ago", (int)_report.fix_type,
- _report.satellites_visible, (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", _report.eph_m, _report.epv_m);
+ warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
+
+ 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("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
+ (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
+ 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);
@@ -459,7 +506,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();
@@ -469,7 +516,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;
@@ -477,7 +524,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;
@@ -568,8 +615,9 @@ 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;
+ bool enable_sat_info = false;
/*
* Start/load the driver.
@@ -591,7 +639,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"))
@@ -616,5 +670,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-06-12 07:51:28 +0000