From 3f37a21aafebc9ab875ea4d4bcbda2345b21005b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 2 Jun 2021 13:06:13 +1000 Subject: [PATCH] GCS_MAVLink: set message intervals from config files --- libraries/GCS_MAVLink/GCS.h | 48 +++++++ libraries/GCS_MAVLink/GCS_Common.cpp | 191 +++++++++++++++++++++++++++ 2 files changed, 239 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index b939c37d4e..dc797f2c1b 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -39,6 +39,10 @@ #define HAL_HIGH_LATENCY2_ENABLED !HAL_MINIMIZE_FEATURES #endif +#ifndef HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED +#define HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED (HAVE_FILESYSTEM_SUPPORT && BOARD_FLASH_SIZE > 1024) +#endif + // macros used to determine if a message will fit in the space available. void gcs_out_of_space_to_send_count(mavlink_channel_t chan); @@ -96,6 +100,37 @@ public: AP_Int16 streamRates[GCS_MAVLINK_NUM_STREAM_RATES]; }; +#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED +class DefaultIntervalsFromFiles +{ + +public: + + DefaultIntervalsFromFiles(uint16_t max_num); + ~DefaultIntervalsFromFiles(); + + void set(ap_message id, uint16_t interval); + uint16_t num_intervals() const { + return _num_intervals; + } + bool get_interval_for_ap_message_id(ap_message id, uint16_t &interval) const; + ap_message id_at(uint8_t ofs) const; + uint16_t interval_at(uint8_t ofs) const; + +private: + + struct from_file_default_interval { + ap_message id; + uint16_t interval; + }; + + from_file_default_interval *_intervals; + + uint16_t _num_intervals; + uint16_t _max_intervals; +}; +#endif + /// /// @class GCS_MAVLINK /// @brief MAVLink transport control class @@ -682,6 +717,13 @@ private: // boolean that indicated that message intervals have been set // from streamrates: bool deferred_messages_initialised; +#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED + // read configuration files from (e.g.) SD and ROMFS, set + // intervals from same + void initialise_message_intervals_from_config_files(); + // read file, set message intervals from it: + void get_intervals_from_filepath(const char *path, DefaultIntervalsFromFiles &); +#endif // return interval deferred message bucket should be sent after. // When sending parameters and waypoints this may be longer than // the interval specified in "deferred" @@ -844,6 +886,12 @@ private: bool signing_enabled(void) const; static void save_signing_timestamp(bool force_save_now); +#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED + // structure containing default intervals read from files for this + // link: + DefaultIntervalsFromFiles *default_intervals_from_files; +#endif + // alternative protocol handler support struct { GCS_MAVLINK::protocol_handler_fn_t handler; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a03d6a7a6b..4acf0063b0 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include @@ -78,6 +79,8 @@ #include #include +#include + extern const AP_HAL::HAL& hal; struct GCS_MAVLINK::LastRadioStatus GCS_MAVLINK::last_radio_status; @@ -1117,6 +1120,9 @@ void GCS_MAVLINK::update_send() if (!deferred_messages_initialised) { initialise_message_intervals_from_streamrates(); +#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED + initialise_message_intervals_from_config_files(); +#endif deferred_messages_initialised = true; } @@ -5259,6 +5265,182 @@ void GCS_MAVLINK::initialise_message_intervals_for_stream(GCS_MAVLINK::streams i } } +#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED +// open and read contents of path, setting message intervals from each +// line +DefaultIntervalsFromFiles::DefaultIntervalsFromFiles(uint16_t max_num) +{ + if (max_num == 0) { + return; + } + _intervals = new from_file_default_interval[max_num]; + _max_intervals = max_num; +} + +DefaultIntervalsFromFiles::~DefaultIntervalsFromFiles() +{ + delete[] (_intervals); +} + +void DefaultIntervalsFromFiles::set(ap_message id, uint16_t interval) +{ + if (_intervals == nullptr) { + return; + } + + // update any existing interval (last-one-in wins) + for (uint8_t i=0; i<_num_intervals; i++) { + if (_intervals[i].id == id) { + _intervals[i].interval = interval; + return; + } + } + + // store an interval we've not seen before: + if (_num_intervals == _max_intervals) { + return; + } + + _intervals[_num_intervals].id = id; + _intervals[_num_intervals].interval = interval; + _num_intervals++; +} + +bool DefaultIntervalsFromFiles::get_interval_for_ap_message_id(ap_message id, uint16_t &interval) const +{ + for (uint16_t i=0; i<_num_intervals; i++) { + if (_intervals[i].id == id) { + interval = _intervals[i].interval; + return true; + } + } + return false; +} + +ap_message DefaultIntervalsFromFiles::id_at(uint8_t ofs) const +{ + if (_intervals == nullptr || ofs >= _num_intervals) { + return MSG_LAST; + } + return _intervals[ofs].id; +} + +uint16_t DefaultIntervalsFromFiles::interval_at(uint8_t ofs) const +{ + if (_intervals == nullptr || ofs >= _num_intervals) { + return -1; // unsigned-integer wrap + } + return _intervals[ofs].interval; +} + +void GCS_MAVLINK::get_intervals_from_filepath(const char *path, DefaultIntervalsFromFiles &intervals) +{ + const int f = AP::FS().open(path, O_RDONLY); + if (f == -1) { + return; + } + + char line[20]; + while (AP::FS().fgets(line, sizeof(line)-1, f)) { + char *saveptr = nullptr; + const char *mavlink_id_str = strtok_r(line, " ", &saveptr); + if (mavlink_id_str == nullptr || strlen(mavlink_id_str) == 0) { + continue; + } + const uint32_t mavlink_id = atoi(mavlink_id_str); + + const ap_message msg_id = mavlink_id_to_ap_message_id(mavlink_id); + if (msg_id == MSG_LAST) { + continue; + } + + const char *interval_str = strtok_r(nullptr, "\r\n", &saveptr); + if (interval_str == nullptr || strlen(interval_str) == 0) { + continue; + } + const uint16_t interval = atoi(interval_str); + + intervals.set(msg_id, interval); + } + + AP::FS().close(f); +} + +void GCS_MAVLINK::initialise_message_intervals_from_config_files() +{ + static const char *path_templates[] { + "@ROMFS/message-intervals-chan%u.txt", + "message-intervals-chan%u.txt" + }; + + // don't do anything at all if no files exist: + bool exists = false; + for (const char * path_template : path_templates) { + struct stat stats; + char *path; + if (asprintf(&path, path_template, chan) == -1) { + continue; + } + if (AP::FS().stat(path, &stats) < 0) { + free(path); + continue; + } + free(path); + if (stats.st_size == 0) { + continue; + } + exists = true; + break; + } + if (!exists) { + return; + } + + // first over-allocate: + DefaultIntervalsFromFiles *overallocated = new DefaultIntervalsFromFiles(128); + if (overallocated == nullptr) { + return; + } + for (const char * path_template : path_templates) { + char *path; + if (asprintf(&path, path_template, chan) == -1) { + continue; + } + get_intervals_from_filepath(path, *overallocated); + free(path); + } + + // then allocate just the right number and redo all of the work: + const uint16_t num_required = overallocated->num_intervals(); + delete overallocated; + overallocated = nullptr; + + default_intervals_from_files = new DefaultIntervalsFromFiles(num_required); + if (default_intervals_from_files == nullptr) { + return; + } + for (const char * path_template : path_templates) { + char *path; + if (asprintf(&path, path_template, chan) == -1) { + continue; + } + get_intervals_from_filepath(path, *default_intervals_from_files); + free(path); + } + + // now actually initialise the intervals: + for (uint8_t i=0; inum_intervals(); i++) { + const ap_message id = default_intervals_from_files->id_at(i); + if (id == MSG_LAST) { + // internal error + break; + } + const uint16_t interval = default_intervals_from_files->interval_at(i); + set_ap_message_interval(id, interval); + } +} +#endif + void GCS_MAVLINK::initialise_message_intervals_from_streamrates() { // this is O(n^2), but it's once at boot and across a 10-entry list... @@ -5276,6 +5458,15 @@ bool GCS_MAVLINK::get_default_interval_for_ap_message(const ap_message id, uint1 return true; } +#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED + // a user can specify default rates in files, which are read close + // to vehicle startup + if (default_intervals_from_files != nullptr && + default_intervals_from_files->get_interval_for_ap_message_id(id, interval)) { + return true; + } +#endif + // find which stream this ap_message is in for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) { const GCS_MAVLINK::stream_entries &entries = all_stream_entries[i];