From 9782aecc73b45ae72ce01e507b1535c43f3780b6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Matej=20Fran=C4=8De=C5=A1kin?= <Matej.Franceskin@gmail.com>
Date: Tue, 19 Feb 2019 14:12:04 +0100
Subject: [PATCH] Added GPS reset command
---
src/drivers/gps/devices | 2 +-
src/drivers/gps/gps.cpp | 93 ++++++++++++++++++++++++++++++++++++++---
2 files changed, 89 insertions(+), 6 deletions(-)
diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices
index a6687961ec..a4999f111d 160000
--- a/src/drivers/gps/devices
+++ b/src/drivers/gps/devices
@@ -1 +1 @@
-Subproject commit a6687961ec328a1694916618f9160eaed555d4be
+Subproject commit a4999f111d13bcb209754823a3c503fa659a0d15
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 8fac347d82..746e5fd740 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013-2019 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
@@ -155,8 +155,17 @@ public:
*/
int print_status() override;
-private:
+ /**
+ * Schedule reset of the GPS device
+ */
+ void schedule_reset(GPSRestartType restart_type);
+
+ /**
+ * Reset device if reset was scheduled
+ */
+ void reset_if_scheduled();
+private:
int _serial_fd{-1}; ///< serial interface to GPS
unsigned _baudrate{0}; ///< current baudrate
const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect)
@@ -199,6 +208,8 @@ private:
static volatile GPS *_secondary_instance;
+ volatile GPSRestartType _scheduled_reset{GPSRestartType::None};
+
/**
* Publish the gps struct
*/
@@ -759,6 +770,8 @@ GPS::run()
publishSatelliteInfo();
}
+ reset_if_scheduled();
+
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
@@ -925,6 +938,38 @@ GPS::print_status()
return 0;
}
+void
+GPS::schedule_reset(GPSRestartType restart_type)
+{
+ _scheduled_reset = restart_type;
+
+ if (_instance == Instance::Main && _secondary_instance) {
+ GPS *secondary_instance = (GPS *)_secondary_instance;
+ secondary_instance->schedule_reset(restart_type);
+ }
+}
+
+void
+GPS::reset_if_scheduled()
+{
+ GPSRestartType restart_type = _scheduled_reset;
+
+ if (restart_type != GPSRestartType::None) {
+ _scheduled_reset = GPSRestartType::None;
+ int res = _helper->reset(restart_type);
+
+ if (res == -1) {
+ PX4_INFO("Reset is not supported on this device.");
+
+ } else if (res < 0) {
+ PX4_INFO("Reset failed.");
+
+ } else {
+ PX4_INFO("Reset succeeded.");
+ }
+ }
+}
+
void
GPS::publish()
{
@@ -950,9 +995,41 @@ GPS::publishSatelliteInfo()
}
}
-int GPS::custom_command(int argc, char *argv[])
+int
+GPS::custom_command(int argc, char *argv[])
{
- return print_usage("unknown command");
+ // Check if the driver is running.
+ if (!is_running()) {
+ PX4_INFO("not running");
+ return PX4_ERROR;
+ }
+
+ GPS *_instance = get_instance();
+
+ bool res = false;
+
+ if (argc == 2 && !strcmp(argv[0], "reset")) {
+
+ if (!strcmp(argv[1], "hot")) {
+ res = true;
+ _instance->schedule_reset(GPSRestartType::Hot);
+
+ } else if (!strcmp(argv[1], "cold")) {
+ res = true;
+ _instance->schedule_reset(GPSRestartType::Cold);
+
+ } else if (!strcmp(argv[1], "warm")) {
+ res = true;
+ _instance->schedule_reset(GPSRestartType::Warm);
+ }
+ }
+
+ if (res) {
+ PX4_INFO("Resetting GPS - %s", argv[1]);
+ return 0;
+ }
+
+ return (res) ? 0 : print_usage("unknown command");
}
int GPS::print_usage(const char *reason)
@@ -979,8 +1056,12 @@ so that they can be used in other projects as well (eg. QGroundControl uses them
For testing it can be useful to fake a GPS signal (it will signal the system that it has a valid position):
$ gps stop
$ gps start -f
+
Starting 2 GPS devices (the main GPS on /dev/ttyS3 and the secondary on /dev/ttyS4):
-gps start -d /dev/ttyS3 -e /dev/ttyS4
+$ gps start -d /dev/ttyS3 -e /dev/ttyS4
+
+Initiate warm restart of GPS device
+$ gps reset warm
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("gps", "driver");
@@ -997,6 +1078,8 @@ gps start -d /dev/ttyS3 -e /dev/ttyS4
PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml", "GPS Protocol (default=auto select)", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+ PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset GPS device");
+ PRINT_MODULE_USAGE_ARG("cold|warm|hot", "Specify reset type", false);
return 0;
}
--
GitLab