aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: send multiple async replies per call to send_queued_param

Browse Source
This commit is contained in:
Peter Barker 2019-01-13 21:13:30 +11:00 committed by Andrew Tridgell
parent 99ea5d1a5e
commit 8fd8ae660b
2 changed files with 18 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -604,7 +604,7 @@ private:
void param_io_timer(void); void param_io_timer(void);
// send an async parameter reply // send an async parameter reply
void send_parameter_reply(void); bool send_parameter_reply(void);
void send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const; void send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const;

27
libraries/GCS_MAVLink/GCS_Param.cpp
View File

@ -39,13 +39,6 @@ GCS_MAVLINK::queued_param_send()
return; return;
} }
// send one parameter async reply if pending
send_parameter_reply();
if (_queued_parameter == nullptr) {
return;
}
uint32_t tnow = AP_HAL::millis(); uint32_t tnow = AP_HAL::millis();
uint32_t tstart = AP_HAL::micros(); uint32_t tstart = AP_HAL::micros();
@ -69,7 +62,18 @@ GCS_MAVLINK::queued_param_send()
count = 5; count = 5;
} }
while (_queued_parameter != nullptr && count--) { // send parameter async replies
while (count && send_parameter_reply()) {
// do nothing
_queued_parameter_send_time_ms = tnow;
count--;
}
if (_queued_parameter == nullptr) {
return;
}
while (count && _queued_parameter != nullptr) {
char param_name[AP_MAX_NAME_SIZE]; char param_name[AP_MAX_NAME_SIZE];
_queued_parameter->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true); _queued_parameter->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
@ -88,6 +92,7 @@ GCS_MAVLINK::queued_param_send()
// don't use more than 1ms sending blocks of parameters // don't use more than 1ms sending blocks of parameters
break; break;
} }
count--;
} }
_queued_parameter_send_time_ms = tnow; _queued_parameter_send_time_ms = tnow;
} }
@ -373,13 +378,13 @@ void GCS_MAVLINK::param_io_timer(void)
/* /*
send a reply to a PARAM_REQUEST_READ send a reply to a PARAM_REQUEST_READ
*/ */
void GCS_MAVLINK::send_parameter_reply(void) bool GCS_MAVLINK::send_parameter_reply(void)
{ {
struct pending_param_reply reply; struct pending_param_reply reply;
if (!param_replies.pop(reply)) { if (!param_replies.pop(reply)) {
// nothing to do // nothing to do
return; return false;
} }
mavlink_msg_param_value_send( mavlink_msg_param_value_send(
@ -389,6 +394,8 @@ void GCS_MAVLINK::send_parameter_reply(void)
mav_var_type(reply.p_type), mav_var_type(reply.p_type),
reply.count, reply.count,
reply.param_index); reply.param_index);
return true;
} }
void GCS_MAVLINK::handle_common_param_message(mavlink_message_t *msg) void GCS_MAVLINK::handle_common_param_message(mavlink_message_t *msg)