MAVLink app: Fix rate handling

src/modules/mavlink/mavlink_main.cpp

@@ -1521,15 +1521,20 @@ Mavlink::update_rate_mult()
 	MavlinkStream *stream;
 	LL_FOREACH(_streams, stream) {
 		if (stream->const_rate()) {
-			const_rate += stream->get_size() * 1000000.0f / stream->get_interval();
+			const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
 
 		} else {
-			rate += stream->get_size() * 1000000.0f / stream->get_interval();
+			rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
 		}
 	}
 
-	/* don't scale up rates, only scale down if needed */
-	float bandwidth_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
+	/* scale up and down as the link permits */
+	float bandwidth_mult = (float)(_datarate - const_rate) / rate;
+
+	/* if we do not have flow control, limit to the set data rate */
+	if (!get_flow_control_enabled()) {
+		bandwidth_mult = fminf(1.0f, bandwidth_mult);
+	}
 
 	/* check if we have radio feedback */
 	struct telemetry_status_s &tstatus = get_rx_status();
@@ -1852,26 +1857,26 @@ Mavlink::task_main(int argc, char *argv[])
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("EXTENDED_SYS_STATE", 1.0f);
-		configure_stream("HIGHRES_IMU", 2.0f);
+		configure_stream("HIGHRES_IMU", 1.5f);
 		configure_stream("ATTITUDE", 20.0f);
-		configure_stream("RC_CHANNELS", 1.0f);
+		configure_stream("RC_CHANNELS", 5.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream("ALTITUDE", 1.0f);
 		configure_stream("GPS_RAW_INT", 1.0f);
 		configure_stream("ADSB_VEHICLE", 2.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
-		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
-		configure_stream("VISION_POSITION_NED", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 1.0f);
+		configure_stream("VISION_POSITION_NED", 1.0f);
 		configure_stream("ESTIMATOR_STATUS", 0.5f);
-		configure_stream("NAV_CONTROLLER_OUTPUT", 2.0f);
-		configure_stream("GLOBAL_POSITION_INT", 3.0f);
-		configure_stream("LOCAL_POSITION_NED", 3.0f);
-		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
-		configure_stream("ATTITUDE_TARGET", 8.0f);
+		configure_stream("NAV_CONTROLLER_OUTPUT", 1.5f);
+		configure_stream("GLOBAL_POSITION_INT", 5.0f);
+		configure_stream("LOCAL_POSITION_NED", 1.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 1.5f);
+		configure_stream("ATTITUDE_TARGET", 2.0f);
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("NAMED_VALUE_FLOAT", 1.0f);
-		configure_stream("VFR_HUD", 8.0f);
-		configure_stream("WIND", 2.0f);
+		configure_stream("VFR_HUD", 4.0f);
+		configure_stream("WIND_COV", 1.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
@@ -1896,7 +1901,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
 		configure_stream("VFR_HUD", 10.0f);
-		configure_stream("WIND", 10.0f);
+		configure_stream("WIND_COV", 10.0f);
 		configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
 		configure_stream("SYSTEM_TIME", 1.0f);
 		configure_stream("TIMESYNC", 10.0f);
@@ -1919,7 +1924,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ATTITUDE_TARGET", 10.0f);
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("VFR_HUD", 25.0f);
-		configure_stream("WIND", 2.0f);
+		configure_stream("WIND_COV", 2.0f);
 		configure_stream("SYSTEM_TIME", 1.0f);
 		break;
 
@@ -1951,7 +1956,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("NAMED_VALUE_FLOAT", 50.0f);
 		configure_stream("VFR_HUD", 20.0f);
-		configure_stream("WIND", 10.0f);
+		configure_stream("WIND_COV", 10.0f);
 		configure_stream("CAMERA_TRIGGER", 500.0f);
 		configure_stream("MISSION_ITEM", 50.0f);
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);

src/modules/mavlink/mavlink_messages.cpp

@@ -1219,7 +1219,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_pos_time > 0) ? MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
@@ -1292,7 +1292,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_trigger_time > 0) ? MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
@@ -1435,7 +1435,7 @@ public:
 
     unsigned get_size()
     {
-        return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+        return (_pos_time > 0) ? MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
     }
 private:
 
@@ -2674,7 +2674,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_debug_time > 0) ? MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
@@ -2739,7 +2739,8 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_fw_pos_ctrl_status_sub->is_published()) ?
+				MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
@@ -3068,7 +3069,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_local_pos_time > 0) ? MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
@@ -3147,7 +3148,7 @@ public:
 
 	static const char *get_name_static()
 	{
-		return "WIND";
+		return "WIND_COV";
 	}
 
 	static uint8_t get_id_static()
@@ -3167,7 +3168,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return (_wind_estimate_time > 0) ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:

src/modules/mavlink/mavlink_parameters.cpp

@@ -73,6 +73,12 @@ MavlinkParametersManager::get_size()
 	return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 }
 
+unsigned
+MavlinkParametersManager::get_size_avg()
+{
+	return 0;
+}
+
 void
 MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 {

src/modules/mavlink/mavlink_parameters.h

@@ -73,6 +73,8 @@ public:
 
 	unsigned get_size();
 
+	unsigned get_size_avg();
+
 	void handle_message(const mavlink_message_t *msg);
 
 private:

src/modules/mavlink/mavlink_stream.h

@@ -86,6 +86,16 @@ public:
 	 */
 	virtual unsigned get_size() = 0;
 
+	/**
+	 * Get the average message size
+	 *
+	 * For a normal stream this equals the message size,
+	 * for something like a parameter or mission message
+	 * this equals usually zero, as no bandwidth
+	 * needs to be reserved
+	 */
+	virtual unsigned get_size_avg() { return get_size(); }
+
 protected:
 	Mavlink     *_mavlink;
 	unsigned int _interval;