Skip to content

Commit 71d150f

Browse files
LorenzMeierLorenzMeier
committed
MAVLink app: Fix rate handling
1 parent c713008 commit 71d150f

File tree

5 files changed

+50
-26
lines changed

5 files changed

+50
-26
lines changed

‎src/modules/mavlink/mavlink_main.cpp‎

Lines changed: 23 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1521,15 +1521,20 @@ Mavlink::update_rate_mult()
15211521
MavlinkStream *stream;
15221522
LL_FOREACH(_streams, stream) {
15231523
if (stream->const_rate()) {
1524-
const_rate += stream->get_size() * 1000000.0f / stream->get_interval();
1524+
const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
15251525

15261526
} else {
1527-
rate += stream->get_size() * 1000000.0f / stream->get_interval();
1527+
rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
15281528
}
15291529
}
15301530

1531-
/* don't scale up rates, only scale down if needed */
1532-
float bandwidth_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
1531+
/* scale up and down as the link permits */
1532+
float bandwidth_mult = (float)(_datarate - const_rate) / rate;
1533+
1534+
/* if we do not have flow control, limit to the set data rate */
1535+
if (!get_flow_control_enabled()) {
1536+
bandwidth_mult = fminf(1.0f, bandwidth_mult);
1537+
}
15331538

15341539
/* check if we have radio feedback */
15351540
struct telemetry_status_s &tstatus = get_rx_status();
@@ -1852,26 +1857,26 @@ Mavlink::task_main(int argc, char *argv[])
18521857
case MAVLINK_MODE_NORMAL:
18531858
configure_stream("SYS_STATUS", 1.0f);
18541859
configure_stream("EXTENDED_SYS_STATE", 1.0f);
1855-
configure_stream("HIGHRES_IMU", 2.0f);
1860+
configure_stream("HIGHRES_IMU", 1.5f);
18561861
configure_stream("ATTITUDE", 20.0f);
1857-
configure_stream("RC_CHANNELS", 1.0f);
1862+
configure_stream("RC_CHANNELS", 5.0f);
18581863
configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
18591864
configure_stream("ALTITUDE", 1.0f);
18601865
configure_stream("GPS_RAW_INT", 1.0f);
18611866
configure_stream("ADSB_VEHICLE", 2.0f);
18621867
configure_stream("DISTANCE_SENSOR", 0.5f);
1863-
configure_stream("OPTICAL_FLOW_RAD", 5.0f);
1864-
configure_stream("VISION_POSITION_NED", 10.0f);
1868+
configure_stream("OPTICAL_FLOW_RAD", 1.0f);
1869+
configure_stream("VISION_POSITION_NED", 1.0f);
18651870
configure_stream("ESTIMATOR_STATUS", 0.5f);
1866-
configure_stream("NAV_CONTROLLER_OUTPUT", 2.0f);
1867-
configure_stream("GLOBAL_POSITION_INT", 3.0f);
1868-
configure_stream("LOCAL_POSITION_NED", 3.0f);
1869-
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
1870-
configure_stream("ATTITUDE_TARGET", 8.0f);
1871+
configure_stream("NAV_CONTROLLER_OUTPUT", 1.5f);
1872+
configure_stream("GLOBAL_POSITION_INT", 5.0f);
1873+
configure_stream("LOCAL_POSITION_NED", 1.0f);
1874+
configure_stream("POSITION_TARGET_GLOBAL_INT", 1.5f);
1875+
configure_stream("ATTITUDE_TARGET", 2.0f);
18711876
configure_stream("HOME_POSITION", 0.5f);
18721877
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
1873-
configure_stream("VFR_HUD", 8.0f);
1874-
configure_stream("WIND", 2.0f);
1878+
configure_stream("VFR_HUD", 4.0f);
1879+
configure_stream("WIND_COV", 1.0f);
18751880
break;
18761881

18771882
case MAVLINK_MODE_ONBOARD:
@@ -1896,7 +1901,7 @@ Mavlink::task_main(int argc, char *argv[])
18961901
configure_stream("HOME_POSITION", 0.5f);
18971902
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
18981903
configure_stream("VFR_HUD", 10.0f);
1899-
configure_stream("WIND", 10.0f);
1904+
configure_stream("WIND_COV", 10.0f);
19001905
configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
19011906
configure_stream("SYSTEM_TIME", 1.0f);
19021907
configure_stream("TIMESYNC", 10.0f);
@@ -1919,7 +1924,7 @@ Mavlink::task_main(int argc, char *argv[])
19191924
configure_stream("ATTITUDE_TARGET", 10.0f);
19201925
configure_stream("HOME_POSITION", 0.5f);
19211926
configure_stream("VFR_HUD", 25.0f);
1922-
configure_stream("WIND", 2.0f);
1927+
configure_stream("WIND_COV", 2.0f);
19231928
configure_stream("SYSTEM_TIME", 1.0f);
19241929
break;
19251930

