fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink command sender: give channels more time to request command

Browse Source 
- if a channel receives an ack for a command, do not immediately remove the
command item from the send queue but wait until the next ack timeout occurs.
This gives other mavlink channels time to try to put identical commands into
the send queue.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2019-08-02 22:05:18 +02:00 committed by Beat Küng
parent 71791fa8f8
commit 43d006aff2
2 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/mavlink/mavlink_command_sender.cpp
View File

@ -126,9 +126,7 @@ void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_
from_sysid == item->command.target_system &&
from_compid == item->command.target_component &&
item->num_sent_per_channel[channel] != -1) {
// Drop it anyway because the command seems to have arrived at the destination, even if we
// receive IN_PROGRESS because we trust that it will be handled after that.
_commands.drop_current();
item->num_sent_per_channel[channel] = -2; // mark this as acknowledged
break;
}
}
@ -148,6 +146,23 @@ void MavlinkCommandSender::check_timeout(mavlink_channel_t channel)
continue;
}
// Loop through num_sent_per_channel and check if any channel has receives an ack for this command
// (indicated by the value -2). We avoid removing the command at the time of receiving the ack
// as some channels might be lagging behind and will end up putting the same command into the buffer.
bool dropped_command = false;
for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) {
if (item->num_sent_per_channel[i] == -2) {
_commands.drop_current();
dropped_command = true;
break;
}
}
if (dropped_command) {
continue;
}
// The goal of this is to retry from all channels. Therefore, we keep
// track of the retry count for each channel.
//

2
src/modules/mavlink/mavlink_command_sender.h
View File

@ -109,7 +109,7 @@ private:
mavlink_command_long_t command = {};
hrt_abstime timestamp_us = 0;
hrt_abstime last_time_sent_us = 0;
int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL] = {-1, -1, -1, -1};
int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL] = {-1, -1, -1, -1}; // -1: channel did not request this command to be sent, -2: channel got an ack for this command
} command_item_t;
TimestampedList<command_item_t> _commands{3};