diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5fc1d32244..d7bb8dfc3c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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)) { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e09f39780d..c96d84862c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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] = ""; - - 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"); - - 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); + } 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); + + // 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); + } } } } } } +#endif }; class MavlinkStreamCommandLong : public MavlinkStream