MAVLink: Improve message handling / tracking

The message handling was not obeying action focused messages and high-rate messages properly before. With this change update rates track the desired rates closely. Critical high-rate messages such as ADS-B are queued additionally to guarantee that all received packets are being correctly forwarded.

src/modules/mavlink/mavlink_messages.cpp

@@ -318,7 +318,7 @@ protected:
 		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status = {};
 
@@ -335,6 +335,8 @@ protected:
 
 		mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
 					   base_mode, custom_mode, system_status);
+
+		return true;
 	}
 };
 
@@ -383,7 +385,7 @@ protected:
 	~MavlinkStreamStatustext() {}
 
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		if (!_mavlink->get_logbuffer()->empty() && _mavlink->is_connected()) {
 
@@ -397,8 +399,12 @@ protected:
 				msg.text[sizeof(msg.text) - 1] = '\0';
 
 				mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
+
+				return true;
 			}
 		}
+
+		return false;
 	}
 };
 
@@ -449,9 +455,10 @@ protected:
 		_cmd_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_command_s cmd;
+		bool sent = false;
 
 		if (_cmd_sub->update(&_cmd_time, &cmd)) {
 
@@ -464,6 +471,7 @@ protected:
 				}
 
 				MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
+				sent = true;
 
 			} else {
 				if (_mavlink->verbose()) {
@@ -473,6 +481,8 @@ protected:
 		}
 
 		MavlinkCommandSender::instance().check_timeout(_mavlink->get_channel());
+
+		return sent;
 	}
 };
 
@@ -525,7 +535,7 @@ protected:
 		_battery_status_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status)))
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status = {};
 		struct cpuload_s cpuload = {};
@@ -576,7 +586,11 @@ protected:
 			}
 
 			mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -642,7 +656,7 @@ protected:
 		_baro_timestamp(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct sensor_combined_s sensor = {};
 		struct differential_pressure_s differential_pressure = {};
@@ -695,7 +709,11 @@ protected:
 			msg.fields_updated = fields_updated;
 
 			mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -748,7 +766,7 @@ protected:
 		_att_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_attitude_s att;
 
@@ -764,7 +782,11 @@ protected:
 			msg.yawspeed = att.yawspeed;
 
 			mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -816,7 +838,7 @@ protected:
 		_att_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_attitude_s att;
 
@@ -833,7 +855,11 @@ protected:
 			msg.yawspeed = att.yawspeed;
 
 			mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -916,7 +942,7 @@ protected:
 		_sensor_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_attitude_s att = {};
 		struct vehicle_global_position_s pos = {};
@@ -966,7 +992,11 @@ protected:
 			msg.climb = -pos.vel_d;
 
 			mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -1018,7 +1048,7 @@ protected:
 		_gps_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_gps_position_s gps;
 
@@ -1042,7 +1072,11 @@ protected:
 				msg.satellites_visible = gps.satellites_used;
 
 			mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -1088,7 +1122,7 @@ protected:
 	explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		mavlink_system_time_t msg = {};
 		timespec tv;
@@ -1099,6 +1133,8 @@ protected:
 		msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
 
 		mavlink_msg_system_time_send_struct(_mavlink->get_channel(), &msg);
+
+		return true;
 	}
 };
 
@@ -1144,7 +1180,7 @@ protected:
 	explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		mavlink_timesync_t msg = {};
 
@@ -1152,6 +1188,8 @@ protected:
 		msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds
 
 		mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &msg);
+
+		return true;
 	}
 };
 
@@ -1183,7 +1221,7 @@ public:
 		return new MavlinkStreamADSBVehicle(mavlink);
 	}
 
-	virtual bool const_rate()
+	bool const_rate()
 	{
 		return true;
 	}
@@ -1207,9 +1245,10 @@ protected:
 		_pos_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct transponder_report_s pos;
+		bool sent = false;
 
 		while (_pos_sub->update(&_pos_time, &pos)) {
 			mavlink_adsb_vehicle_t msg = {};
@@ -1229,7 +1268,10 @@ protected:
 			msg.squawk = pos.squawk;
 
 			mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg);
+			sent = true;
 		}
+
+		return sent;
 	}
 };
 
@@ -1280,11 +1322,12 @@ protected:
 		_collision_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct collision_report_s report;
+		bool sent = false;
 
