Line data Source code
1 : /// @file AP_Mission.h
2 : /// @brief Handles the MAVLINK command mission stack. Reads and writes mission to storage.
3 :
4 : /*
5 : * The AP_Mission library:
6 : * - responsible for managing a list of commands made up of "nav", "do" and "conditional" commands
7 : * - reads and writes the mission commands to storage.
8 : * - provides easy access to current, previous and upcoming waypoints
9 : * - calls main program's command execution and verify functions.
10 : * - accounts for the DO_JUMP command
11 : *
12 : */
13 : #pragma once
14 :
15 : #include "AP_Mission_config.h"
16 :
17 : #include <GCS_MAVLink/GCS_MAVLink.h>
18 : #include <AP_Math/AP_Math.h>
19 : #include <AP_Common/AP_Common.h>
20 : #include <AP_Common/Location.h>
21 : #include <AP_Param/AP_Param.h>
22 : #include <StorageManager/StorageManager.h>
23 : #include <AP_Common/float16.h>
24 :
25 : // definitions
26 : #define AP_MISSION_EEPROM_VERSION 0x65AE // version number stored in first four bytes of eeprom. increment this by one when eeprom format is changed
27 : #define AP_MISSION_EEPROM_COMMAND_SIZE 15 // size in bytes of all mission commands
28 :
29 : #ifndef AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS
30 : #if HAL_MEM_CLASS >= HAL_MEM_CLASS_500
31 : #define AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS 100 // allow up to 100 do-jump commands
32 : #else
33 : #define AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS 15 // allow up to 15 do-jump commands
34 : #endif
35 : #endif
36 :
37 : #define AP_MISSION_JUMP_REPEAT_FOREVER -1 // when do-jump command's repeat count is -1 this means endless repeat
38 :
39 : #define AP_MISSION_CMD_ID_NONE 0 // mavlink cmd id of zero means invalid or missing command
40 : #define AP_MISSION_CMD_INDEX_NONE 65535 // command index of 65535 means invalid or missing command
41 : #define AP_MISSION_JUMP_TIMES_MAX 32767 // maximum number of times a jump can be executed. Used when jump tracking fails (i.e. when too many jumps in mission)
42 :
43 : #define AP_MISSION_FIRST_REAL_COMMAND 1 // command #0 reserved to hold home position
44 :
45 : #define AP_MISSION_RESTART_DEFAULT 0 // resume the mission from the last command run by default
46 :
47 : #define AP_MISSION_OPTIONS_DEFAULT 0 // Do not clear the mission when rebooting
48 :
49 : #define AP_MISSION_MAX_WP_HISTORY 7 // The maximum number of previous wp commands that will be stored from the active missions history
50 : #define LAST_WP_PASSED (AP_MISSION_MAX_WP_HISTORY-2)
51 :
52 : #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
53 : #define AP_MISSION_SDCARD_FILENAME "APM/mission.stg"
54 : #else
55 : #define AP_MISSION_SDCARD_FILENAME "mission.stg"
56 : #endif
57 :
58 : union PackedContent;
59 :
60 : /// @class AP_Mission
61 : /// @brief Object managing Mission
62 : class AP_Mission
63 : {
64 :
65 : public:
66 : // jump command structure
67 : struct PACKED Jump_Command {
68 : uint16_t target; // target command id
69 : int16_t num_times; // num times to repeat. -1 = repeat forever
70 : };
71 :
72 : // condition delay command structure
73 : struct PACKED Conditional_Delay_Command {
74 : float seconds; // period of delay in seconds
75 : };
76 :
77 : // condition delay command structure
78 : struct PACKED Conditional_Distance_Command {
79 : float meters; // distance from next waypoint in meters
80 : };
81 :
82 : // condition yaw command structure
83 : struct PACKED Yaw_Command {
84 : float angle_deg; // target angle in degrees (0=north, 90=east)
85 : float turn_rate_dps; // turn rate in degrees / second (0=use default)
86 : int8_t direction; // -1 = ccw, +1 = cw
87 : uint8_t relative_angle; // 0 = absolute angle, 1 = relative angle
88 : };
89 :
90 : // change speed command structure
91 : struct PACKED Change_Speed_Command {
92 : uint8_t speed_type; // 0=airspeed, 1=ground speed
93 : float target_ms; // target speed in m/s, -1 means no change
94 : float throttle_pct; // throttle as a percentage (i.e. 1 ~ 100), 0 means no change
95 : };
96 :
97 : // set relay command structure
98 : struct PACKED Set_Relay_Command {
99 : uint8_t num; // relay number from 1 to 4
100 : uint8_t state; // on = 3.3V or 5V (depending upon board), off = 0V. only used for do-set-relay, not for do-repeat-relay
101 : };
102 :
103 : // repeat relay command structure
104 : struct PACKED Repeat_Relay_Command {
105 : uint8_t num; // relay number from 1 to 4
106 : int16_t repeat_count; // number of times to trigger the relay
107 : float cycle_time; // cycle time in seconds (the time between peaks or the time the relay is on and off for each cycle?)
108 : };
109 :
110 : // set servo command structure
111 : struct PACKED Set_Servo_Command {
112 : uint8_t channel; // servo channel
113 : uint16_t pwm; // pwm value for servo
114 : };
115 :
116 : // repeat servo command structure
117 : struct PACKED Repeat_Servo_Command {
118 : uint8_t channel; // servo channel
119 : uint16_t pwm; // pwm value for servo
120 : int16_t repeat_count; // number of times to move the servo (returns to trim in between)
121 : float cycle_time; // cycle time in seconds (the time between peaks or the time the servo is at the specified pwm value for each cycle?)
122 : };
123 :
124 : // mount control command structure
125 : struct PACKED Mount_Control {
126 : float pitch; // pitch angle in degrees
127 : float roll; // roll angle in degrees
128 : float yaw; // yaw angle (relative to vehicle heading) in degrees
129 : };
130 :
131 : // digicam control command structure
132 : struct PACKED Digicam_Configure {
133 : uint8_t shooting_mode; // ProgramAuto = 1, AV = 2, TV = 3, Man=4, IntelligentAuto=5, SuperiorAuto=6
134 : uint16_t shutter_speed;
135 : uint8_t aperture; // F stop number * 10
136 : uint16_t ISO; // 80, 100, 200, etc
137 : uint8_t exposure_type;
138 : uint8_t cmd_id;
139 : float engine_cutoff_time; // seconds
140 : };
141 :
142 : // digicam control command structure
143 : struct PACKED Digicam_Control {
144 : uint8_t session; // 1 = on, 0 = off
145 : uint8_t zoom_pos;
146 : int8_t zoom_step; // +1 = zoom in, -1 = zoom out
147 : uint8_t focus_lock;
148 : uint8_t shooting_cmd;
149 : uint8_t cmd_id;
150 : };
151 :
152 : // set cam trigger distance command structure
153 : struct PACKED Cam_Trigg_Distance {
154 : float meters; // distance
155 : uint8_t trigger; // triggers one image capture immediately
156 : uint8_t camera_id; // which camera to trigger
157 : };
158 :
159 : // gripper command structure
160 : struct PACKED Gripper_Command {
161 : uint8_t num; // gripper number
162 : uint8_t action; // action (0 = release, 1 = grab)
163 : };
164 :
165 : // AUX_FUNCTION command structure
166 : struct PACKED AuxFunction {
167 : uint16_t function; // from RC_Channel::AUX_FUNC
168 : uint8_t switchpos; // from RC_Channel::AuxSwitchPos
169 : };
170 :
171 : // high altitude balloon altitude wait
172 : struct PACKED Altitude_Wait {
173 : float altitude; // meters
174 : float descent_rate; // m/s
175 : uint8_t wiggle_time; // seconds
176 : };
177 :
178 : // nav guided command
179 : struct PACKED Guided_Limits_Command {
180 : // max time is held in p1 field
181 : float alt_min; // min alt below which the command will be aborted. 0 for no lower alt limit
182 : float alt_max; // max alt above which the command will be aborted. 0 for no upper alt limit
183 : float horiz_max; // max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit
184 : };
185 :
186 : // do VTOL transition
187 : struct PACKED Do_VTOL_Transition {
188 : uint8_t target_state;
189 : };
190 :
191 : // navigation delay command structure
192 : struct PACKED Navigation_Delay_Command {
193 : float seconds; // period of delay in seconds
194 : int8_t hour_utc; // absolute time's hour (utc)
195 : int8_t min_utc; // absolute time's min (utc)
196 : int8_t sec_utc; // absolute time's sec (utc)
197 : };
198 :
199 : // DO_ENGINE_CONTROL support
200 : struct PACKED Do_Engine_Control {
201 : bool start_control; // start or stop engine
202 : bool cold_start; // use cold start procedure
203 : uint16_t height_delay_cm; // height delay for start
204 : bool allow_disarmed_start; // allow starting the engine while disarmed
205 : };
206 :
207 : // NAV_SET_YAW_SPEED support
208 : struct PACKED Set_Yaw_Speed {
209 : float angle_deg; // target angle in degrees (0=north, 90=east)
210 : float speed; // speed in meters/second
211 : uint8_t relative_angle; // 0 = absolute angle, 1 = relative angle
212 : };
213 :
214 : // winch command structure
215 : struct PACKED Winch_Command {
216 : uint8_t num; // winch number
217 : uint8_t action; // action (0 = relax, 1 = length control, 2 = rate control)
218 : float release_length; // cable distance to unwind in meters, negative numbers to wind in cable
219 : float release_rate; // release rate in meters/second
220 : };
221 :
222 : // Scripting command structure
223 : struct PACKED scripting_Command {
224 : float p1;
225 : float p2;
226 : float p3;
227 : };
228 :
229 : #if AP_SCRIPTING_ENABLED
230 : // Scripting NAV command old version of storage format
231 : struct PACKED nav_script_time_Command_tag0 {
232 : uint8_t command;
233 : uint8_t timeout_s;
234 : float arg1;
235 : float arg2;
236 : };
237 :
238 : // Scripting NAV command, new version of storage format
239 : struct PACKED nav_script_time_Command {
240 : uint8_t command;
241 : uint8_t timeout_s;
242 : Float16_t arg1;
243 : Float16_t arg2;
244 : // last 2 arguments need to be integers due to MISSION_ITEM_INT encoding
245 : int16_t arg3;
246 : int16_t arg4;
247 : };
248 : #endif
249 :
250 : // Scripting NAV command (with verify)
251 : struct PACKED nav_attitude_time_Command {
252 : uint16_t time_sec;
253 : int16_t roll_deg;
254 : int8_t pitch_deg;
255 : int16_t yaw_deg;
256 : int16_t climb_rate;
257 : };
258 :
259 : // MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW support
260 : struct PACKED gimbal_manager_pitchyaw_Command {
261 : int8_t pitch_angle_deg;
262 : int16_t yaw_angle_deg;
263 : int8_t pitch_rate_degs;
264 : int8_t yaw_rate_degs;
265 : uint8_t flags;
266 : uint8_t gimbal_id;
267 : };
268 :
269 : // MAV_CMD_IMAGE_START_CAPTURE support
270 : struct PACKED image_start_capture_Command {
271 : uint8_t instance;
272 : float interval_s;
273 : uint16_t total_num_images;
274 : uint16_t start_seq_number;
275 : };
276 :
277 : // MAV_CMD_SET_CAMERA_ZOOM support
278 : struct PACKED set_camera_zoom_Command {
279 : uint8_t zoom_type;
280 : float zoom_value;
281 : uint8_t camera_id;
282 : };
283 :
284 : // MAV_CMD_SET_CAMERA_FOCUS support
285 : struct PACKED set_camera_focus_Command {
286 : uint8_t focus_type;
287 : float focus_value;
288 : uint8_t camera_id;
289 : };
290 :
291 : // MAV_CMD_SET_CAMERA_SOURCE support
292 : struct PACKED set_camera_source_Command {
293 : uint8_t instance;
294 : uint8_t primary_source;
295 : uint8_t secondary_source;
296 : };
297 :
298 : // MAV_CMD_VIDEO_START_CAPTURE support
299 : struct PACKED video_start_capture_Command {
300 : uint8_t video_stream_id;
301 : };
302 :
303 : // MAV_CMD_VIDEO_STOP_CAPTURE support
304 : struct PACKED video_stop_capture_Command {
305 : uint8_t video_stream_id;
306 : };
307 :
308 : #if AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
309 : // MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET support
310 : struct PACKED ROI_WPNext_Offset_Command {
311 : int16_t roll_offset_cd;
312 : int16_t pitch_offset_cd;
313 : int16_t yaw_offset_cd;
314 : uint8_t gimbal_id;
315 : };
316 : #endif // AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
317 :
318 : union Content {
319 : // jump structure
320 : Jump_Command jump;
321 :
322 : // conditional delay
323 : Conditional_Delay_Command delay;
324 :
325 : // conditional distance
326 : Conditional_Distance_Command distance;
327 :
328 : // conditional yaw
329 : Yaw_Command yaw;
330 :
331 : // change speed
332 : Change_Speed_Command speed;
333 :
334 : // do-set-relay
335 : Set_Relay_Command relay;
336 :
337 : // do-repeat-relay
338 : Repeat_Relay_Command repeat_relay;
339 :
340 : // do-set-servo
341 : Set_Servo_Command servo;
342 :
343 : // do-repeate-servo
344 : Repeat_Servo_Command repeat_servo;
345 :
346 : // mount control
347 : Mount_Control mount_control;
348 :
349 : // camera configure
350 : Digicam_Configure digicam_configure;
351 :
352 : // camera control
353 : Digicam_Control digicam_control;
354 :
355 : // cam trigg distance
356 : Cam_Trigg_Distance cam_trigg_dist;
357 :
358 : // do-gripper
359 : Gripper_Command gripper;
360 :
361 : // arbitrary aux function
362 : AuxFunction auxfunction;
363 :
364 : // do-guided-limits
365 : Guided_Limits_Command guided_limits;
366 :
367 : // high altitude balloon altitude wait
368 : Altitude_Wait altitude_wait;
369 :
370 : // do vtol transition
371 : Do_VTOL_Transition do_vtol_transition;
372 :
373 : // DO_ENGINE_CONTROL
374 : Do_Engine_Control do_engine_control;
375 :
376 : // navigation delay
377 : Navigation_Delay_Command nav_delay;
378 :
379 : // NAV_SET_YAW_SPEED support
380 : Set_Yaw_Speed set_yaw_speed;
381 :
382 : // do-winch
383 : Winch_Command winch;
384 :
385 : // do scripting
386 : scripting_Command scripting;
387 :
388 : #if AP_SCRIPTING_ENABLED
389 : // nav scripting
390 : nav_script_time_Command nav_script_time;
391 : #endif
392 :
393 : // nav attitude time
394 : nav_attitude_time_Command nav_attitude_time;
395 :
396 : // MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW
397 : gimbal_manager_pitchyaw_Command gimbal_manager_pitchyaw;
398 :
399 : // MAV_CMD_IMAGE_START_CAPTURE support
400 : image_start_capture_Command image_start_capture;
401 :
402 : // MAV_CMD_SET_CAMERA_ZOOM support
403 : set_camera_zoom_Command set_camera_zoom;
404 :
405 : // MAV_CMD_SET_CAMERA_FOCUS support
406 : set_camera_focus_Command set_camera_focus;
407 :
408 : // MAV_CMD_SET_CAMEARA_SOURCE support
409 : set_camera_source_Command set_camera_source;
410 :
411 : // MAV_CMD_VIDEO_START_CAPTURE support
412 : video_start_capture_Command video_start_capture;
413 :
414 : // MAV_CMD_VIDEO_STOP_CAPTURE support
415 : video_stop_capture_Command video_stop_capture;
416 :
417 : #if AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
418 : // MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET
419 : ROI_WPNext_Offset_Command wpnext_offset;
420 : #endif // AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
421 :
422 : // location
423 : Location location{}; // Waypoint location
424 : };
425 :
426 : // command structure
427 : struct Mission_Command {
428 : uint16_t index; // this commands position in the command list
429 : uint16_t id; // mavlink command id
430 : uint16_t p1; // general purpose parameter 1
431 : Content content;
432 :
433 : // for items which store in location, we offer a few more bits
434 : // of storage:
435 : uint8_t type_specific_bits; // bitmask of set/unset bits
436 :
437 : // return a human-readable interpretation of the ID stored in this command
438 : const char *type() const;
439 :
440 : // comparison operator (relies on all bytes in the structure even if they may not be used)
441 1234 : bool operator ==(const Mission_Command &b) const { return (memcmp(this, &b, sizeof(Mission_Command)) == 0); }
442 1234 : bool operator !=(const Mission_Command &b) const { return !operator==(b); }
443 :
444 : /*
445 : return the number of turns for a LOITER_TURNS command
446 : this has special handling for loiter turns from cmd.p1 and type_specific_bits
447 : */
448 129092 : float get_loiter_turns(void) const {
449 129092 : float turns = LOWBYTE(p1);
450 129092 : if (type_specific_bits & (1U<<1)) {
451 : // special storage handling allows for fractional turns
452 1 : turns *= (1.0/256.0);
453 : }
454 129092 : return turns;
455 : }
456 :
457 : /*
458 : return the arc angle in radians for an ARC_WAYPOINT command
459 : this has special handling for arc waypoints using cmd.p1 and loiter_ccw
460 : */
461 31 : float get_arc_angle_rad(void) const {
462 31 : const float sign = (content.location.loiter_ccw == 0) ? 1.0f : -1.0f;
463 31 : return radians(float(p1) * sign);
464 : }
465 : };
466 :
467 :
468 : // main program function pointers
469 : FUNCTOR_TYPEDEF(mission_cmd_fn_t, bool, const Mission_Command&);
470 : FUNCTOR_TYPEDEF(mission_complete_fn_t, void);
471 :
472 : // constructor
473 2096 : AP_Mission(mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn) :
474 2096 : _cmd_start_fn(cmd_start_fn),
475 2096 : _cmd_verify_fn(cmd_verify_fn),
476 2096 : _mission_complete_fn(mission_complete_fn),
477 2096 : _prev_nav_cmd_id(AP_MISSION_CMD_ID_NONE),
478 2096 : _prev_nav_cmd_index(AP_MISSION_CMD_INDEX_NONE),
479 2096 : _prev_nav_cmd_wp_index(AP_MISSION_CMD_INDEX_NONE)
480 : {
481 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
482 2095 : if (_singleton != nullptr) {
483 0 : AP_HAL::panic("Mission must be singleton");
484 : }
485 : #endif
486 2096 : _singleton = this;
487 :
488 : // load parameter defaults
489 2096 : AP_Param::setup_object_defaults(this, var_info);
490 :
491 : // clear commands
492 2096 : _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
493 2096 : _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
494 2096 : }
495 :
496 : // get singleton instance
497 6419017 : static AP_Mission *get_singleton()
498 : {
499 6419017 : return _singleton;
500 : }
501 :
502 : /* Do not allow copies */
503 : CLASS_NO_COPY(AP_Mission);
504 :
505 : // mission state enumeration
506 : enum mission_state {
507 : MISSION_STOPPED=0,
508 : MISSION_RUNNING=1,
509 : MISSION_COMPLETE=2
510 : };
511 :
512 : ///
513 : /// public mission methods
514 : ///
515 :
516 : /// init - initialises this library including checks the version in eeprom matches this library
517 : void init();
518 :
519 : /// status - returns the status of the mission (i.e. Mission_Started, Mission_Complete, Mission_Stopped
520 7278670 : mission_state state() const
521 : {
522 7278670 : return _flags.state;
523 : }
524 :
525 : /// num_commands - returns total number of commands in the mission
526 : /// this number includes offset 0, the home location
527 1366099 : uint16_t num_commands() const
528 : {
529 1366099 : return _cmd_total;
530 : }
531 :
532 : /// num_commands_max - returns maximum number of commands that can be stored
533 16597 : uint16_t num_commands_max() const {
534 16597 : return _commands_max;
535 : }
536 :
537 : // Present - returns true if there is a mission currently loaded, ignoring home which is stored in index 0
538 1132259 : bool present() const { return _cmd_total > 1; }
539 :
540 : /// start - resets current commands to point to the beginning of the mission
541 : /// To-Do: should we validate the mission first and return true/false?
542 : void start();
543 :
544 : /// stop - stops mission execution. subsequent calls to update() will have no effect until the mission is started or resumed
545 : void stop();
546 :
547 : /// resume - continues the mission execution from where we last left off
548 : /// previous running commands will be re-initialised
549 : void resume();
550 :
551 : /// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()
552 : void start_or_resume();
553 :
554 : /// check mission starts with a takeoff command
555 : bool starts_with_takeoff_cmd();
556 :
557 : /// reset - reset mission to the first command
558 : void reset();
559 :
560 : /// reset_jump_counters - reset DO_JUMP counters to their initial values without affecting current position
561 3 : void reset_jump_counters() { init_jump_tracking(); }
562 :
563 : /// clear - clears out mission
564 : bool clear();
565 :
566 : /// truncate - truncate any mission items beyond given index
567 : void truncate(uint16_t index);
568 :
569 : /// 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
570 : /// should be called at 10hz or higher
571 : void update();
572 :
573 : ///
574 : /// public command methods
575 : ///
576 :
577 : /// add_cmd - adds a command to the end of the command list and writes to storage
578 : /// returns true if successfully added, false on failure
579 : /// cmd.index is updated with it's new position in the mission
580 : bool add_cmd(Mission_Command& cmd);
581 :
582 : /// replace_cmd - replaces the command at position 'index' in the command list with the provided cmd
583 : /// replacing the current active command will have no effect until the command is restarted
584 : /// returns true if successfully replaced, false on failure
585 : bool replace_cmd(uint16_t index, const Mission_Command& cmd);
586 :
587 : /// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
588 : static bool is_nav_cmd(const Mission_Command& cmd);
589 :
590 : // check if command is a takeoff type command.
591 : bool is_takeoff_type_cmd(uint16_t id) const;
592 :
593 : /// get_current_nav_cmd - returns the current "navigation" command
594 249099966 : const Mission_Command& get_current_nav_cmd() const
595 : {
596 249099966 : return _nav_cmd;
597 : }
598 :
599 : /// get_current_nav_index - returns the current "navigation" command index
600 : /// Note that this will return 0 if there is no command. This is
601 : /// used in MAVLink reporting of the mission command
602 4827536 : uint16_t get_current_nav_index() const
603 : {
604 4827536 : return _nav_cmd.index==AP_MISSION_CMD_INDEX_NONE?0:_nav_cmd.index;
605 : }
606 :
607 : /// get_current_nav_id - return the id of the current nav command
608 37676 : uint16_t get_current_nav_id() const
609 : {
610 37676 : return _nav_cmd.id;
611 : }
612 :
613 : /// get_prev_nav_cmd_id - returns the previous "navigation" command id
614 : /// if there was no previous nav command it returns AP_MISSION_CMD_ID_NONE
615 : /// we do not return the entire command to save on RAM
616 5862 : uint16_t get_prev_nav_cmd_id() const
617 : {
618 5862 : return _prev_nav_cmd_id;
619 : }
620 :
621 : /// get_prev_nav_cmd_index - returns the previous "navigation" commands index (i.e. position in the mission command list)
622 : /// if there was no previous nav command it returns AP_MISSION_CMD_INDEX_NONE
623 : /// we do not return the entire command to save on RAM
624 1237 : uint16_t get_prev_nav_cmd_index() const
625 : {
626 1237 : return _prev_nav_cmd_index;
627 : }
628 :
629 : /// get_prev_nav_cmd_with_wp_index - returns the previous "navigation" commands index that contains a waypoint (i.e. position in the mission command list)
630 : /// if there was no previous nav command it returns AP_MISSION_CMD_INDEX_NONE
631 : /// we do not return the entire command to save on RAM
632 13676 : uint16_t get_prev_nav_cmd_with_wp_index() const
633 : {
634 13676 : return _prev_nav_cmd_wp_index;
635 : }
636 :
637 : /// get_next_nav_cmd - gets next "navigation" command found at or after start_index
638 : /// returns true if found, false if not found (i.e. reached end of mission command list)
639 : /// accounts for do_jump commands
640 : bool get_next_nav_cmd(uint16_t start_index, Mission_Command& cmd);
641 :
642 : /// get the ground course of the next navigation leg in centidegrees
643 : /// from 0 36000. Return default_angle if next navigation
644 : /// leg cannot be determined
645 : int32_t get_next_ground_course_cd(int32_t default_angle);
646 :
647 : /// get_current_do_cmd - returns active "do" command
648 : const Mission_Command& get_current_do_cmd() const
649 : {
650 : return _do_cmd;
651 : }
652 :
653 : /// get_current_do_cmd_id - returns id of the active "do" command
654 0 : uint16_t get_current_do_cmd_id() const
655 : {
656 0 : return _do_cmd.id;
657 : }
658 :
659 : // set_current_cmd - jumps to command specified by index
660 : bool set_current_cmd(uint16_t index);
661 :
662 : // restart current navigation command. Used to handle external changes to mission
663 : // returns true on success, false if current nav command has been deleted
664 : bool restart_current_nav_cmd();
665 :
666 : // fast call to get command ID of a mission index
667 : uint16_t get_command_id(uint16_t index) const;
668 :
669 : /// load_cmd_from_storage - load command from storage
670 : /// true is return if successful
671 : bool read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const;
672 :
673 : /// write_home_to_storage - writes the special purpose cmd 0 (home) to storage
674 : /// home is taken directly from ahrs
675 : void write_home_to_storage();
676 :
677 : static MAV_MISSION_RESULT convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t &mission_item,
678 : mavlink_mission_item_int_t &mission_item_int) WARN_IF_UNUSED;
679 : static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_MISSION_ITEM(const mavlink_mission_item_int_t &mission_item_int,
680 : mavlink_mission_item_t &mission_item) WARN_IF_UNUSED;
681 :
682 : // mavlink_int_to_mission_cmd - converts mavlink message to an AP_Mission::Mission_Command object which can be stored to eeprom
683 : // return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
684 : static MAV_MISSION_RESULT mavlink_int_to_mission_cmd(const mavlink_mission_item_int_t& packet, AP_Mission::Mission_Command& cmd);
685 :
686 : // mission_cmd_to_mavlink_int - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS
687 : // return true on success, false on failure
688 : static bool mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& cmd, mavlink_mission_item_int_t& packet);
689 :
690 : // return the last time the mission changed in milliseconds
691 4870316 : uint32_t last_change_time_ms(void) const
692 : {
693 4870316 : return _last_change_time_ms;
694 : }
695 :
696 : // find the nearest landing sequence starting point (DO_LAND_START) and
697 : // return its index. Returns 0 if no appropriate DO_LAND_START point can
698 : // be found.
699 : uint16_t get_landing_sequence_start(const Location ¤t_loc);
700 :
701 : // find the nearest landing sequence starting point (DO_LAND_START) and
702 : // switch to that mission item. Returns false if no DO_LAND_START
703 : // available.
704 : bool jump_to_landing_sequence(const Location ¤t_loc);
705 :
706 : // jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort
707 : bool jump_to_abort_landing_sequence(const Location ¤t_loc);
708 :
709 : // Scripting helpers for the above functions to fill in the location
710 : #if AP_SCRIPTING_ENABLED
711 : bool jump_to_landing_sequence(void);
712 : bool jump_to_abort_landing_sequence(void);
713 : #endif
714 :
715 : // find the closest point on the mission after a DO_RETURN_PATH_START and before DO_LAND_START or landing
716 : bool jump_to_closest_mission_leg(const Location ¤t_loc);
717 :
718 : // check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan
719 : bool is_best_land_sequence(const Location ¤t_loc);
720 :
721 : // set in_landing_sequence flag
722 0 : void set_in_landing_sequence_flag(bool flag)
723 : {
724 0 : _flags.in_landing_sequence = flag;
725 0 : }
726 :
727 : // get in_landing_sequence flag
728 6256678 : bool get_in_landing_sequence_flag() const {
729 6256678 : return _flags.in_landing_sequence;
730 : }
731 :
732 : // get in_return_path flag
733 3277922 : bool get_in_return_path_flag() const {
734 3277922 : return _flags.in_return_path;
735 : }
736 :
737 : // force mission to resume when start_or_resume() is called
738 26 : void set_force_resume(bool force_resume)
739 : {
740 26 : _force_resume = force_resume;
741 26 : }
742 :
743 : // returns true if configured to resume
744 40 : bool is_resume() const { return _restart == 0 || _force_resume; }
745 :
746 : // get a reference to the AP_Mission semaphore, allowing an external caller to lock the
747 : // storage while working with multiple waypoints
748 0 : HAL_Semaphore &get_semaphore(void)
749 : {
750 0 : return _rsem;
751 : }
752 :
753 : // returns true if the mission contains the requested items
754 : bool contains_item(MAV_CMD command) const;
755 :
756 : // returns true if the mission has a terrain relative mission item
757 : bool contains_terrain_alt_items(void);
758 :
759 : // returns true if the mission cmd has a location
760 : static bool cmd_has_location(const uint16_t command);
761 :
762 : // reset the mission history to prevent recalling previous mission histories when restarting missions.
763 : void reset_wp_history(void);
764 :
765 : /*
766 : Option::FailsafeToBestLanding - continue mission
767 : logic after a land if the next waypoint is a takeoff. If this
768 : is false then after a landing is complete the vehicle should
769 : disarm and mission logic should stop
770 : */
771 : enum class Option {
772 : CLEAR_ON_BOOT = (1U<<0), // clear mission on vehicle boot
773 : FAILSAFE_TO_BEST_LANDING = (1U<<1), // on failsafe, find fastest path along mission home
774 : CONTINUE_AFTER_LAND = (1U<<2), // continue running mission (do not disarm) after land if takeoff is next waypoint
775 : DONT_ZERO_COUNTER = (1U<<3), // don't zero counter on completion
776 : };
777 40169 : bool option_is_set(Option option) const {
778 40169 : return (_options.get() & (uint16_t)option) != 0;
779 : }
780 :
781 : bool continue_after_land_check_for_takeoff(void);
782 38081 : bool continue_after_land(void) const {
783 38081 : return option_is_set(Option::CONTINUE_AFTER_LAND);
784 : }
785 :
786 : // user settable parameters
787 : static const struct AP_Param::GroupInfo var_info[];
788 :
789 : // allow lua to get/set any WP items in any order in a mavlink-ish kinda way.
790 : bool get_item(uint16_t index, mavlink_mission_item_int_t& result) const ;
791 : bool set_item(uint16_t index, mavlink_mission_item_int_t& source) ;
792 :
793 : // Jump Tags. When a JUMP_TAG is run in the mission, either via DO_JUMP_TAG or
794 : // by just being the next item, the tag is remembered and the age is set to 1.
795 : // Only the most recent tag is remembered. It's age is how many NAV items have
796 : // progressed since the tag was seen. While executing the tag, the
797 : // age will be 1. The next NAV command after it will tick the age to 2, and so on.
798 : bool get_last_jump_tag(uint16_t &tag, uint16_t &age) const;
799 :
800 : // Set the mission index to the first JUMP_TAG with this tag.
801 : // Returns true on success, else false if no appropriate JUMP_TAG match can be found or if setting the index failed
802 : bool jump_to_tag(const uint16_t tag);
803 :
804 : // find the first JUMP_TAG with this tag and return its index.
805 : // Returns 0 if no appropriate JUMP_TAG match can be found.
806 : uint16_t get_index_of_jump_tag(const uint16_t tag) const;
807 :
808 9 : bool is_valid_index(const uint16_t index) const { return index < _cmd_total; }
809 :
810 : #if AP_SDCARD_STORAGE_ENABLED
811 133080 : bool failed_sdcard_storage(void) const {
812 133080 : return _failed_sdcard_storage;
813 : }
814 : #endif
815 :
816 : #if HAL_LOGGING_ENABLED
817 2054 : void set_log_start_mission_item_bit(uint32_t bit) { log_start_mission_item_bit = bit; }
818 : #endif
819 :
820 : private:
821 : static AP_Mission *_singleton;
822 :
823 : static StorageAccess _storage;
824 :
825 : static bool stored_in_location(uint16_t id);
826 :
827 : struct {
828 : uint16_t age; // a value of 0 means we have never seen a tag. Once a tag is seen, age will increment every time the mission index changes.
829 : uint16_t tag; // most recent tag that was successfully jumped to. Only valid if age > 0
830 : } _jump_tag;
831 :
832 : struct Mission_Flags {
833 : mission_state state;
834 : bool nav_cmd_loaded; // true if a "navigation" command has been loaded into _nav_cmd
835 : bool do_cmd_loaded; // true if a "do"/"conditional" command has been loaded into _do_cmd
836 : bool do_cmd_all_done; // true if all "do"/"conditional" commands have been completed (stops unnecessary searching through eeprom for do commands)
837 : bool in_landing_sequence; // true if the mission has jumped to a landing
838 : bool resuming_mission; // true if the mission is resuming and set false once the aircraft attains the interrupted WP
839 : bool in_return_path; // true if the mission has passed a DO_RETURN_PATH_START waypoint either in the course of the mission or via a `jump_to_closest_mission_leg` call
840 : } _flags;
841 :
842 : // mission WP resume history
843 : uint16_t _wp_index_history[AP_MISSION_MAX_WP_HISTORY]; // storing the nav_cmd index for the last 6 WPs
844 :
845 : ///
846 : /// private methods
847 : ///
848 :
849 : /// write_cmd_to_storage - write a command to storage
850 : /// cmd.index is used to calculate the storage location
851 : /// true is returned if successful
852 : bool write_cmd_to_storage(uint16_t index, const Mission_Command& cmd);
853 :
854 : /// complete - mission is marked complete and clean-up performed including calling the mission_complete_fn
855 : void complete();
856 :
857 : bool verify_command(const Mission_Command& cmd);
858 : bool start_command(const Mission_Command& cmd);
859 :
860 : /// advance_current_nav_cmd - moves current nav command forward
861 : // starting_index is used to set the index from which searching will begin, leave as 0 to search from the current navigation target
862 : /// do command will also be loaded
863 : /// accounts for do-jump commands
864 : // returns true if command is advanced, false if failed (i.e. mission completed)
865 : bool advance_current_nav_cmd(uint16_t starting_index = 0);
866 :
867 : /// advance_current_do_cmd - moves current do command forward
868 : /// accounts for do-jump commands
869 : /// returns true if successfully advanced (can it ever be unsuccessful?)
870 : void advance_current_do_cmd();
871 :
872 : /// get_next_cmd - gets next command found at or after start_index
873 : /// returns true if found, false if not found (i.e. mission complete)
874 : /// accounts for do_jump commands
875 : /// increment_jump_num_times_if_found should be set to true if advancing the active navigation command
876 : bool get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool increment_jump_num_times_if_found, bool send_gcs_msg = true);
877 :
878 : /// get_next_do_cmd - gets next "do" or "conditional" command after start_index
879 : /// returns true if found, false if not found
880 : /// stops and returns false if it hits another navigation command before it finds the first do or conditional command
881 : /// accounts for do_jump commands but never increments the jump's num_times_run (get_next_nav_cmd is responsible for this)
882 : bool get_next_do_cmd(uint16_t start_index, Mission_Command& cmd);
883 :
884 : ///
885 : /// jump handling methods
886 : ///
887 : // init_jump_tracking - initialise jump_tracking variables
888 : void init_jump_tracking();
889 :
890 : /// get_jump_times_run - returns number of times the jump command has been run
891 : /// return is signed to be consistent with do-jump cmd's repeat count which can be -1 (to signify to repeat forever)
892 : int16_t get_jump_times_run(const Mission_Command& cmd);
893 :
894 : /// increment_jump_times_run - increments the recorded number of times the jump command has been run
895 : void increment_jump_times_run(Mission_Command& cmd, bool send_gcs_msg = true);
896 :
897 : /// check_eeprom_version - checks version of missions stored in eeprom matches this library
898 : /// command list will be cleared if they do not match
899 : void check_eeprom_version();
900 :
901 : // check if command is a landing type command. Asside the obvious, MAV_CMD_DO_PARACHUTE is considered a type of landing
902 : bool is_landing_type_cmd(uint16_t id) const;
903 :
904 : // approximate the distance travelled to get to a landing. DO_JUMP commands are observed in look forward.
905 : bool distance_to_landing(uint16_t index, float &tot_distance,Location current_loc);
906 :
907 : // Approximate the distance traveled to return to the mission path. DO_JUMP commands are observed in look forward.
908 : // Stop searching once reaching a landing or do-land-start
909 : bool distance_to_mission_leg(uint16_t index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc);
910 :
911 : // calculate the location of a resume cmd wp
912 : bool calc_rewind_pos(Mission_Command& rewind_cmd);
913 :
914 : // update progress made in mission to store last position in the event of mission exit
915 : void update_exit_position(void);
916 :
917 : void on_mission_timestamp_change();
918 :
919 : /// sanity checks that the masked fields are not NaN's or infinite
920 : static MAV_MISSION_RESULT sanity_check_params(const mavlink_mission_item_int_t& packet);
921 :
922 : /// check if the next nav command is a takeoff, skipping delays
923 : bool is_takeoff_next(uint16_t start_index);
924 :
925 : // pointer to main program functions
926 : mission_cmd_fn_t _cmd_start_fn; // pointer to function which will be called when a new command is started
927 : mission_cmd_fn_t _cmd_verify_fn; // pointer to function which will be called repeatedly to ensure a command is progressing
928 : mission_complete_fn_t _mission_complete_fn; // pointer to function which will be called when mission completes
929 :
930 : // parameters
931 : AP_Int16 _cmd_total; // total number of commands in the mission
932 : AP_Int16 _options; // bitmask options for missions, currently for mission clearing on reboot but can be expanded as required
933 : AP_Int8 _restart; // controls mission starting point when entering Auto mode (either restart from beginning of mission or resume from last command run)
934 :
935 : // internal variables
936 : bool _force_resume; // when set true it forces mission to resume irrespective of MIS_RESTART param.
937 : uint16_t _repeat_dist; // Distance to repeat on mission resume (m), can be set with MAV_CMD_DO_SET_RESUME_REPEAT_DIST
938 : struct Mission_Command _nav_cmd; // current "navigation" command. It's position in the command list is held in _nav_cmd.index
939 : struct Mission_Command _do_cmd; // current "do" command. It's position in the command list is held in _do_cmd.index
940 : struct Mission_Command _resume_cmd; // virtual wp command that is used to resume mission if the mission needs to be rewound on resume.
941 : uint16_t _prev_nav_cmd_id; // id of the previous "navigation" command. (WAYPOINT, LOITER_TO_ALT, ect etc)
942 : uint16_t _prev_nav_cmd_index; // index of the previous "navigation" command. Rarely used which is why we don't store the whole command
943 : uint16_t _prev_nav_cmd_wp_index; // index of the previous "navigation" command that contains a waypoint. Rarely used which is why we don't store the whole command
944 : Location _exit_position; // the position in the mission that the mission was exited
945 :
946 : // jump related variables
947 : struct jump_tracking_struct {
948 : uint16_t index; // index of do-jump commands in mission
949 : int16_t num_times_run; // number of times this jump command has been run
950 : } _jump_tracking[AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS];
951 :
952 : // last time that mission changed
953 : uint32_t _last_change_time_ms;
954 : uint32_t _last_change_time_prev_ms;
955 :
956 : // maximum number of commands that will fit in storage
957 : uint16_t _commands_max;
958 :
959 : #if AP_SDCARD_STORAGE_ENABLED
960 : bool _failed_sdcard_storage;
961 : #endif
962 :
963 : // memoisation of contains-relative:
964 : bool _contains_terrain_alt_items; // true if the mission has terrain-relative items
965 : uint32_t _last_contains_relative_calculated_ms; // will be equal to _last_change_time_ms if _contains_terrain_alt_items is up-to-date
966 : bool calculate_contains_terrain_alt_items(void) const;
967 :
968 : // multi-thread support. This is static so it can be used from
969 : // const functions
970 : static HAL_Semaphore _rsem;
971 :
972 : // mission items common to all vehicles:
973 : bool start_command_do_aux_function(const AP_Mission::Mission_Command& cmd);
974 : bool start_command_do_gripper(const AP_Mission::Mission_Command& cmd);
975 : bool start_command_do_servorelayevents(const AP_Mission::Mission_Command& cmd);
976 : bool start_command_camera(const AP_Mission::Mission_Command& cmd);
977 : bool start_command_parachute(const AP_Mission::Mission_Command& cmd);
978 : bool command_do_set_repeat_dist(const AP_Mission::Mission_Command& cmd);
979 :
980 : bool start_command_do_sprayer(const AP_Mission::Mission_Command& cmd);
981 : bool start_command_do_scripting(const AP_Mission::Mission_Command& cmd);
982 : bool start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd);
983 : bool start_command_fence(const AP_Mission::Mission_Command& cmd);
984 : #if AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
985 : bool start_command_do_set_roi_wpnext_offset(const AP_Mission::Mission_Command& cmd);
986 : #endif // AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
987 :
988 : /*
989 : handle format conversion of storage format to allow us to update
990 : format to take advantage of new packing
991 : */
992 : void format_conversion(uint8_t tag_byte, const Mission_Command &cmd, PackedContent &packed_content) const;
993 :
994 : #if HAL_LOGGING_ENABLED
995 : // if not -1, this bit in LOG_BITMASK specifies whether to log a message each time we start a command:
996 : uint32_t log_start_mission_item_bit = -1;
997 : #endif
998 : };
999 :
1000 : namespace AP
1001 : {
1002 : AP_Mission &mission();
1003 : };
|
