Line data Source code
1 : #include "GCS_config.h"
2 :
3 : #if HAL_GCS_ENABLED
4 :
5 : #include "MissionItemProtocol.h"
6 :
7 : #include "GCS.h"
8 :
9 394 : void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link,
10 : const mavlink_message_t &msg,
11 : const int16_t _request_first,
12 : const int16_t _request_last)
13 : {
14 : // set variables to help handle the expected receiving of commands from the GCS
15 394 : timelast_receive_ms = AP_HAL::millis(); // set time we last received commands to now
16 394 : receiving = true; // record that we expect to receive commands
17 394 : request_i = _request_first; // reset the next expected command number to zero
18 394 : request_last = _request_last; // record how many commands we expect to receive
19 :
20 394 : dest_sysid = msg.sysid; // record system id of GCS who wants to upload the mission
21 394 : dest_compid = msg.compid; // record component id of GCS who wants to upload the mission
22 :
23 394 : link = &_link;
24 :
25 394 : timelast_request_ms = AP_HAL::millis();
26 394 : link->send_message(next_item_ap_message_id());
27 :
28 394 : mission_item_warning_sent = false;
29 394 : mission_request_warning_sent = false;
30 394 : }
31 :
32 4 : void MissionItemProtocol::handle_mission_clear_all(const GCS_MAVLINK &_link,
33 : const mavlink_message_t &msg)
34 : {
35 4 : bool success = true;
36 4 : success = success && cancel_upload(_link, msg);
37 4 : success = success && clear_all_items();
38 4 : send_mission_ack(_link, msg, success ? MAV_MISSION_ACCEPTED : MAV_MISSION_ERROR);
39 4 : }
40 :
41 9231 : bool MissionItemProtocol::mavlink2_requirement_met(const GCS_MAVLINK &_link, const mavlink_message_t &msg) const
42 : {
43 : // need mavlink2 to do mission types other than mission:
44 9231 : if (mission_type() == MAV_MISSION_TYPE_MISSION) {
45 6337 : return true;
46 : }
47 2894 : if (!_link.sending_mavlink1()) {
48 2894 : return true;
49 : }
50 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Need mavlink2 for item transfer");
51 0 : send_mission_ack(_link, msg, MAV_MISSION_UNSUPPORTED);
52 0 : return false;
53 : }
54 :
55 : // returns true if we are either not receiving, or we successfully
56 : // cancelled an existing upload:
57 3181 : bool MissionItemProtocol::cancel_upload(const GCS_MAVLINK &_link, const mavlink_message_t &msg)
58 : {
59 3181 : if (receiving) {
60 : // someone is already uploading a mission. If we are
61 : // receiving from someone then we will allow them to restart -
62 : // otherwise we deny.
63 3 : if (msg.sysid != dest_sysid || msg.compid != dest_compid) {
64 : // reject another upload until
65 1 : send_mission_ack(_link, msg, MAV_MISSION_DENIED);
66 1 : return false;
67 : }
68 : // the upload count may have changed; free resources and
69 : // allocate them again:
70 2 : free_upload_resources();
71 2 : receiving = false;
72 2 : link = nullptr;
73 : }
74 :
75 3180 : return true;
76 : }
77 :
78 3177 : void MissionItemProtocol::handle_mission_count(
79 : GCS_MAVLINK &_link,
80 : const mavlink_mission_count_t &packet,
81 : const mavlink_message_t &msg)
82 : {
83 3177 : if (!mavlink2_requirement_met(_link, msg)) {
84 0 : return;
85 : }
86 :
87 3177 : if (!cancel_upload(_link, msg)) {
88 0 : return;
89 : }
90 :
91 3177 : if (packet.count > max_items()) {
92 : // FIXME: different items take up different storage space!
93 1 : send_mission_ack(_link, msg, MAV_MISSION_NO_SPACE);
94 1 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Only %u items are supported", (unsigned)max_items());
95 1 : return;
96 : }
97 :
98 3176 : MAV_MISSION_RESULT ret_alloc = allocate_receive_resources(packet.count);
99 3176 : if (ret_alloc != MAV_MISSION_ACCEPTED) {
100 0 : send_mission_ack(_link, msg, ret_alloc);
101 0 : return;
102 : }
103 :
104 3176 : truncate(packet);
105 :
106 3176 : if (packet.count == 0) {
107 : // no requests to send...
108 2786 : transfer_is_complete(_link, msg);
109 2786 : return;
110 : }
111 :
112 : // start waypoint receiving
113 390 : init_send_requests(_link, msg, 0, packet.count-1);
114 : }
115 :
116 467 : void MissionItemProtocol::handle_mission_request_list(
117 : const GCS_MAVLINK &_link,
118 : const mavlink_mission_request_list_t &packet,
119 : const mavlink_message_t &msg)
120 : {
121 467 : if (!mavlink2_requirement_met(_link, msg)) {
122 0 : return;
123 : }
124 :
125 467 : if (receiving) {
126 : // someone is uploading a mission; reject fetching of points
127 : // until done or timeout
128 0 : send_mission_ack(_link, msg, MAV_MISSION_DENIED);
129 0 : return;
130 : }
131 :
132 : // reply with number of commands in the mission. The GCS will
133 : // then request each command separately
134 467 : CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_COUNT);
135 467 : mavlink_msg_mission_count_send(_link.get_chan(),
136 467 : msg.sysid,
137 467 : msg.compid,
138 467 : item_count(),
139 467 : mission_type());
140 : }
141 :
142 5582 : void MissionItemProtocol::handle_mission_request_int(GCS_MAVLINK &_link,
143 : const mavlink_mission_request_int_t &packet,
144 : const mavlink_message_t &msg)
145 : {
146 5582 : if (!mavlink2_requirement_met(_link, msg)) {
147 1 : return;
148 : }
149 :
150 5582 : if (receiving) {
151 : // someone is uploading a mission; reject fetching of points
152 : // until done or timeout
153 0 : send_mission_ack(_link, msg, MAV_MISSION_DENIED);
154 0 : return;
155 : }
156 :
157 : mavlink_mission_item_int_t ret_packet;
158 5582 : const MAV_MISSION_RESULT result_code = get_item(packet.seq, ret_packet);
159 5582 : if (result_code != MAV_MISSION_ACCEPTED) {
160 1 : if (result_code == MAV_MISSION_INVALID_SEQUENCE) {
161 : // try to educate the GCS on the actual size of the mission:
162 1 : const mavlink_channel_t chan = _link.get_chan();
163 1 : if (HAVE_PAYLOAD_SPACE(chan, MISSION_COUNT)) {
164 2 : mavlink_msg_mission_count_send(chan,
165 1 : msg.sysid,
166 1 : msg.compid,
167 1 : item_count(),
168 1 : mission_type());
169 : }
170 : }
171 : // send failure message
172 1 : send_mission_ack(_link, msg, result_code);
173 1 : return;
174 : }
175 :
176 5581 : ret_packet.target_system = msg.sysid;
177 5581 : ret_packet.target_component = msg.compid;
178 :
179 5581 : _link.send_message(MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char*)&ret_packet);
180 : }
181 :
182 : #if AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED
183 0 : void MissionItemProtocol::handle_mission_request(GCS_MAVLINK &_link,
184 : const mavlink_mission_request_t &packet,
185 : const mavlink_message_t &msg
186 : )
187 : {
188 0 : if (!mavlink2_requirement_met(_link, msg)) {
189 0 : return;
190 : }
191 :
192 : mavlink_mission_item_int_t item_int;
193 0 : MAV_MISSION_RESULT ret = get_item(packet.seq, item_int);
194 0 : if (ret != MAV_MISSION_ACCEPTED) {
195 0 : send_mission_ack(_link, msg, ret);
196 0 : return;
197 : }
198 :
199 0 : item_int.target_system = msg.sysid;
200 0 : item_int.target_component = msg.compid;
201 :
202 0 : mavlink_mission_item_t ret_packet{};
203 0 : ret = AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(item_int, ret_packet);
204 0 : if (ret != MAV_MISSION_ACCEPTED) {
205 0 : send_mission_ack(_link, msg, ret);
206 0 : return;
207 : }
208 :
209 0 : if (!mission_request_warning_sent) {
210 0 : mission_request_warning_sent = true;
211 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "got MISSION_REQUEST; use MISSION_REQUEST_INT!");
212 : }
213 :
214 : // buffer space is checked by send_message
215 0 : _link.send_message(MAVLINK_MSG_ID_MISSION_ITEM, (const char*)&ret_packet);
216 : }
217 : #endif // AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED
218 :
219 4 : void MissionItemProtocol::send_mission_item_warning()
220 : {
221 4 : if (mission_item_warning_sent) {
222 3 : return;
223 : }
224 1 : mission_item_warning_sent = true;
225 1 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "got MISSION_ITEM; GCS should send MISSION_ITEM_INT");
226 : }
227 :
228 5 : void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link,
229 : const mavlink_message_t &msg,
230 : const mavlink_mission_write_partial_list_t &packet)
231 : {
232 5 : if (!mavlink2_requirement_met(_link, msg)) {
233 0 : return;
234 : }
235 :
236 5 : if (receiving) {
237 : // someone is already uploading a mission. Deny ability to
238 : // write a partial list here as they might be trying to
239 : // overwrite a subset of the waypoints which the current
240 : // transfer is uploading, and that may lead to storing a whole
241 : // bunch of empty items.
242 1 : send_mission_ack(_link, msg, MAV_MISSION_DENIED);
243 1 : return;
244 : }
245 :
246 : // start waypoint receiving
247 4 : if ((unsigned)packet.start_index > item_count() ||
248 8 : (unsigned)packet.end_index > item_count() ||
249 4 : packet.end_index < packet.start_index) {
250 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"Flight plan update rejected"); // FIXME: Remove this anytime after 2020-01-22
251 0 : send_mission_ack(_link, msg, MAV_MISSION_ERROR);
252 0 : return;
253 : }
254 :
255 4 : MAV_MISSION_RESULT ret_alloc = allocate_update_resources();
256 4 : if (ret_alloc != MAV_MISSION_ACCEPTED) {
257 0 : send_mission_ack(_link, msg, ret_alloc);
258 0 : return;
259 : }
260 :
261 4 : init_send_requests(_link, msg, packet.start_index, packet.end_index);
262 : }
263 :
264 5239 : void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, const mavlink_mission_item_int_t &cmd)
265 : {
266 5239 : if (link == nullptr) {
267 0 : INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
268 0 : return;
269 : }
270 :
271 : // check if this is the requested waypoint
272 5239 : if (cmd.seq != request_i) {
273 0 : send_mission_ack(msg, MAV_MISSION_INVALID_SEQUENCE);
274 0 : return;
275 : }
276 : // make sure the item is coming from the system that initiated the upload
277 5239 : if (msg.sysid != dest_sysid) {
278 0 : send_mission_ack(msg, MAV_MISSION_DENIED);
279 0 : return;
280 : }
281 5239 : if (msg.compid != dest_compid) {
282 0 : send_mission_ack(msg, MAV_MISSION_DENIED);
283 0 : return;
284 : }
285 :
286 5239 : const uint16_t _item_count = item_count();
287 :
288 : MAV_MISSION_RESULT result;
289 5239 : if (cmd.seq < _item_count) {
290 : // command index is within the existing list, replace the command
291 2458 : result = replace_item(cmd);
292 2781 : } else if (cmd.seq == _item_count) {
293 : // command is at the end of command list, add the command
294 2781 : result = append_item(cmd);
295 : } else {
296 : // beyond the end of the command list, return an error
297 0 : result = MAV_MISSION_ERROR;
298 : }
299 5239 : if (result != MAV_MISSION_ACCEPTED) {
300 0 : send_mission_ack(msg, result);
301 0 : receiving = false;
302 0 : link = nullptr;
303 0 : free_upload_resources();
304 0 : return;
305 : }
306 :
307 : // update waypoint receiving state machine
308 5239 : timelast_receive_ms = AP_HAL::millis();
309 5239 : request_i++;
310 :
311 5239 : if (request_i > request_last) {
312 388 : transfer_is_complete(*link, msg);
313 388 : return;
314 : }
315 : // if we have enough space, then send the next WP request immediately
316 4851 : if (HAVE_PAYLOAD_SPACE(link->get_chan(), MISSION_REQUEST)) {
317 4851 : queued_request_send();
318 : } else {
319 0 : link->send_message(next_item_ap_message_id());
320 : }
321 : }
322 :
323 3174 : void MissionItemProtocol::transfer_is_complete(const GCS_MAVLINK &_link, const mavlink_message_t &msg)
324 : {
325 3174 : const MAV_MISSION_RESULT result = complete(_link);
326 3174 : send_mission_ack(_link, msg, result);
327 3174 : free_upload_resources();
328 3174 : receiving = false;
329 3174 : link = nullptr;
330 3174 : }
331 :
332 0 : void MissionItemProtocol::send_mission_ack(const mavlink_message_t &msg,
333 : MAV_MISSION_RESULT result) const
334 : {
335 0 : if (link == nullptr) {
336 0 : INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
337 0 : return;
338 : }
339 0 : send_mission_ack(*link, msg, result);
340 : }
341 3182 : void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link,
342 : const mavlink_message_t &msg,
343 : MAV_MISSION_RESULT result) const
344 : {
345 3182 : CHECK_PAYLOAD_SIZE2_VOID(_link.get_chan(), MISSION_ACK);
346 3182 : mavlink_msg_mission_ack_send(_link.get_chan(),
347 3182 : msg.sysid,
348 3182 : msg.compid,
349 : result,
350 3182 : mission_type());
351 : }
352 :
353 : /**
354 : * @brief Send the next pending waypoint, called from deferred message
355 : * handling code
356 : */
357 5273 : void MissionItemProtocol::queued_request_send()
358 : {
359 5273 : if (!receiving) {
360 0 : return;
361 : }
362 5273 : if (request_i > request_last) {
363 0 : return;
364 : }
365 5273 : if (link == nullptr) {
366 0 : INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
367 0 : return;
368 : }
369 5273 : CHECK_PAYLOAD_SIZE2_VOID(link->get_chan(), MISSION_REQUEST);
370 5273 : mavlink_msg_mission_request_send(
371 5273 : link->get_chan(),
372 5273 : dest_sysid,
373 5273 : dest_compid,
374 5273 : request_i,
375 5273 : mission_type());
376 5273 : timelast_request_ms = AP_HAL::millis();
377 : }
378 :
379 125596513 : void MissionItemProtocol::update()
380 : {
381 125596513 : if (!receiving) {
382 : // we don't need to do anything unless we're sending requests
383 125587184 : return;
384 : }
385 9329 : if (link == nullptr) {
386 0 : INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
387 0 : return;
388 : }
389 : // stop waypoint receiving if timeout
390 9329 : const uint32_t tnow = AP_HAL::millis();
391 9329 : if (tnow - timelast_receive_ms > upload_timeout_ms) {
392 4 : receiving = false;
393 4 : timeout();
394 4 : const mavlink_channel_t chan = link->get_chan();
395 4 : if (HAVE_PAYLOAD_SPACE(chan, MISSION_ACK)) {
396 4 : mavlink_msg_mission_ack_send(chan,
397 4 : dest_sysid,
398 4 : dest_compid,
399 : MAV_MISSION_OPERATION_CANCELLED,
400 4 : mission_type());
401 : }
402 4 : link = nullptr;
403 4 : free_upload_resources();
404 4 : return;
405 : }
406 : // resend request if we haven't gotten one:
407 9325 : const uint32_t wp_recv_timeout_ms = 1000U + link->get_stream_slowdown_ms();
408 9325 : if (tnow - timelast_request_ms > wp_recv_timeout_ms) {
409 28 : timelast_request_ms = tnow;
410 28 : link->send_message(next_item_ap_message_id());
411 : }
412 : }
413 :
414 : #endif // HAL_GCS_ENABLED
|
