diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3a2acd07c36..a23b3f8736a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -115,6 +115,8 @@ extern mavlink_system_t mavlink_system; static void usage(void); +bool Mavlink::_boot_complete = false; + Mavlink::Mavlink() : _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), @@ -1909,6 +1911,10 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream_command(argc, argv); + } else if (!strcmp(argv[1], "boot_complete")) { + Mavlink::set_boot_complete(); + return 0; + } else { usage(); exit(1); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 777836f6880..5ecf8e69565 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -145,6 +145,14 @@ public: bool get_forwarding_on() { return _forwarding_on; } + /** + * Set the boot complete flag on all instances + * + * Setting the flag unblocks parameter transmissions, which are gated + * beforehand to ensure that the system is fully initialized. + */ + static void set_boot_complete() { _boot_complete = true; } + /** * Get the free space in the transmit buffer * @@ -294,6 +302,8 @@ public: unsigned get_system_type() { return _system_type; } + static bool boot_complete() { return _boot_complete; } + protected: Mavlink *next; @@ -302,6 +312,7 @@ private: int _mavlink_fd; bool _task_running; + static bool _boot_complete; /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 73d7580d219..66c6a871762 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -193,7 +193,7 @@ void MavlinkParametersManager::send(const hrt_abstime t) { /* send all parameters if requested, but only after the system has booted */ - if (_send_all_index >= 0 && t > 4 * 1000 * 1000) { + if (_send_all_index >= 0 && _mavlink->boot_complete()) { /* skip if no space is available */ if (_mavlink->get_free_tx_buf() < get_size()) {