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