-		if (_collision_sub->update(&_collision_time, &report)) {
+		while (_collision_sub->update(&_collision_time, &report)) {
 			mavlink_collision_t msg = {};
 
 			msg.src = report.src;
@@ -1296,7 +1339,10 @@ protected:
 			msg.horizontal_minimum_delta = report.horizontal_minimum_delta;
 
 			mavlink_msg_collision_send_struct(_mavlink->get_channel(), &msg);
+			sent = true;
 		}
+
+		return sent;
 	}
 };
 
@@ -1328,7 +1374,7 @@ public:
 		return new MavlinkStreamCameraTrigger(mavlink);
 	}
 
-	virtual bool const_rate()
+	bool const_rate()
 	{
 		return true;
 	}
@@ -1352,7 +1398,7 @@ protected:
 		_trigger_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct camera_trigger_s trigger;
 
@@ -1400,8 +1446,12 @@ protected:
 				digicam_ctrl_cmd.param7 = NAN;
 
 				mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &digicam_ctrl_cmd);
+
+				return true;
 			}
 		}
+
+		return false;
 	}
 };
 
@@ -1457,7 +1507,7 @@ protected:
 		_capture_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct camera_capture_s capture;
 
@@ -1481,7 +1531,11 @@ protected:
 			msg.file_url[0] = '\0';
 
 			mavlink_msg_camera_image_captured_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -1537,7 +1591,7 @@ protected:
 		_home_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_global_position_s pos = {};
 		struct home_position_s home = {};
@@ -1560,6 +1614,8 @@ protected:
 
 			mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
 		}
+
+		return updated;
 	}
 };
 
@@ -1615,7 +1671,7 @@ protected:
 		_att_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_local_position_s vpos = {};
 		struct vehicle_attitude_s vatt = {};
@@ -1637,6 +1693,8 @@ protected:
 
 			mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg);
 		}
+
+		return (pos_updated || att_updated);
 	}
 };
 
@@ -1687,7 +1745,7 @@ protected:
 		_pos_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_local_position_s pos;
 
@@ -1703,7 +1761,11 @@ protected:
 			msg.vz = pos.vz;
 
 			mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -1755,7 +1817,7 @@ protected:
 		_est_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct estimator_status_s est = {};
 
@@ -1782,7 +1844,11 @@ protected:
 			msg.covariance[11] = est.timeout_flags;
 
 			mavlink_msg_local_position_ned_cov_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -1833,7 +1899,7 @@ protected:
 		_est_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct estimator_status_s est;
 
@@ -1863,7 +1929,11 @@ protected:
 			msg.vibration_z = est.vibe[2];
 
 			mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -1914,7 +1984,7 @@ protected:
 		_mocap_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct att_pos_mocap_s mocap;
 
@@ -1931,7 +2001,11 @@ protected:
 			msg.z = mocap.z;
 
 			mavlink_msg_att_pos_mocap_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -1981,7 +2055,7 @@ protected:
 		_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position)))
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		/* we're sending the GPS home periodically to ensure the
 		 * the GCS does pick it up at one point */
@@ -2012,8 +2086,12 @@ protected:
 				msg.approach_z = 0.0f;
 
 				mavlink_msg_home_position_send_struct(_mavlink->get_channel(), &msg);
+
+				return true;
 			}
 		}
+
+		return false;
 	}
 };
 
@@ -2080,7 +2158,7 @@ protected:
 		_act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N);
 	}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct actuator_outputs_s act;
 
@@ -2099,7 +2177,11 @@ protected:
 			msg.servo8_raw = act.output[7];
 
 			mavlink_msg_servo_output_raw_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -2182,7 +2264,7 @@ protected:
 		}
 	}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct actuator_controls_s att_ctrl;
 
@@ -2197,7 +2279,11 @@ protected:
 			}
 
 			mavlink_msg_actuator_control_target_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -2255,7 +2341,7 @@ protected:
 		_act_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
 		struct actuator_outputs_s act;
@@ -2363,6 +2449,8 @@ protected:
 
 			mavlink_msg_hil_controls_send_struct(_mavlink->get_channel(), &msg);
 		}
+
+		return updated;
 	}
 };
 
@@ -2418,7 +2506,7 @@ protected:
 		_act_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
 		struct actuator_outputs_s act;
@@ -2521,6 +2609,8 @@ protected:
 
 			mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);
 		}
+
+		return (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED));
 	}
 };
 
@@ -2569,7 +2659,7 @@ protected:
 		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
@@ -2583,7 +2673,11 @@ protected:
 			msg.alt = pos_sp_triplet.current.alt;
 
 			mavlink_msg_position_target_global_int_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -2635,7 +2729,7 @@ protected:
 		_pos_sp_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_local_position_setpoint_s pos_sp;
 
