fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 21:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

gps: fixed code style, more informative and clean messages

Browse Source
This commit is contained in:
Anton Babushkin
2013-09-21 08:59:04 +02:00
parent cd8854e622
commit 8d6a47546a
6 changed files with 142 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/gps.cpp
+78 -38
View File
@@ -85,7 +85,7 @@ static const int ERROR = -1;
class GPS : public device::CDev
{
public:
GPS(const char* uart_path);
GPS(const char *uart_path);
virtual ~GPS();
virtual int init();
@@ -156,7 +156,7 @@ GPS *g_dev;
}
GPS::GPS(const char* uart_path) :
GPS::GPS(const char *uart_path) :
CDev("gps", GPS_DEVICE_PATH),
_task_should_exit(false),
_healthy(false),
@@ -192,6 +192,7 @@ GPS::~GPS()
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
task_delete(_task);
g_dev = nullptr;
}
@@ -270,19 +271,24 @@ GPS::task_main()
}
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_NMEA:
//_Helper = new NMEA(); //TODO: add NMEA
break;
default:
break;
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_NMEA:
//_Helper = new NMEA(); //TODO: add NMEA
break;
default:
break;
}
unlock();
if (_Helper->configure(_baudrate) == 0) {
unlock();
@@ -294,6 +300,7 @@ GPS::task_main()
/* opportunistic publishing - else invalid data would end up on the bus */
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
@@ -310,10 +317,30 @@ GPS::task_main()
}
if (!_healthy) {
warnx("module found");
char *mode_str = "unknown";
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
mode_str = "ubx";
break;
case GPS_DRIVER_MODE_MTK:
mode_str = "mtk";
break;
case GPS_DRIVER_MODE_NMEA:
mode_str = "nmea";
break;
default:
break;
}
warnx("module found: %s", mode_str);
_healthy = true;
}
}
if (_healthy) {
warnx("module lost");
_healthy = false;
@@ -322,24 +349,28 @@ GPS::task_main()
lock();
}
lock();
/* select next mode */
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_UBX;
break;
// case GPS_DRIVER_MODE_NMEA:
// _mode = GPS_DRIVER_MODE_UBX;
// break;
default:
break;
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_UBX;
break;
// case GPS_DRIVER_MODE_NMEA:
// _mode = GPS_DRIVER_MODE_UBX;
// break;
default:
break;
}
}
debug("exiting");
::close(_serial_fd);
@@ -361,22 +392,27 @@ void
GPS::print_info()
{
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX");
break;
case GPS_DRIVER_MODE_MTK:
warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_NMEA:
warnx("protocol: NMEA");
break;
default:
break;
case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX");
break;
case GPS_DRIVER_MODE_MTK:
warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_NMEA:
warnx("protocol: NMEA");
break;
default:
break;
}
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
if (_report.timestamp_position != 0) {
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
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());
@@ -428,6 +464,7 @@ start(const char *uart_path)
errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
goto fail;
}
exit(0);
fail:
@@ -503,7 +540,7 @@ gps_main(int argc, char *argv[])
{
/* set to default */
char* device_name = GPS_DEFAULT_UART_PORT;
char *device_name = GPS_DEFAULT_UART_PORT;
/*
* Start/load the driver.
@@ -513,15 +550,18 @@ gps_main(int argc, char *argv[])
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();
/*
* Test the driver/device.
*/