diff options
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); |