@@ -2656,7 +2750,11 @@ protected:
 			msg.afz = pos_sp.acc_z;
 
 			mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -2712,7 +2810,7 @@ protected:
 		_att_rates_sp_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_attitude_setpoint_s att_sp = {};
 
@@ -2739,7 +2837,11 @@ protected:
 			msg.thrust = att_sp.thrust;
 
 			mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -2791,7 +2893,7 @@ protected:
 		_rc_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct rc_input_values rc = {};
 
@@ -2840,7 +2942,11 @@ protected:
 			over.chan8_raw = msg.chan8_raw;
 
 			mavlink_msg_rc_channels_override_send_struct(_mavlink->get_channel(), &over);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -2892,7 +2998,7 @@ protected:
 		_manual_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct manual_control_setpoint_s manual = {};
 
@@ -2914,7 +3020,11 @@ protected:
 			msg.buttons |= (manual.offboard_switch << (shift * 5));
 
 			mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -2965,7 +3075,7 @@ protected:
 		_flow_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct optical_flow_s flow = {};
 
@@ -2987,7 +3097,11 @@ protected:
 			msg.temperature = flow.gyro_temperature;
 
 			mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -3038,7 +3152,7 @@ protected:
 		_debug_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct debug_key_value_s debug = {};
 
@@ -3052,7 +3166,11 @@ protected:
 			msg.value = debug.value;
 
 			mavlink_msg_named_value_float_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -3104,7 +3222,7 @@ protected:
 		_tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status)))
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct fw_pos_ctrl_status_s _fw_pos_ctrl_status = {};
 		struct tecs_status_s _tecs_status = {};
@@ -3125,7 +3243,11 @@ protected:
 			msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp;
 
 			mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -3174,7 +3296,7 @@ protected:
 		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status = {};
 		(void)_status_sub->update(&status);
@@ -3196,6 +3318,8 @@ protected:
 		msg.param7 = 0;
 
 		mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg);
+
+		return true;
 	}
 };
 
@@ -3246,7 +3370,7 @@ protected:
 		_dist_sensor_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct distance_sensor_s dist_sensor = {};
 
@@ -3284,7 +3408,11 @@ protected:
 			msg.covariance = dist_sensor.covariance;
 
 			mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -3344,7 +3472,7 @@ protected:
 		_msg.landed_state = MAV_LANDED_STATE_UNDEFINED;
 	}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status = {};
 		struct vehicle_land_detected_s land_detected = {};
@@ -3400,6 +3528,8 @@ protected:
 		if (updated) {
 			mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg);
 		}
+
+		return updated;
 	}
 };
 
@@ -3465,7 +3595,7 @@ protected:
 		_sensor_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		mavlink_altitude_t msg = {};
 		bool updated = false;
@@ -3538,6 +3668,8 @@ protected:
 
 			mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg);
 		}
+
+		return updated;
 	}
 };
 
@@ -3593,7 +3725,7 @@ protected:
 		_global_pos_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct wind_estimate_s wind_estimate = {};
 
@@ -3621,7 +3753,11 @@ protected:
 			msg.vert_accuracy = 0.0f;
 
 			mavlink_msg_wind_cov_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -3672,7 +3808,7 @@ protected:
 		_mount_orientation_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct mount_orientation_s mount_orientation = {};
 
@@ -3687,7 +3823,11 @@ protected:
 			msg.yaw = 180.0f / M_PI_F * mount_orientation.attitude_euler_angle[2];
 
 			mavlink_msg_mount_orientation_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };
 
@@ -3806,7 +3946,7 @@ protected:
 		_tecs_time(0)
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		struct actuator_controls_s actuator = {};
 		struct airspeed_s airspeed = {};
@@ -3893,6 +4033,8 @@ protected:
 
 			mavlink_msg_high_latency_send_struct(_mavlink->get_channel(), &msg);
 		}
+
+		return updated;
 	}
 };
 
@@ -3953,7 +4095,7 @@ protected:
 		_gpos()
 	{}
 
-	void send(const hrt_abstime t)
+	bool send(const hrt_abstime t)
 	{
 		bool att_updated = _att_sub->update(&_att_time, &_att);
 		bool gpos_updated = _gpos_sub->update(&_gpos_time, &_gpos);
@@ -3987,7 +4129,11 @@ protected:
 			}
 
 			mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 		}
+
+		return false;
 	}
 };