gps file dump: re-implement with an uORB topic & write to the log file (#4987)

Drawbacks of the previous method: when writing to the SD card, there are
high delays in the write() call of several 100ms, every now and then. The
frequency and length of these events depend on:
- SD card
- used logger bandwidth
- bandwidth of gps data (RTCM)
Since the whole gps thread was blocked during this period, it lead to
gps timeouts and lost module.

What we do now is: publish an orb topic with queuing. This makes it async
and the logger takes care of buffering. This means it's best to:
- use high logger rate
- use large logger buffer
- reduce logger bandwith by disabling unused topics

msg/CMakeLists.txt

@@ -61,6 +61,7 @@ set(msg_file_names
 	fw_virtual_attitude_setpoint.msg
 	fw_virtual_rates_setpoint.msg
 	geofence_result.msg
+	gps_dump.msg
 	gps_inject_data.msg
 	hil_sensor.msg
 	home_position.msg

msg/gps_dump.msg

@@ -0,0 +1,6 @@
+# This message is used to dump the raw gps communication to the log.
+# Set the parameter GPS_DUMP_COMM to 1 to use this.
+
+uint8 len			# length of data, MSB bit set = message to the gps device,
+				# clear = message from the device
+uint8[79] data			# data to write to the log

src/drivers/gps/gps.cpp

@@ -76,9 +76,9 @@
 #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>
+#include <uORB/topics/gps_dump.h>
 
 #include <board_config.h>
 
@@ -142,16 +142,9 @@ private:
 	int _orb_inject_data_fd[_orb_inject_data_fd_count];
 	int _orb_inject_data_next = 0;
 
-	// dump communication to file
+	orb_advert_t _dump_communication_pub;			///< if non-null, dump communication
-#ifdef __PX4_POSIX_EAGLE
+	gps_dump_s *_dump_to_device;
-	static constexpr const char	*DUMP_ROOT = PX4_ROOTFSDIR;
+	gps_dump_s *_dump_from_device;
-#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
@@ -225,13 +218,14 @@ private:
 	static int callback(GPSCallbackType type, void *data1, int data2, void *user);
 
 	/**
-	 * Setup dumping of the GPS device input and output stream to a file.
+	 * Dump gps communication.
+	 * @param data message
+	 * @param len length of the message
+	 * @param msg_to_gps_device if true, this is a message sent to the gps device, otherwise it's from the device
 	 */
-	void initializeCommunicationDump();
+	void dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device);
 
-	int getDumpFileName(char *file_name, size_t file_name_size, const char *suffix);
+	void initializeCommunicationDump();
-
-	static bool fileExist(const char *filename);
 };
 
 
@@ -264,7 +258,10 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num
 	_rate_rtcm_injection(0.0f),
 	_last_rate_rtcm_injection_count(0),
 	_fake_gps(fake_gps),
-	_gps_num(gps_num)
+	_gps_num(gps_num),
+	_dump_communication_pub(nullptr),
+	_dump_to_device(nullptr),
+	_dump_from_device(nullptr)
 {
 	/* store port name */
 	strncpy(_port, uart_path, sizeof(_port));
@@ -305,6 +302,13 @@ GPS::~GPS()
 		delete(_sat_info);
 	}
 
+	if (_dump_to_device) {
+		delete(_dump_to_device);
+	}
+
+	if (_dump_from_device) {
+		delete(_dump_from_device);
+	}
 
 }
 
@@ -340,21 +344,15 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 	case GPSCallbackType::readDeviceData: {
 			int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
 
-			if (num_read > 0 && gps->_dump_from_gps_device_fd >= 0) {
+			if (num_read > 0) {
-				if (write(gps->_dump_from_gps_device_fd, data1, (size_t)num_read) != (size_t)num_read) {
+				gps->dumpGpsData((uint8_t *)data1, (size_t)num_read, false);
-					PX4_WARN("gps dump failed");
-				}
 			}
 
 			return num_read;
 		}
 
 	case GPSCallbackType::writeDeviceData:
-		if (gps->_dump_to_gps_device_fd >= 0) {
+		gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true);
-			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);
 
@@ -457,11 +455,7 @@ void GPS::handleInjectDataTopic()
 
 bool GPS::injectData(uint8_t *data, size_t len)
 {
-	if (_dump_to_gps_device_fd >= 0) {
+	dumpGpsData(data, len, true);
-		if (write(_dump_to_gps_device_fd, data, len) != len) {
-			PX4_WARN("gps dump failed");
-		}
-	}
 
 	size_t written = ::write(_serial_fd, data, len);
 	::fsync(_serial_fd);
@@ -580,17 +574,6 @@ int GPS::setBaudrate(unsigned baud)
 	return 0;
 }
 
