Line data Source code
1 : /// @file AP_Mission.cpp
2 : /// @brief Handles the MAVLINK command mission stack. Reads and writes mission to storage.
3 :
4 : #include "AP_Mission_config.h"
5 :
6 : #if AP_MISSION_ENABLED
7 :
8 : #include <AP_AHRS/AP_AHRS.h>
9 : #include <AP_BoardConfig/AP_BoardConfig.h>
10 : #include <AP_Camera/AP_Camera.h>
11 : #include <AP_Gripper/AP_Gripper_config.h>
12 : #include "AP_Mission.h"
13 : #include <AP_Scripting/AP_Scripting.h>
14 : #include <AP_ServoRelayEvents/AP_ServoRelayEvents_config.h>
15 : #include <AP_Terrain/AP_Terrain.h>
16 : #include <AP_Vehicle/AP_Vehicle_Type.h>
17 : #include <GCS_MAVLink/GCS.h>
18 : #include <RC_Channel/RC_Channel_config.h>
19 : #include <AC_Fence/AC_Fence.h>
20 : #include <AP_Logger/AP_Logger.h>
21 :
22 : const AP_Param::GroupInfo AP_Mission::var_info[] = {
23 :
24 : // @Param: TOTAL
25 : // @DisplayName: Total mission commands
26 : // @Description: The number of mission mission items that has been loaded by the ground station. Do not change this manually.
27 : // @Range: 0 32766
28 : // @Increment: 1
29 : // @User: Advanced
30 : // @ReadOnly: True
31 : AP_GROUPINFO_FLAGS("TOTAL", 0, AP_Mission, _cmd_total, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
32 :
33 : // @Param: RESTART
34 : // @DisplayName: Mission Restart when entering Auto mode
35 : // @Description: Controls mission starting point when entering Auto mode (either restart from beginning of mission or resume from last command run)
36 : // @Values: 0:Resume Mission, 1:Restart Mission
37 : // @User: Advanced
38 : AP_GROUPINFO("RESTART", 1, AP_Mission, _restart, AP_MISSION_RESTART_DEFAULT),
39 :
40 : // @Param: OPTIONS
41 : // @DisplayName: Mission options bitmask
42 : // @Description: Bitmask of what options to use in missions. If the DontZeroCounter counter option is set than on completion of a jump loop the counter is left at zero, so the jump will not happen again if the loop is re-entered.
43 : // @Bitmask: 0:Clear Mission on reboot, 1:Use distance to land calc on battery failsafe,2:ContinueAfterLand, 3:DontZeroCounter
44 : // @Bitmask{Copter}: 0:Clear Mission on reboot, 2:ContinueAfterLand
45 : // @Bitmask{Rover, Sub}: 0:Clear Mission on reboot
46 : // @User: Advanced
47 : AP_GROUPINFO("OPTIONS", 2, AP_Mission, _options, AP_MISSION_OPTIONS_DEFAULT),
48 :
49 : AP_GROUPEND
50 : };
51 :
52 : extern const AP_HAL::HAL& hal;
53 :
54 : // storage object
55 : StorageAccess AP_Mission::_storage(StorageManager::StorageMission);
56 :
57 : HAL_Semaphore AP_Mission::_rsem;
58 :
59 : ///
60 : /// public mission methods
61 : ///
62 :
63 : /// init - initialises this library including checks the version in eeprom matches this library
64 2054 : void AP_Mission::init()
65 : {
66 : #if AP_SDCARD_STORAGE_ENABLED
67 : // check for extra storage on microsd
68 2054 : const auto *bc = AP::boardConfig();
69 2054 : if (bc != nullptr) {
70 2054 : const auto size_kb = bc->get_sdcard_mission_kb();
71 2054 : if (size_kb > 0) {
72 2 : _failed_sdcard_storage = !_storage.attach_file(AP_MISSION_SDCARD_FILENAME, size_kb);
73 2 : if (_failed_sdcard_storage) {
74 : // wipe mission if storage not available, but don't save. This allows sdcard error to be fixed and reboot
75 0 : _cmd_total.set(0);
76 : }
77 : }
78 : }
79 : #endif
80 :
81 : // work out maximum index for our storage size
82 2054 : if (_storage.size() >= AP_MISSION_EEPROM_COMMAND_SIZE+4) {
83 2054 : _commands_max = (_storage.size()-4U) / AP_MISSION_EEPROM_COMMAND_SIZE;
84 : }
85 2054 : if (_cmd_total.get() > _commands_max) {
86 : // wipe mission if storage not available, but don't save. This allows sdcard error to be fixed and reboot
87 1 : _cmd_total.set(0);
88 : }
89 :
90 :
91 : // check_eeprom_version - checks version of missions stored in eeprom matches this library
92 : // command list will be cleared if they do not match
93 2054 : check_eeprom_version();
94 :
95 : // initialize the jump tracking array
96 2054 : init_jump_tracking();
97 :
98 : // If Mission Clear bit is set then it should clear the mission, otherwise retain the mission.
99 2054 : if (option_is_set(Option::CLEAR_ON_BOOT)) {
100 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Clearing Mission");
101 1 : clear();
102 : }
103 :
104 2054 : _last_change_time_ms = AP_HAL::millis();
105 2054 : }
106 :
107 : /// start - resets current commands to point to the beginning of the mission
108 : /// To-Do: should we validate the mission first and return true/false?
109 96 : void AP_Mission::start()
110 : {
111 96 : _flags.state = MISSION_RUNNING;
112 :
113 96 : reset(); // reset mission to the first command, resets jump tracking
114 :
115 : // advance to the first command
116 96 : if (!advance_current_nav_cmd()) {
117 : // on failure set mission complete
118 2 : complete();
119 : }
120 96 : }
121 :
122 : /// stop - stops mission execution. subsequent calls to update() will have no effect until the mission is started or resumed
123 178 : void AP_Mission::stop()
124 : {
125 178 : _flags.state = MISSION_STOPPED;
126 178 : }
127 :
128 : /// resume - continues the mission execution from where we last left off
129 : /// previous running commands will be re-initialized
130 365 : void AP_Mission::resume()
131 : {
132 : // if mission had completed then start it from the first command
133 365 : if (_flags.state == MISSION_COMPLETE) {
134 24 : start();
135 24 : return;
136 : }
137 :
138 : // if mission had stopped then restart it
139 341 : if (_flags.state == MISSION_STOPPED) {
140 317 : _flags.state = MISSION_RUNNING;
141 :
142 : // if no valid nav command index restart from beginning
143 317 : if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
144 62 : start();
145 62 : return;
146 : }
147 : }
148 :
149 : // ensure cache coherence
150 279 : if (!read_cmd_from_storage(_nav_cmd.index, _nav_cmd)) {
151 : // if we failed to read the command from storage, then the command may have
152 : // been from a previously loaded mission it is illogical to ever resume
153 : // flying to a command that has been excluded from the current mission
154 10 : start();
155 10 : return;
156 : }
157 :
158 : // rewind the mission wp if the repeat distance has been set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
159 269 : if (_repeat_dist > 0 && _wp_index_history[LAST_WP_PASSED] != AP_MISSION_CMD_INDEX_NONE) {
160 : // if not already in a resume state calculate the position to rewind to
161 0 : Mission_Command tmp_cmd;
162 0 : if (!_flags.resuming_mission && calc_rewind_pos(tmp_cmd)) {
163 0 : _resume_cmd = tmp_cmd;
164 : }
165 :
166 : // resume mission to rewound position
167 0 : if (_resume_cmd.index != AP_MISSION_CMD_INDEX_NONE && start_command(_resume_cmd)) {
168 0 : _nav_cmd = _resume_cmd;
169 0 : _flags.nav_cmd_loaded = true;
170 : // set flag to prevent history being re-written
171 0 : _flags.resuming_mission = true;
172 0 : return;
173 : }
174 : }
175 :
176 : // restart active navigation command. We run these on resume()
177 : // regardless of whether the mission was stopped, as we may be
178 : // re-entering AUTO mode and the nav_cmd callback needs to be run
179 : // to setup the current target waypoint
180 :
181 269 : if (_flags.do_cmd_loaded && _do_cmd.index != AP_MISSION_CMD_INDEX_NONE) {
182 : // restart the active do command, which will also load the nav command for us
183 61 : set_current_cmd(_do_cmd.index);
184 208 : } else if (_flags.nav_cmd_loaded) {
185 : // restart the active nav command
186 208 : set_current_cmd(_nav_cmd.index);
187 : }
188 :
189 : // Note: if there is no active command then the mission must have been stopped just after the previous nav command completed
190 : // update will take care of finding and starting the nav command
191 : }
192 :
193 : /// check if the next nav command is a takeoff, skipping delays
194 1356 : bool AP_Mission::is_takeoff_next(uint16_t cmd_index)
195 : {
196 1356 : Mission_Command cmd = {};
197 : // check a maximum of 16 items, remembering that missions can have
198 : // loops in them
199 1357 : for (uint8_t i=0; i<16; i++, cmd_index++) {
200 1357 : if (!get_next_nav_cmd(cmd_index, cmd)) {
201 27 : return false;
202 : }
203 1330 : switch (cmd.id) {
204 : // any of these are considered a takeoff command:
205 593 : case MAV_CMD_NAV_VTOL_TAKEOFF:
206 : case MAV_CMD_NAV_TAKEOFF:
207 : case MAV_CMD_NAV_TAKEOFF_LOCAL:
208 593 : return true;
209 : // any of these are considered "skippable" when determining if
210 : // we "start with a takeoff command"
211 1 : case MAV_CMD_DO_AUX_FUNCTION:
212 : case MAV_CMD_NAV_DELAY:
213 1 : continue;
214 736 : default:
215 736 : return false;
216 : }
217 : }
218 0 : return false;
219 : }
220 :
221 : /// check mission starts with a takeoff command
222 1356 : bool AP_Mission::starts_with_takeoff_cmd()
223 : {
224 1356 : uint16_t cmd_index = _restart ? AP_MISSION_CMD_INDEX_NONE : _nav_cmd.index;
225 1356 : if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
226 4 : cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
227 : }
228 1356 : return is_takeoff_next(cmd_index);
229 : }
230 :
231 : /*
232 : return true if MIS_OPTIONS is set to allow continue of mission
233 : logic after a land and the next waypoint is a takeoff. If this
234 : is false then after a landing is complete the vehicle should
235 : disarm and mission logic should stop
236 : */
237 9 : bool AP_Mission::continue_after_land_check_for_takeoff()
238 : {
239 9 : if (!continue_after_land()) {
240 9 : return false;
241 : }
242 0 : if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
243 0 : return false;
244 : }
245 0 : return is_takeoff_next(_nav_cmd.index+1);
246 : }
247 :
248 : /// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()
249 339 : void AP_Mission::start_or_resume()
250 : {
251 339 : if (_restart == 1 && !_force_resume) {
252 0 : start();
253 : } else {
254 339 : resume();
255 339 : _force_resume = false;
256 : }
257 339 : }
258 :
259 : /// reset - reset mission to the first command
260 1037 : void AP_Mission::reset()
261 : {
262 1037 : _flags.nav_cmd_loaded = false;
263 1037 : _flags.do_cmd_loaded = false;
264 1037 : _flags.do_cmd_all_done = false;
265 1037 : _flags.in_landing_sequence = false;
266 1037 : _flags.in_return_path = false;
267 1037 : _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
268 1037 : _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
269 1037 : _prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
270 1037 : _prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
271 1037 : _prev_nav_cmd_id = AP_MISSION_CMD_ID_NONE;
272 1037 : init_jump_tracking();
273 1037 : reset_wp_history();
274 1037 : }
275 :
276 : /// clear - clears out mission
277 : /// returns false if mission was running so it could not be cleared
278 300 : bool AP_Mission::clear()
279 : {
280 : // do not allow clearing the mission while it is running unless disarmed
281 300 : if (hal.util->get_soft_armed() && _flags.state == MISSION_RUNNING) {
282 0 : return false;
283 : }
284 :
285 : // remove all commands
286 300 : truncate(0);
287 :
288 : // clear index to commands
289 300 : _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
290 300 : _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
291 300 : _flags.nav_cmd_loaded = false;
292 300 : _flags.do_cmd_loaded = false;
293 300 : _flags.state = MISSION_STOPPED;
294 : // return success
295 300 : return true;
296 : }
297 :
298 :
299 : /// trucate - truncate any mission items beyond index
300 1584 : void AP_Mission::truncate(uint16_t index)
301 : {
302 1584 : if ((unsigned)_cmd_total > index) {
303 699 : _cmd_total.set_and_save(index);
304 699 : _last_change_time_ms = AP_HAL::millis();
305 : }
306 1584 : }
307 :
308 : /// update - ensures the command queues are loaded with the next command and calls main programs command_init and command_verify functions to progress the mission
309 : /// should be called at 10hz or higher
310 4363001 : void AP_Mission::update()
311 : {
312 : // exit immediately if not running or no mission commands
313 4363001 : if (_flags.state != MISSION_RUNNING || _cmd_total == 0) {
314 115761 : return;
315 : }
316 :
317 4247240 : update_exit_position();
318 :
319 : // mission_change events
320 4247240 : if (_last_change_time_prev_ms != _last_change_time_ms) {
321 340 : _last_change_time_prev_ms = _last_change_time_ms;
322 340 : on_mission_timestamp_change();
323 : }
324 :
325 : // save persistent waypoint_num for watchdog restore
326 4247240 : hal.util->persistent_data.waypoint_num = _nav_cmd.index;
327 :
328 : // check if we have an active nav command
329 4247240 : if (!_flags.nav_cmd_loaded || _nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
330 : // advance in mission if no active nav command
331 19 : if (!advance_current_nav_cmd()) {
332 : // failure to advance nav command means mission has completed
333 0 : complete();
334 0 : return;
335 : }
336 : } else {
337 : // run the active nav command
338 4247221 : if (verify_command(_nav_cmd)) {
339 : // market _nav_cmd as complete (it will be started on the next iteration)
340 1436 : _flags.nav_cmd_loaded = false;
341 : // immediately advance to the next mission command
342 1436 : if (!advance_current_nav_cmd()) {
343 : // failure to advance nav command means mission has completed
344 82 : complete();
345 82 : return;
346 : }
347 : }
348 : }
349 :
350 : // check if we have an active do command
351 4247158 : if (!_flags.do_cmd_loaded) {
352 4236782 : advance_current_do_cmd();
353 : } else {
354 : // check the active do command
355 10376 : if (verify_command(_do_cmd)) {
356 : // mark _do_cmd as complete
357 151 : _flags.do_cmd_loaded = false;
358 : }
359 : }
360 : }
361 :
362 : // handle events for when the mission has been updated (but maybe not changed)
363 340 : void AP_Mission::on_mission_timestamp_change()
364 : {
365 340 : _jump_tag.age = 0;
366 340 : }
367 :
368 4257597 : bool AP_Mission::verify_command(const Mission_Command& cmd)
369 : {
370 4257597 : switch (cmd.id) {
371 : // do-commands always return true for verify:
372 : #if AP_GRIPPER_ENABLED
373 38 : case MAV_CMD_DO_GRIPPER:
374 : #endif
375 : case MAV_CMD_DO_SET_SERVO:
376 : case MAV_CMD_DO_SET_RELAY:
377 : case MAV_CMD_DO_REPEAT_SERVO:
378 : case MAV_CMD_DO_REPEAT_RELAY:
379 : case MAV_CMD_DO_DIGICAM_CONFIGURE:
380 : case MAV_CMD_DO_DIGICAM_CONTROL:
381 : case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
382 : #if AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
383 : case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
384 : #endif // AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
385 : case MAV_CMD_DO_PARACHUTE:
386 : case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
387 : case MAV_CMD_DO_SPRAYER:
388 : case MAV_CMD_DO_AUX_FUNCTION:
389 : case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
390 : case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
391 : case MAV_CMD_JUMP_TAG:
392 : case MAV_CMD_IMAGE_START_CAPTURE:
393 : case MAV_CMD_IMAGE_STOP_CAPTURE:
394 : case MAV_CMD_SET_CAMERA_ZOOM:
395 : case MAV_CMD_SET_CAMERA_FOCUS:
396 : case MAV_CMD_SET_CAMERA_SOURCE:
397 : case MAV_CMD_VIDEO_START_CAPTURE:
398 : case MAV_CMD_VIDEO_STOP_CAPTURE:
399 38 : return true;
400 4257559 : default:
401 4257559 : return _cmd_verify_fn(cmd);
402 : }
403 : }
404 :
405 2030 : bool AP_Mission::start_command(const Mission_Command& cmd)
406 : {
407 : #if HAL_LOGGING_ENABLED
408 2030 : if (log_start_mission_item_bit != (uint32_t)-1) {
409 2030 : auto &logger = AP::logger();
410 2030 : if (logger.should_log(log_start_mission_item_bit)) {
411 2026 : logger.Write_MISE(*this, cmd);
412 : }
413 : }
414 : #endif
415 :
416 : // check for landing related commands and set flags
417 2030 : if (is_landing_type_cmd(cmd.id) || cmd.id == MAV_CMD_DO_LAND_START) {
418 234 : _flags.in_landing_sequence = true;
419 :
420 1796 : } else if (cmd.id == MAV_CMD_DO_RETURN_PATH_START) {
421 0 : _flags.in_return_path = true;
422 :
423 1796 : } else if (is_takeoff_type_cmd(cmd.id)) {
424 : // Clear landing and return path flags on takeoff
425 283 : _flags.in_landing_sequence = false;
426 283 : _flags.in_return_path = false;
427 :
428 : }
429 :
430 2030 : switch (cmd.id) {
431 1 : case MAV_CMD_DO_JUMP:
432 : case MAV_CMD_JUMP_TAG:
433 : case MAV_CMD_DO_JUMP_TAG:
434 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u %s %u", cmd.index, cmd.type(), (unsigned)cmd.p1);
435 1 : break;
436 :
437 2029 : default:
438 2029 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u %s", cmd.index, cmd.type());
439 2029 : break;
440 :
441 : }
442 :
443 2030 : switch (cmd.id) {
444 : #if AP_RC_CHANNEL_ENABLED
445 1 : case MAV_CMD_DO_AUX_FUNCTION:
446 1 : return start_command_do_aux_function(cmd);
447 : #endif
448 : #if AP_GRIPPER_ENABLED
449 12 : case MAV_CMD_DO_GRIPPER:
450 12 : return start_command_do_gripper(cmd);
451 : #endif
452 : #if AP_SERVORELAYEVENTS_ENABLED
453 0 : case MAV_CMD_DO_SET_SERVO:
454 : case MAV_CMD_DO_SET_RELAY:
455 : case MAV_CMD_DO_REPEAT_SERVO:
456 : case MAV_CMD_DO_REPEAT_RELAY:
457 0 : return start_command_do_servorelayevents(cmd);
458 : #endif
459 : #if AP_CAMERA_ENABLED
460 21 : case MAV_CMD_DO_DIGICAM_CONFIGURE:
461 : case MAV_CMD_DO_DIGICAM_CONTROL:
462 : case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
463 : case MAV_CMD_IMAGE_START_CAPTURE:
464 : case MAV_CMD_IMAGE_STOP_CAPTURE:
465 : case MAV_CMD_SET_CAMERA_ZOOM:
466 : case MAV_CMD_SET_CAMERA_FOCUS:
467 : case MAV_CMD_SET_CAMERA_SOURCE:
468 : case MAV_CMD_VIDEO_START_CAPTURE:
469 : case MAV_CMD_VIDEO_STOP_CAPTURE:
470 21 : return start_command_camera(cmd);
471 : #endif
472 : #if AP_FENCE_ENABLED
473 0 : case MAV_CMD_DO_FENCE_ENABLE:
474 0 : return start_command_fence(cmd);
475 : #endif
476 3 : case MAV_CMD_DO_PARACHUTE:
477 3 : return start_command_parachute(cmd);
478 0 : case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
479 0 : return start_command_do_scripting(cmd);
480 1 : case MAV_CMD_DO_SPRAYER:
481 1 : return start_command_do_sprayer(cmd);
482 0 : case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
483 0 : return command_do_set_repeat_dist(cmd);
484 0 : case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
485 0 : return start_command_do_gimbal_manager_pitchyaw(cmd);
486 : #if AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
487 3 : case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
488 3 : return start_command_do_set_roi_wpnext_offset(cmd);
489 : #endif // AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
490 1 : case MAV_CMD_JUMP_TAG:
491 1 : _jump_tag.tag = cmd.content.jump.target;
492 1 : _jump_tag.age = 1;
493 : FALLTHROUGH; // fall through in case the vehicle handles tag events
494 1989 : default:
495 1989 : return _cmd_start_fn(cmd);
496 : }
497 : }
498 :
499 : ///
500 : /// public command methods
501 : ///
502 :
503 : /// add_cmd - adds a command to the end of the command list and writes to storage
504 : /// returns true if successfully added, false on failure
505 : /// cmd.index is updated with it's new position in the mission
506 7159 : bool AP_Mission::add_cmd(Mission_Command& cmd)
507 : {
508 : // Add home if its not already present
509 7159 : if (_cmd_total < 1) {
510 112 : write_home_to_storage();
511 : }
512 :
513 : // attempt to write the command to storage
514 7159 : bool ret = write_cmd_to_storage(_cmd_total, cmd);
515 :
516 7159 : if (ret) {
517 : // update command's index
518 7159 : cmd.index = _cmd_total;
519 : // increment total number of commands
520 7159 : _cmd_total.set_and_save(_cmd_total + 1);
521 : }
522 :
523 7159 : return ret;
524 : }
525 :
526 : /// replace_cmd - replaces the command at position 'index' in the command list with the provided cmd
527 : /// replacing the current active command will have no effect until the command is restarted
528 : /// returns true if successfully replaced, false on failure
529 1498 : bool AP_Mission::replace_cmd(uint16_t index, const Mission_Command& cmd)
530 : {
531 : // sanity check index
532 1498 : if (index >= (unsigned)_cmd_total) {
533 0 : return false;
534 : }
535 :
536 : // Writing index zero is not allowed, it must be home
537 1498 : if (index == 0) {
538 : // Really should be returning false in this case, but in order to not break things we return true
539 220 : return true;
540 : }
541 :
542 : // attempt to write the command to storage
543 1278 : return write_cmd_to_storage(index, cmd);
544 : }
545 :
546 : /// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
547 29967 : bool AP_Mission::is_nav_cmd(const Mission_Command& cmd)
548 : {
549 : // NAV commands all have ids below MAV_CMD_NAV_LAST, plus some exceptions
550 30799 : return (cmd.id <= MAV_CMD_NAV_LAST ||
551 832 : cmd.id == MAV_CMD_NAV_SET_YAW_SPEED ||
552 31631 : cmd.id == MAV_CMD_NAV_SCRIPT_TIME ||
553 30799 : cmd.id == MAV_CMD_NAV_ATTITUDE_TIME);
554 : }
555 :
556 : /// get_next_nav_cmd - gets next "navigation" command found at or after start_index
557 : /// returns true if found, false if not found (i.e. reached end of mission command list)
558 : /// accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this)
559 25077 : bool AP_Mission::get_next_nav_cmd(uint16_t start_index, Mission_Command& cmd)
560 : {
561 : // search until the end of the mission command list
562 25459 : for (uint16_t cmd_index = start_index; cmd_index < (unsigned)_cmd_total; cmd_index++) {
563 : // get next command
564 24737 : if (!get_next_cmd(cmd_index, cmd, false)) {
565 : // no more commands so return failure
566 5 : return false;
567 : }
568 : // if found a "navigation" command then return it
569 24732 : if (is_nav_cmd(cmd)) {
570 24350 : return true;
571 : }
572 : }
573 :
574 : // if we got this far we did not find a navigation command
575 722 : return false;
576 : }
577 :
578 : /// get the ground course of the next navigation leg in centidegrees
579 : /// from 0 36000. Return default_angle if next navigation
580 : /// leg cannot be determined
581 4165 : int32_t AP_Mission::get_next_ground_course_cd(int32_t default_angle)
582 : {
583 4165 : Mission_Command cmd;
584 4165 : if (!get_next_nav_cmd(_nav_cmd.index+1, cmd)) {
585 252 : return default_angle;
586 : }
587 : // special handling for nav commands with no target location
588 3913 : switch (cmd.id) {
589 0 : case MAV_CMD_NAV_GUIDED_ENABLE:
590 : case MAV_CMD_NAV_DELAY:
591 0 : return default_angle;
592 :
593 0 : case MAV_CMD_NAV_SET_YAW_SPEED:
594 0 : return (_nav_cmd.content.set_yaw_speed.angle_deg * 100);
595 :
596 3913 : default:
597 3913 : break;
598 : }
599 :
600 3913 : return _nav_cmd.content.location.get_bearing_to(cmd.content.location);
601 : }
602 :
603 : // set_current_cmd - jumps to command specified by index
604 2612 : bool AP_Mission::set_current_cmd(uint16_t index)
605 : {
606 : // Clear flags
607 2612 : _flags.in_landing_sequence = false;
608 2612 : _flags.in_return_path = false;
609 :
610 : // read command to check for DO_LAND_START and DO_RETURN_PATH_START
611 2612 : Mission_Command cmd;
612 2612 : if (read_cmd_from_storage(index, cmd)) {
613 2560 : switch (cmd.id) {
614 65 : case MAV_CMD_DO_LAND_START:
615 65 : _flags.in_landing_sequence = true;
616 65 : break;
617 :
618 0 : case MAV_CMD_DO_RETURN_PATH_START:
619 0 : _flags.in_return_path = true;
620 0 : break;
621 :
622 2495 : default:
623 2495 : break;
624 :
625 : }
626 : }
627 :
628 : // mission command has been set, don't track history.
629 2612 : reset_wp_history();
630 :
631 : // sanity check index and that we have a mission
632 2612 : if (index >= (unsigned)_cmd_total || _cmd_total == 1) {
633 1926 : return false;
634 : }
635 :
636 : // stop the current running do command
637 686 : _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
638 686 : _flags.do_cmd_loaded = false;
639 686 : _flags.do_cmd_all_done = false;
640 :
641 : // stop current nav cmd
642 686 : _flags.nav_cmd_loaded = false;
643 :
644 : // if index is zero then the user wants to completely restart the mission
645 686 : if (index == 0 || _flags.state == MISSION_COMPLETE) {
646 275 : _prev_nav_cmd_id = AP_MISSION_CMD_ID_NONE;
647 275 : _prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
648 275 : _prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
649 : // reset the jump tracking to zero
650 275 : init_jump_tracking();
651 275 : if (index == 0) {
652 274 : index = 1;
653 : }
654 : }
655 :
656 : // if the mission is stopped or completed move the nav_cmd index to the specified point and set the state to stopped
657 : // so that if the user resumes the mission it will begin at the specified index
658 686 : if (_flags.state != MISSION_RUNNING) {
659 : // search until we find next nav command or reach end of command list
660 688 : while (!_flags.nav_cmd_loaded) {
661 : // get next command
662 383 : if (!get_next_cmd(index, cmd, true)) {
663 0 : _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
664 0 : return false;
665 : }
666 :
667 : // check if navigation or "do" command
668 383 : if (is_nav_cmd(cmd)) {
669 : // set current navigation command
670 305 : _nav_cmd = cmd;
671 305 : _flags.nav_cmd_loaded = true;
672 : } else {
673 : // set current do command
674 78 : if (!_flags.do_cmd_loaded) {
675 78 : _do_cmd = cmd;
676 78 : _flags.do_cmd_loaded = true;
677 : }
678 : }
679 : // move onto next command
680 383 : index = cmd.index+1;
681 : }
682 :
683 : // if we have not found a do command then set flag to show there are no do-commands to be run before nav command completes
684 305 : if (!_flags.do_cmd_loaded) {
685 227 : _flags.do_cmd_all_done = true;
686 : }
687 :
688 : // if we got this far then the mission can safely be "resumed" from the specified index so we set the state to "stopped"
689 305 : _flags.state = MISSION_STOPPED;
690 305 : return true;
691 : }
692 :
693 : // the state must be MISSION_RUNNING, allow advance_current_nav_cmd() to manage starting the item
694 381 : if (!advance_current_nav_cmd(index)) {
695 : // on failure set mission complete
696 2 : complete();
697 2 : return false;
698 : }
699 :
700 : // if we got this far we must have successfully advanced the nav command
701 379 : return true;
702 : }
703 :
704 : // restart current navigation command. Used to handle external changes to mission
705 : // returns true on success, false if mission is not running or current nav command is invalid
706 1 : bool AP_Mission::restart_current_nav_cmd()
707 : {
708 : // return immediately if mission is not running
709 1 : if (_flags.state != MISSION_RUNNING) {
710 0 : return false;
711 : }
712 :
713 : // return immediately if nav command index is invalid
714 1 : const uint16_t nav_cmd_index = get_current_nav_index();
715 1 : if ((nav_cmd_index == 0) || (nav_cmd_index >= num_commands())) {
716 1 : return false;
717 : }
718 :
719 0 : return set_current_cmd(_nav_cmd.index);
720 : }
721 :
722 : // returns false on any issue at all.
723 4373 : bool AP_Mission::set_item(uint16_t index, mavlink_mission_item_int_t& src_packet)
724 : {
725 : // this is the on-storage format
726 4373 : AP_Mission::Mission_Command cmd {};
727 :
728 : // can't handle request for anything bigger than the mission size+1...
729 4373 : if (index > num_commands()) {
730 0 : return false;
731 : }
732 :
733 : // convert from mavlink-ish format to storage format, if we can.
734 4373 : if (mavlink_int_to_mission_cmd(src_packet, cmd) != MAV_MISSION_ACCEPTED) {
735 0 : return false;
736 : }
737 :
738 : // Writing index zero is not allowed, it must be home
739 4373 : if (index == 0) {
740 : // If home is not already loaded add it so cmd_total is incremented to 1 as would be expected when the returning true
741 2 : if (_cmd_total < 1) {
742 2 : write_home_to_storage();
743 : }
744 : // Really should be returning false in this case, but in order to not break things we return true
745 2 : return true;
746 : }
747 :
748 : // A request to set the 'next' item after the end is how we add an extra
749 : // item to the list, thus allowing us to write entire missions if needed.
750 4371 : if (index == num_commands()) {
751 4371 : return add_cmd(cmd);
752 : }
753 :
754 : // replacing an existing mission item...
755 0 : return AP_Mission::replace_cmd(index, cmd);
756 : }
757 :
758 9435 : bool AP_Mission::get_item(uint16_t index, mavlink_mission_item_int_t& ret_packet) const
759 : {
760 : mavlink_mission_item_int_t tmp;
761 :
762 : // can't handle request for anything bigger than the mission size...
763 9435 : if (index >= num_commands()) {
764 5 : return false;
765 : }
766 :
767 : // retrieve mission from eeprom
768 9430 : AP_Mission::Mission_Command cmd {};
769 9430 : if (!read_cmd_from_storage(index, cmd)) {
770 0 : return false;
771 : }
772 : // convert into mavlink-ish format for lua and friends.
773 9430 : if (!mission_cmd_to_mavlink_int(cmd, tmp)) {
774 0 : return false;
775 : }
776 :
777 : // set packet's current field to 1 if this is the command being executed
778 9430 : if (cmd.id == (uint16_t)get_current_nav_cmd().index) {
779 0 : tmp.current = 1;
780 : }
781 :
782 9430 : ret_packet = tmp;
783 :
784 9430 : return true;
785 : }
786 :
787 :
788 : struct PACKED Packed_Location_Option_Flags {
789 : uint8_t relative_alt : 1; // 1 if altitude is relative to home
790 : uint8_t unused1 : 1; // unused flag (defined so that loiter_ccw uses the correct bit)
791 : uint8_t loiter_ccw : 1; // 0 if clockwise, 1 if counter clockwise
792 : uint8_t terrain_alt : 1; // this altitude is above terrain
793 : uint8_t origin_alt : 1; // this altitude is above ekf origin
794 : uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location
795 : uint8_t type_specific_bit_0 : 1; // each mission item type can use this for storing 1 bit of extra data
796 : uint8_t type_specific_bit_1 : 1; // each mission item type can use this for storing 1 bit of extra data
797 : };
798 :
799 : struct PACKED PackedLocation {
800 : union {
801 : Packed_Location_Option_Flags flags; ///< options bitmask (1<<0 = relative altitude)
802 : uint8_t options; /// allows writing all flags to eeprom as one byte
803 : };
804 : // by making alt 24 bit we can make p1 in a command 16 bit,
805 : // allowing an accurate angle in centi-degrees. This keeps the
806 : // storage cost per mission item at 15 bytes, and allows mission
807 : // altitudes of up to +/- 83km
808 : int32_t alt:24; ///< param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
809 : int32_t lat; ///< param 3 - Latitude * 10**7
810 : int32_t lng; ///< param 4 - Longitude * 10**7
811 : };
812 :
813 : union PackedContent {
814 : // location
815 : PackedLocation location; // Waypoint location
816 :
817 : // raw bytes, for reading/writing to eeprom. Note that only 10
818 : // bytes are available if a 16 bit command ID is used
819 : uint8_t bytes[12];
820 :
821 : };
822 :
823 : /// load_cmd_from_storage - load command from storage
824 : /// true is return if successful
825 104916 : bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const
826 : {
827 : ASSERT_STORAGE_SIZE(PackedContent, 12);
828 :
829 104916 : WITH_SEMAPHORE(_rsem);
830 :
831 : // special handling for command #0 which is home
832 104916 : if (index == 0) {
833 4417 : cmd = {};
834 4417 : cmd.id = MAV_CMD_NAV_WAYPOINT;
835 4417 : cmd.content.location = AP::ahrs().get_home();
836 4417 : return true;
837 : }
838 100499 : if (index >= (unsigned)_cmd_total || index >= _commands_max) {
839 2699 : return false;
840 : }
841 :
842 : // ensure all bytes of cmd are zeroed
843 97800 : cmd = {};
844 :
845 : // Find out proper location in memory by using the start_byte position + the index
846 : // we can load a command, we don't process it yet
847 : // read WP position
848 97800 : const uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
849 :
850 97800 : PackedContent packed_content {};
851 :
852 97800 : const uint8_t b1 = _storage.read_byte(pos_in_storage);
853 97800 : if (b1 == 0 || b1 == 1) {
854 259 : cmd.id = _storage.read_uint16(pos_in_storage+1);
855 259 : cmd.p1 = _storage.read_uint16(pos_in_storage+3);
856 259 : _storage.read_block(packed_content.bytes, pos_in_storage+5, 10);
857 259 : format_conversion(b1, cmd, packed_content);
858 : } else {
859 97541 : cmd.id = b1;
860 97541 : cmd.p1 = _storage.read_uint16(pos_in_storage+1);
861 97541 : _storage.read_block(packed_content.bytes, pos_in_storage+3, 12);
862 : }
863 :
864 97800 : if (stored_in_location(cmd.id)) {
865 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
866 : // NOTE! no 16-bit command may be stored_in_location as only
867 : // 10 bytes are available for storage and lat/lon/alt required
868 : // 4*sizeof(float) == 12 bytes of storage.
869 94098 : if (b1 == 0) {
870 0 : AP_HAL::panic("May not store location for 16-bit commands");
871 : }
872 : #endif
873 : // Location is not PACKED; field-wise copy it:
874 94098 : cmd.content.location.relative_alt = packed_content.location.flags.relative_alt;
875 94098 : cmd.content.location.loiter_ccw = packed_content.location.flags.loiter_ccw;
876 94098 : cmd.content.location.terrain_alt = packed_content.location.flags.terrain_alt;
877 94098 : cmd.content.location.origin_alt = packed_content.location.flags.origin_alt;
878 94098 : cmd.content.location.loiter_xtrack = packed_content.location.flags.loiter_xtrack;
879 94098 : cmd.content.location.alt = packed_content.location.alt;
880 94098 : cmd.content.location.lat = packed_content.location.lat;
881 94098 : cmd.content.location.lng = packed_content.location.lng;
882 :
883 94098 : if (packed_content.location.flags.type_specific_bit_0) {
884 143 : cmd.type_specific_bits |= 1U << 0;
885 : }
886 94098 : if (packed_content.location.flags.type_specific_bit_1) {
887 7 : cmd.type_specific_bits |= 1U << 1;
888 : }
889 : } else {
890 : // all other options in Content are assumed to be packed:
891 : static_assert(sizeof(cmd.content) >= 12,
892 : "content is big enough to take bytes");
893 : // (void *) cast to specify gcc that we know that we are copy byte into a non trivial type and leaving 4 bytes untouched
894 3702 : memcpy((void *)&cmd.content, packed_content.bytes, 12);
895 : }
896 :
897 : // set command's index to it's position in eeprom
898 97800 : cmd.index = index;
899 :
900 : // return success
901 97800 : return true;
902 104916 : }
903 :
904 176479 : bool AP_Mission::stored_in_location(uint16_t id)
905 : {
906 176479 : switch (id) {
907 170495 : case MAV_CMD_NAV_WAYPOINT:
908 : case MAV_CMD_NAV_LOITER_UNLIM:
909 : case MAV_CMD_NAV_LOITER_TURNS:
910 : case MAV_CMD_NAV_LOITER_TIME:
911 : case MAV_CMD_NAV_LAND:
912 : case MAV_CMD_NAV_TAKEOFF:
913 : case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
914 : case MAV_CMD_NAV_LOITER_TO_ALT:
915 : case MAV_CMD_NAV_SPLINE_WAYPOINT:
916 : case MAV_CMD_NAV_GUIDED_ENABLE:
917 : case MAV_CMD_DO_SET_HOME:
918 : case MAV_CMD_DO_RETURN_PATH_START:
919 : case MAV_CMD_DO_LAND_START:
920 : case MAV_CMD_DO_GO_AROUND:
921 : case MAV_CMD_DO_SET_ROI_LOCATION:
922 : case MAV_CMD_DO_SET_ROI:
923 : case MAV_CMD_NAV_VTOL_TAKEOFF:
924 : case MAV_CMD_NAV_VTOL_LAND:
925 : case MAV_CMD_NAV_PAYLOAD_PLACE:
926 : case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
927 : case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
928 : case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION:
929 : case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION:
930 : case MAV_CMD_NAV_FENCE_RETURN_POINT:
931 : case MAV_CMD_NAV_RALLY_POINT:
932 : case MAV_CMD_NAV_ARC_WAYPOINT:
933 170495 : return true;
934 5984 : default:
935 5984 : return false;
936 : }
937 : }
938 :
939 : /// write_cmd_to_storage - write a command to storage
940 : /// index is used to calculate the storage location
941 : /// true is returned if successful
942 15313 : bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd)
943 : {
944 15313 : WITH_SEMAPHORE(_rsem);
945 :
946 : // range check cmd's index
947 15313 : if (index >= num_commands_max()) {
948 29 : return false;
949 : }
950 :
951 15284 : PackedContent packed {};
952 15284 : if (stored_in_location(cmd.id)) {
953 : // Location is not PACKED; field-wise copy it:
954 14927 : packed.location.flags.relative_alt = cmd.content.location.relative_alt;
955 14927 : packed.location.flags.loiter_ccw = cmd.content.location.loiter_ccw;
956 14927 : packed.location.flags.terrain_alt = cmd.content.location.terrain_alt;
957 14927 : packed.location.flags.origin_alt = cmd.content.location.origin_alt;
958 14927 : packed.location.flags.loiter_xtrack = cmd.content.location.loiter_xtrack;
959 14927 : packed.location.flags.type_specific_bit_0 = (cmd.type_specific_bits & (1U<<0)) >> 0;
960 14927 : packed.location.flags.type_specific_bit_1 = (cmd.type_specific_bits & (1U<<1)) >> 1;
961 14927 : packed.location.alt = cmd.content.location.alt;
962 14927 : packed.location.lat = cmd.content.location.lat;
963 14927 : packed.location.lng = cmd.content.location.lng;
964 : } else {
965 : // all other options in Content are assumed to be packed:
966 : static_assert(sizeof(packed.bytes) >= 12,
967 : "packed.bytes is big enough to take content");
968 357 : memcpy(packed.bytes, &cmd.content, 12);
969 : }
970 :
971 : // calculate where in storage the command should be placed
972 15284 : uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
973 :
974 15284 : if (cmd.id < 256) {
975 : // for commands below 256 we store up to 12 bytes
976 15271 : _storage.write_byte(pos_in_storage, cmd.id);
977 15271 : _storage.write_uint16(pos_in_storage+1, cmd.p1);
978 15271 : _storage.write_block(pos_in_storage+3, packed.bytes, 12);
979 : } else {
980 : // if the command ID is above 256 we store a tag byte followed
981 : // by the 16 bit command ID. The tag byte is 1 for commands
982 : // where we have changed the storage format (see
983 : // format_conversion), 0 otherwise
984 13 : uint8_t tag_byte = 0;
985 : // currently the only converted structure is NAV_SCRIPT_TIME
986 13 : if (cmd.id == MAV_CMD_NAV_SCRIPT_TIME) {
987 0 : tag_byte = 1;
988 : }
989 13 : _storage.write_byte(pos_in_storage, tag_byte);
990 13 : _storage.write_uint16(pos_in_storage+1, cmd.id);
991 13 : _storage.write_uint16(pos_in_storage+3, cmd.p1);
992 13 : _storage.write_block(pos_in_storage+5, packed.bytes, 10);
993 : }
994 :
995 : // remember when the mission last changed
996 15284 : if (index != 0) {
997 : // Update of home location is not a true change
998 8437 : _last_change_time_ms = AP_HAL::millis();
999 : }
1000 :
1001 : // return success
1002 15284 : return true;
1003 15313 : }
1004 :
1005 : /// write_home_to_storage - writes the special purpose cmd 0 (home) to storage
1006 : /// home is taken directly from ahrs
1007 6876 : void AP_Mission::write_home_to_storage()
1008 : {
1009 6876 : Mission_Command home_cmd = {};
1010 6876 : home_cmd.id = MAV_CMD_NAV_WAYPOINT;
1011 6876 : home_cmd.content.location = AP::ahrs().get_home();
1012 6876 : write_cmd_to_storage(0,home_cmd);
1013 :
1014 : // Make sure command total reflects that home has been added
1015 6876 : if (_cmd_total < 1) {
1016 932 : _cmd_total.set_and_save(1);
1017 : }
1018 6876 : }
1019 :
1020 8654 : MAV_MISSION_RESULT AP_Mission::sanity_check_params(const mavlink_mission_item_int_t& packet)
1021 : {
1022 : uint8_t nan_mask;
1023 8654 : switch (packet.command) {
1024 7641 : case MAV_CMD_NAV_WAYPOINT:
1025 7641 : nan_mask = ~(1 << 3); // param 4 can be nan
1026 7641 : break;
1027 14 : case MAV_CMD_NAV_LOITER_UNLIM:
1028 14 : nan_mask = ~(1 << 3); // param 4 can be nan
1029 14 : break;
1030 134 : case MAV_CMD_NAV_LAND:
1031 134 : nan_mask = ~(1 << 3); // param 4 can be nan
1032 134 : break;
1033 224 : case MAV_CMD_NAV_TAKEOFF:
1034 224 : nan_mask = ~(1 << 3); // param 4 can be nan
1035 224 : break;
1036 35 : case MAV_CMD_NAV_ARC_WAYPOINT:
1037 35 : nan_mask = ~((1 << 1) | (1 << 2) | (1 << 3)); // param 2,3 & 4 can be nan
1038 35 : break;
1039 24 : case MAV_CMD_NAV_VTOL_TAKEOFF:
1040 24 : nan_mask = ~(1 << 3); // param 4 can be nan
1041 24 : break;
1042 33 : case MAV_CMD_NAV_VTOL_LAND:
1043 33 : nan_mask = ~((1 << 2) | (1 << 3)); // param 3 and 4 can be nan
1044 33 : break;
1045 549 : default:
1046 549 : nan_mask = 0xff;
1047 549 : break;
1048 : }
1049 :
1050 17308 : if (((nan_mask & (1 << 0)) && isnan(packet.param1)) ||
1051 8654 : isinf(packet.param1)) {
1052 0 : return MAV_MISSION_INVALID_PARAM1;
1053 : }
1054 17308 : if (((nan_mask & (1 << 1)) && isnan(packet.param2)) ||
1055 8654 : isinf(packet.param2)) {
1056 0 : return MAV_MISSION_INVALID_PARAM2;
1057 : }
1058 17308 : if (((nan_mask & (1 << 2)) && isnan(packet.param3)) ||
1059 8654 : isinf(packet.param3)) {
1060 0 : return MAV_MISSION_INVALID_PARAM3;
1061 : }
1062 17308 : if (((nan_mask & (1 << 3)) && isnan(packet.param4)) ||
1063 8654 : isinf(packet.param4)) {
1064 0 : return MAV_MISSION_INVALID_PARAM4;
1065 : }
1066 8654 : return MAV_MISSION_ACCEPTED;
1067 : }
1068 :
1069 : // mavlink_int_to_mission_cmd - converts mavlink message to an AP_Mission::Mission_Command object which can be stored to eeprom
1070 : // return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
1071 8654 : MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_item_int_t& packet, AP_Mission::Mission_Command& cmd)
1072 : {
1073 8654 : cmd = {};
1074 :
1075 : // command's position in mission list and mavlink id
1076 8654 : cmd.index = packet.seq;
1077 8654 : cmd.id = packet.command;
1078 8654 : cmd.content.location = {};
1079 :
1080 8654 : MAV_MISSION_RESULT param_check = sanity_check_params(packet);
1081 8654 : if (param_check != MAV_MISSION_ACCEPTED) {
1082 0 : return param_check;
1083 : }
1084 :
1085 : // command specific conversions from mavlink packet to mission command
1086 8654 : switch (cmd.id) {
1087 :
1088 0 : case 0 ... 1:
1089 : // these are reserved for storing 16 bit command IDs
1090 0 : return MAV_MISSION_INVALID;
1091 :
1092 7641 : case MAV_CMD_NAV_WAYPOINT: { // MAV ID: 16
1093 : /*
1094 : the 15 byte limit means we can't fit both delay and radius
1095 : in the cmd structure. When we expand the mission structure
1096 : we can do this properly
1097 : */
1098 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1099 : // acceptance radius in meters and pass by distance in meters
1100 7160 : uint16_t acp = packet.param2; // param 2 is acceptance radius in meters is held in low p1
1101 7160 : uint16_t passby = packet.param3; // param 3 is pass by distance in meters is held in high p1
1102 :
1103 : // limit to 255 so it does not wrap during the shift or mask operation
1104 7160 : passby = MIN(0xFF,passby);
1105 7160 : acp = MIN(0xFF,acp);
1106 :
1107 7160 : cmd.p1 = (passby << 8) | (acp & 0x00FF);
1108 : #else
1109 : // delay at waypoint in seconds (this is for copters???)
1110 : // reject invalid param1 values to prevent floating point exception
1111 481 : if (packet.param1 < 0 || packet.param1 > UINT16_MAX) {
1112 0 : return MAV_MISSION_INVALID_PARAM1;
1113 : }
1114 481 : cmd.p1 = (uint16_t)packet.param1;
1115 : #endif
1116 : }
1117 7641 : break;
1118 :
1119 14 : case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17
1120 14 : cmd.p1 = fabsf(packet.param3); // store radius as 16bit since no other params are competing for space
1121 14 : cmd.content.location.loiter_ccw = (packet.param3 < 0); // -1 = counter clockwise, +1 = clockwise
1122 14 : break;
1123 :
1124 12 : case MAV_CMD_NAV_LOITER_TURNS: { // MAV ID: 18
1125 : // number of turns is stored in the lowest bits. Number of
1126 : // turns 0 < N < 1 are stored multiplied by 256 and a bit set
1127 : // in storage so that on retrieval they are divided by 256.
1128 : // Radii below 255m are stored in the top 8 bits as an 8-bit integer.
1129 : // Radii above 255m are stored divided by 10 and a bit set in
1130 : // storage so that on retrieval they are multiplied by 10
1131 12 : float param1_stored = packet.param1;
1132 12 : if (param1_stored > 0 && param1_stored < 1) {
1133 1 : param1_stored *= 256.0;
1134 1 : cmd.type_specific_bits |= (1U << 1);
1135 : }
1136 12 : cmd.p1 = MIN(255, param1_stored); // store number of times to circle in low p1
1137 : uint8_t radius_m;
1138 12 : const float abs_radius = fabsf(packet.param3);
1139 12 : if (abs_radius <= 255) {
1140 10 : radius_m = abs_radius;
1141 : } else {
1142 2 : radius_m = MIN(255, abs_radius * 0.1);
1143 2 : cmd.type_specific_bits |= (1U << 0);
1144 : }
1145 12 : cmd.p1 |= (radius_m<<8); // store radius in high byte of p1
1146 12 : cmd.content.location.loiter_ccw = (packet.param3 < 0);
1147 12 : cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
1148 : }
1149 12 : break;
1150 :
1151 15 : case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19
1152 15 : cmd.p1 = packet.param1; // loiter time in seconds uses all 16 bits, 8bit seconds is too small. No room for radius.
1153 15 : cmd.content.location.loiter_ccw = (packet.param3 < 0);
1154 15 : cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
1155 15 : break;
1156 :
1157 95 : case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20
1158 95 : break;
1159 :
1160 134 : case MAV_CMD_NAV_LAND: // MAV ID: 21
1161 134 : cmd.p1 = packet.param1; // abort target altitude(m) (plane only)
1162 134 : if (!isnan(packet.param4)) {
1163 134 : cmd.content.location.loiter_ccw = is_negative(packet.param4); // yaw direction, (plane deepstall only)
1164 : }
1165 134 : break;
1166 :
1167 224 : case MAV_CMD_NAV_TAKEOFF: // MAV ID: 22
1168 224 : cmd.p1 = packet.param1; // minimum pitch (plane only)
1169 224 : break;
1170 :
1171 0 : case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: // MAV ID: 30
1172 0 : cmd.p1 = packet.param1; // Climb/Descend
1173 : // 0 = Neutral, cmd complete at +/- 5 of indicated alt.
1174 : // 1 = Climb, cmd complete at or above indicated alt.
1175 : // 2 = Descend, cmd complete at or below indicated alt.
1176 0 : break;
1177 :
1178 15 : case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31
1179 15 : cmd.p1 = fabsf(packet.param2); // param2 is radius in meters
1180 15 : cmd.content.location.loiter_ccw = (packet.param2 < 0);
1181 15 : cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
1182 15 : break;
1183 :
1184 35 : case MAV_CMD_NAV_ARC_WAYPOINT: // MAV ID: 36
1185 35 : cmd.content.location.loiter_ccw = is_negative(packet.param1); // re-use loiter_cw for arc direction
1186 35 : cmd.p1 = fabsf(packet.param1); // arc angle in deg
1187 35 : break;
1188 :
1189 16 : case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82
1190 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1191 0 : return MAV_MISSION_UNSUPPORTED;
1192 : #else
1193 16 : cmd.p1 = packet.param1; // delay at waypoint in seconds
1194 16 : break;
1195 : #endif
1196 :
1197 1 : case MAV_CMD_NAV_GUIDED_ENABLE: // MAV ID: 92
1198 1 : cmd.p1 = packet.param1; // on/off. >0.5 means "on", hand-over control to external controller
1199 1 : break;
1200 :
1201 7 : case MAV_CMD_NAV_DELAY: // MAV ID: 93
1202 7 : cmd.content.nav_delay.seconds = packet.param1; // delay in seconds
1203 7 : cmd.content.nav_delay.hour_utc = packet.param2;// absolute time's hour (utc)
1204 7 : cmd.content.nav_delay.min_utc = packet.param3;// absolute time's min (utc)
1205 7 : cmd.content.nav_delay.sec_utc = packet.param4; // absolute time's second (utc)
1206 7 : break;
1207 :
1208 0 : case MAV_CMD_CONDITION_DELAY: // MAV ID: 112
1209 0 : cmd.content.delay.seconds = packet.param1; // delay in seconds
1210 0 : break;
1211 :
1212 0 : case MAV_CMD_CONDITION_DISTANCE: // MAV ID: 114
1213 0 : cmd.content.distance.meters = packet.param1; // distance in meters from next waypoint
1214 0 : break;
1215 :
1216 6 : case MAV_CMD_CONDITION_YAW: // MAV ID: 115
1217 6 : cmd.content.yaw.angle_deg = packet.param1; // target angle in degrees
1218 6 : cmd.content.yaw.turn_rate_dps = packet.param2; // 0 = use default turn rate otherwise specific turn rate in deg/sec
1219 6 : cmd.content.yaw.direction = packet.param3; // -1 = ccw, +1 = cw
1220 6 : cmd.content.yaw.relative_angle = packet.param4; // lng=0: absolute angle provided, lng=1: relative angle provided
1221 6 : break;
1222 :
1223 131 : case MAV_CMD_DO_JUMP: // MAV ID: 177
1224 : case MAV_CMD_DO_JUMP_TAG: // MAV ID: 601
1225 131 : cmd.content.jump.target = packet.param1; // jump-to command/tag number
1226 131 : cmd.content.jump.num_times = packet.param2; // repeat count
1227 131 : break;
1228 :
1229 2 : case MAV_CMD_JUMP_TAG: // MAV ID: 600
1230 2 : cmd.content.jump.target = packet.param1; // jump-to tag number
1231 2 : break;
1232 :
1233 41 : case MAV_CMD_DO_CHANGE_SPEED: // MAV ID: 178
1234 41 : cmd.content.speed.speed_type = packet.param1; // 0 = airspeed, 1 = ground speed
1235 41 : cmd.content.speed.target_ms = packet.param2; // target speed in m/s
1236 41 : cmd.content.speed.throttle_pct = packet.param3; // throttle as a percentage from 1 ~ 100%
1237 41 : break;
1238 :
1239 0 : case MAV_CMD_DO_SET_HOME:
1240 0 : cmd.p1 = packet.param1; // p1=0 means use current location, p=1 means use provided location
1241 0 : break;
1242 :
1243 0 : case MAV_CMD_DO_SET_RELAY: // MAV ID: 181
1244 0 : cmd.content.relay.num = packet.param1; // relay number
1245 0 : cmd.content.relay.state = packet.param2; // 0:off, 1:on
1246 0 : break;
1247 :
1248 0 : case MAV_CMD_DO_REPEAT_RELAY: // MAV ID: 182
1249 0 : cmd.content.repeat_relay.num = packet.param1; // relay number
1250 0 : cmd.content.repeat_relay.repeat_count = packet.param2; // count
1251 0 : cmd.content.repeat_relay.cycle_time = packet.param3; // time converted from seconds to milliseconds
1252 0 : break;
1253 :
1254 2 : case MAV_CMD_DO_SET_SERVO: // MAV ID: 183
1255 2 : cmd.content.servo.channel = packet.param1; // channel
1256 2 : cmd.content.servo.pwm = packet.param2; // PWM
1257 2 : break;
1258 :
1259 0 : case MAV_CMD_DO_REPEAT_SERVO: // MAV ID: 184
1260 0 : cmd.content.repeat_servo.channel = packet.param1; // channel
1261 0 : cmd.content.repeat_servo.pwm = packet.param2; // PWM
1262 0 : cmd.content.repeat_servo.repeat_count = packet.param3; // count
1263 0 : cmd.content.repeat_servo.cycle_time = packet.param4; // time in seconds
1264 0 : break;
1265 :
1266 131 : case MAV_CMD_DO_RETURN_PATH_START: // MAV ID: 188
1267 : case MAV_CMD_DO_LAND_START: // MAV ID: 189
1268 131 : break;
1269 :
1270 0 : case MAV_CMD_DO_GO_AROUND: // MAV ID: 191
1271 0 : break;
1272 :
1273 0 : case MAV_CMD_DO_SET_ROI_LOCATION: // MAV ID: 195
1274 : case MAV_CMD_DO_SET_ROI_NONE: // MAV ID: 197
1275 0 : cmd.p1 = packet.param1; // gimbal device id
1276 0 : break;
1277 :
1278 0 : case MAV_CMD_DO_SET_ROI: // MAV ID: 201
1279 0 : cmd.p1 = packet.param1; // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported)
1280 0 : break;
1281 :
1282 0 : case MAV_CMD_DO_DIGICAM_CONFIGURE: // MAV ID: 202
1283 0 : cmd.content.digicam_configure.shooting_mode = packet.param1;
1284 0 : cmd.content.digicam_configure.shutter_speed = packet.param2;
1285 0 : cmd.content.digicam_configure.aperture = packet.param3;
1286 0 : cmd.content.digicam_configure.ISO = packet.param4;
1287 0 : cmd.content.digicam_configure.exposure_type = packet.x;
1288 0 : cmd.content.digicam_configure.cmd_id = packet.y;
1289 0 : cmd.content.digicam_configure.engine_cutoff_time = packet.z;
1290 0 : break;
1291 :
1292 1 : case MAV_CMD_DO_DIGICAM_CONTROL: // MAV ID: 203
1293 1 : cmd.content.digicam_control.session = packet.param1;
1294 1 : cmd.content.digicam_control.zoom_pos = packet.param2;
1295 1 : cmd.content.digicam_control.zoom_step = packet.param3;
1296 1 : cmd.content.digicam_control.focus_lock = packet.param4;
1297 1 : cmd.content.digicam_control.shooting_cmd = packet.x;
1298 1 : cmd.content.digicam_control.cmd_id = packet.y;
1299 1 : break;
1300 :
1301 0 : case MAV_CMD_DO_MOUNT_CONTROL: // MAV ID: 205
1302 : // TODO: this is only valid if packet.z == MAV_MOUNT_MODE_MAVLINK_TARGETING
1303 0 : cmd.content.mount_control.pitch = packet.param1;
1304 0 : cmd.content.mount_control.roll = packet.param2;
1305 0 : cmd.content.mount_control.yaw = packet.param3;
1306 0 : break;
1307 :
1308 12 : case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206
1309 12 : cmd.content.cam_trigg_dist.meters = packet.param1; // distance between camera shots in meters
1310 12 : cmd.content.cam_trigg_dist.trigger = packet.param3; // when enabled, camera triggers once immediately
1311 12 : cmd.content.cam_trigg_dist.camera_id = packet.param4; // which camera to trigger
1312 12 : break;
1313 :
1314 0 : case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207
1315 0 : cmd.p1 = packet.param1; // action 0=disable, 1=enable, 2=disable floor
1316 0 : break;
1317 :
1318 1 : case MAV_CMD_DO_AUX_FUNCTION:
1319 1 : cmd.content.auxfunction.function = packet.param1;
1320 1 : cmd.content.auxfunction.switchpos = packet.param2;
1321 1 : break;
1322 :
1323 4 : case MAV_CMD_DO_PARACHUTE: // MAV ID: 208
1324 4 : cmd.p1 = packet.param1; // action 0=disable, 1=enable, 2=release. See PARACHUTE_ACTION enum
1325 4 : break;
1326 :
1327 2 : case MAV_CMD_DO_INVERTED_FLIGHT: // MAV ID: 210
1328 2 : cmd.p1 = packet.param1; // normal=0 inverted=1
1329 2 : break;
1330 :
1331 : #if AP_GRIPPER_ENABLED
1332 14 : case MAV_CMD_DO_GRIPPER: // MAV ID: 211
1333 14 : cmd.content.gripper.num = packet.param1; // gripper number
1334 14 : cmd.content.gripper.action = packet.param2; // action 0=release, 1=grab. See GRIPPER_ACTION enum
1335 14 : break;
1336 : #endif
1337 :
1338 1 : case MAV_CMD_DO_GUIDED_LIMITS: // MAV ID: 222
1339 1 : cmd.p1 = packet.param1; // max time in seconds the external controller will be allowed to control the vehicle
1340 1 : cmd.content.guided_limits.alt_min = packet.param2; // min alt below which the command will be aborted. 0 for no lower alt limit
1341 1 : cmd.content.guided_limits.alt_max = packet.param3; // max alt above which the command will be aborted. 0 for no upper alt limit
1342 1 : cmd.content.guided_limits.horiz_max = packet.param4;// max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit
1343 1 : break;
1344 :
1345 0 : case MAV_CMD_DO_AUTOTUNE_ENABLE: // MAV ID: 211
1346 0 : cmd.p1 = packet.param1; // disable=0 enable=1
1347 0 : break;
1348 :
1349 2 : case MAV_CMD_NAV_ALTITUDE_WAIT: // MAV ID: 83
1350 2 : cmd.content.altitude_wait.altitude = packet.param1;
1351 2 : cmd.content.altitude_wait.descent_rate = packet.param2;
1352 2 : cmd.content.altitude_wait.wiggle_time = packet.param3;
1353 2 : break;
1354 :
1355 24 : case MAV_CMD_NAV_VTOL_TAKEOFF:
1356 24 : break;
1357 :
1358 33 : case MAV_CMD_NAV_VTOL_LAND:
1359 33 : cmd.p1 = (NAV_VTOL_LAND_OPTIONS)packet.param1;
1360 33 : break;
1361 :
1362 2 : case MAV_CMD_DO_VTOL_TRANSITION:
1363 2 : cmd.content.do_vtol_transition.target_state = packet.param1;
1364 2 : break;
1365 :
1366 0 : case MAV_CMD_DO_SET_REVERSE:
1367 0 : cmd.p1 = packet.param1; // 0 = forward, 1 = reverse
1368 0 : break;
1369 :
1370 21 : case MAV_CMD_DO_ENGINE_CONTROL:
1371 21 : cmd.content.do_engine_control.start_control = (packet.param1>0);
1372 21 : cmd.content.do_engine_control.cold_start = (packet.param2>0);
1373 21 : cmd.content.do_engine_control.height_delay_cm = packet.param3*100;
1374 21 : cmd.content.do_engine_control.allow_disarmed_start = (((uint32_t)packet.param4) & ENGINE_CONTROL_OPTIONS_ALLOW_START_WHILE_DISARMED) != 0;
1375 21 : break;
1376 :
1377 : #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED
1378 2 : case MAV_CMD_NAV_PAYLOAD_PLACE:
1379 2 : cmd.p1 = packet.param1*100; // copy max-descend parameter (m->cm)
1380 2 : break;
1381 : #endif
1382 :
1383 0 : case MAV_CMD_NAV_SET_YAW_SPEED:
1384 0 : cmd.content.set_yaw_speed.angle_deg = packet.param1; // target angle in degrees
1385 0 : cmd.content.set_yaw_speed.speed = packet.param2; // speed in meters/second
1386 0 : cmd.content.set_yaw_speed.relative_angle = packet.param3; // 0 = absolute angle, 1 = relative angle
1387 0 : break;
1388 :
1389 0 : case MAV_CMD_DO_WINCH: // MAV ID: 42600
1390 0 : cmd.content.winch.num = packet.param1; // winch number
1391 0 : cmd.content.winch.action = packet.param2; // action (0 = relax, 1 = length control, 2 = rate control). See WINCH_ACTION enum
1392 0 : cmd.content.winch.release_length = packet.param3; // cable distance to unwind in meters, negative numbers to wind in cable
1393 0 : cmd.content.winch.release_rate = packet.param4; // release rate in meters/second
1394 0 : break;
1395 :
1396 0 : case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
1397 0 : cmd.p1 = packet.param1; // Resume repeat distance (m)
1398 0 : break;
1399 :
1400 2 : case MAV_CMD_DO_SPRAYER:
1401 2 : cmd.p1 = packet.param1; // action 0=disable, 1=enable
1402 2 : break;
1403 :
1404 0 : case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
1405 0 : cmd.p1 = packet.param1;
1406 0 : cmd.content.scripting.p1 = packet.param2;
1407 0 : cmd.content.scripting.p2 = packet.param3;
1408 0 : cmd.content.scripting.p3 = packet.param4;
1409 0 : break;
1410 :
1411 : #if AP_SCRIPTING_ENABLED
1412 0 : case MAV_CMD_NAV_SCRIPT_TIME:
1413 0 : cmd.content.nav_script_time.command = packet.param1;
1414 0 : cmd.content.nav_script_time.timeout_s = packet.param2;
1415 0 : cmd.content.nav_script_time.arg1.set(packet.param3);
1416 0 : cmd.content.nav_script_time.arg2.set(packet.param4);
1417 0 : cmd.content.nav_script_time.arg3 = int16_t(packet.x);
1418 0 : cmd.content.nav_script_time.arg4 = int16_t(packet.y);
1419 0 : break;
1420 : #endif
1421 :
1422 0 : case MAV_CMD_NAV_ATTITUDE_TIME:
1423 0 : cmd.content.nav_attitude_time.time_sec = constrain_float(packet.param1, 0, UINT16_MAX);
1424 0 : cmd.content.nav_attitude_time.roll_deg = (fabsf(packet.param2) <= 180) ? packet.param2 : 0;
1425 0 : cmd.content.nav_attitude_time.pitch_deg = (fabsf(packet.param3) <= 90) ? packet.param3 : 0;
1426 0 : cmd.content.nav_attitude_time.yaw_deg = ((packet.param4 >= -180) && (packet.param4 <= 360)) ? packet.param4 : 0;
1427 0 : cmd.content.nav_attitude_time.climb_rate = packet.x;
1428 0 : break;
1429 :
1430 0 : case MAV_CMD_DO_PAUSE_CONTINUE:
1431 0 : cmd.p1 = packet.param1;
1432 0 : break;
1433 :
1434 0 : case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
1435 0 : cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg = packet.param1;
1436 0 : cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg = packet.param2;
1437 0 : cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs = packet.param3;
1438 0 : cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs = packet.param4;
1439 0 : cmd.content.gimbal_manager_pitchyaw.flags = packet.x;
1440 0 : cmd.content.gimbal_manager_pitchyaw.gimbal_id = packet.z;
1441 0 : break;
1442 :
1443 3 : case MAV_CMD_IMAGE_START_CAPTURE:
1444 3 : cmd.content.image_start_capture.instance = packet.param1;
1445 3 : cmd.content.image_start_capture.interval_s = packet.param2;
1446 3 : cmd.content.image_start_capture.total_num_images = packet.param3;
1447 3 : cmd.content.image_start_capture.start_seq_number = packet.param4;
1448 3 : break;
1449 :
1450 1 : case MAV_CMD_IMAGE_STOP_CAPTURE:
1451 1 : cmd.p1 = packet.param1;
1452 1 : break;
1453 :
1454 2 : case MAV_CMD_SET_CAMERA_ZOOM:
1455 2 : cmd.content.set_camera_zoom.zoom_type = packet.param1;
1456 2 : cmd.content.set_camera_zoom.zoom_value = packet.param2;
1457 2 : cmd.content.set_camera_zoom.camera_id = packet.param3;
1458 2 : break;
1459 :
1460 2 : case MAV_CMD_SET_CAMERA_FOCUS:
1461 2 : cmd.content.set_camera_focus.focus_type = packet.param1;
1462 2 : cmd.content.set_camera_focus.focus_value = packet.param2;
1463 2 : cmd.content.set_camera_focus.camera_id = packet.param3;
1464 2 : break;
1465 :
1466 0 : case MAV_CMD_SET_CAMERA_SOURCE:
1467 0 : cmd.content.set_camera_source.instance = packet.param1;
1468 0 : cmd.content.set_camera_source.primary_source = packet.param2;
1469 0 : cmd.content.set_camera_source.secondary_source = packet.param3;
1470 0 : break;
1471 :
1472 0 : case MAV_CMD_VIDEO_START_CAPTURE:
1473 0 : cmd.content.video_start_capture.video_stream_id = packet.param1;
1474 0 : break;
1475 :
1476 0 : case MAV_CMD_VIDEO_STOP_CAPTURE:
1477 0 : cmd.content.video_stop_capture.video_stream_id = packet.param1;
1478 0 : break;
1479 :
1480 : #if AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
1481 3 : case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
1482 3 : cmd.content.wpnext_offset.gimbal_id = packet.param1;
1483 3 : cmd.content.wpnext_offset.roll_offset_cd = packet.y * 100;
1484 3 : cmd.content.wpnext_offset.pitch_offset_cd = packet.x * 100;
1485 3 : cmd.content.wpnext_offset.yaw_offset_cd = packet.z * 100;
1486 3 : break;
1487 : #endif // AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
1488 :
1489 0 : default:
1490 : // unrecognised command
1491 0 : return MAV_MISSION_UNSUPPORTED;
1492 : }
1493 :
1494 : // copy location from mavlink to command
1495 8654 : if (stored_in_location(cmd.id)) {
1496 :
1497 : // sanity check location
1498 8297 : if (!check_lat(packet.x)) {
1499 0 : return MAV_MISSION_INVALID_PARAM5_X;
1500 : }
1501 8297 : if (!check_lng(packet.y)) {
1502 0 : return MAV_MISSION_INVALID_PARAM6_Y;
1503 : }
1504 8297 : if (isnan(packet.z) || fabsf(packet.z) >= LOCATION_ALT_MAX_M) {
1505 0 : return MAV_MISSION_INVALID_PARAM7;
1506 : }
1507 :
1508 8297 : cmd.content.location.lat = packet.x;
1509 8297 : cmd.content.location.lng = packet.y;
1510 :
1511 8297 : cmd.content.location.alt = packet.z * 100.0f; // convert packet's alt (m) to cmd alt (cm)
1512 :
1513 8297 : switch (packet.frame) {
1514 :
1515 254 : case MAV_FRAME_MISSION:
1516 : case MAV_FRAME_GLOBAL:
1517 : case MAV_FRAME_GLOBAL_INT:
1518 254 : cmd.content.location.relative_alt = 0;
1519 254 : break;
1520 :
1521 6899 : case MAV_FRAME_GLOBAL_RELATIVE_ALT:
1522 : case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
1523 6899 : cmd.content.location.relative_alt = 1;
1524 6899 : break;
1525 :
1526 : #if AP_TERRAIN_AVAILABLE
1527 1144 : case MAV_FRAME_GLOBAL_TERRAIN_ALT:
1528 : case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
1529 : // we mark it as a relative altitude, as it doesn't have
1530 : // home alt added
1531 1144 : cmd.content.location.relative_alt = 1;
1532 : // mark altitude as above terrain, not above home
1533 1144 : cmd.content.location.terrain_alt = 1;
1534 1144 : break;
1535 : #endif
1536 :
1537 0 : default:
1538 0 : return MAV_MISSION_UNSUPPORTED_FRAME;
1539 : }
1540 : }
1541 :
1542 : // if we got this far then it must have been successful
1543 8654 : return MAV_MISSION_ACCEPTED;
1544 : }
1545 :
1546 9 : MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t &packet,
1547 : mavlink_mission_item_int_t &mav_cmd)
1548 : {
1549 : // TODO: rename mav_cmd to mission_item_int
1550 : // TODO: rename packet to mission_item
1551 9 : mav_cmd.param1 = packet.param1;
1552 9 : mav_cmd.param2 = packet.param2;
1553 9 : mav_cmd.param3 = packet.param3;
1554 9 : mav_cmd.param4 = packet.param4;
1555 9 : mav_cmd.z = packet.z;
1556 9 : mav_cmd.seq = packet.seq;
1557 9 : mav_cmd.command = packet.command;
1558 9 : mav_cmd.target_system = packet.target_system;
1559 9 : mav_cmd.target_component = packet.target_component;
1560 9 : mav_cmd.frame = packet.frame;
1561 9 : mav_cmd.current = packet.current;
1562 9 : mav_cmd.autocontinue = packet.autocontinue;
1563 9 : mav_cmd.mission_type = packet.mission_type;
1564 :
1565 : /*
1566 : the strategy for handling both MISSION_ITEM and MISSION_ITEM_INT
1567 : is to pass the lat/lng in MISSION_ITEM_INT straight through, and
1568 : for MISSION_ITEM multiply by 1e7 here. We need an exception for
1569 : any commands which use the x and y fields not as
1570 : latitude/longitude.
1571 : */
1572 9 : if (!cmd_has_location(packet.command)) {
1573 0 : mav_cmd.x = packet.x;
1574 0 : mav_cmd.y = packet.y;
1575 :
1576 : } else {
1577 : //these commands use x and y as lat/lon. We need to
1578 : // multiply by 1e7 to convert to int32_t
1579 9 : if (!check_lat(packet.x)) {
1580 0 : return MAV_MISSION_INVALID_PARAM5_X;
1581 : }
1582 9 : if (!check_lng(packet.y)) {
1583 0 : return MAV_MISSION_INVALID_PARAM6_Y;
1584 : }
1585 9 : mav_cmd.x = packet.x * 1.0e7f;
1586 9 : mav_cmd.y = packet.y * 1.0e7f;
1587 : }
1588 :
1589 9 : return MAV_MISSION_ACCEPTED;
1590 : }
1591 :
1592 0 : MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(const mavlink_mission_item_int_t &item_int,
1593 : mavlink_mission_item_t &item)
1594 : {
1595 0 : item.param1 = item_int.param1;
1596 0 : item.param2 = item_int.param2;
1597 0 : item.param3 = item_int.param3;
1598 0 : item.param4 = item_int.param4;
1599 0 : item.z = item_int.z;
1600 0 : item.seq = item_int.seq;
1601 0 : item.command = item_int.command;
1602 0 : item.target_system = item_int.target_system;
1603 0 : item.target_component = item_int.target_component;
1604 0 : item.frame = item_int.frame;
1605 0 : item.current = item_int.current;
1606 0 : item.autocontinue = item_int.autocontinue;
1607 0 : item.mission_type = item_int.mission_type;
1608 :
1609 0 : if (!cmd_has_location(item_int.command)) {
1610 0 : item.x = item_int.x;
1611 0 : item.y = item_int.y;
1612 :
1613 : } else {
1614 : // These commands use x and y as lat/lon. We need to
1615 : // multiply by 1e-7 to convert to float
1616 0 : item.x = item_int.x * 1.0e-7f;
1617 0 : item.y = item_int.y * 1.0e-7f;
1618 0 : if (!check_lat(item.x)) {
1619 0 : return MAV_MISSION_INVALID_PARAM5_X;
1620 : }
1621 0 : if (!check_lng(item.y)) {
1622 0 : return MAV_MISSION_INVALID_PARAM6_Y;
1623 : }
1624 : }
1625 :
1626 0 : return MAV_MISSION_ACCEPTED;
1627 : }
1628 :
1629 : // mission_cmd_to_mavlink_int - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS
1630 : // return true on success, false on failure
1631 : // NOTE: callers to this method current fill parts of "packet" in before calling this method, so do NOT attempt to zero the entire packet in here
1632 26892 : bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& cmd, mavlink_mission_item_int_t& packet)
1633 : {
1634 : // Make sure return packed is zeroed
1635 : // Strictly params 1 to 4 and alt should be defaulted to NaN
1636 : // x and y should be defaulted to INT32_MAX
1637 26892 : packet = {
1638 26892 : seq: cmd.index,
1639 26892 : command: cmd.id,
1640 : autocontinue: 1,
1641 : };
1642 :
1643 : // command specific conversions from mission command to mavlink packet
1644 26892 : switch (cmd.id) {
1645 0 : case 0:
1646 : // this is reserved for 16 bit command IDs
1647 0 : return false;
1648 :
1649 23333 : case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16
1650 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1651 : // acceptance radius in meters
1652 :
1653 21007 : packet.param2 = LOWBYTE(cmd.p1); // param 2 is acceptance radius in meters is held in low p1
1654 21007 : packet.param3 = HIGHBYTE(cmd.p1); // param 3 is pass by distance in meters is held in high p1
1655 : #else
1656 : // delay at waypoint in seconds
1657 2326 : packet.param1 = cmd.p1;
1658 : #endif
1659 23333 : break;
1660 :
1661 38 : case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17
1662 38 : packet.param3 = (float)cmd.p1;
1663 38 : if (cmd.content.location.loiter_ccw) {
1664 0 : packet.param3 *= -1;
1665 : }
1666 38 : break;
1667 :
1668 51 : case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18
1669 51 : packet.param1 = LOWBYTE(cmd.p1); // number of times to circle is held in low byte of p1
1670 51 : packet.param3 = HIGHBYTE(cmd.p1); // radius is held in high byte of p1
1671 51 : if (cmd.content.location.loiter_ccw) {
1672 13 : packet.param3 = -packet.param3;
1673 : }
1674 51 : if (cmd.type_specific_bits & (1U<<0)) {
1675 6 : packet.param3 *= 10;
1676 : }
1677 51 : if (cmd.type_specific_bits & (1U<<1)) {
1678 3 : packet.param1 /= 256.0;
1679 : }
1680 51 : packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
1681 51 : break;
1682 :
1683 43 : case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19
1684 43 : packet.param1 = cmd.p1; // loiter time in seconds
1685 43 : if (cmd.content.location.loiter_ccw) {
1686 0 : packet.param3 = -1;
1687 : } else {
1688 43 : packet.param3 = 1;
1689 : }
1690 43 : packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
1691 43 : break;
1692 :
1693 303 : case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20
1694 303 : break;
1695 :
1696 519 : case MAV_CMD_NAV_LAND: // MAV ID: 21
1697 519 : packet.param1 = cmd.p1; // abort target altitude(m) (plane only)
1698 519 : packet.param4 = cmd.content.location.loiter_ccw ? -1 : 1; // yaw direction, (plane deepstall only)
1699 519 : break;
1700 :
1701 895 : case MAV_CMD_NAV_TAKEOFF: // MAV ID: 22
1702 895 : packet.param1 = cmd.p1; // minimum pitch (plane only)
1703 895 : break;
1704 :
1705 0 : case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: // MAV ID: 30
1706 0 : packet.param1 = cmd.p1; // Climb/Descend
1707 : // 0 = Neutral, cmd complete at +/- 5 of indicated alt.
1708 : // 1 = Climb, cmd complete at or above indicated alt.
1709 : // 2 = Descend, cmd complete at or below indicated alt.
1710 0 : break;
1711 :
1712 55 : case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31
1713 55 : packet.param2 = cmd.p1; // loiter radius(m)
1714 55 : if (cmd.content.location.loiter_ccw) {
1715 3 : packet.param2 = -packet.param2;
1716 : }
1717 55 : packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
1718 55 : break;
1719 :
1720 123 : case MAV_CMD_NAV_ARC_WAYPOINT: { // MAV ID: 36
1721 123 : const float sign = cmd.content.location.loiter_ccw == 0 ? 1.0 : -1.0; // Mavlink command specifies positiive is CW
1722 123 : packet.param1 = float(cmd.p1) * sign;
1723 123 : break;
1724 : }
1725 57 : case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82
1726 57 : packet.param1 = cmd.p1; // delay at waypoint in seconds
1727 57 : break;
1728 :
1729 4 : case MAV_CMD_NAV_GUIDED_ENABLE: // MAV ID: 92
1730 4 : packet.param1 = cmd.p1; // on/off. >0.5 means "on", hand-over control to external controller
1731 4 : break;
1732 :
1733 20 : case MAV_CMD_NAV_DELAY: // MAV ID: 93
1734 20 : packet.param1 = cmd.content.nav_delay.seconds; // delay in seconds
1735 20 : packet.param2 = cmd.content.nav_delay.hour_utc; // absolute time's day of week (utc)
1736 20 : packet.param3 = cmd.content.nav_delay.min_utc; // absolute time's hour (utc)
1737 20 : packet.param4 = cmd.content.nav_delay.sec_utc; // absolute time's min (utc)
1738 20 : break;
1739 :
1740 0 : case MAV_CMD_CONDITION_DELAY: // MAV ID: 112
1741 0 : packet.param1 = cmd.content.delay.seconds; // delay in seconds
1742 0 : break;
1743 :
1744 0 : case MAV_CMD_CONDITION_DISTANCE: // MAV ID: 114
1745 0 : packet.param1 = cmd.content.distance.meters; // distance in meters from next waypoint
1746 0 : break;
1747 :
1748 23 : case MAV_CMD_CONDITION_YAW: // MAV ID: 115
1749 23 : packet.param1 = cmd.content.yaw.angle_deg; // target angle in degrees
1750 23 : packet.param2 = cmd.content.yaw.turn_rate_dps; // 0 = use default turn rate otherwise specific turn rate in deg/sec
1751 23 : packet.param3 = cmd.content.yaw.direction; // -1 = ccw, +1 = cw
1752 23 : packet.param4 = cmd.content.yaw.relative_angle; // 0 = absolute angle provided, 1 = relative angle provided
1753 23 : break;
1754 :
1755 372 : case MAV_CMD_DO_JUMP: // MAV ID: 177
1756 : case MAV_CMD_DO_JUMP_TAG: // MAV ID: 601
1757 372 : packet.param1 = cmd.content.jump.target; // jump-to command/tag number
1758 372 : packet.param2 = cmd.content.jump.num_times; // repeat count
1759 372 : break;
1760 :
1761 5 : case MAV_CMD_JUMP_TAG: // MAV ID: 600
1762 5 : packet.param1 = cmd.content.jump.target; // jump-to tag number
1763 5 : break;
1764 :
1765 124 : case MAV_CMD_DO_CHANGE_SPEED: // MAV ID: 178
1766 124 : packet.param1 = cmd.content.speed.speed_type; // 0 = airspeed, 1 = ground speed
1767 124 : packet.param2 = cmd.content.speed.target_ms; // speed in m/s
1768 124 : packet.param3 = cmd.content.speed.throttle_pct; // throttle as a percentage from 1 ~ 100%
1769 124 : break;
1770 :
1771 0 : case MAV_CMD_DO_SET_HOME: // MAV ID: 179
1772 0 : packet.param1 = cmd.p1; // p1=0 means use current location, p=1 means use provided location
1773 0 : break;
1774 :
1775 0 : case MAV_CMD_DO_SET_RELAY: // MAV ID: 181
1776 0 : packet.param1 = cmd.content.relay.num; // relay number
1777 0 : packet.param2 = cmd.content.relay.state; // 0:off, 1:on
1778 0 : break;
1779 :
1780 0 : case MAV_CMD_DO_REPEAT_RELAY: // MAV ID: 182
1781 0 : packet.param1 = cmd.content.repeat_relay.num; // relay number
1782 0 : packet.param2 = cmd.content.repeat_relay.repeat_count; // count
1783 0 : packet.param3 = cmd.content.repeat_relay.cycle_time; // time in seconds
1784 0 : break;
1785 :
1786 4 : case MAV_CMD_DO_SET_SERVO: // MAV ID: 183
1787 4 : packet.param1 = cmd.content.servo.channel; // channel
1788 4 : packet.param2 = cmd.content.servo.pwm; // PWM
1789 4 : break;
1790 :
1791 0 : case MAV_CMD_DO_REPEAT_SERVO: // MAV ID: 184
1792 0 : packet.param1 = cmd.content.repeat_servo.channel; // channel
1793 0 : packet.param2 = cmd.content.repeat_servo.pwm; // PWM
1794 0 : packet.param3 = cmd.content.repeat_servo.repeat_count; // count
1795 0 : packet.param4 = cmd.content.repeat_servo.cycle_time; // time in milliseconds converted to seconds
1796 0 : break;
1797 :
1798 442 : case MAV_CMD_DO_RETURN_PATH_START: // MAV ID: 188
1799 : case MAV_CMD_DO_LAND_START: // MAV ID: 189
1800 442 : break;
1801 :
1802 0 : case MAV_CMD_DO_GO_AROUND: // MAV ID: 191
1803 0 : break;
1804 :
1805 0 : case MAV_CMD_DO_SET_ROI_LOCATION: // MAV ID: 195
1806 : case MAV_CMD_DO_SET_ROI_NONE: // MAV ID: 197
1807 0 : packet.param1 = cmd.p1; // gimbal device id
1808 0 : break;
1809 :
1810 0 : case MAV_CMD_DO_SET_ROI: // MAV ID: 201
1811 0 : packet.param1 = cmd.p1; // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported)
1812 0 : break;
1813 :
1814 0 : case MAV_CMD_DO_DIGICAM_CONFIGURE: // MAV ID: 202
1815 0 : packet.param1 = cmd.content.digicam_configure.shooting_mode;
1816 0 : packet.param2 = cmd.content.digicam_configure.shutter_speed;
1817 0 : packet.param3 = cmd.content.digicam_configure.aperture;
1818 0 : packet.param4 = cmd.content.digicam_configure.ISO;
1819 0 : packet.x = cmd.content.digicam_configure.exposure_type;
1820 0 : packet.y = cmd.content.digicam_configure.cmd_id;
1821 0 : packet.z = cmd.content.digicam_configure.engine_cutoff_time;
1822 0 : break;
1823 :
1824 3 : case MAV_CMD_DO_DIGICAM_CONTROL: // MAV ID: 203
1825 3 : packet.param1 = cmd.content.digicam_control.session;
1826 3 : packet.param2 = cmd.content.digicam_control.zoom_pos;
1827 3 : packet.param3 = cmd.content.digicam_control.zoom_step;
1828 3 : packet.param4 = cmd.content.digicam_control.focus_lock;
1829 3 : packet.x = cmd.content.digicam_control.shooting_cmd;
1830 3 : packet.y = cmd.content.digicam_control.cmd_id;
1831 3 : break;
1832 :
1833 0 : case MAV_CMD_DO_MOUNT_CONTROL: // MAV ID: 205
1834 0 : packet.param1 = cmd.content.mount_control.pitch;
1835 0 : packet.param2 = cmd.content.mount_control.roll;
1836 0 : packet.param3 = cmd.content.mount_control.yaw;
1837 0 : packet.z = MAV_MOUNT_MODE_MAVLINK_TARGETING;
1838 0 : break;
1839 :
1840 68 : case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206
1841 68 : packet.param1 = cmd.content.cam_trigg_dist.meters; // distance between camera shots in meters
1842 68 : packet.param3 = cmd.content.cam_trigg_dist.trigger; // when enabled, camera triggers once immediately
1843 68 : packet.param4 = cmd.content.cam_trigg_dist.camera_id; // which camera to trigger
1844 68 : break;
1845 :
1846 0 : case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207
1847 0 : packet.param1 = cmd.p1; // action 0=disable, 1=enable, 2=disable floor, 3=enable except floor
1848 0 : break;
1849 :
1850 34 : case MAV_CMD_DO_PARACHUTE: // MAV ID: 208
1851 34 : packet.param1 = cmd.p1; // action 0=disable, 1=enable, 2=release. See PARACHUTE_ACTION enum
1852 34 : break;
1853 :
1854 7 : case MAV_CMD_DO_SPRAYER:
1855 7 : packet.param1 = cmd.p1; // action 0=disable, 1=enable
1856 7 : break;
1857 :
1858 3 : case MAV_CMD_DO_AUX_FUNCTION:
1859 3 : packet.param1 = cmd.content.auxfunction.function;
1860 3 : packet.param2 = cmd.content.auxfunction.switchpos;
1861 3 : break;
1862 :
1863 6 : case MAV_CMD_DO_INVERTED_FLIGHT: // MAV ID: 210
1864 6 : packet.param1 = cmd.p1; // normal=0 inverted=1
1865 6 : break;
1866 :
1867 : #if AP_GRIPPER_ENABLED
1868 51 : case MAV_CMD_DO_GRIPPER: // MAV ID: 211
1869 51 : packet.param1 = cmd.content.gripper.num; // gripper number
1870 51 : packet.param2 = cmd.content.gripper.action; // action 0=release, 1=grab. See GRIPPER_ACTION enum
1871 51 : break;
1872 : #endif
1873 :
1874 4 : case MAV_CMD_DO_GUIDED_LIMITS: // MAV ID: 222
1875 4 : packet.param1 = cmd.p1; // max time in seconds the external controller will be allowed to control the vehicle
1876 4 : packet.param2 = cmd.content.guided_limits.alt_min; // min alt below which the command will be aborted. 0 for no lower alt limit
1877 4 : packet.param3 = cmd.content.guided_limits.alt_max; // max alt above which the command will be aborted. 0 for no upper alt limit
1878 4 : packet.param4 = cmd.content.guided_limits.horiz_max;// max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit
1879 4 : break;
1880 :
1881 0 : case MAV_CMD_DO_AUTOTUNE_ENABLE:
1882 0 : packet.param1 = cmd.p1; // disable=0 enable=1
1883 0 : break;
1884 :
1885 0 : case MAV_CMD_DO_SET_REVERSE:
1886 0 : packet.param1 = cmd.p1; // 0 = forward, 1 = reverse
1887 0 : break;
1888 :
1889 6 : case MAV_CMD_NAV_ALTITUDE_WAIT: // MAV ID: 83
1890 6 : packet.param1 = cmd.content.altitude_wait.altitude;
1891 6 : packet.param2 = cmd.content.altitude_wait.descent_rate;
1892 6 : packet.param3 = cmd.content.altitude_wait.wiggle_time;
1893 6 : break;
1894 :
1895 89 : case MAV_CMD_NAV_VTOL_TAKEOFF:
1896 89 : break;
1897 :
1898 97 : case MAV_CMD_NAV_VTOL_LAND:
1899 97 : packet.param1 = cmd.p1;
1900 97 : break;
1901 :
1902 6 : case MAV_CMD_DO_VTOL_TRANSITION:
1903 6 : packet.param1 = cmd.content.do_vtol_transition.target_state;
1904 6 : break;
1905 :
1906 63 : case MAV_CMD_DO_ENGINE_CONTROL:
1907 63 : packet.param1 = cmd.content.do_engine_control.start_control?1:0;
1908 63 : packet.param2 = cmd.content.do_engine_control.cold_start?1:0;
1909 63 : packet.param3 = cmd.content.do_engine_control.height_delay_cm*0.01f;
1910 63 : if (cmd.content.do_engine_control.allow_disarmed_start) {
1911 0 : packet.param4 = ENGINE_CONTROL_OPTIONS_ALLOW_START_WHILE_DISARMED;
1912 : }
1913 63 : break;
1914 :
1915 : #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED
1916 8 : case MAV_CMD_NAV_PAYLOAD_PLACE:
1917 8 : packet.param1 = cmd.p1*0.01f; // copy max-descend parameter (cm->m)
1918 8 : break;
1919 : #endif
1920 :
1921 0 : case MAV_CMD_NAV_SET_YAW_SPEED:
1922 0 : packet.param1 = cmd.content.set_yaw_speed.angle_deg; // target angle in degrees
1923 0 : packet.param2 = cmd.content.set_yaw_speed.speed; // speed in meters/second
1924 0 : packet.param3 = cmd.content.set_yaw_speed.relative_angle; // 0 = absolute angle, 1 = relative angle
1925 0 : break;
1926 :
1927 0 : case MAV_CMD_DO_WINCH:
1928 0 : packet.param1 = cmd.content.winch.num; // winch number
1929 0 : packet.param2 = cmd.content.winch.action; // action (0 = relax, 1 = length control, 2 = rate control). See WINCH_ACTION enum
1930 0 : packet.param3 = cmd.content.winch.release_length; // cable distance to unwind in meters, negative numbers to wind in cable
1931 0 : packet.param4 = cmd.content.winch.release_rate; // release rate in meters/second
1932 0 : break;
1933 :
1934 0 : case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
1935 0 : packet.param1 = cmd.p1; // Resume repeat distance (m)
1936 0 : break;
1937 :
1938 0 : case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
1939 0 : packet.param1 = cmd.p1;
1940 0 : packet.param2 = cmd.content.scripting.p1;
1941 0 : packet.param3 = cmd.content.scripting.p2;
1942 0 : packet.param4 = cmd.content.scripting.p3;
1943 0 : break;
1944 :
1945 : #if AP_SCRIPTING_ENABLED
1946 0 : case MAV_CMD_NAV_SCRIPT_TIME:
1947 0 : packet.param1 = cmd.content.nav_script_time.command;
1948 0 : packet.param2 = cmd.content.nav_script_time.timeout_s;
1949 0 : packet.param3 = cmd.content.nav_script_time.arg1.get();
1950 0 : packet.param4 = cmd.content.nav_script_time.arg2.get();
1951 0 : packet.x = cmd.content.nav_script_time.arg3;
1952 0 : packet.y = cmd.content.nav_script_time.arg4;
1953 0 : break;
1954 : #endif
1955 :
1956 0 : case MAV_CMD_NAV_ATTITUDE_TIME:
1957 0 : packet.param1 = cmd.content.nav_attitude_time.time_sec;
1958 0 : packet.param2 = cmd.content.nav_attitude_time.roll_deg;
1959 0 : packet.param3 = cmd.content.nav_attitude_time.pitch_deg;
1960 0 : packet.param4 = cmd.content.nav_attitude_time.yaw_deg;
1961 0 : packet.x = cmd.content.nav_attitude_time.climb_rate;
1962 0 : break;
1963 :
1964 0 : case MAV_CMD_DO_PAUSE_CONTINUE:
1965 0 : packet.param1 = cmd.p1;
1966 0 : break;
1967 :
1968 0 : case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
1969 0 : packet.param1 = cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg;
1970 0 : packet.param2 = cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg;
1971 0 : packet.param3 = cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs;
1972 0 : packet.param4 = cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs;
1973 0 : packet.x = cmd.content.gimbal_manager_pitchyaw.flags;
1974 0 : packet.z = cmd.content.gimbal_manager_pitchyaw.gimbal_id;
1975 0 : break;
1976 :
1977 9 : case MAV_CMD_IMAGE_START_CAPTURE:
1978 9 : packet.param1 = cmd.content.image_start_capture.instance;
1979 9 : packet.param2 = cmd.content.image_start_capture.interval_s;
1980 9 : packet.param3 = cmd.content.image_start_capture.total_num_images;
1981 9 : packet.param4 = cmd.content.image_start_capture.start_seq_number;
1982 9 : break;
1983 :
1984 3 : case MAV_CMD_IMAGE_STOP_CAPTURE:
1985 3 : packet.param1 = cmd.p1;
1986 3 : break;
1987 :
1988 6 : case MAV_CMD_SET_CAMERA_ZOOM:
1989 6 : packet.param1 = cmd.content.set_camera_zoom.zoom_type;
1990 6 : packet.param2 = cmd.content.set_camera_zoom.zoom_value;
1991 6 : packet.param3 = cmd.content.set_camera_zoom.camera_id;
1992 6 : break;
1993 :
1994 6 : case MAV_CMD_SET_CAMERA_FOCUS:
1995 6 : packet.param1 = cmd.content.set_camera_focus.focus_type;
1996 6 : packet.param2 = cmd.content.set_camera_focus.focus_value;
1997 6 : packet.param3 = cmd.content.set_camera_focus.camera_id;
1998 6 : break;
1999 :
2000 0 : case MAV_CMD_SET_CAMERA_SOURCE:
2001 0 : packet.param1 = cmd.content.set_camera_source.instance;
2002 0 : packet.param2 = cmd.content.set_camera_source.primary_source;
2003 0 : packet.param3 = cmd.content.set_camera_source.secondary_source;
2004 0 : break;
2005 :
2006 0 : case MAV_CMD_VIDEO_START_CAPTURE:
2007 0 : packet.param1 = cmd.content.video_start_capture.video_stream_id;
2008 0 : break;
2009 :
2010 0 : case MAV_CMD_VIDEO_STOP_CAPTURE:
2011 0 : packet.param1 = cmd.content.video_stop_capture.video_stream_id;
2012 0 : break;
2013 :
2014 : #if AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
2015 12 : case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
2016 12 : packet.param1 = cmd.content.wpnext_offset.gimbal_id;
2017 12 : packet.y = cmd.content.wpnext_offset.roll_offset_cd * 0.01;
2018 12 : packet.x = cmd.content.wpnext_offset.pitch_offset_cd * 0.01;
2019 12 : packet.z = cmd.content.wpnext_offset.yaw_offset_cd * 0.01;
2020 12 : break;
2021 : #endif // AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
2022 :
2023 0 : default:
2024 : // unrecognised command
2025 0 : return false;
2026 : }
2027 :
2028 : // copy location from mavlink to command
2029 26892 : if (stored_in_location(cmd.id)) {
2030 25754 : packet.x = cmd.content.location.lat;
2031 25754 : packet.y = cmd.content.location.lng;
2032 :
2033 25754 : packet.z = cmd.content.location.alt * 0.01f; // cmd alt in cm to m
2034 25754 : if (cmd.content.location.relative_alt) {
2035 23687 : packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
2036 : } else {
2037 2067 : packet.frame = MAV_FRAME_GLOBAL;
2038 : }
2039 : #if AP_TERRAIN_AVAILABLE
2040 25754 : if (cmd.content.location.terrain_alt) {
2041 : // this is a above-terrain altitude
2042 2510 : if (!cmd.content.location.relative_alt) {
2043 : // refuse to return non-relative terrain mission
2044 : // items. Internally we do have these, and they
2045 : // have home.alt added, but we should never be
2046 : // returning them to the GCS, as the GCS doesn't know
2047 : // our home.alt, so it would have no way to properly
2048 : // interpret it
2049 0 : return false;
2050 : }
2051 2510 : packet.z = cmd.content.location.alt * 0.01f;
2052 2510 : packet.frame = MAV_FRAME_GLOBAL_TERRAIN_ALT;
2053 : }
2054 : #else
2055 : // don't ever return terrain mission items if no terrain support
2056 : if (cmd.content.location.terrain_alt) {
2057 : return false;
2058 : }
2059 : #endif
2060 : }
2061 :
2062 : // if we got this far then it must have been successful
2063 26892 : return true;
2064 : }
2065 :
2066 : ///
2067 : /// private methods
2068 : ///
2069 :
2070 : /// complete - mission is marked complete and clean-up performed including calling the mission_complete_fn
2071 86 : void AP_Mission::complete()
2072 : {
2073 : // flag mission as complete
2074 86 : _flags.state = MISSION_COMPLETE;
2075 86 : _flags.in_landing_sequence = false;
2076 86 : _flags.in_return_path = false;
2077 :
2078 : // callback to main program's mission complete function
2079 86 : _mission_complete_fn();
2080 86 : }
2081 :
2082 : /// advance_current_nav_cmd - moves current nav command forward
2083 : /// do command will also be loaded
2084 : /// accounts for do-jump commands
2085 : // returns true if command is advanced, false if failed (i.e. mission completed)
2086 1932 : bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
2087 : {
2088 : // exit immediately if we're not running
2089 1932 : if (_flags.state != MISSION_RUNNING) {
2090 0 : return false;
2091 : }
2092 :
2093 : // exit immediately if current nav command has not completed
2094 1932 : if (_flags.nav_cmd_loaded) {
2095 0 : return false;
2096 : }
2097 :
2098 : // stop the current running do command
2099 1932 : _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
2100 1932 : _flags.do_cmd_loaded = false;
2101 1932 : _flags.do_cmd_all_done = false;
2102 :
2103 : // get starting point for search
2104 1932 : uint16_t cmd_index = starting_index > 0 ? starting_index - 1 : _nav_cmd.index;
2105 1932 : if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
2106 : // start from beginning of the mission command list
2107 115 : cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
2108 : } else {
2109 : // start from one position past the current nav command
2110 1817 : cmd_index++;
2111 : }
2112 :
2113 : // avoid endless loops
2114 1932 : uint8_t max_loops = 255;
2115 :
2116 : // search until we find next nav command or reach end of command list
2117 3962 : while (!_flags.nav_cmd_loaded && max_loops-- > 0) {
2118 : // get next command
2119 2116 : Mission_Command cmd;
2120 2116 : if (!get_next_cmd(cmd_index, cmd, true)) {
2121 86 : return false;
2122 : }
2123 :
2124 : // check if navigation or "do" command
2125 2030 : if (is_nav_cmd(cmd)) {
2126 : // save previous nav command index
2127 1853 : _prev_nav_cmd_id = _nav_cmd.id;
2128 1853 : _prev_nav_cmd_index = _nav_cmd.index;
2129 : // save separate previous nav command index if it contains lat,long,alt
2130 1853 : if (!(cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
2131 1615 : _prev_nav_cmd_wp_index = _nav_cmd.index;
2132 : }
2133 : // set current navigation command and start it
2134 1853 : _nav_cmd = cmd;
2135 1853 : if (start_command(_nav_cmd)) {
2136 1846 : _flags.nav_cmd_loaded = true;
2137 1846 : if (_jump_tag.age > 0 && _jump_tag.age < UINT16_MAX) {
2138 : // we're tracking a tag so increase it's age on every new NAV item
2139 1 : _jump_tag.age++;
2140 : }
2141 : }
2142 : // save a loaded wp index in history array for when _repeat_dist is set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
2143 : // and prevent history being re-written until vehicle returns to interrupted position
2144 1853 : if (_repeat_dist > 0 && !_flags.resuming_mission && _nav_cmd.index != AP_MISSION_CMD_INDEX_NONE && !(_nav_cmd.content.location.lat == 0 && _nav_cmd.content.location.lng == 0)) {
2145 : // update mission history. last index position is always the most recent wp loaded.
2146 0 : for (uint8_t i=0; i<AP_MISSION_MAX_WP_HISTORY-1; i++) {
2147 0 : _wp_index_history[i] = _wp_index_history[i+1];
2148 : }
2149 0 : _wp_index_history[AP_MISSION_MAX_WP_HISTORY-1] = _nav_cmd.index;
2150 : }
2151 : // check if the vehicle is resuming and has returned to where it was interrupted
2152 1853 : if (_flags.resuming_mission && _nav_cmd.index == _wp_index_history[AP_MISSION_MAX_WP_HISTORY-1]) {
2153 : // vehicle has resumed previous position
2154 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: Returned to interrupted WP");
2155 0 : _flags.resuming_mission = false;
2156 : }
2157 :
2158 : } else {
2159 : // set current do command and start it (if not already set)
2160 177 : if (!_flags.do_cmd_loaded) {
2161 166 : _do_cmd = cmd;
2162 166 : _flags.do_cmd_loaded = true;
2163 166 : start_command(_do_cmd);
2164 : }
2165 : }
2166 : // move onto next command
2167 2030 : cmd_index = cmd.index+1;
2168 : }
2169 :
2170 1846 : if (max_loops == 0) {
2171 : // infinite loop. This can happen if there's a loop involving
2172 : // only nav commands (no DO commands) which won't start()
2173 0 : return false;
2174 : }
2175 :
2176 : // if we have not found a do command then set flag to show there are no do-commands to be run before nav command completes
2177 1846 : if (!_flags.do_cmd_loaded) {
2178 1684 : _flags.do_cmd_all_done = true;
2179 : }
2180 :
2181 : // if we got this far we must have successfully advanced the nav command
2182 1846 : return true;
2183 : }
2184 :
2185 : /// advance_current_do_cmd - moves current do command forward
2186 : /// accounts for do-jump commands
2187 4236782 : void AP_Mission::advance_current_do_cmd()
2188 : {
2189 : // exit immediately if we're not running or we've completed all possible "do" commands
2190 4236782 : if (_flags.state != MISSION_RUNNING || _flags.do_cmd_all_done) {
2191 4236771 : return;
2192 : }
2193 :
2194 : // get starting point for search
2195 157 : uint16_t cmd_index = _do_cmd.index;
2196 157 : if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
2197 9 : cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
2198 : } else {
2199 : // start from one position past the current do command
2200 148 : cmd_index = _do_cmd.index + 1;
2201 : }
2202 :
2203 : // find next do command
2204 157 : Mission_Command cmd;
2205 157 : if (!get_next_do_cmd(cmd_index, cmd)) {
2206 : // set flag to stop unnecessarily searching for do commands
2207 146 : _flags.do_cmd_all_done = true;
2208 146 : return;
2209 : }
2210 :
2211 : // set current do command and start it
2212 11 : _do_cmd = cmd;
2213 11 : _flags.do_cmd_loaded = true;
2214 11 : start_command(_do_cmd);
2215 : }
2216 :
2217 : /// get_next_cmd - gets next command found at or after start_index
2218 : /// returns true if found, false if not found (i.e. mission complete)
2219 : /// accounts for do_jump commands
2220 : /// increment_jump_num_times_if_found should be set to true if advancing the active navigation command
2221 27416 : bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool increment_jump_num_times_if_found, bool send_gcs_msg)
2222 : {
2223 27416 : uint16_t cmd_index = start_index;
2224 27416 : Mission_Command temp_cmd;
2225 27416 : uint16_t jump_index = AP_MISSION_CMD_INDEX_NONE;
2226 :
2227 : // search until the end of the mission command list
2228 27416 : uint8_t max_loops = 64;
2229 27941 : while (cmd_index < (unsigned)_cmd_total) {
2230 : // load the next command
2231 27854 : if (!read_cmd_from_storage(cmd_index, temp_cmd)) {
2232 : // this should never happen because of check above but just in case
2233 0 : return false;
2234 : }
2235 :
2236 : // check for do-jump-tag command and convert target tag to do-jump target index and do-jump to it
2237 27854 : if (temp_cmd.id == MAV_CMD_DO_JUMP_TAG) {
2238 : // convert tmp_cmd target from a target tag to a target index
2239 4 : temp_cmd.content.jump.target = get_index_of_jump_tag(temp_cmd.content.jump.target);
2240 4 : temp_cmd.id = MAV_CMD_DO_JUMP;
2241 : }
2242 :
2243 : // check for do-jump command
2244 27854 : if (temp_cmd.id == MAV_CMD_DO_JUMP) {
2245 :
2246 529 : if (max_loops-- == 0) {
2247 0 : return false;
2248 : }
2249 :
2250 : // check for invalid target
2251 529 : if ((temp_cmd.content.jump.target >= (unsigned)_cmd_total) || (temp_cmd.content.jump.target == 0)) {
2252 : // To-Do: log an error?
2253 4 : return false;
2254 : }
2255 :
2256 : // check for endless loops
2257 525 : if (!increment_jump_num_times_if_found && jump_index == cmd_index) {
2258 : // we have somehow reached this jump command twice and there is no chance it will complete
2259 : // To-Do: log an error?
2260 0 : return false;
2261 : }
2262 :
2263 : // record this command so we can check for endless loops
2264 525 : if (jump_index == AP_MISSION_CMD_INDEX_NONE) {
2265 525 : jump_index = cmd_index;
2266 : }
2267 :
2268 : // get number of times jump command has already been run
2269 1039 : if (temp_cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER ||
2270 514 : get_jump_times_run(temp_cmd) < temp_cmd.content.jump.num_times) {
2271 : // update the record of the number of times run
2272 413 : if (increment_jump_num_times_if_found && !_flags.resuming_mission) {
2273 50 : increment_jump_times_run(temp_cmd, send_gcs_msg);
2274 : }
2275 : // continue searching from jump target
2276 413 : cmd_index = temp_cmd.content.jump.target;
2277 : } else {
2278 112 : if (increment_jump_num_times_if_found && !option_is_set(Option::DONT_ZERO_COUNTER)) {
2279 : /*
2280 : when we have completed a jump loop reset the counter
2281 : so if the user changes WP back to restart the loop
2282 : they get the count again
2283 : */
2284 34 : for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
2285 34 : if (_jump_tracking[i].index == cmd_index) {
2286 34 : _jump_tracking[i].num_times_run = 0;
2287 34 : break;
2288 : }
2289 : }
2290 : }
2291 : // jump has been run specified number of times so move search to next command in mission
2292 112 : cmd_index++;
2293 : }
2294 : } else {
2295 : // this is a non-jump command so return it
2296 27325 : cmd = temp_cmd;
2297 27325 : return true;
2298 : }
2299 : }
2300 :
2301 : // if we got this far we did not find a navigation command
2302 87 : return false;
2303 : }
2304 :
2305 : /// get_next_do_cmd - gets next "do" or "conditional" command after start_index
2306 : /// returns true if found, false if not found
2307 : /// stops and returns false if it hits another navigation command before it finds the first do or conditional command
2308 : /// accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this)
2309 157 : bool AP_Mission::get_next_do_cmd(uint16_t start_index, Mission_Command& cmd)
2310 : {
2311 157 : Mission_Command temp_cmd;
2312 :
2313 : // check we have not passed the end of the mission list
2314 157 : if (start_index >= (unsigned)_cmd_total) {
2315 1 : return false;
2316 : }
2317 :
2318 : // get next command
2319 156 : if (!get_next_cmd(start_index, temp_cmd, false)) {
2320 : // no more commands so return failure
2321 0 : return false;
2322 156 : } else if (is_nav_cmd(temp_cmd)) {
2323 : // if it's a "navigation" command then return false because we do not progress past nav commands
2324 145 : return false;
2325 : } else {
2326 : // this must be a "do" or "conditional" and is not a do-jump command so return it
2327 11 : cmd = temp_cmd;
2328 11 : return true;
2329 : }
2330 : }
2331 :
2332 : ///
2333 : /// jump handling methods
2334 : ///
2335 :
2336 : // Set the mission index to the first JUMP_TAG with this tag.
2337 : // Returns true on success, else false if no appropriate JUMP_TAG match can be found or if setting the index failed
2338 4 : bool AP_Mission::jump_to_tag(const uint16_t tag)
2339 : {
2340 4 : const uint16_t index = get_index_of_jump_tag(tag);
2341 4 : if (index == 0) {
2342 2 : return false;
2343 : }
2344 2 : return set_current_cmd(index);
2345 : }
2346 :
2347 : // find the first JUMP_TAG with this tag and return its index.
2348 : // Returns 0 if no appropriate JUMP_TAG match can be found.
2349 8 : uint16_t AP_Mission::get_index_of_jump_tag(const uint16_t tag) const
2350 : {
2351 8 : const auto count = num_commands();
2352 28 : for (uint16_t i = 1; i < count; i++) {
2353 22 : if (get_command_id(i) != uint16_t(MAV_CMD_JUMP_TAG)) {
2354 19 : continue;
2355 : }
2356 3 : Mission_Command tmp;
2357 3 : if (!read_cmd_from_storage(i, tmp)) {
2358 0 : continue;
2359 : }
2360 3 : if (tmp.id == MAV_CMD_JUMP_TAG && tmp.content.jump.target == tag) {
2361 2 : return i;
2362 : }
2363 : }
2364 6 : return 0;
2365 : }
2366 :
2367 : #if AP_SCRIPTING_ENABLED
2368 0 : bool AP_Mission::get_last_jump_tag(uint16_t &tag, uint16_t &age) const
2369 : {
2370 0 : if (_jump_tag.age == 0) {
2371 0 : return false;
2372 : }
2373 0 : tag = _jump_tag.tag;
2374 0 : age = _jump_tag.age;
2375 0 : return true;
2376 : }
2377 : #endif
2378 :
2379 : // init_jump_tracking - initialise jump_tracking variables
2380 3369 : void AP_Mission::init_jump_tracking()
2381 : {
2382 340269 : for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
2383 336900 : _jump_tracking[i].index = AP_MISSION_CMD_INDEX_NONE;
2384 336900 : _jump_tracking[i].num_times_run = 0;
2385 : }
2386 3369 : }
2387 :
2388 : /// get_jump_times_run - returns number of times the jump command has been run
2389 514 : int16_t AP_Mission::get_jump_times_run(const Mission_Command& cmd)
2390 : {
2391 : // exit immediately if cmd is not a do-jump command or target is invalid
2392 514 : if ((cmd.id != MAV_CMD_DO_JUMP) || (cmd.content.jump.target >= (unsigned)_cmd_total) || (cmd.content.jump.target == 0)) {
2393 : // To-Do: log an error?
2394 0 : return AP_MISSION_JUMP_TIMES_MAX;
2395 : }
2396 :
2397 : // search through jump_tracking array for this cmd
2398 514 : for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
2399 514 : if (_jump_tracking[i].index == cmd.index) {
2400 477 : return _jump_tracking[i].num_times_run;
2401 37 : } else if (_jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
2402 : // we've searched through all known jump commands and haven't found it so allocate new space in _jump_tracking array
2403 37 : _jump_tracking[i].index = cmd.index;
2404 37 : _jump_tracking[i].num_times_run = 0;
2405 37 : return 0;
2406 : }
2407 : }
2408 :
2409 : // if we've gotten this far then the _jump_tracking array must be full
2410 : // To-Do: log an error?
2411 0 : return AP_MISSION_JUMP_TIMES_MAX;
2412 : }
2413 :
2414 : /// increment_jump_times_run - increments the recorded number of times the jump command has been run
2415 50 : void AP_Mission::increment_jump_times_run(Mission_Command& cmd, bool send_gcs_msg)
2416 : {
2417 : // exit immediately if cmd is not a do-jump command
2418 50 : if (cmd.id != MAV_CMD_DO_JUMP) {
2419 : // To-Do: log an error?
2420 0 : return;
2421 : }
2422 :
2423 : // search through jump_tracking array for this cmd
2424 50 : for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
2425 50 : if (_jump_tracking[i].index == cmd.index) {
2426 46 : _jump_tracking[i].num_times_run++;
2427 46 : if (send_gcs_msg) {
2428 46 : if (cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER) {
2429 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u Jump %i/unlimited", _jump_tracking[i].index, _jump_tracking[i].num_times_run);
2430 : } else {
2431 45 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u Jump %i/%i", _jump_tracking[i].index, _jump_tracking[i].num_times_run, cmd.content.jump.num_times);
2432 : }
2433 : }
2434 46 : return;
2435 4 : } else if (_jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
2436 : // we've searched through all known jump commands and haven't found it so allocate new space in _jump_tracking array
2437 4 : _jump_tracking[i].index = cmd.index;
2438 4 : _jump_tracking[i].num_times_run = 1;
2439 4 : return;
2440 : }
2441 : }
2442 :
2443 : // if we've gotten this far then the _jump_tracking array must be full
2444 : // To-Do: log an error
2445 0 : return;
2446 : }
2447 :
2448 : // check_eeprom_version - checks version of missions stored in eeprom matches this library
2449 : // command list will be cleared if they do not match
2450 2054 : void AP_Mission::check_eeprom_version()
2451 : {
2452 2054 : uint32_t eeprom_version = _storage.read_uint32(0);
2453 :
2454 : // if eeprom version does not match, clear the command list and update the eeprom version
2455 2054 : if (eeprom_version != AP_MISSION_EEPROM_VERSION) {
2456 291 : if (clear()) {
2457 291 : _storage.write_uint32(0, AP_MISSION_EEPROM_VERSION);
2458 : }
2459 : }
2460 2054 : }
2461 :
2462 : // find the nearest landing sequence starting point (DO_LAND_START) and
2463 : // return its index. Returns 0 if no appropriate DO_LAND_START point can
2464 : // be found.
2465 97 : uint16_t AP_Mission::get_landing_sequence_start(const Location ¤t_loc)
2466 : {
2467 97 : uint16_t landing_start_index = 0;
2468 97 : float min_distance = -1;
2469 :
2470 : // Go through mission looking for nearest landing start command
2471 97 : const auto count = num_commands();
2472 265 : for (uint16_t i = 1; i < count; i++) {
2473 168 : if (get_command_id(i) != uint16_t(MAV_CMD_DO_LAND_START)) {
2474 140 : continue;
2475 : }
2476 28 : Mission_Command tmp;
2477 28 : if (!read_cmd_from_storage(i, tmp)) {
2478 0 : continue;
2479 : }
2480 28 : if (tmp.id == MAV_CMD_DO_LAND_START) {
2481 28 : if (!tmp.content.location.initialised() && !get_next_nav_cmd(i, tmp)) {
2482 : // command does not have a valid location and cannot get next valid
2483 0 : continue;
2484 : }
2485 :
2486 28 : const float tmp_distance = tmp.content.location.get_distance_NED_alt_frame(current_loc).length();
2487 28 : if (min_distance < 0 || tmp_distance < min_distance) {
2488 28 : min_distance = tmp_distance;
2489 28 : landing_start_index = i;
2490 : }
2491 : }
2492 : }
2493 :
2494 97 : return landing_start_index;
2495 : }
2496 :
2497 : /*
2498 : find the nearest landing sequence starting point (DO_LAND_START) and
2499 : switch to that mission item. Returns false if no DO_LAND_START
2500 : available.
2501 : */
2502 39 : bool AP_Mission::jump_to_landing_sequence(const Location ¤t_loc)
2503 : {
2504 39 : uint16_t land_idx = get_landing_sequence_start(current_loc);
2505 39 : if (land_idx != 0 && set_current_cmd(land_idx)) {
2506 :
2507 : //if the mission has ended it has to be restarted
2508 22 : if (state() == AP_Mission::MISSION_STOPPED) {
2509 17 : resume();
2510 : }
2511 :
2512 22 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing sequence start");
2513 22 : return true;
2514 : }
2515 :
2516 17 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start landing sequence");
2517 17 : return false;
2518 : }
2519 :
2520 : /*
2521 : find the closest point on the mission after a DO_RETURN_PATH_START and before DO_LAND_START or landing
2522 : */
2523 18 : bool AP_Mission::jump_to_closest_mission_leg(const Location ¤t_loc)
2524 : {
2525 18 : if (_flags.state == MISSION_RUNNING) {
2526 : // if mission is already running don't switch away from a active landing or return path
2527 0 : if (_flags.in_landing_sequence) {
2528 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing sequence active");
2529 0 : return true;
2530 :
2531 0 : } else if (_flags.in_return_path) {
2532 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Return path active");
2533 0 : return true;
2534 : }
2535 : }
2536 :
2537 18 : uint16_t landing_start_index = 0;
2538 18 : float min_distance = -1;
2539 :
2540 : // This defines the maximum number of waypoints that will be searched, this limits the worst case runtime
2541 18 : uint16_t search_remaining = 1000;
2542 :
2543 : // Go through mission and check each DO_RETURN_PATH_START
2544 68 : for (uint16_t i = 1; i < num_commands(); i++) {
2545 50 : if (get_command_id(i) == uint16_t(MAV_CMD_DO_RETURN_PATH_START)) {
2546 : uint16_t tmp_index;
2547 : float tmp_distance;
2548 4 : if (distance_to_mission_leg(i, search_remaining, tmp_distance, tmp_index, current_loc) && (min_distance < 0 || tmp_distance <= min_distance)){
2549 4 : min_distance = tmp_distance;
2550 4 : landing_start_index = tmp_index;
2551 : }
2552 4 : if (search_remaining == 0) {
2553 : // Run out of time to search, stop and return the best so far
2554 0 : break;
2555 : }
2556 : }
2557 : }
2558 :
2559 18 : if (landing_start_index != 0 && set_current_cmd(landing_start_index)) {
2560 :
2561 : // if the mission has ended it has to be restarted
2562 4 : if (state() == AP_Mission::MISSION_STOPPED) {
2563 4 : resume();
2564 : }
2565 :
2566 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Return path started");
2567 4 : _flags.in_return_path = true;
2568 4 : return true;
2569 : }
2570 :
2571 : // Failed to find do land start
2572 14 : return false;
2573 : }
2574 :
2575 : // jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort
2576 2 : bool AP_Mission::jump_to_abort_landing_sequence(const Location ¤t_loc)
2577 : {
2578 2 : uint16_t abort_index = 0;
2579 2 : float min_distance = FLT_MAX;
2580 :
2581 2 : const auto count = num_commands();
2582 14 : for (uint16_t i = 1; i < count; i++) {
2583 12 : if (get_command_id(i) != uint16_t(MAV_CMD_DO_GO_AROUND)) {
2584 12 : continue;
2585 : }
2586 0 : Mission_Command tmp;
2587 0 : if (!read_cmd_from_storage(i, tmp)) {
2588 0 : continue;
2589 : }
2590 0 : if (tmp.id == MAV_CMD_DO_GO_AROUND) {
2591 0 : float tmp_distance = tmp.content.location.get_distance(current_loc);
2592 0 : if (tmp_distance < min_distance) {
2593 0 : min_distance = tmp_distance;
2594 0 : abort_index = i;
2595 : }
2596 : }
2597 : }
2598 :
2599 2 : if (abort_index != 0 && set_current_cmd(abort_index)) {
2600 :
2601 : //if the mission has ended it has to be restarted
2602 0 : if (state() == AP_Mission::MISSION_STOPPED) {
2603 0 : resume();
2604 : }
2605 :
2606 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing abort sequence start");
2607 0 : return true;
2608 : }
2609 :
2610 2 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start find a landing abort sequence");
2611 2 : return false;
2612 : }
2613 :
2614 : // check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan
2615 0 : bool AP_Mission::is_best_land_sequence(const Location ¤t_loc)
2616 : {
2617 : // check if there is even a running mission to interupt
2618 0 : if (_flags.state != MISSION_RUNNING) {
2619 0 : return false;
2620 : }
2621 :
2622 : // check if aircraft has already jumped to a landing sequence
2623 0 : if (_flags.in_landing_sequence) {
2624 0 : return true;
2625 : }
2626 :
2627 : // check if MIS_OPTIONS bit set to allow distance calculation to be done
2628 0 : if (!option_is_set(Option::FAILSAFE_TO_BEST_LANDING)) {
2629 0 : return false;
2630 : }
2631 :
2632 : // The decision to allow a failsafe to interupt a potential landing approach
2633 : // is a distance travelled minimization problem. Look forward in
2634 : // mission to evaluate the shortest remaining distance to land.
2635 :
2636 : // go through the mission for the nearest DO_LAND_START first as this is the most probable route
2637 : // to a landing with the minimum number of WP.
2638 0 : uint16_t do_land_start_index = get_landing_sequence_start(current_loc);
2639 0 : if (do_land_start_index == 0) {
2640 : // then no DO_LAND_START commands are in mission and normal failsafe behaviour should be maintained
2641 0 : return false;
2642 : }
2643 :
2644 : // get distance to landing if travelled to nearest DO_LAND_START via RTL
2645 : float dist_via_do_land;
2646 0 : if (!distance_to_landing(do_land_start_index, dist_via_do_land, current_loc)) {
2647 : // cant get a valid distance to landing
2648 0 : return false;
2649 : }
2650 :
2651 : // get distance to landing if continue along current mission path
2652 : float dist_continue_to_land;
2653 0 : if (!distance_to_landing(_nav_cmd.index, dist_continue_to_land, current_loc)) {
2654 : // cant get a valid distance to landing
2655 0 : return false;
2656 : }
2657 :
2658 : // compare distances
2659 0 : if (dist_via_do_land >= dist_continue_to_land) {
2660 : // then the mission should carry on uninterrupted as that is the shorter distance
2661 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Rejecting RTL: closer land if mis continued");
2662 0 : return true;
2663 : } else {
2664 : // allow failsafes to interrupt the current mission
2665 0 : return false;
2666 : }
2667 : }
2668 :
2669 : // Approximate the distance travelled to get to a landing. DO_JUMP commands are observed in look forward.
2670 0 : bool AP_Mission::distance_to_landing(uint16_t index, float &tot_distance, Location prev_loc)
2671 : {
2672 0 : Mission_Command temp_cmd;
2673 0 : tot_distance = 0.0f;
2674 0 : bool ret = false; // reached end of loop without getting to a landing
2675 :
2676 : // back up jump tracking to reset after distance calculation
2677 : jump_tracking_struct _jump_tracking_backup[AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS];
2678 0 : for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
2679 0 : _jump_tracking_backup[i] = _jump_tracking[i];
2680 : }
2681 :
2682 : // run through remainder of mission to approximate a distance to landing
2683 0 : for (uint8_t i=0; i<UINT8_MAX; i++) {
2684 : // search until the end of the mission command list
2685 0 : for (uint16_t cmd_index = index; cmd_index < (unsigned)_cmd_total; cmd_index++) {
2686 : // get next command
2687 0 : if (!get_next_cmd(cmd_index, temp_cmd, true, false)) {
2688 : // we got to the end of the mission
2689 0 : goto reset_do_jump_tracking;
2690 : }
2691 0 : if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT || is_landing_type_cmd(temp_cmd.id)) {
2692 0 : break;
2693 0 : } else if (is_nav_cmd(temp_cmd) || temp_cmd.id == MAV_CMD_CONDITION_DELAY) {
2694 : // if we receive a nav command that we dont handle then give up as cant measure the distance e.g. MAV_CMD_NAV_LOITER_UNLIM
2695 0 : goto reset_do_jump_tracking;
2696 : }
2697 : }
2698 0 : index = temp_cmd.index+1;
2699 :
2700 0 : if (!(temp_cmd.content.location.lat == 0 && temp_cmd.content.location.lng == 0)) {
2701 : // add distance to running total
2702 0 : float disttemp = prev_loc.get_distance(temp_cmd.content.location);
2703 0 : tot_distance = tot_distance + disttemp;
2704 :
2705 : // store wp location as previous
2706 0 : prev_loc = temp_cmd.content.location;
2707 : }
2708 :
2709 0 : if (is_landing_type_cmd(temp_cmd.id)) {
2710 : // reached a landing!
2711 0 : ret = true;
2712 0 : goto reset_do_jump_tracking;
2713 : }
2714 : }
2715 :
2716 0 : reset_do_jump_tracking:
2717 0 : for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
2718 0 : _jump_tracking[i] = _jump_tracking_backup[i];
2719 : }
2720 :
2721 0 : return ret;
2722 : }
2723 :
2724 : // Approximate the distance travelled to return to the mission path. DO_JUMP commands are observed in look forward.
2725 : // Stop searching once reaching a landing or do-land-start
2726 4 : bool AP_Mission::distance_to_mission_leg(uint16_t start_index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc)
2727 : {
2728 4 : Location prev_loc;
2729 4 : Mission_Command temp_cmd;
2730 4 : rejoin_distance = -1;
2731 4 : rejoin_index = -1;
2732 4 : bool ret = false;
2733 :
2734 : // back up jump tracking to reset after distance calculation
2735 : jump_tracking_struct _jump_tracking_backup[AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS];
2736 404 : for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
2737 400 : _jump_tracking_backup[i] = _jump_tracking[i];
2738 : }
2739 :
2740 : // run through remainder of mission to approximate a distance to landing
2741 4 : uint16_t index = start_index;
2742 24 : for (; search_remaining > 0; search_remaining--) {
2743 : // search until the end of the mission command list
2744 24 : for (uint16_t cmd_index = index; cmd_index <= (unsigned)_cmd_total; cmd_index++) {
2745 24 : if (get_next_cmd(cmd_index, temp_cmd, true, false)) {
2746 24 : break;
2747 : } else {
2748 : // got to the end of the mission
2749 0 : goto reset_do_jump_tracking;
2750 : }
2751 : }
2752 24 : index = temp_cmd.index + 1;
2753 :
2754 24 : if (stored_in_location(temp_cmd.id) && temp_cmd.content.location.initialised()) {
2755 16 : if (prev_loc.lat == 0 && prev_loc.lng == 0) {
2756 : // Need a valid previous location to do distance to leg calculation
2757 4 : prev_loc = temp_cmd.content.location;
2758 :
2759 : // single point dist calc
2760 4 : rejoin_distance = prev_loc.get_distance_NED_alt_frame(current_loc).length();
2761 4 : rejoin_index = temp_cmd.index;
2762 4 : ret = true;
2763 :
2764 : } else {
2765 : // Calculate the distance to rejoin
2766 12 : const Vector3f mission_vector = prev_loc.get_distance_NED_alt_frame(temp_cmd.content.location);
2767 12 : if (!mission_vector.is_zero()) {
2768 12 : Vector3f pos = prev_loc.get_distance_NED_alt_frame(current_loc);
2769 :
2770 : // project pos vector on to mission vector
2771 12 : Vector3f p = pos.projected(mission_vector);
2772 :
2773 : // constrain to mission line
2774 12 : p.x = constrain_float(p.x, MIN(0,mission_vector.x), MAX(0,mission_vector.x));
2775 12 : p.y = constrain_float(p.y, MIN(0,mission_vector.y), MAX(0,mission_vector.y));
2776 12 : p.z = constrain_float(p.z, MIN(0,mission_vector.z), MAX(0,mission_vector.z));
2777 :
2778 12 : const float disttemp = (p - pos).length();
2779 :
2780 : // store wp location as previous
2781 12 : prev_loc = temp_cmd.content.location;
2782 :
2783 12 : if (disttemp < rejoin_distance || is_negative(rejoin_distance)) {
2784 5 : rejoin_distance = disttemp;
2785 5 : rejoin_index = temp_cmd.index;
2786 : }
2787 12 : ret = true;
2788 : }
2789 : }
2790 : }
2791 :
2792 24 : if (is_landing_type_cmd(temp_cmd.id) || (temp_cmd.id == MAV_CMD_DO_LAND_START)) {
2793 : // reached a landing!
2794 4 : goto reset_do_jump_tracking;
2795 : }
2796 : }
2797 : // reached end of loop without getting to a landing or DO_LAND_START
2798 0 : ret = false;
2799 :
2800 4 : reset_do_jump_tracking:
2801 404 : for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
2802 400 : _jump_tracking[i] = _jump_tracking_backup[i];
2803 : }
2804 :
2805 4 : return ret;
2806 : }
2807 :
2808 : // check if command is a landing type command.
2809 2054 : bool AP_Mission::is_landing_type_cmd(uint16_t id) const
2810 : {
2811 2054 : switch (id) {
2812 157 : case MAV_CMD_NAV_LAND:
2813 : case MAV_CMD_NAV_VTOL_LAND:
2814 : case MAV_CMD_DO_PARACHUTE:
2815 157 : return true;
2816 1897 : default:
2817 1897 : return false;
2818 : }
2819 : }
2820 :
2821 : // check if command is a takeoff type command.
2822 307237 : bool AP_Mission::is_takeoff_type_cmd(uint16_t id) const
2823 : {
2824 307237 : switch (id) {
2825 5190 : case MAV_CMD_NAV_TAKEOFF:
2826 : case MAV_CMD_NAV_VTOL_TAKEOFF:
2827 5190 : return true;
2828 302047 : default:
2829 302047 : return false;
2830 : }
2831 : }
2832 :
2833 2030 : const char *AP_Mission::Mission_Command::type() const
2834 : {
2835 2030 : switch (id) {
2836 1232 : case MAV_CMD_NAV_WAYPOINT:
2837 1232 : return "WP";
2838 24 : case MAV_CMD_NAV_SPLINE_WAYPOINT:
2839 24 : return "SplineWP";
2840 37 : case MAV_CMD_NAV_ARC_WAYPOINT:
2841 37 : return "ArcWP";
2842 70 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
2843 70 : return "RTL";
2844 9 : case MAV_CMD_NAV_LOITER_UNLIM:
2845 9 : return "LoitUnlim";
2846 6 : case MAV_CMD_NAV_LOITER_TIME:
2847 6 : return "LoitTime";
2848 1 : case MAV_CMD_NAV_GUIDED_ENABLE:
2849 1 : return "GuidedEnable";
2850 15 : case MAV_CMD_NAV_LOITER_TURNS:
2851 15 : return "LoitTurns";
2852 13 : case MAV_CMD_NAV_LOITER_TO_ALT:
2853 13 : return "LoitAltitude";
2854 0 : case MAV_CMD_NAV_SET_YAW_SPEED:
2855 0 : return "SetYawSpd";
2856 0 : case MAV_CMD_CONDITION_DELAY:
2857 0 : return "CondDelay";
2858 0 : case MAV_CMD_CONDITION_DISTANCE:
2859 0 : return "CondDist";
2860 28 : case MAV_CMD_DO_CHANGE_SPEED:
2861 28 : return "ChangeSpeed";
2862 0 : case MAV_CMD_DO_SET_HOME:
2863 0 : return "SetHome";
2864 0 : case MAV_CMD_DO_SET_SERVO:
2865 0 : return "SetServo";
2866 0 : case MAV_CMD_DO_SET_RELAY:
2867 0 : return "SetRelay";
2868 0 : case MAV_CMD_DO_REPEAT_SERVO:
2869 0 : return "RepeatServo";
2870 0 : case MAV_CMD_DO_REPEAT_RELAY:
2871 0 : return "RepeatRelay";
2872 0 : case MAV_CMD_DO_DIGICAM_CONFIGURE:
2873 0 : return "DigiCamCfg";
2874 1 : case MAV_CMD_DO_DIGICAM_CONTROL:
2875 1 : return "DigiCamCtrl";
2876 12 : case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
2877 12 : return "SetCamTrigDst";
2878 0 : case MAV_CMD_DO_SET_ROI_LOCATION:
2879 0 : return "SetROILocation";
2880 0 : case MAV_CMD_DO_SET_ROI_NONE:
2881 0 : return "SetROINone";
2882 0 : case MAV_CMD_DO_SET_ROI:
2883 0 : return "SetROI";
2884 0 : case MAV_CMD_DO_SET_REVERSE:
2885 0 : return "SetReverse";
2886 0 : case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
2887 0 : return "SetRepeatDist";
2888 1 : case MAV_CMD_DO_GUIDED_LIMITS:
2889 1 : return "GuidedLimits";
2890 247 : case MAV_CMD_NAV_TAKEOFF:
2891 247 : return "Takeoff";
2892 125 : case MAV_CMD_NAV_LAND:
2893 125 : return "Land";
2894 0 : case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
2895 0 : return "ContinueAndChangeAlt";
2896 2 : case MAV_CMD_NAV_ALTITUDE_WAIT:
2897 2 : return "AltitudeWait";
2898 36 : case MAV_CMD_NAV_VTOL_TAKEOFF:
2899 36 : return "VTOLTakeoff";
2900 29 : case MAV_CMD_NAV_VTOL_LAND:
2901 29 : return "VTOLLand";
2902 2 : case MAV_CMD_DO_INVERTED_FLIGHT:
2903 2 : return "InvertedFlight";
2904 0 : case MAV_CMD_DO_FENCE_ENABLE:
2905 0 : return "FenceEnable";
2906 0 : case MAV_CMD_DO_AUTOTUNE_ENABLE:
2907 0 : return "AutoTuneEnable";
2908 2 : case MAV_CMD_DO_VTOL_TRANSITION:
2909 2 : return "VTOLTransition";
2910 21 : case MAV_CMD_DO_ENGINE_CONTROL:
2911 21 : return "EngineControl";
2912 4 : case MAV_CMD_CONDITION_YAW:
2913 4 : return "CondYaw";
2914 0 : case MAV_CMD_DO_RETURN_PATH_START:
2915 0 : return "ReturnPathStart";
2916 77 : case MAV_CMD_DO_LAND_START:
2917 77 : return "LandStart";
2918 5 : case MAV_CMD_NAV_DELAY:
2919 5 : return "Delay";
2920 : #if AP_GRIPPER_ENABLED
2921 12 : case MAV_CMD_DO_GRIPPER:
2922 12 : return "Gripper";
2923 : #endif
2924 : #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED
2925 2 : case MAV_CMD_NAV_PAYLOAD_PLACE:
2926 2 : return "PayloadPlace";
2927 : #endif
2928 3 : case MAV_CMD_DO_PARACHUTE:
2929 3 : return "Parachute";
2930 1 : case MAV_CMD_DO_SPRAYER:
2931 1 : return "Sprayer";
2932 1 : case MAV_CMD_DO_AUX_FUNCTION:
2933 1 : return "AuxFunction";
2934 0 : case MAV_CMD_DO_MOUNT_CONTROL:
2935 0 : return "MountControl";
2936 0 : case MAV_CMD_DO_WINCH:
2937 0 : return "Winch";
2938 0 : case MAV_CMD_DO_SEND_SCRIPT_MESSAGE:
2939 0 : return "Scripting";
2940 0 : case MAV_CMD_DO_JUMP:
2941 0 : return "Jump";
2942 0 : case MAV_CMD_DO_JUMP_TAG:
2943 0 : return "JumpToTag";
2944 1 : case MAV_CMD_JUMP_TAG:
2945 1 : return "Tag";
2946 0 : case MAV_CMD_DO_GO_AROUND:
2947 0 : return "Go Around";
2948 : #if AP_SCRIPTING_ENABLED
2949 0 : case MAV_CMD_NAV_SCRIPT_TIME:
2950 0 : return "NavScriptTime";
2951 : #endif
2952 0 : case MAV_CMD_NAV_ATTITUDE_TIME:
2953 0 : return "NavAttitudeTime";
2954 0 : case MAV_CMD_DO_PAUSE_CONTINUE:
2955 0 : return "PauseContinue";
2956 0 : case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
2957 0 : return "GimbalPitchYaw";
2958 3 : case MAV_CMD_IMAGE_START_CAPTURE:
2959 3 : return "ImageStartCapture";
2960 1 : case MAV_CMD_IMAGE_STOP_CAPTURE:
2961 1 : return "ImageStopCapture";
2962 2 : case MAV_CMD_SET_CAMERA_ZOOM:
2963 2 : return "SetCameraZoom";
2964 2 : case MAV_CMD_SET_CAMERA_FOCUS:
2965 2 : return "SetCameraFocus";
2966 0 : case MAV_CMD_SET_CAMERA_SOURCE:
2967 0 : return "SetCameraSource";
2968 0 : case MAV_CMD_VIDEO_START_CAPTURE:
2969 0 : return "VideoStartCapture";
2970 0 : case MAV_CMD_VIDEO_STOP_CAPTURE:
2971 0 : return "VideoStopCapture";
2972 : #if AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
2973 3 : case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
2974 3 : return "ROIWPNextOffset";
2975 : #endif // AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
2976 0 : default:
2977 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2978 0 : AP_HAL::panic("Mission command with ID %u has no string", id);
2979 : #endif
2980 0 : return "?";
2981 : }
2982 : }
2983 :
2984 : /*
2985 : get the command ID of a mission index. Caller should have checked the index is in range
2986 : */
2987 883564 : uint16_t AP_Mission::get_command_id(uint16_t index) const
2988 : {
2989 883564 : const uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
2990 883564 : uint8_t b[3] {};
2991 883564 : if (!_storage.read_block(b, pos_in_storage, sizeof(b))) {
2992 0 : return 0U;
2993 : }
2994 883564 : uint16_t id = 0;
2995 883564 : if (b[0] == 0 || b[0] == 1) {
2996 750 : memcpy((void*)&id, (void*)&b[1], 2);
2997 : } else {
2998 882814 : id = b[0];
2999 : }
3000 883564 : return id;
3001 : }
3002 :
3003 : /*
3004 : see if the mission contains a particular item
3005 : */
3006 50272 : bool AP_Mission::contains_item(MAV_CMD command) const
3007 : {
3008 50272 : const auto count = num_commands();
3009 613284 : for (uint16_t i = 1; i < count; i++) {
3010 563748 : if (get_command_id(i) != uint16_t(command)) {
3011 563012 : continue;
3012 : }
3013 : // confirm with full read
3014 736 : Mission_Command tmp;
3015 736 : if (!read_cmd_from_storage(i, tmp)) {
3016 0 : continue;
3017 : }
3018 736 : if (tmp.id == command) {
3019 736 : return true;
3020 : }
3021 : }
3022 49536 : return false;
3023 : }
3024 :
3025 : /*
3026 : return true if the mission item has a location
3027 : */
3028 :
3029 9 : bool AP_Mission::cmd_has_location(const uint16_t command)
3030 : {
3031 9 : return stored_in_location(command);
3032 : }
3033 :
3034 : /*
3035 : return true if the mission has a terrain relative item. ~2200us for 530 items on H7
3036 : */
3037 130880 : bool AP_Mission::contains_terrain_alt_items(void)
3038 : {
3039 130880 : if (_last_contains_relative_calculated_ms != _last_change_time_ms) {
3040 2796 : _contains_terrain_alt_items = calculate_contains_terrain_alt_items();
3041 2796 : _last_contains_relative_calculated_ms = _last_change_time_ms;
3042 : }
3043 130880 : return _contains_terrain_alt_items;
3044 : }
3045 :
3046 2796 : bool AP_Mission::calculate_contains_terrain_alt_items(void) const
3047 : {
3048 2796 : const auto count = num_commands();
3049 16873 : for (uint16_t i = 1; i < count; i++) {
3050 14123 : if (!stored_in_location(get_command_id(i))) {
3051 430 : continue;
3052 : }
3053 13693 : Mission_Command tmp;
3054 13693 : if (!read_cmd_from_storage(i, tmp)) {
3055 0 : continue;
3056 : }
3057 13693 : if (stored_in_location(tmp.id) && tmp.content.location.terrain_alt) {
3058 46 : return true;
3059 : }
3060 : }
3061 2750 : return false;
3062 : }
3063 :
3064 : // reset the mission history to prevent recalling previous mission histories after a mission restart.
3065 3649 : void AP_Mission::reset_wp_history(void)
3066 : {
3067 29192 : for (uint8_t i = 0; i<AP_MISSION_MAX_WP_HISTORY; i++) {
3068 25543 : _wp_index_history[i] = AP_MISSION_CMD_INDEX_NONE;
3069 : }
3070 3649 : _resume_cmd.index = AP_MISSION_CMD_INDEX_NONE;
3071 3649 : _flags.resuming_mission = false;
3072 3649 : _repeat_dist = 0;
3073 3649 : }
3074 :
3075 : // store the latest reported position incase of mission exit and rewind resume
3076 4247240 : void AP_Mission::update_exit_position(void)
3077 : {
3078 4247240 : if (!AP::ahrs().get_location(_exit_position)) {
3079 0 : _exit_position.lat = 0;
3080 0 : _exit_position.lng = 0;
3081 : }
3082 4247240 : }
3083 :
3084 : // calculate the location of the _resume_cmd wp and set as current
3085 0 : bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd)
3086 : {
3087 : // check for a recent history
3088 0 : if (_wp_index_history[LAST_WP_PASSED] == AP_MISSION_CMD_INDEX_NONE) {
3089 : // no saved history so can't rewind
3090 0 : return false;
3091 : }
3092 :
3093 : // check that we have a valid exit position
3094 0 : if (_exit_position.lat == 0 && _exit_position.lng == 0) {
3095 0 : return false;
3096 : }
3097 :
3098 0 : Mission_Command temp_cmd;
3099 0 : float rewind_distance = _repeat_dist; //(m)
3100 : uint16_t resume_index;
3101 0 : Location prev_loc = _exit_position;
3102 :
3103 0 : for (int8_t i = (LAST_WP_PASSED); i>=0; i--) {
3104 :
3105 : // to get this far there has to be at least one 'passed wp' stored in history. This is to check incase
3106 : // of history array no being completely filled with valid waypoints upon resume.
3107 0 : if (_wp_index_history[i] == AP_MISSION_CMD_INDEX_NONE) {
3108 : // no more stored history
3109 0 : resume_index = i+1;
3110 0 : break;
3111 : }
3112 :
3113 0 : if (!read_cmd_from_storage(_wp_index_history[i], temp_cmd)) {
3114 : // if read from storage failed then don't use rewind
3115 0 : return false;
3116 : }
3117 :
3118 : // calculate distance
3119 0 : float disttemp = prev_loc.get_distance(temp_cmd.content.location); //(m)
3120 0 : rewind_distance -= disttemp;
3121 0 : resume_index = i;
3122 :
3123 0 : if (rewind_distance <= 0.0f) {
3124 : // history rewound enough distance to meet _repeat_dist requirement
3125 0 : rewind_cmd = temp_cmd;
3126 0 : break;
3127 : }
3128 :
3129 : // store wp location as previous
3130 0 : prev_loc = temp_cmd.content.location;
3131 : }
3132 :
3133 0 : if (rewind_distance > 0.0f) {
3134 : // then the history array was rewound all of the way without finding a wp distance > _repeat_dist distance.
3135 : // the last read temp_cmd will be the furthest cmd back in the history array so resume to that.
3136 0 : rewind_cmd = temp_cmd;
3137 0 : return true;
3138 : }
3139 :
3140 : // if we have got this far the desired rewind distance lies between two waypoints stored in history array.
3141 : // calculate the location for the mission to resume
3142 :
3143 : // the last wp read from storage is the wp that is before the resume wp in the mission order
3144 0 : Location passed_wp_loc = temp_cmd.content.location;
3145 :
3146 : // fetch next destination wp
3147 0 : if (!read_cmd_from_storage(_wp_index_history[resume_index+1], temp_cmd)) {
3148 : // if read from storage failed then don't use rewind
3149 0 : return false;
3150 : }
3151 :
3152 : // determine the length of the mission leg that the resume wp lies in
3153 0 : float leg_length = passed_wp_loc.get_distance(temp_cmd.content.location); //(m)
3154 :
3155 : // calculate the percentage along the leg that resume wp will be positioned
3156 0 : float leg_percent = fabsf(rewind_distance)/leg_length;
3157 :
3158 : // calculate difference vector of mission leg
3159 0 : Vector3f dist_vec = passed_wp_loc.get_distance_NED(temp_cmd.content.location);
3160 :
3161 : // calculate the resume wp position
3162 0 : rewind_cmd.content.location.offset(dist_vec.x * leg_percent, dist_vec.y * leg_percent);
3163 0 : rewind_cmd.content.location.offset_up_m(-dist_vec.z * leg_percent);
3164 :
3165 : // The rewind_cmd.index has the index of the 'last passed wp' from the history array. This ensures that the mission order
3166 : // continues as planned without further intervention. The resume wp is not written to memory so will not perminantely change the mission.
3167 :
3168 : // if we got this far then mission rewind position was successfully calculated
3169 0 : return true;
3170 : }
3171 :
3172 : /*
3173 : handle format conversion of storage format to allow us to update
3174 : format to take advantage of new packing. This is particularly useful
3175 : for conversion to Float16 to get extra parameter space
3176 : */
3177 259 : void AP_Mission::format_conversion(uint8_t tag_byte, const Mission_Command &cmd, PackedContent &packed_content) const
3178 : {
3179 : // currently only one conversion needed, more can be added
3180 : #if AP_SCRIPTING_ENABLED
3181 259 : if (tag_byte == 0 && cmd.id == MAV_CMD_NAV_SCRIPT_TIME) {
3182 : // PARAMETER_CONVERSION - Added: Oct-2022 for ArduPilot-4.4
3183 : struct nav_script_time_Command_tag0 old_fmt;
3184 : struct nav_script_time_Command new_fmt;
3185 0 : memcpy((void*)&old_fmt, packed_content.bytes, sizeof(old_fmt));
3186 0 : new_fmt.command = old_fmt.command;
3187 0 : new_fmt.timeout_s = old_fmt.timeout_s;
3188 0 : new_fmt.arg1.set(old_fmt.arg1);
3189 0 : new_fmt.arg2.set(old_fmt.arg2);
3190 0 : new_fmt.arg3 = 0;
3191 0 : new_fmt.arg4 = 0;
3192 0 : memcpy(packed_content.bytes, (void*)&new_fmt, sizeof(new_fmt));
3193 : }
3194 : #endif
3195 259 : }
3196 :
3197 : // Helpers to fill in location for scripting
3198 : #if AP_SCRIPTING_ENABLED
3199 0 : bool AP_Mission::jump_to_landing_sequence(void)
3200 : {
3201 0 : Location loc;
3202 0 : if (AP::ahrs().get_location(loc)) {
3203 0 : return jump_to_landing_sequence(loc);
3204 : }
3205 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start landing sequence");
3206 0 : return false;
3207 : }
3208 :
3209 0 : bool AP_Mission::jump_to_abort_landing_sequence(void)
3210 : {
3211 0 : Location loc;
3212 0 : if (AP::ahrs().get_location(loc)) {
3213 0 : return jump_to_abort_landing_sequence(loc);
3214 : }
3215 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start find a landing abort sequence");
3216 0 : return false;
3217 : }
3218 : #endif // AP_SCRIPTING_ENABLED
3219 :
3220 :
3221 : // singleton instance
3222 : AP_Mission *AP_Mission::_singleton;
3223 :
3224 : namespace AP
3225 : {
3226 :
3227 6405083 : AP_Mission &mission()
3228 : {
3229 6405083 : return *(AP_Mission::get_singleton());
3230 : }
3231 :
3232 : }
3233 :
3234 : #endif // AP_MISSION_ENABLED
|