@@ -1951,7 +1956,7 @@ Mavlink::task_main(int argc, char *argv[])
19511956
configure_stream("HOME_POSITION", 0.5f);
19521957
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
19531958
configure_stream("VFR_HUD", 20.0f);
1954-
configure_stream("WIND", 10.0f);
1959+
configure_stream("WIND_COV", 10.0f);
19551960
configure_stream("CAMERA_TRIGGER", 500.0f);
19561961
configure_stream("MISSION_ITEM", 50.0f);
19571962
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);

‎src/modules/mavlink/mavlink_messages.cpp‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1219,7 +1219,7 @@ class MavlinkStreamADSBVehicle : public MavlinkStream
12191219

12201220
unsigned get_size()
12211221
{
1222-
return MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
1222+
return (_pos_time > 0) ? MAVLINK_MSG_ID_ADSB_VEHICLE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
12231223
}
12241224

12251225
private:
@@ -1292,7 +1292,7 @@ class MavlinkStreamCameraTrigger : public MavlinkStream
12921292

12931293
unsigned get_size()
12941294
{
1295-
return MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
1295+
return (_trigger_time > 0) ? MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
12961296
}
12971297

12981298
private:
@@ -1435,7 +1435,7 @@ class MavlinkStreamVisionPositionNED : public MavlinkStream
14351435

14361436
unsigned get_size()
14371437
{
1438-
return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
1438+
return (_pos_time > 0) ? MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
14391439
}
14401440
private:
14411441

@@ -2674,7 +2674,7 @@ class MavlinkStreamNamedValueFloat : public MavlinkStream
26742674

26752675
unsigned get_size()
26762676
{
2677-
return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
2677+
return (_debug_time > 0) ? MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
26782678
}
26792679

26802680
private:
@@ -2739,7 +2739,8 @@ class MavlinkStreamNavControllerOutput : public MavlinkStream
27392739

27402740
unsigned get_size()
27412741
{
2742-
return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES;
2742+
return (_fw_pos_ctrl_status_sub->is_published()) ?
2743+
MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
27432744
}
27442745

27452746
private:
@@ -3068,7 +3069,7 @@ class MavlinkStreamAltitude : public MavlinkStream
30683069

30693070
unsigned get_size()
30703071
{
3071-
return MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
3072+
return (_local_pos_time > 0) ? MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
30723073
}
30733074

30743075
private:
@@ -3147,7 +3148,7 @@ class MavlinkStreamWind : public MavlinkStream
31473148

31483149
static const char *get_name_static()
31493150
{
3150-
return "WIND";
3151+
return "WIND_COV";
31513152
}
31523153

31533154
static uint8_t get_id_static()
@@ -3167,7 +3168,7 @@ class MavlinkStreamWind : public MavlinkStream
31673168

31683169
unsigned get_size()
31693170
{
3170-
return MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
3171+
return (_wind_estimate_time > 0) ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
31713172
}
31723173

31733174
private:

‎src/modules/mavlink/mavlink_parameters.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,12 @@ MavlinkParametersManager::get_size()
7373
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
7474
}
7575

76+
unsigned
77+
MavlinkParametersManager::get_size_avg()
78+
{
79+
return 0;
80+
}
81+
7682
void
7783
MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
7884
{

‎src/modules/mavlink/mavlink_parameters.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,8 @@ class MavlinkParametersManager : public MavlinkStream
7373

7474
unsigned get_size();
7575

76+
unsigned get_size_avg();
77+
7678
void handle_message(const mavlink_message_t *msg);
7779

7880
private:

‎src/modules/mavlink/mavlink_stream.h‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,16 @@ class MavlinkStream
8686
*/
8787
virtual unsigned get_size() = 0;
8888

89+
/**
90+
* Get the average message size
91+
*
92+
* For a normal stream this equals the message size,
93+
* for something like a parameter or mission message
94+
* this equals usually zero, as no bandwidth
95+
* needs to be reserved
96+
*/
97+
virtual unsigned get_size_avg() { return get_size(); }
98+
8999
protected:
90100
Mavlink *_mavlink;
91101
unsigned int _interval;

0 commit comments

Comments
 (0)