ArduPilot开源飞控之AP_Mission

1. 源由

ArduPilot中的AP_Mission主要用于处理存储器内部Mission任务相关指令的读/写操作。

• responsible for managing a list of commands made up of “nav”, “do” and “conditional” commands

负责管理任务命令列表：“nav”, “do” , “conditional”

• 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

负责DO_JUMP命令

飞控的自主导航任务，通常有三种方式：

1. 起飞前，将规划导航任务相关的指令按照顺序存储在飞控内部存储器
2. 实际飞行过程中，通过存储获取每一步的操作指令，依次执行命令
3. 在遇到异常情况时，根据飞行配置参数执行相关异常规避策略

因此，AP_Mission类的理解对于飞控自主导航任务是重中之重，为此，研读下这部分代码逻辑。

2. AP_Mission类

研读会从四个方面进行展开：

1. 类的声明

==》详见：AP_Mission

2. 实例初始化/运行过程如下：

Copter::init_ardupilot└──> <MODE_AUTO_ENABLED == ENABLED> mode_auto.mission.init()Copter::update_flight_mode└──> flightmode->run (ModeAuto::run)└──> mission.update()

3. 简令结构

==》专门独立章节进行展开

4. 方法接口

==》专门独立章节进行展开

3 简令结构

3.1 导航相关

3.1.1 jump command

    // jump command structurestruct PACKED Jump_Command {uint16_t target;        // target command idint16_t num_times;      // num times to repeat.  -1 = repeat forever};

3.1.2 condition delay command

    // condition delay command structurestruct PACKED Conditional_Delay_Command {float seconds;          // period of delay in seconds};

3.1.3 condition distance command

    // condition delay command structurestruct PACKED Conditional_Distance_Command {float meters;           // distance from next waypoint in meters};

注：代码注释似乎有拼写错误，这里给纠正下。

3.1.4 condition yaw command

    // condition yaw command structurestruct 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 = cwuint8_t relative_angle; // 0 = absolute angle, 1 = relative angle};

3.1.5 change speed command

    // change speed command structurestruct PACKED Change_Speed_Command {uint8_t speed_type;     // 0=airspeed, 1=ground speedfloat target_ms;        // target speed in m/s, -1 means no changefloat throttle_pct;     // throttle as a percentage (i.e. 1 ~ 100), 0 means no change};

3.1.6 nav guided command

    // nav guided commandstruct PACKED Guided_Limits_Command {// max time is held in p1 fieldfloat alt_min;          // min alt below which the command will be aborted.  0 for no lower alt limitfloat alt_max;          // max alt above which the command will be aborted.  0 for no upper alt limitfloat horiz_max;        // max horizontal distance the vehicle can move before the command will be aborted.  0 for no horizontal limit};

3.1.7 do VTOL transition

    // do VTOL transitionstruct PACKED Do_VTOL_Transition {uint8_t target_state;};

3.1.8 navigation delay command

    // navigation delay command structurestruct PACKED Navigation_Delay_Command {float seconds; // period of delay in secondsint8_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)};

3.1.9 DO_ENGINE_CONTROL support

    // DO_ENGINE_CONTROL supportstruct PACKED Do_Engine_Control {bool start_control; // start or stop enginebool cold_start; // use cold start procedureuint16_t height_delay_cm; // height delay for start};

3.1.10 NAV_SET_YAW_SPEED support

    // NAV_SET_YAW_SPEED supportstruct PACKED Set_Yaw_Speed {float angle_deg;        // target angle in degrees (0=north, 90=east)float speed;            // speed in meters/seconduint8_t relative_angle; // 0 = absolute angle, 1 = relative angle};

3.1.11 high altitude balloon altitude wait

    // high altitude balloon altitude waitstruct PACKED Altitude_Wait {float altitude; // metersfloat descent_rate; // m/suint8_t wiggle_time; // seconds};

3.2 外围设备

3.2.1 继电器操作

    // set relay command structurestruct PACKED Set_Relay_Command {uint8_t num;            // relay number from 1 to 4uint8_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 structurestruct PACKED Repeat_Relay_Command {uint8_t num;            // relay number from 1 to 4int16_t repeat_count;   // number of times to trigger the relayfloat cycle_time;       // cycle time in seconds (the time between peaks or the time the relay is on and off for each cycle?)};

3.2.2 伺服器操作

    // set servo command structurestruct PACKED Set_Servo_Command {uint8_t channel;        // servo channeluint16_t pwm;           // pwm value for servo};

    // repeat servo command structurestruct PACKED Repeat_Servo_Command {uint8_t channel;        // servo channeluint16_t pwm;           // pwm value for servoint16_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?)};

3.3.3 云台角度

    // mount control command structurestruct PACKED Mount_Control {float pitch;            // pitch angle in degreesfloat roll;             // roll angle in degreesfloat yaw;              // yaw angle (relative to vehicle heading) in degrees};

