Mavlink set last sent timestamp to space out initial publication

- remove QURT defines no longer required

src/modules/mavlink/mavlink_command_sender.cpp

@@ -57,15 +57,9 @@ void MavlinkCommandSender::initialize()
 
 MavlinkCommandSender &MavlinkCommandSender::instance()
 {
-
 	return *_instance;
 }
 
-MavlinkCommandSender::MavlinkCommandSender() :
-	_commands(3)
-{
-}
-
 MavlinkCommandSender::~MavlinkCommandSender()
 {
 	px4_sem_destroy(&_lock);

src/modules/mavlink/mavlink_command_sender.h

@@ -84,7 +84,7 @@ public:
 	void handle_mavlink_command_ack(const mavlink_command_ack_t &ack, uint8_t from_sysid, uint8_t from_compid);
 
 private:
-	MavlinkCommandSender();
+	MavlinkCommandSender() = default;
 
 	~MavlinkCommandSender();
 
@@ -111,11 +111,11 @@ private:
 		int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL] = {-1, -1, -1, -1};
 	} command_item_t;
 
-	TimestampedList<command_item_t> _commands;
+	TimestampedList<command_item_t> _commands{3};
 
 	bool _debug_enabled = false;
-	static const uint8_t RETRIES = 3;
-	static const uint64_t TIMEOUT_US = 500000;
+	static constexpr uint8_t RETRIES = 3;
+	static constexpr uint64_t TIMEOUT_US = 500000;
 
 	/* do not allow copying or assigning this class */
 	MavlinkCommandSender(const MavlinkCommandSender &) = delete;

src/modules/mavlink/mavlink_ftp.cpp

@@ -49,13 +49,7 @@
 constexpr const char MavlinkFTP::_root_dir[];
 
 MavlinkFTP::MavlinkFTP(Mavlink *mavlink) :
-	_session_info{},
-	_utRcvMsgFunc{},
-	_worker_data{},
-	_mavlink(mavlink),
-	_work_buffer1{nullptr},
-	_work_buffer2{nullptr},
-	_last_work_buffer_access{0}
+	_mavlink(mavlink)
 {
 	// initialize session
 	_session_info.fd = -1;
@@ -70,7 +64,6 @@ MavlinkFTP::~MavlinkFTP()
 	if (_work_buffer2) {
 		delete[] _work_buffer2;
 	}
-
 }
 
 unsigned

src/modules/mavlink/mavlink_ftp.h

@@ -172,10 +172,10 @@ private:
 		uint8_t		stream_target_system_id;
 		unsigned	stream_chunk_transmitted;
 	};
-	struct SessionInfo _session_info;	///< Session info, fd=-1 for no active session
+	struct SessionInfo _session_info {};	///< Session info, fd=-1 for no active session
 
-	ReceiveMessageFunc_t	_utRcvMsgFunc;	///< Unit test override for mavlink message sending
-	void			*_worker_data;	///< Additional parameter to _utRcvMsgFunc;
+	ReceiveMessageFunc_t	_utRcvMsgFunc{};	///< Unit test override for mavlink message sending
+	void			*_worker_data{nullptr};	///< Additional parameter to _utRcvMsgFunc;
 
 	Mavlink *_mavlink;
 
@@ -184,11 +184,11 @@ private:
 	MavlinkFTP operator=(const MavlinkFTP &);
 
 	/* work buffers: they're allocated as soon as we get the first request (lazy, since FTP is rarely used) */
-	char *_work_buffer1;
+	char *_work_buffer1{nullptr};
 	static constexpr int _work_buffer1_len = kMaxDataLength;
-	char *_work_buffer2;
+	char *_work_buffer2{nullptr};
 	static constexpr int _work_buffer2_len = 256;
-	hrt_abstime _last_work_buffer_access; ///< timestamp when the buffers were last accessed
+	hrt_abstime _last_work_buffer_access{0}; ///< timestamp when the buffers were last accessed
 
 	// prepend a root directory to each file/dir access to avoid enumerating the full FS tree (e.g. on Linux).
 	// Note that requests can still fall outside of the root dir by using ../..

src/modules/mavlink/mavlink_main.h

@@ -316,14 +316,12 @@ public:
 
 	int			get_instance_id();
 
-#ifndef __PX4_QURT
 	/**
 	 * Enable / disable hardware flow control.
 	 *
 	 * @param enabled	True if hardware flow control should be enabled
 	 */
 	int			enable_flow_control(enum FLOW_CONTROL_MODE enabled);
