PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_command_sender.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file mavlink_command_sender.cpp
36  * Mavlink commands sender with support for retransmission.
37  *
38  * @author Julian Oes <julian@oes.ch>
39  */
40 
41 #include "mavlink_command_sender.h"
42 #include <px4_platform_common/log.h>
43 
44 #define CMD_DEBUG(FMT, ...) PX4_LOG_NAMED_COND("cmd sender", _debug_enabled, FMT, ##__VA_ARGS__)
45 
48 
50 {
51  px4_sem_init(&_lock, 1, 1);
52 
53  if (_instance == nullptr) {
55  }
56 }
57 
59 {
60  return *_instance;
61 }
62 
64 {
65  px4_sem_destroy(&_lock);
66 }
67 
68 int MavlinkCommandSender::handle_vehicle_command(const struct vehicle_command_s &command, mavlink_channel_t channel)
69 {
70  lock();
71  CMD_DEBUG("new command: %d (channel: %d)", command.command, channel);
72 
73  mavlink_command_long_t msg = {};
74  msg.target_system = command.target_system;
75  msg.target_component = command.target_component;
76  msg.command = command.command;
77  msg.confirmation = command.confirmation;
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);
86 
87  bool already_existing = false;
88  _commands.reset_to_start();
89 
90  while (command_item_t *item = _commands.get_next()) {
91  if (item->timestamp_us == command.timestamp) {
92 
93  // We should activate the channel by setting num_sent_per_channel from -1 to 0.
94  item->num_sent_per_channel[channel] = 0;
95  already_existing = true;
96  break;
97  }
98  }
99 
100  if (!already_existing) {
101 
102  command_item_t new_item;
103  new_item.command = msg;
104  new_item.timestamp_us = command.timestamp;
105  new_item.num_sent_per_channel[channel] = 0;
107  _commands.put(new_item);
108  }
109 
110  unlock();
111  return 0;
112 }
113 
114 void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_t &ack,
115  uint8_t from_sysid, uint8_t from_compid, uint8_t channel)
116 {
117  CMD_DEBUG("handling result %d for command %d (from %d:%d)",
118  ack.result, ack.command, from_sysid, from_compid);
119  lock();
120 
121  _commands.reset_to_start();
122 
123  while (command_item_t *item = _commands.get_next()) {
124  // Check if the incoming ack matches any of the commands that we have sent.
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; // mark this as acknowledged
130  break;
131  }
132  }
133 
134  unlock();
135 }
136 
137 void MavlinkCommandSender::check_timeout(mavlink_channel_t channel)
138 {
139  lock();
140 
141  _commands.reset_to_start();
142 
143  while (command_item_t *item = _commands.get_next()) {
144  if (hrt_elapsed_time(&item->last_time_sent_us) <= TIMEOUT_US) {
145  // We keep waiting for the timeout.
146  continue;
147  }
148 
149  // Loop through num_sent_per_channel and check if any channel has receives an ack for this command
150  // (indicated by the value -2). We avoid removing the command at the time of receiving the ack
151  // as some channels might be lagging behind and will end up putting the same command into the buffer.
152  bool dropped_command = false;
153 
154  for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) {
155  if (item->num_sent_per_channel[i] == -2) {
156  _commands.drop_current();
157  dropped_command = true;
158  break;
159  }
160  }
161 
162  if (dropped_command) {
163  continue;
164  }
165 
166  // The goal of this is to retry from all channels. Therefore, we keep
167  // track of the retry count for each channel.
168  //
169  // When the first channel does a retry, the timeout is reset.
170  // (e.g. all channel have done 2 retries, then channel 0 is called
171  // and does retry number 3, and also resets the timeout timestamp).
172 
173  // First, we need to determine what the current max and min retry level
174  // are because we can only level up, if all have caught up.
175  // If num_sent_per_channel is at -1, the channel is inactive.
176  int8_t max_sent = 0;
177  int8_t min_sent = INT8_MAX;
178 
179  for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) {
180  if (item->num_sent_per_channel[i] > max_sent) {
181  max_sent = item->num_sent_per_channel[i];
182  }
183 
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];
187  }
188  }
189 
190  if (item->num_sent_per_channel[channel] < max_sent && item->num_sent_per_channel[channel] != -1) {
191  // We are behind and need to do a retransmission.
192  mavlink_msg_command_long_send_struct(channel, &item->command);
193  item->num_sent_per_channel[channel]++;
194 
195  CMD_DEBUG("command %d sent (not first, retries: %d/%d, channel: %d)",
196  item->command.command,
197  item->num_sent_per_channel[channel],
198  max_sent,
199  channel);
200 
201  } else if (item->num_sent_per_channel[channel] == max_sent &&
202  min_sent == max_sent) {
203 
204  // If the next retry would be above the needed retries anyway, we can
205  // drop the item, and continue with other items.
206  if (item->num_sent_per_channel[channel] + 1 > RETRIES) {
207  CMD_DEBUG("command %d dropped", item->command.command);
208  _commands.drop_current();
209  continue;
210  }
211 
212  // We are the first of a new retransmission series.
213  mavlink_msg_command_long_send_struct(channel, &item->command);
214  item->num_sent_per_channel[channel]++;
215  // Therefore, we are the ones setting the timestamp of this retry round.
216  item->last_time_sent_us = hrt_absolute_time();
217 
218  CMD_DEBUG("command %d sent (first, retries: %d/%d, channel: %d)",
219  item->command.command,
220  item->num_sent_per_channel[channel],
221  max_sent,
222  channel);
223 
224  } else {
225  // We are already ahead, so this should not happen.
226  // If it ever does, just ignore it. It will timeout eventually.
227  continue;
228  }
229  }
230 
231  unlock();
232 }
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).