3.3.4 摄像头操作

    // digicam configure command structurestruct PACKED Digicam_Configure {uint8_t shooting_mode;  // ProgramAuto = 1, AV = 2, TV = 3, Man=4, IntelligentAuto=5, SuperiorAuto=6uint16_t shutter_speed;uint8_t aperture;       // F stop number * 10uint16_t ISO;           // 80, 100, 200, etcuint8_t exposure_type;uint8_t cmd_id;float engine_cutoff_time;   // seconds};

    // digicam control command structurestruct PACKED Digicam_Control {uint8_t session;        // 1 = on, 0 = offuint8_t zoom_pos;int8_t zoom_step;       // +1 = zoom in, -1 = zoom outuint8_t focus_lock;uint8_t shooting_cmd;uint8_t cmd_id;};

    // set cam trigger distance command structurestruct PACKED Cam_Trigg_Distance {float meters;           // distanceuint8_t trigger;        // triggers one image capture immediately};

3.3.5 夹具操作

    // gripper command structurestruct PACKED Gripper_Command {uint8_t num;            // gripper numberuint8_t action;         // action (0 = release, 1 = grab)};

3.3.6 辅助开关

    // AUX_FUNCTION command structurestruct PACKED AuxFunction {uint16_t function;  // from RC_Channel::AUX_FUNCuint8_t switchpos;  // from RC_Channel::AuxSwitchPos};

3.3.7 绞车命令

    // winch command structurestruct PACKED Winch_Command {uint8_t num;            // winch numberuint8_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 cablefloat release_rate;     // release rate in meters/second};

3.4 脚本命令

3.4.1 scripting command

    // Scripting command structurestruct PACKED scripting_Command {float p1;float p2;float p3;};

3.4.2 scripting NAV command

    // Scripting NAV command old version of storage formatstruct PACKED nav_script_time_Command_tag0 {uint8_t command;uint8_t timeout_s;float arg1;float arg2;};// Scripting NAV command, new version of storage formatstruct 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 encodingint16_t arg3;int16_t arg4;};

3.4.3 Scripting NAV command (with verify)

    // 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;};

3.5 MAVLink命令

3.5.1 MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW support

    // MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW supportstruct 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;};

3.5.2 MAV_CMD_IMAGE_START_CAPTURE support

    // MAV_CMD_IMAGE_START_CAPTURE supportstruct PACKED image_start_capture_Command {float interval_s;uint16_t total_num_images;uint16_t start_seq_number;};

3.5.3 MAV_CMD_SET_CAMERA_ZOOM support

    // MAV_CMD_SET_CAMERA_ZOOM supportstruct PACKED set_camera_zoom_Command {uint8_t zoom_type;float zoom_value;};

3.5.4 MAV_CMD_SET_CAMERA_FOCUS support

    // MAV_CMD_SET_CAMERA_FOCUS supportstruct PACKED set_camera_focus_Command {uint8_t focus_type;float focus_value;};

3.5.5 MAV_CMD_VIDEO_START_CAPTURE support

    // MAV_CMD_VIDEO_START_CAPTURE supportstruct PACKED video_start_capture_Command {uint8_t video_stream_id;};

3.5.6 MAV_CMD_VIDEO_STOP_CAPTURE support

    // MAV_CMD_VIDEO_STOP_CAPTURE supportstruct PACKED video_stop_capture_Command {uint8_t video_stream_id;};

4. 方法

4.1 init

    /// init - initialises this library including checks the version in eeprom matches this libraryvoid init();

注：在Copter::init_ardupilot中被调用。

4.2 state

    /// status - returns the status of the mission (i.e. Mission_Started, Mission_Complete, Mission_Stoppedmission_state state() const{return _flags.state;}// mission state enumerationenum mission_state {MISSION_STOPPED=0,MISSION_RUNNING=1,MISSION_COMPLETE=2};

4.3 num_commands

    /// num_commands - returns total number of commands in the mission///                 this number includes offset 0, the home locationuint16_t num_commands() const{return _cmd_total;}

4.4 num_commands_max

    /// num_commands_max - returns maximum number of commands that can be storeduint16_t num_commands_max() const {return _commands_max;}

4.5 start

    /// 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();

4.6 stop

    /// stop - stops mission execution.  subsequent calls to update() will have no effect until the mission is started or resumedvoid stop();

4.7 resume

    /// resume - continues the mission execution from where we last left off///     previous running commands will be re-initialisedvoid resume();

4.8 start_or_resume

    /// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()void start_or_resume();

4.9 starts_with_takeoff_cmd

    /// check mission starts with a takeoff commandbool starts_with_takeoff_cmd();

4.10 reset

    /// reset - reset mission to the first commandvoid reset();

4.11 clear

    /// clear - clears out missionbool clear();

4.12 truncate

    /// truncate - truncate any mission items beyond given indexvoid truncate(uint16_t index);

4.13 update

    /// 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 highervoid update();

4.14 add_cmd

    /// 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 missionbool add_cmd(Mission_Command& cmd);

4.15 replace_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 failurebool replace_cmd(uint16_t index, const Mission_Command& cmd);

4.16 is_nav_cmd

    /// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" commandstatic bool is_nav_cmd(const Mission_Command& cmd);

