fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 01:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: bring buffering for log messages back

Browse Source
This commit is contained in:
Julian Oes
2016-03-24 13:10:02 +01:00
parent 1fc9b99307
commit de0e467150
2 changed files with 51 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+7
View File
@@ -1637,6 +1637,8 @@ Mavlink::task_main(int argc, char *argv[])
uint64_t status_time = 0;
MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack));
uint64_t ack_time = 0;
MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
uint64_t mavlink_log_time = 0;
struct vehicle_status_s status;
status_sub->update(&status_time, &status);
@@ -1875,6 +1877,11 @@ Mavlink::task_main(int argc, char *argv[])
send_message(MAVLINK_MSG_ID_COMMAND_ACK, &msg);
}
struct mavlink_log_s mavlink_log;
if (mavlink_log_sub->update(&mavlink_log_time, &mavlink_log)) {
_logbuffer.put(&mavlink_log);
}
/* check for requested subscriptions */
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
src/modules/mavlink/mavlink_messages.cpp
+44 -32
View File
@@ -365,35 +365,45 @@ public:
}
private:
MavlinkOrbSubscription *_mavlink_log_sub;
uint64_t _mavlink_log_time;
/* do not allow top copying this class */
MavlinkStreamStatustext(MavlinkStreamStatustext &);
MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
unsigned write_err_count = 0;
static const unsigned write_err_threshold = 5;
#ifndef __PX4_QURT
FILE *fp = nullptr;
#endif
protected:
explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink),
_mavlink_log_sub(_mavlink->add_orb_subscription(ORB_ID(mavlink_log)))
explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
~MavlinkStreamStatustext() {
#ifndef __PX4_QURT
if (fp) {
fclose(fp);
}
#endif
}
void send(const hrt_abstime t)
{
struct mavlink_log_s mavlink_log;
if (!_mavlink->get_logbuffer()->empty()) {
if (_mavlink_log_sub->update(&_mavlink_log_time, &mavlink_log)) {
struct mavlink_log_s mavlink_log;
if (_mavlink->get_logbuffer()->get(&mavlink_log)) {
mavlink_statustext_t msg;
msg.severity = mavlink_log.severity;
mavlink_statustext_t msg;
msg.severity = mavlink_log.severity;
strncpy(msg.text, (const char *)mavlink_log.text, sizeof(msg.text));
strncpy(msg.text, (const char*)mavlink_log.text, sizeof(msg.text));
_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
// TODO: the logging doesn't work on Snapdragon yet because of file paths.
#ifndef __PX4_QURT
/* write log messages in first instance to disk */
if (_mavlink->get_instance_id() == 0) {
if (fp) {
@@ -411,35 +421,37 @@ protected:
(void)fsync(fileno(fp));
}
} else if (write_err_count < write_err_threshold) {
/* string to hold the path to the log */
char log_file_name[32] = "";
char log_file_path[70] = "";
} else if (write_err_count < write_err_threshold) {
/* string to hold the path to the log */
char log_file_name[32] = "";
char log_file_path[70] = "";
timespec ts;
px4_clock_gettime(CLOCK_REALTIME, &ts);
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
struct tm tt;
gmtime_r(&gps_time_sec, &tt);
timespec ts;
px4_clock_gettime(CLOCK_REALTIME, &ts);
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
struct tm tt;
gmtime_r(&gps_time_sec, &tt);
// XXX we do not want to interfere here with the SD log app
strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
snprintf(log_file_path, sizeof(log_file_path), PX4_ROOTFSDIR"/fs/microsd/%s", log_file_name);
fp = fopen(log_file_path, "ab");
// XXX we do not want to interfere here with the SD log app
strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
snprintf(log_file_path, sizeof(log_file_path), PX4_ROOTFSDIR"/fs/microsd/%s", log_file_name);
fp = fopen(log_file_path, "ab");
if (fp != NULL) {
/* write first message */
fputs(msg.text, fp);
fputs("\n", fp);
}
else {
warn("Failed to open %s errno=%d", log_file_path, errno);
if (fp != NULL) {
/* write first message */
fputs(msg.text, fp);
fputs("\n", fp);
}
else {
warn("Failed to open %s errno=%d", log_file_path, errno);
}
}
}
}
}
}
#endif
};
class MavlinkStreamCommandLong : public MavlinkStream