gps: add GPS_DUMP_COMM param: if 1, dump all gps communication to a file

src/drivers/gps/gps.cpp

@@ -52,6 +52,7 @@
 
 
 #include <fcntl.h>
+#include <sys/stat.h>
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdio.h>
@@ -71,9 +72,11 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/scheduling_priorities.h>
 #include <systemlib/err.h>
+#include <systemlib/param/param.h>
 #include <drivers/drv_gps.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/gps_inject_data.h>
 
@@ -139,6 +142,17 @@ private:
 	int _orb_inject_data_fd[_orb_inject_data_fd_count];
 	int _orb_inject_data_next = 0;
 
+	// dump communication to file
+#ifdef __PX4_POSIX_EAGLE
+	static constexpr const char	*DUMP_ROOT = PX4_ROOTFSDIR;
+#else
+	static constexpr const char 	*DUMP_ROOT = PX4_ROOTFSDIR"/fs/microsd";
+#endif
+	int _dump_to_gps_device_fd = -1;              ///< file descriptors, to safe all device communication to a file
+	int _dump_from_gps_device_fd = -1;
+	int _vehicle_status_sub = -1;
+	bool _is_armed = false;                       ///< current arming state (only updated if communication dump is enabled)
+
 	/**
 	 * Try to configure the GPS, handle outgoing communication to the GPS
 	 */
@@ -209,6 +223,15 @@ private:
 	 * callback from the driver for the platform specific stuff
 	 */
 	static int callback(GPSCallbackType type, void *data1, int data2, void *user);
+
+	/**
+	 * Setup dumping of the GPS device input and output stream to a file.
+	 */
+	void initializeCommunicationDump();
+
+	int getDumpFileName(char *file_name, size_t file_name_size, const char *suffix);
+
+	static bool fileExist(const char *filename);
 };
 
 
@@ -314,10 +337,25 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 	GPS *gps = (GPS *)user;
 
 	switch (type) {
-	case GPSCallbackType::readDeviceData:
+	case GPSCallbackType::readDeviceData: {
-		return gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
+			int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
+
+			if (num_read > 0 && gps->_dump_from_gps_device_fd >= 0) {
+				if (write(gps->_dump_from_gps_device_fd, data1, (size_t)num_read) != (size_t)num_read) {
+					PX4_WARN("gps dump failed");
+				}
+			}
+
+			return num_read;
+		}
 
 	case GPSCallbackType::writeDeviceData:
+		if (gps->_dump_to_gps_device_fd >= 0) {
+			if (write(gps->_dump_to_gps_device_fd, data1, (size_t)data2) != (size_t)data2) {
+				PX4_WARN("gps dump failed");
+			}
+		}
+
 		return write(gps->_serial_fd, data1, (size_t)data2);
 
 	case GPSCallbackType::setBaudrate:
@@ -529,6 +567,73 @@ int GPS::setBaudrate(unsigned baud)
 	return 0;
 }
 
+bool GPS::fileExist(const char *filename)
+{
+	struct stat buffer;
+	return stat(filename, &buffer) == 0;
+}
+
+void GPS::initializeCommunicationDump()
+{
+	param_t gps_dump_comm_ph = param_find("GPS_DUMP_COMM");
+	int32_t param_dump_comm;
+
+	if (gps_dump_comm_ph == PARAM_INVALID || param_get(gps_dump_comm_ph, &param_dump_comm) != 0) {
+		return;
+	}
+
+	if (param_dump_comm != 1) {
+		return; //dumping disabled
+	}
+
+	char to_device_file_name[128] = "";
+	char from_device_file_name[128] = "";
+
+	if (getDumpFileName(to_device_file_name, sizeof(to_device_file_name), "to")
+	    || getDumpFileName(from_device_file_name, sizeof(from_device_file_name), "from")) {
+		PX4_ERR("Failed to get GPS dump file name");
+		return;
+	}
+
+	//open files
+	if ((_dump_to_gps_device_fd = open(to_device_file_name, O_CREAT | O_WRONLY, PX4_O_MODE_666)) < 0) {
+		return;
+	}
+
+	if ((_dump_from_gps_device_fd = open(from_device_file_name, O_CREAT | O_WRONLY, PX4_O_MODE_666)) < 0) {
+		close(_dump_to_gps_device_fd);
+		_dump_to_gps_device_fd = -1;
+		return;
+	}
+
+	PX4_INFO("Dumping GPS comm to files %s, %s", to_device_file_name, from_device_file_name);
+
+	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+}
+
+int GPS::getDumpFileName(char *file_name, size_t file_name_size, const char *suffix)
+{
+	uint16_t file_number = 1; // start with file gps_dump_<num>_<suffix>_dev_001.dat
+
+	const uint16_t max_num_files = 999;
+
+	while (file_number <= max_num_files) {
+		snprintf(file_name, file_name_size, "%s/gps_dump_%u_%s_dev_%03u.dat",
+			 DUMP_ROOT, _gps_num, suffix, file_number);
+
+		if (!fileExist(file_name)) {
+			break;
+		}
+
+		file_number++;
+	}
+
+	if (file_number > max_num_files) {
+		return -1;
+	}
+
+	return 0;
+}
 
 void
 GPS::task_main()
@@ -555,6 +660,8 @@ GPS::task_main()
 		_orb_inject_data_fd[i] = orb_subscribe_multi(ORB_ID(gps_inject_data), i);
 	}
 
+	initializeCommunicationDump();
+
 	uint64_t last_rate_measurement = hrt_absolute_time();
 	unsigned last_rate_count = 0;
 
@@ -697,6 +804,32 @@ GPS::task_main()
 //						PX4_WARN("module found: %s", mode_str);
 						_healthy = true;
 					}
+
+					//check for disarming
+					if (_vehicle_status_sub != -1 && _dump_from_gps_device_fd != -1) {
+						bool updated;
+						orb_check(_vehicle_status_sub, &updated);
+
+						if (updated) {
+							vehicle_status_s vehicle_status;
+							orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status);
+							bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
+								     (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
+
+							if (armed) {
+								_is_armed = true;
+
+							} else if (_is_armed) {
+								//disable communication dump when disarming
+								close(_dump_from_gps_device_fd);
+								_dump_from_gps_device_fd = -1;
+								close(_dump_to_gps_device_fd);
+								_dump_to_gps_device_fd = -1;
+								_is_armed = false;
+							}
+
+						}
+					}
 				}
 
 				if (_healthy) {
@@ -735,6 +868,21 @@ GPS::task_main()
 		_orb_inject_data_fd[i] = -1;
 	}
 
+	if (_dump_to_gps_device_fd != -1) {
+		close(_dump_to_gps_device_fd);
+		_dump_to_gps_device_fd = -1;
+	}
+
+	if (_dump_from_gps_device_fd != -1) {
+		close(_dump_from_gps_device_fd);
+		_dump_from_gps_device_fd = -1;
+	}
+
+	if (_vehicle_status_sub != -1) {
+		orb_unsubscribe(_vehicle_status_sub);
+		_vehicle_status_sub = -1;
+	}
+
 	::close(_serial_fd);
 
 	orb_unadvertise(_report_gps_pos_pub);

src/drivers/gps/params.c

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Dump GPS communication to a file.
+ *
+ * If this is set to 1, all GPS communication data will be written to a
+ * file. Two files will be created, for reading and writing. All
+ * communication from startup until device disarm will be dumped.
+ *
+ * @min 0
+ * @max 1
+ * @value 0 Disable
+ * @value 1 Enable
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0);
+