diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index dedd4983ac..5a94a5ba54 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/gps_dump.msg b/msg/gps_dump.msg new file mode 100644 index 0000000000..8a91149d46 --- /dev/null +++ b/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 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6d9d0f07c8..bd3eb10807 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -76,9 +76,9 @@ #include #include #include -#include #include #include +#include #include @@ -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 -#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) + orb_advert_t _dump_communication_pub; ///< if non-null, dump communication + gps_dump_s *_dump_to_device; + gps_dump_s *_dump_from_device; /** * 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 dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device); + void initializeCommunicationDump(); - - int getDumpFileName(char *file_name, size_t file_name_size, const char *suffix); - - 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 (write(gps->_dump_from_gps_device_fd, data1, (size_t)num_read) != (size_t)num_read) { - PX4_WARN("gps dump failed"); - } + if (num_read > 0) { + gps->dumpGpsData((uint8_t *)data1, (size_t)num_read, false); } 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"); - } - } + gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true); 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) { - if (write(_dump_to_gps_device_fd, data, len) != len) { - PX4_WARN("gps dump failed"); - } - } + dumpGpsData(data, len, true); 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] = ""; - char from_device_file_name[128] = ""; + _dump_from_device = new gps_dump_s(); + _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"); + if (!_dump_from_device || !_dump_to_device) { + PX4_ERR("failed to allocated dump data"); 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; - } + memset(_dump_to_device, 0, sizeof(gps_dump_s)); + memset(_dump_from_device, 0, sizeof(gps_dump_s)); - if ((_dump_from_gps_device_fd = open(from_device_file_name, O_CREAT | O_WRONLY | O_TRUNC, 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 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___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) { - snprintf(file_name, file_name_size, "%s/gps_dump_%u_%s_dev_%03u.dat", - DUMP_ROOT, _gps_num, suffix, file_number); + while (len > 0) { + size_t write_len = len; - if (!fileExist(file_name)) { - break; + if (write_len > sizeof(dump_data->data) - dump_data->len) { + 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) { - return -1; - } + if (dump_data->len >= sizeof(dump_data->data)) { + 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) { - 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; + if (_dump_communication_pub) { + orb_unadvertise(_dump_communication_pub); } ::close(_serial_fd); diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c index 9baba96507..75f4c3dc22 100644 --- a/src/drivers/gps/params.c +++ b/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 - * file. Two files will be created, for reading and writing. All - * communication from startup until device disarm will be dumped. - * + * If this is set to 1, all GPS communication data will be published via uORB, + * and written to the log file as gps_dump message. * @min 0 * @max 1 * @value 0 Disable diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 609333011f..4412809f79 100644 --- a/src/modules/logger/logger.cpp +++ b/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)