diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
| -rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 113 |
1 files changed, 109 insertions, 4 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0f25c969d..6765100c7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -342,6 +342,8 @@ private: MavlinkStreamStatustext(MavlinkStreamStatustext &); MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); FILE *fp = nullptr; + unsigned write_err_count = 0; + static const unsigned write_err_threshold = 5; protected: explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) @@ -370,10 +372,21 @@ protected: /* write log messages in first instance to disk */ if (_mavlink->get_instance_id() == 0) { if (fp) { - fputs(msg.text, fp); - fputs("\n", fp); - fsync(fileno(fp)); - } else { + if (EOF == fputs(msg.text, fp)) { + write_err_count++; + } else { + write_err_count = 0; + } + + if (write_err_count >= write_err_threshold) { + (void)fclose(fp); + fp = nullptr; + } else { + (void)fputs("\n", fp); + (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[64] = ""; @@ -389,6 +402,10 @@ protected: 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), "/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); + + /* write first message */ + fputs(msg.text, fp); + fputs("\n", fp); } } } @@ -923,6 +940,92 @@ protected: } }; +class MavlinkStreamSystemTime : public MavlinkStream +{ +public: + const char *get_name() const { + return MavlinkStreamSystemTime::get_name_static(); + } + + static const char *get_name_static() { + return "SYSTEM_TIME"; + } + + uint8_t get_id() { + return MAVLINK_MSG_ID_SYSTEM_TIME; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) { + return new MavlinkStreamSystemTime(mavlink); + } + + unsigned get_size() { + return MAVLINK_MSG_ID_SYSTEM_TIME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + /* do not allow top copying this class */ + MavlinkStreamSystemTime(MavlinkStreamSystemTime &); + MavlinkStreamSystemTime &operator = (const MavlinkStreamSystemTime &); + +protected: + explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) { + mavlink_system_time_t msg; + timespec tv; + + clock_gettime(CLOCK_REALTIME, &tv); + + msg.time_boot_ms = hrt_absolute_time() / 1000; + msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; + + _mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg); + } +}; + +class MavlinkStreamTimesync : public MavlinkStream +{ +public: + const char *get_name() const { + return MavlinkStreamTimesync::get_name_static(); + } + + static const char *get_name_static() { + return "TIMESYNC"; + } + + uint8_t get_id() { + return MAVLINK_MSG_ID_TIMESYNC; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) { + return new MavlinkStreamTimesync(mavlink); + } + + unsigned get_size() { + return MAVLINK_MSG_ID_TIMESYNC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + /* do not allow top copying this class */ + MavlinkStreamTimesync(MavlinkStreamTimesync &); + MavlinkStreamTimesync &operator = (const MavlinkStreamTimesync &); + +protected: + explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) { + mavlink_timesync_t msg; + + msg.tc1 = 0; + msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds + + _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg); + } +}; class MavlinkStreamGlobalPositionInt : public MavlinkStream { @@ -2197,6 +2300,8 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), + new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static), + new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), |