forked from Archive/PX4-Autopilot
Added option to select port name, minor tweaks to status printing, sacrificied 20 bytes for better status / user debuggability
This commit is contained in:
Lorenz Meier
parent
53c11f87cb
commit
368ba0056f
|
|
@ -42,6 +42,7 @@
|
|||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <semaphore.h>
|
||||
|
|
@ -93,7 +94,7 @@ static const int ERROR = -1;
|
|||
class GPS : public device::CDev
|
||||
{
|
||||
public:
|
||||
GPS();
|
||||
GPS(const char* uart_path);
|
||||
~GPS();
|
||||
|
||||
virtual int init();
|
||||
|
|
@ -110,13 +111,14 @@ private:
|
|||
bool _task_should_exit; ///< flag to make the main worker task exit
|
||||
int _serial_fd; ///< serial interface to GPS
|
||||
unsigned _baudrate; ///< current baudrate
|
||||
char _port[20]; ///< device / serial port path
|
||||
const unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to
|
||||
volatile int _task; ///< worker task
|
||||
volatile int _task; //< worker task
|
||||
bool _config_needed; ///< flag to signal that configuration of GPS is needed
|
||||
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; ///< Class for a GPS interface
|
||||
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
|
||||
float _rate; ///< position update rate
|
||||
|
|
@ -164,7 +166,7 @@ GPS *g_dev;
|
|||
}
|
||||
|
||||
|
||||
GPS::GPS() :
|
||||
GPS::GPS(const char* uart_path) :
|
||||
CDev("gps", GPS_DEVICE_PATH),
|
||||
_task_should_exit(false),
|
||||
_baudrates_to_try({9600, 38400, 57600, 115200}),
|
||||
|
|
@ -176,6 +178,11 @@ GPS::GPS() :
|
|||
_report_pub(-1),
|
||||
_rate(0.0f)
|
||||
{
|
||||
/* store port name */
|
||||
strncpy(_port, uart_path, sizeof(_port));
|
||||
/* enforce null termination */
|
||||
_port[sizeof(_port) - 1] = '\0';
|
||||
|
||||
/* we need this potentially before it could be set in task_main */
|
||||
g_dev = this;
|
||||
memset(&_report, 0, sizeof(_report));
|
||||
|
|
@ -198,6 +205,7 @@ GPS::~GPS()
|
|||
if (_task != -1)
|
||||
task_delete(_task);
|
||||
g_dev = nullptr;
|
||||
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -302,13 +310,13 @@ GPS::task_main()
|
|||
log("starting");
|
||||
|
||||
/* open the serial port */
|
||||
_serial_fd = ::open("/dev/ttyS3", O_RDWR); //TODO make the device dynamic depending on startup parameters
|
||||
_serial_fd = ::open(_port, O_RDWR); //TODO make the device dynamic depending on startup parameters
|
||||
|
||||
/* buffer to read from the serial port */
|
||||
uint8_t buf[32];
|
||||
|
||||
if (_serial_fd < 0) {
|
||||
log("failed to open serial port: %d", errno);
|
||||
log("failed to open serial port: %s err: %d", _port, errno);
|
||||
/* tell the dtor that we are exiting, set error code */
|
||||
_task = -1;
|
||||
_exit(1);
|
||||
|
|
@ -318,7 +326,6 @@ GPS::task_main()
|
|||
pollfd fds[1];
|
||||
fds[0].fd = _serial_fd;
|
||||
fds[0].events = POLLIN;
|
||||
debug("ready");
|
||||
|
||||
/* lock against the ioctl handler */
|
||||
lock();
|
||||
|
|
@ -418,14 +425,12 @@ GPS::task_main()
|
|||
/* This means something went wrong in the parser, let's reconfigure */
|
||||
if (!_config_needed) {
|
||||
_config_needed = true;
|
||||
debug("Lost GPS module");
|
||||
}
|
||||
config();
|
||||
} else if (ret_parse > 0) {
|
||||
/* Looks like we got a valid position update, stop configuring and publish it */
|
||||
if (_config_needed) {
|
||||
_config_needed = false;
|
||||
debug("Found GPS module");
|
||||
}
|
||||
|
||||
/* opportunistic publishing - else invalid data would end up on the bus */
|
||||
|
|
@ -530,7 +535,7 @@ GPS::print_info()
|
|||
default:
|
||||
break;
|
||||
}
|
||||
warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK");
|
||||
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK");
|
||||
if (_report.timestamp != 0) {
|
||||
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
|
||||
(double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f));
|
||||
|
|
@ -549,7 +554,7 @@ namespace gps
|
|||
|
||||
GPS *g_dev;
|
||||
|
||||
void start();
|
||||
void start(const char *uart_path);
|
||||
void stop();
|
||||
void test();
|
||||
void reset();
|
||||
|
|
@ -559,7 +564,7 @@ void info();
|
|||
* Start the driver.
|
||||
*/
|
||||
void
|
||||
start()
|
||||
start(const char *uart_path)
|
||||
{
|
||||
int fd;
|
||||
|
||||
|
|
@ -567,7 +572,7 @@ start()
|
|||
errx(1, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new GPS;
|
||||
g_dev = new GPS(uart_path);
|
||||
|
||||
if (g_dev == nullptr)
|
||||
goto fail;
|
||||
|
|
@ -644,7 +649,6 @@ info()
|
|||
if (g_dev == nullptr)
|
||||
errx(1, "driver not running");
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
||||
exit(0);
|
||||
|
|
@ -656,11 +660,24 @@ info()
|
|||
int
|
||||
gps_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
/* set to default */
|
||||
char* device_name = "/dev/ttyS3";
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "start"))
|
||||
gps::start();
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
/* work around getopt unreliability */
|
||||
if (argc > 3) {
|
||||
if (!strcmp(argv[2], "-d")) {
|
||||
device_name = argv[3];
|
||||
} else {
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
gps::start(device_name);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
gps::stop();
|
||||
|
|
@ -682,5 +699,6 @@ gps_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "status"))
|
||||
gps::info();
|
||||
|
||||
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'");
|
||||
out:
|
||||
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue