fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 11:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink app: Fix rate handling

Browse Source
This commit is contained in:
Lorenz Meier
2016-07-10 17:05:33 +02:00
parent c7130081cc
commit 71d150f6ec
5 changed files with 50 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+23 -18
View File
@@ -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
+9 -8
View File
@@ -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
+6
View File
@@ -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
+2
View File
@@ -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
+10
View File
@@ -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;