From 71d150f6ec0789d36d4f7af85a2e44c225846d51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Jul 2016 17:05:33 +0200 Subject: [PATCH] MAVLink app: Fix rate handling --- src/modules/mavlink/mavlink_main.cpp | 41 ++++++++++++---------- src/modules/mavlink/mavlink_messages.cpp | 17 ++++----- src/modules/mavlink/mavlink_parameters.cpp | 6 ++++ src/modules/mavlink/mavlink_parameters.h | 2 ++ src/modules/mavlink/mavlink_stream.h | 10 ++++++ 5 files changed, 50 insertions(+), 26 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ee804996fd..d160320dc0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b12ea1fb9d..b366c9940e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/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: diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index d6b7ef090c..b846daf1e0 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/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) { diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index d0a14d1cd8..1ef29411c7 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/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: diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index c37b791b04..f77a8d94a1 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/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;