diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-22 23:16:45 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-28 13:07:01 -0700 |
commit | 78741c87e5415c9e894f619e28b145e127576a56 (patch) | |
tree | 424876901d5b92ab661fc5ab67791e78ea414806 /src/modules/mavlink/mavlink_parameters.cpp | |
parent | 4c6ddf93724e51d8af914671bfd1090f42ce09a6 (diff) | |
download | px4-firmware-78741c87e5415c9e894f619e28b145e127576a56.tar.gz px4-firmware-78741c87e5415c9e894f619e28b145e127576a56.tar.bz2 px4-firmware-78741c87e5415c9e894f619e28b145e127576a56.zip |
MAVLink app: 1) only transmit active params, 2) send params faster, 3) ensure no overflow occurs on buffer when sending at higher rate.
Diffstat (limited to 'src/modules/mavlink/mavlink_parameters.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_parameters.cpp | 23 |
1 files changed, 19 insertions, 4 deletions
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index e910f674a..e7e0c11df 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -184,8 +184,23 @@ MavlinkParametersManager::send(const hrt_abstime t) { /* send all parameters if requested */ if (_send_all_index >= 0) { - send_param(param_for_index(_send_all_index)); - _send_all_index++; + + /* skip if no space is available */ + if (_mavlink->get_free_tx_buf() < get_size()) { + return; + } + + /* look for the first parameter which is used */ + param_t p; + do { + p = param_for_index(_send_all_index); + _send_all_index++; + } while (p != PARAM_INVALID && !param_used(p)); + + if (p != PARAM_INVALID) { + send_param(p); + } + if (_send_all_index >= (int) param_count()) { _send_all_index = -1; } @@ -209,8 +224,8 @@ MavlinkParametersManager::send_param(param_t param) return; } - msg.param_count = param_count(); - msg.param_index = param_get_index(param); + msg.param_count = param_count_used(); + msg.param_index = param_get_used_index(param); /* copy parameter name */ strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); |