4.17 get_current_nav_cmd

    /// get_current_nav_cmd - returns the current "navigation" commandconst Mission_Command& get_current_nav_cmd() const{return _nav_cmd;}

4.18 get_current_nav_index

    /// 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 commanduint16_t get_current_nav_index() const{return _nav_cmd.index==AP_MISSION_CMD_INDEX_NONE?0:_nav_cmd.index;}

4.19 get_current_nav_id

    /// get_current_nav_id - return the id of the current nav commanduint16_t get_current_nav_id() const{return _nav_cmd.id;}

4.20 get_prev_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 RAMuint16_t get_prev_nav_cmd_id() const{return _prev_nav_cmd_id;}

4.21 get_prev_nav_cmd_index

    /// 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 RAMuint16_t get_prev_nav_cmd_index() const{return _prev_nav_cmd_index;}

4.22 get_prev_nav_cmd_with_wp_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 RAMuint16_t get_prev_nav_cmd_with_wp_index() const{return _prev_nav_cmd_wp_index;}

4.23 get_next_nav_cmd

    /// 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 commandsbool get_next_nav_cmd(uint16_t start_index, Mission_Command& cmd);

4.24 get_next_ground_course_cd

    /// get the ground course of the next navigation leg in centidegrees/// from 0 36000. Return default_angle if next navigation/// leg cannot be determinedint32_t get_next_ground_course_cd(int32_t default_angle);

4.25 get_current_do_cmd

    /// get_current_do_cmd - returns active "do" commandconst Mission_Command& get_current_do_cmd() const{return _do_cmd;}

4.26 get_current_do_cmd_id

    /// get_current_do_cmd_id - returns id of the active "do" commanduint16_t get_current_do_cmd_id() const{return _do_cmd.id;}

4.27 set_current_cmd

    // set_current_cmd - jumps to command specified by indexbool set_current_cmd(uint16_t index, bool rewind = false);

4.28 restart_current_nav_cmd

    // restart current navigation command.  Used to handle external changes to mission// returns true on success, false if current nav command has been deletedbool restart_current_nav_cmd();

4.29 read_cmd_from_storage

    /// load_cmd_from_storage - load command from storage///     true is return if successfulbool read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const;

4.30 write_cmd_to_storage

    /// write_cmd_to_storage - write a command to storage///     cmd.index is used to calculate the storage location///     true is returned if successfulbool write_cmd_to_storage(uint16_t index, const Mission_Command& cmd);

4.31 write_home_to_storage

    /// write_home_to_storage - writes the special purpose cmd 0 (home) to storage///     home is taken directly from ahrsvoid write_home_to_storage();

4.32 last_change_time_ms

    // return the last time the mission changed in millisecondsuint32_t last_change_time_ms(void) const{return _last_change_time_ms;}

4.33 get_landing_sequence_start

    // 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();

4.34 jump_to_landing_sequence

    // 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);

4.35 jump_to_abort_landing_sequence

    // jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abortbool jump_to_abort_landing_sequence(void);

4.36 is_best_land_sequence

    // check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission planbool is_best_land_sequence(void);

4.37 set_in_landing_sequence_flag

    // set in_landing_sequence flagvoid set_in_landing_sequence_flag(bool flag){_flags.in_landing_sequence = flag;}

4.38 get_in_landing_sequence_flag

    // get in_landing_sequence flagbool get_in_landing_sequence_flag() const {return _flags.in_landing_sequence;}

4.39 set_force_resume

    // force mission to resume when start_or_resume() is calledvoid set_force_resume(bool force_resume){_force_resume = force_resume;}

4.40 get_semaphore

    // get a reference to the AP_Mission semaphore, allowing an external caller to lock the// storage while working with multiple waypointsHAL_Semaphore &get_semaphore(void){return _rsem;}

4.41 contains_item

    // returns true if the mission contains the requested itemsbool contains_item(MAV_CMD command) const;

4.42 contains_terrain_alt_items

    // returns true if the mission has a terrain relative mission itembool contains_terrain_alt_items(void);

4.43 cmd_has_location

    // returns true if the mission cmd has a locationstatic bool cmd_has_location(const uint16_t command);

4.44 reset_wp_history

    // reset the mission history to prevent recalling previous mission histories when restarting missions.void reset_wp_history(void);

4.45 continue_after_land_check_for_takeoff/continue_after_land

    /*return true if MIS_OPTIONS is set to allow continue of missionlogic after a land and the next waypoint is a takeoff. If thisis 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;}

4.46 get_item/set_item

    // 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) ;

4.47 get_last_jump_tag

    // 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;

4.48 jump_to_tag

    // 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 failedbool jump_to_tag(const uint16_t tag);

4.40 get_index_of_jump_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;

4.50 failed_sdcard_storage

#if AP_SDCARD_STORAGE_ENABLEDbool failed_sdcard_storage(void) const {return _failed_sdcard_storage;}
#endif

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot飞控启动&运行过程简介
【3】ArduPilot之开源代码Library&Sketches设计