-bool GPS::fileExist(const char *filename)
-{
-#ifdef __PX4_QURT
-	//FIXME: QuRT neither has stat() nor access(). Just pretend file does not exist.
-	return false;
-#else
-	struct stat buffer;
-	return stat(filename, &buffer) == 0;
-#endif
-}
-
 void GPS::initializeCommunicationDump()
 {
 	param_t gps_dump_comm_ph = param_find("GPS_DUMP_COMM");
@@ -604,53 +587,54 @@ void GPS::initializeCommunicationDump()
 		return; //dumping disabled
 	}
 
-	char to_device_file_name[128] = "";
+	_dump_from_device = new gps_dump_s();
-	char from_device_file_name[128] = "";
+	_dump_to_device = new gps_dump_s();
-
-	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 | O_TRUNC, PX4_O_MODE_666)) < 0) {
-		return;
-	}
 
-	if ((_dump_from_gps_device_fd = open(from_device_file_name, O_CREAT | O_WRONLY | O_TRUNC, PX4_O_MODE_666)) < 0) {
+	if (!_dump_from_device || !_dump_to_device) {
-		close(_dump_to_gps_device_fd);
+		PX4_ERR("failed to allocated dump data");
-		_dump_to_gps_device_fd = -1;
 		return;
 	}
 
-	PX4_INFO("Dumping GPS comm to files %s, %s", to_device_file_name, from_device_file_name);
+	memset(_dump_to_device, 0, sizeof(gps_dump_s));
+	memset(_dump_from_device, 0, sizeof(gps_dump_s));
 
-	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int instance;
+	//make sure to use a large enough queue size, so that we don't lose messages. You may also want
+	//to increase the logger rate for that.
+	_dump_communication_pub = orb_advertise_multi_queue(ORB_ID(gps_dump), _dump_from_device, &instance,
+				  ORB_PRIO_DEFAULT, 8);
 }
 
-int GPS::getDumpFileName(char *file_name, size_t file_name_size, const char *suffix)
+void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
 {
-	uint16_t file_number = 1; // start with file gps_dump_<num>_<suffix>_dev_001.dat
+	if (!_dump_communication_pub) {
+		return;
+	}
 
-	const uint16_t max_num_files = 999;
+	gps_dump_s *dump_data = msg_to_gps_device ? _dump_to_device : _dump_from_device;
 
-	while (file_number <= max_num_files) {
+	while (len > 0) {
-		snprintf(file_name, file_name_size, "%s/gps_dump_%u_%s_dev_%03u.dat",
+		size_t write_len = len;
-			 DUMP_ROOT, _gps_num, suffix, file_number);
 
-		if (!fileExist(file_name)) {
+		if (write_len > sizeof(dump_data->data) - dump_data->len) {
-			break;
+			write_len = sizeof(dump_data->data) - dump_data->len;
 		}
 
-		file_number++;
+		memcpy(dump_data->data + dump_data->len, data, write_len);
-	}
+		data += write_len;
+		dump_data->len += write_len;
+		len -= write_len;
 
-	if (file_number > max_num_files) {
+		if (dump_data->len >= sizeof(dump_data->data)) {
-		return -1;
+			if (msg_to_gps_device) {
-	}
+				dump_data->len |= 1 << 7;
+			}
 
-	return 0;
+			dump_data->timestamp = hrt_absolute_time();
+			orb_publish(ORB_ID(gps_dump), _dump_communication_pub, dump_data);
+			dump_data->len = 0;
+		}
+	}
 }
 
 void
@@ -817,32 +801,6 @@ 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) {
@@ -874,26 +832,15 @@ GPS::task_main()
 
 	}
 
-	PX4_WARN("exiting");
+	PX4_INFO("exiting");
 
 	for (size_t i = 0; i < _orb_inject_data_fd_count; ++i) {
 		orb_unsubscribe(_orb_inject_data_fd[i]);
 		_orb_inject_data_fd[i] = -1;
 	}
 
-	if (_dump_to_gps_device_fd != -1) {
+	if (_dump_communication_pub) {
-		close(_dump_to_gps_device_fd);
+		orb_unadvertise(_dump_communication_pub);
-		_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);

src/drivers/gps/params.c

@@ -34,10 +34,8 @@
 /**
  * Dump GPS communication to a file.
  *
- * If this is set to 1, all GPS communication data will be written to a
+ * If this is set to 1, all GPS communication data will be published via uORB,
- * file. Two files will be created, for reading and writing. All
+ * and written to the log file as gps_dump message.
- * communication from startup until device disarm will be dumped.
- *
  * @min 0
  * @max 1
  * @value 0 Disable

src/modules/logger/logger.cpp

@@ -469,6 +469,7 @@ void Logger::add_default_topics()
 	add_topic("servorail_status", 200);
 	add_topic("mc_att_ctrl_status", 50);
 	add_topic("vehicle_status", 200);
+	add_topic("gps_dump"); //this will only be published if GPS_DUMP_COMM is set
 }
 
 int Logger::add_topics_from_file(const char *fname)