-#endif
 
 	mavlink_channel_t	get_channel();
 
@@ -542,9 +540,9 @@ private:
 
 	bool			_forwarding_on;
 	bool			_ftp_on;
-#ifndef __PX4_QURT
+
 	int			_uart_fd;
-#endif
+
 	int			_baudrate;
 	int			_datarate;		///< data rate for normal streams (attitude, position, etc.)
 	int			_datarate_events;	///< data rate for params, waypoints, text messages
@@ -631,9 +629,7 @@ private:
 
 	void			mavlink_update_system();
 
-#ifndef __PX4_QURT
 	int			mavlink_open_uart(int baudrate, const char *uart_name, bool force_flow_control);
-#endif
 
 	static int		interval_from_rate(float rate);
 

src/modules/mavlink/mavlink_messages.cpp

@@ -405,9 +405,6 @@ protected:
 	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{}
 
-	~MavlinkStreamStatustext() = default;
-
-
 	bool send(const hrt_abstime t)
 	{
 		if (!_mavlink->get_logbuffer()->empty() && _mavlink->is_connected()) {

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -50,13 +50,8 @@
 #include <uORB/uORB.h>
 
 MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) :
-	next(nullptr),
 	_topic(topic),
-	_fd(-1),
-	_instance(instance),
-	_published(false),
-	_subscribe_from_beginning(false),
-	_last_pub_check(0)
+	_instance(instance)
 {
 }
 

src/modules/mavlink/mavlink_orb_subscription.h

@@ -48,7 +48,7 @@
 class MavlinkOrbSubscription
 {
 public:
-	MavlinkOrbSubscription *next;	///< pointer to next subscription in list
+	MavlinkOrbSubscription *next{nullptr};	///< pointer to next subscription in list
 
 	MavlinkOrbSubscription(const orb_id_t topic, int instance);
 	~MavlinkOrbSubscription();
@@ -97,11 +97,15 @@ public:
 
 private:
 	const orb_id_t _topic;		///< topic metadata
-	int _fd;			///< subscription handle
 	const uint8_t _instance;		///< get topic instance
-	bool _published;		///< topic was ever published
-	bool _subscribe_from_beginning; ///< we need to subscribe from the beginning, e.g. for vehicle_command_acks
-	hrt_abstime _last_pub_check;	///< when we checked last
+
+	int _fd{-1};			///< subscription handle
+
+	bool _published{false};		///< topic was ever published
+
+	bool _subscribe_from_beginning{false}; ///< we need to subscribe from the beginning, e.g. for vehicle_command_acks
+
+	hrt_abstime _last_pub_check{0};	///< when we checked last
 
 	/* do not allow copying this class */
 	MavlinkOrbSubscription(const MavlinkOrbSubscription &);

src/modules/mavlink/mavlink_rate_limiter.cpp

@@ -40,24 +40,8 @@
 
 #include "mavlink_rate_limiter.h"
 
-MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000)
-{
-}
-
-MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval)
-{
-}
-
-MavlinkRateLimiter::~MavlinkRateLimiter() = default;
-
-void
-MavlinkRateLimiter::set_interval(unsigned int interval)
-{
-	_interval = interval;
-}
-
 bool
-MavlinkRateLimiter::check(hrt_abstime t)
+MavlinkRateLimiter::check(const hrt_abstime &t)
 {
 	uint64_t dt = t - _last_sent;
 

src/modules/mavlink/mavlink_rate_limiter.h

@@ -47,15 +47,18 @@
 class MavlinkRateLimiter
 {
 private:
-	hrt_abstime _last_sent;
-	hrt_abstime _interval;
+	hrt_abstime _last_sent{0};
+	hrt_abstime _interval{1000000};
 
 public:
-	MavlinkRateLimiter();
-	MavlinkRateLimiter(unsigned int interval);
-	~MavlinkRateLimiter();
-	void set_interval(unsigned int interval);
-	bool check(hrt_abstime t);
+	MavlinkRateLimiter() = default;
+	MavlinkRateLimiter(unsigned int interval) : _interval(interval) {};
+
+	~MavlinkRateLimiter() = default;
+
+	void set_interval(unsigned int interval) { _interval = interval; }
+
+	bool check(const hrt_abstime &t);
 };
 
 

src/modules/mavlink/mavlink_shell.h

@@ -59,7 +59,6 @@ public:
 	 */
 	int		start();
 
-
 	/**
 	 * Write to the shell
 	 * @return number of written bytes

src/modules/mavlink/mavlink_stream.cpp

@@ -45,29 +45,16 @@
 #include "mavlink_main.h"
 
 MavlinkStream::MavlinkStream(Mavlink *mavlink) :
-	next(nullptr),
-	_mavlink(mavlink),
-	_interval(1000000),
-	_last_sent(0 /* 0 means unlimited - updates on every iteration */)
+	_mavlink(mavlink)
 {
-}
-
-MavlinkStream::~MavlinkStream() = default;
-
-/**
- * Set messages interval in ms
- */
-void
-MavlinkStream::set_interval(const int interval)
-{
-	_interval = interval;
+	_last_sent = hrt_absolute_time();
 }
 
 /**
  * Update subscriptions and send message if necessary
  */
 int
