dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
Beat Küng 2016-07-06 09:32:37 +02:00 committed by Lorenz Meier
parent 7e883809e3
commit 09ecc84cc7
5 changed files with 71 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -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

6
msg/gps_dump.msg Normal file
View File

@ -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

175
src/drivers/gps/gps.cpp
View File

@ -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
#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_<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) {
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);

6
src/drivers/gps/params.c
View File

@ -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

1
src/modules/logger/logger.cpp
View File

@ -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)