From aef78b8aeb5bd196d7856899ea60933806500890 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 12 May 2016 17:31:08 +0200 Subject: [PATCH] gps: add GPS_DUMP_COMM param: if 1, dump all gps communication to a file --- src/drivers/gps/gps.cpp | 152 ++++++++++++++++++++++++++++++++++++++- src/drivers/gps/params.c | 48 +++++++++++++ 2 files changed, 198 insertions(+), 2 deletions(-) create mode 100644 src/drivers/gps/params.c diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index ff359146cb..1088360cdd 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -52,6 +52,7 @@ #include +#include #include #include #include @@ -71,9 +72,11 @@ #include #include #include +#include #include #include #include +#include #include #include @@ -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: - return gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1)); + 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"); + } + } + + 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, ¶m_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___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); diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c new file mode 100644 index 0000000000..9baba96507 --- /dev/null +++ b/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); +