-MavlinkStream::update(const hrt_abstime t)
+MavlinkStream::update(const hrt_abstime &t)
 {
 	update_data();
 
@@ -77,12 +64,10 @@ MavlinkStream::update(const hrt_abstime t)
 		// this will give different messages on the same run a different
 		// initial timestamp which will help spacing them out
 		// on the link scheduling
-#ifndef __PX4_QURT
 		if (send(t)) {
 			_last_sent = hrt_absolute_time();
 		}
 
-#endif
 		return 0;
 	}
 
@@ -111,16 +96,12 @@ MavlinkStream::update(const hrt_abstime t)
 
 	if (interval == 0 || (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 3))) {
 		// interval expired, send message
-		bool sent = true;
-#ifndef __PX4_QURT
-		sent = send(t);
-#endif
 
 		// If the interval is non-zero and dt is smaller than 1.5 times the interval
 		// do not use the actual time but increment at a fixed rate, so that processing delays do not
 		// distort the average rate. The check of the maximum interval is done to ensure that after a
 		// long time not sending anything, sending multiple messages in a short time is avoided.
-		if (sent) {
+		if (send(t)) {
 			_last_sent = ((interval > 0) && ((int64_t)(1.5f * interval) > dt)) ? _last_sent + interval : t;
 			return 0;
 

src/modules/mavlink/mavlink_stream.h

@@ -49,17 +49,23 @@ class MavlinkStream
 {
 
 public:
-	MavlinkStream *next;
+	MavlinkStream *next{nullptr};
 
 	MavlinkStream(Mavlink *mavlink);
-	virtual ~MavlinkStream();
+	virtual ~MavlinkStream() = default;
+
+	// no copy, assignment, move, move assignment
+	MavlinkStream(const MavlinkStream &) = delete;
+	MavlinkStream &operator=(const MavlinkStream &) = delete;
+	MavlinkStream(MavlinkStream &&) = delete;
+	MavlinkStream &operator=(MavlinkStream &&) = delete;
 
 	/**
 	 * Get the interval
 	 *
 	 * @param interval the interval in microseconds (us) between messages
 	 */
-	void set_interval(const int interval);
+	void set_interval(const int interval) { _interval = interval; }
 
 	/**
 	 * Get the interval
@@ -71,7 +77,7 @@ public:
 	/**
 	 * @return 0 if updated / sent, -1 if unchanged
 	 */
-	int update(const hrt_abstime t);
+	int update(const hrt_abstime &t);
 	virtual const char *get_name() const = 0;
 	virtual uint16_t get_id() = 0;
 
@@ -96,12 +102,10 @@ public:
 	virtual unsigned get_size_avg() { return get_size(); }
 
 protected:
-	Mavlink     *_mavlink;
-	int _interval;		///< if set to negative value = unlimited rate
+	Mavlink      *const _mavlink;
+	int _interval{1000000};		///< if set to negative value = unlimited rate
 
-#ifndef __PX4_QURT
 	virtual bool send(const hrt_abstime t) = 0;
-#endif
 
 	/**
 	 * Function to collect/update data for the streams at a high rate independant of
@@ -112,11 +116,7 @@ protected:
 	virtual void update_data() { }
 
 private:
-	hrt_abstime _last_sent;
-
-	/* do not allow top copying this class */
-	MavlinkStream(const MavlinkStream &);
-	MavlinkStream &operator=(const MavlinkStream &);
+	hrt_abstime _last_sent{0};
 };
 
 

src/modules/mavlink/mavlink_timesync.cpp

@@ -42,17 +42,10 @@
 #include "mavlink_main.h"
 
 MavlinkTimesync::MavlinkTimesync(Mavlink *mavlink) :
-	_timesync_status_pub(nullptr),
-	_sequence(0),
-	_time_offset(0.0),
-	_time_skew(0.0),
-	_filter_alpha(ALPHA_GAIN_INITIAL),
-	_filter_beta(BETA_GAIN_INITIAL),
-	_high_deviation_count(0),
-	_high_rtt_count(0),
 	_mavlink(mavlink)
 {
 }
+
 MavlinkTimesync::~MavlinkTimesync()
 {
 	if (_timesync_status_pub) {
@@ -253,4 +246,4 @@ MavlinkTimesync::reset_filter()
 	_high_deviation_count = 0;
 	_high_rtt_count = 0;
 
-}
+}

src/modules/mavlink/mavlink_timesync.h

@@ -50,7 +50,9 @@
 #include <math.h>
 #include <float.h>
 
-#define PX4_EPOCH_SECS 1234567890ULL
+using namespace time_literals;
+
+static constexpr time_t PX4_EPOCH_SECS = 1234567890ULL;
 
 // Filter gains
 //
@@ -62,10 +64,10 @@
 // tighter estimation of the skew (derivative), but will negatively affect how fast the
 // filter reacts to clock skewing (e.g cause by temperature changes to the oscillator).
 // Larger values will cause large-amplitude oscillations.
-#define ALPHA_GAIN_INITIAL 				0.05
-#define BETA_GAIN_INITIAL 				0.05
-#define ALPHA_GAIN_FINAL 				0.003
-#define BETA_GAIN_FINAL 				0.003
+static constexpr double ALPHA_GAIN_INITIAL = 0.05;
+static constexpr double BETA_GAIN_INITIAL = 0.05;
+static constexpr double ALPHA_GAIN_FINAL = 0.003;
+static constexpr double BETA_GAIN_FINAL = 0.003;
 
 // Filter gain scheduling
 //
@@ -73,7 +75,7 @@
 // exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will
 // allow the timesync to converge faster, but with potentially less accurate initial
 // offset and skew estimates.
-#define CONVERGENCE_WINDOW 				500
+static constexpr uint32_t CONVERGENCE_WINDOW = 500;
 
 // Outlier rejection and filter reset
 //
@@ -85,10 +87,10 @@
 // of such events in a row will reset the filter. This usually happens only due to a time jump
 // on the remote system.
 // TODO : automatically determine these using ping statistics?
-#define MAX_RTT_SAMPLE 					10000ULL 	// 10ms
-#define MAX_DEVIATION_SAMPLE 			100000ULL 	// 100ms
-#define MAX_CONSECUTIVE_HIGH_RTT 		5
-#define MAX_CONSECUTIVE_HIGH_DEVIATION 	5
+static constexpr uint64_t MAX_RTT_SAMPLE = 10_ms;
+static constexpr uint64_t MAX_DEVIATION_SAMPLE = 100_ms;
+static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT = 5;
+static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 5;
 
 class Mavlink;
 
@@ -130,21 +132,21 @@ protected:
 	 */
 	void reset_filter();
 
-	orb_advert_t _timesync_status_pub;
+	orb_advert_t _timesync_status_pub{nullptr};
 
-	uint32_t _sequence;
+	uint32_t _sequence{0};
 
 	// Timesync statistics
-	double _time_offset;
-	double _time_skew;
+	double _time_offset{0};
+	double _time_skew{0};
 
 	// Filter parameters
-	double _filter_alpha;
-	double _filter_beta;
+	double _filter_alpha{ALPHA_GAIN_INITIAL};
+	double _filter_beta{BETA_GAIN_INITIAL};
 
 	// Outlier rejection and filter reset
-	uint32_t _high_deviation_count;
-	uint32_t _high_rtt_count;
+	uint32_t _high_deviation_count{0};
+	uint32_t _high_rtt_count{0};
 
-	Mavlink *_mavlink;
+	Mavlink *const _mavlink;
 };

src/modules/mavlink/timestamped_list.h

@@ -58,6 +58,7 @@ public:
 		_list = new item_t[num_items];
 		_list_len = num_items;
 	}
+
 	~TimestampedList()
 	{
 		delete[] _list;