42 #include <px4_platform_common/log.h> 44 #define CMD_DEBUG(FMT, ...) PX4_LOG_NAMED_COND("cmd sender", _debug_enabled, FMT, ##__VA_ARGS__) 51 px4_sem_init(&
_lock, 1, 1);
65 px4_sem_destroy(&
_lock);
73 mavlink_command_long_t
msg = {};
78 msg.param1 = command.
param1;
79 msg.param2 = command.
param2;
80 msg.param3 = command.
param3;
81 msg.param4 = command.
param4;
82 msg.param5 = command.
param5;
83 msg.param6 = command.
param6;
84 msg.param7 = command.
param7;
85 mavlink_msg_command_long_send_struct(channel, &msg);
87 bool already_existing =
false;
91 if (item->timestamp_us == command.
timestamp) {
94 item->num_sent_per_channel[channel] = 0;
95 already_existing =
true;
100 if (!already_existing) {
115 uint8_t from_sysid, uint8_t from_compid, uint8_t channel)
117 CMD_DEBUG(
"handling result %d for command %d (from %d:%d)",
118 ack.result, ack.command, from_sysid, from_compid);
125 if (item->command.command == ack.command &&
126 (item->command.target_system == 0 || from_sysid == item->command.target_system) &&
127 (item->command.target_component == 0 || from_compid == item->command.target_component) &&
128 item->num_sent_per_channel[channel] != -1) {
129 item->num_sent_per_channel[channel] = -2;
152 bool dropped_command =
false;
155 if (item->num_sent_per_channel[i] == -2) {
157 dropped_command =
true;
162 if (dropped_command) {
177 int8_t min_sent = INT8_MAX;
180 if (item->num_sent_per_channel[i] > max_sent) {
181 max_sent = item->num_sent_per_channel[i];
184 if ((item->num_sent_per_channel[i] != -1) &&
185 (item->num_sent_per_channel[i] < min_sent)) {
186 min_sent = item->num_sent_per_channel[i];
190 if (item->num_sent_per_channel[channel] < max_sent && item->num_sent_per_channel[channel] != -1) {
192 mavlink_msg_command_long_send_struct(channel, &item->command);
193 item->num_sent_per_channel[channel]++;
195 CMD_DEBUG(
"command %d sent (not first, retries: %d/%d, channel: %d)",
196 item->command.command,
197 item->num_sent_per_channel[channel],
201 }
else if (item->num_sent_per_channel[channel] == max_sent &&
202 min_sent == max_sent) {
206 if (item->num_sent_per_channel[channel] + 1 >
RETRIES) {
207 CMD_DEBUG(
"command %d dropped", item->command.command);
213 mavlink_msg_command_long_send_struct(channel, &item->command);
214 item->num_sent_per_channel[channel]++;
218 CMD_DEBUG(
"command %d sent (first, retries: %d/%d, channel: %d)",
219 item->command.command,
220 item->num_sent_per_channel[channel],
TimestampedList< command_item_t > _commands
mavlink_command_long_t command
static const unsigned MAX_MAVLINK_CHANNEL
#define CMD_DEBUG(FMT,...)
static constexpr uint64_t TIMEOUT_US
static MavlinkCommandSender * _instance
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
void handle_mavlink_command_ack(const mavlink_command_ack_t &ack, uint8_t from_sysid, uint8_t from_compid, uint8_t channel)
Handle mavlink command_ack.
Mavlink commands sender with support for retransmission.
static constexpr uint8_t RETRIES
static void initialize()
initialize: call this once on startup (this function is not thread-safe!)
static MavlinkCommandSender & instance()
int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL]
MavlinkCommandSender()=default
void check_timeout(mavlink_channel_t channel)
Check timeouts to verify if an commands need retransmission.
hrt_abstime last_time_sent_us
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
int handle_vehicle_command(const struct vehicle_command_s &command, mavlink_channel_t channel)
Send a command on a channel and keep it in a queue for retransmission.