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.mission.init()

Copter::update_flight_mode
 └──> flightmode->run (ModeAuto::run)
     └──> mission.update()
1
2
3
4
5
6

3. 简令结构

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

4. 方法接口

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

3 简令结构

3.1 导航相关

3.1.1 jump command

    // jump command structure
    struct PACKED Jump_Command {
        uint16_t target;        // target command id
        int16_t num_times;      // num times to repeat.  -1 = repeat forever
    };
1
2
3
4
5

3.1.2 condition delay command

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

1
2
3
4
5

3.1.3 condition distance command

    // condition delay command structure
    struct PACKED Conditional_Distance_Command {
        float meters;           // distance from next waypoint in meters
    };
1
2
3
4

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

3.1.4 condition yaw command

    // 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
    };
1
2
3
4
5
6
7

3.1.5 change speed command

    // 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
    };
1
2
3
4
5
6

3.1.6 nav guided command

    // 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
    };
1
2
3
4
5
6
7

3.1.7 do VTOL transition

    // do VTOL transition
    struct PACKED Do_VTOL_Transition {
        uint8_t target_state;
    };
1
2
3
4

3.1.8 navigation delay command

    // 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)
    };
1
2
3
4
5
6
7

3.1.9 DO_ENGINE_CONTROL support

    // 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
    };
1
2
3
4
5
6

3.1.10 NAV_SET_YAW_SPEED support

    // 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
    };
1
2
3
4
5
6

3.1.11 high altitude balloon altitude wait

    // high altitude balloon altitude wait
    struct PACKED Altitude_Wait {
        float altitude; // meters
        float descent_rate; // m/s
        uint8_t wiggle_time; // seconds
    };
1
2
3
4
5
6

3.2 外围设备

3.2.1 继电器操作

    // 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
    };
1
2
3
4
5

    // 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?)
    };
1
2
3
4
5
6

3.2.2 伺服器操作

    // set servo command structure
    struct PACKED Set_Servo_Command {
        uint8_t channel;        // servo channel
        uint16_t pwm;           // pwm value for servo
    };
1
2
3
4
5

    // 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?)
    };
1
2
3
4
5
6
7

3.3.3 云台角度

    // 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
    };
1
2
3
4
5
6

3.3.4 摄像头操作

    // digicam configure 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
    };
1
2
3
4
5
6
7
8
9
10

    // 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;
    };
1
2
3
4
5
6
7
8
9

    // set cam trigger distance command structure
    struct PACKED Cam_Trigg_Distance {
        float meters;           // distance
        uint8_t trigger;        // triggers one image capture immediately
    };
1
2
3
4
5

3.3.5 夹具操作

    // gripper command structure
    struct PACKED Gripper_Command {
        uint8_t num;            // gripper number
        uint8_t action;         // action (0 = release, 1 = grab)
    };
1
2
3
4
5

3.3.6 辅助开关

    // AUX_FUNCTION command structure
    struct PACKED AuxFunction {
        uint16_t function;  // from RC_Channel::AUX_FUNC
        uint8_t switchpos;  // from RC_Channel::AuxSwitchPos
    };
1
2
3
4
5

3.3.7 绞车命令

    // 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
    };
1
2
3
4
5
6
7

3.4 脚本命令

3.4.1 scripting command

    // Scripting command structure
    struct PACKED scripting_Command {
        float p1;
        float p2;
        float p3;
    };
1
2
3
4
5
6

3.4.2 scripting NAV command

    // 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;
    };
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

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;
    };
1
2
3
4
5
6
7
8

3.5 MAVLink命令

3.5.1 MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW support

    // 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;
    };
1
2
3
4
5
6
7
8
9

3.5.2 MAV_CMD_IMAGE_START_CAPTURE support

    // MAV_CMD_IMAGE_START_CAPTURE support
    struct PACKED image_start_capture_Command {
        float interval_s;
        uint16_t total_num_images;
        uint16_t start_seq_number;
    };
1
2
3
4
5
6

3.5.3 MAV_CMD_SET_CAMERA_ZOOM support

    // MAV_CMD_SET_CAMERA_ZOOM support
    struct PACKED set_camera_zoom_Command {
        uint8_t zoom_type;
        float zoom_value;
    };
1
2
3
4
5

3.5.4 MAV_CMD_SET_CAMERA_FOCUS support

    // MAV_CMD_SET_CAMERA_FOCUS support
    struct PACKED set_camera_focus_Command {
        uint8_t focus_type;
        float focus_value;
    };
1
2
3
4
5

3.5.5 MAV_CMD_VIDEO_START_CAPTURE support

    // MAV_CMD_VIDEO_START_CAPTURE support
    struct PACKED video_start_capture_Command {
        uint8_t video_stream_id;
    };
1
2
3
4

3.5.6 MAV_CMD_VIDEO_STOP_CAPTURE support

    // MAV_CMD_VIDEO_STOP_CAPTURE support
    struct PACKED video_stop_capture_Command {
        uint8_t video_stream_id;
    };
1
2
3
4

4. 方法

4.1 init

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

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

4.2 state

    /// status - returns the status of the mission (i.e. Mission_Started, Mission_Complete, Mission_Stopped
    mission_state state() const
    {
        return _flags.state;
    }

    // mission state enumeration
    enum mission_state {
        MISSION_STOPPED=0,
        MISSION_RUNNING=1,
        MISSION_COMPLETE=2
    };
1
2
3
4
5
6
7
8
9
10
11
12

4.3 num_commands

    /// 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;
    }
1
2
3
4
5
6

4.4 num_commands_max

    /// num_commands_max - returns maximum number of commands that can be stored
    uint16_t num_commands_max() const {
        return _commands_max;
    }
1
2
3
4

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();
1
2
3

4.6 stop

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

4.7 resume

    /// resume - continues the mission execution from where we last left off
    ///     previous running commands will be re-initialised
    void resume();
1
2
3

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();
1
2

4.9 starts_with_takeoff_cmd

    /// check mission starts with a takeoff command
    bool starts_with_takeoff_cmd();
1
2

4.10 reset

    /// reset - reset mission to the first command
    void reset();
1
2

4.11 clear

    /// clear - clears out mission
    bool clear();
1
2

4.12 truncate

    /// truncate - truncate any mission items beyond given index
    void truncate(uint16_t index);
1
2

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 higher
    void update();
1
2
3

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 mission
    bool add_cmd(Mission_Command& cmd);
1
2
3
4

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 failure
    bool replace_cmd(uint16_t index, const Mission_Command& cmd);
1
2
3
4

4.16 is_nav_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);
1
2

4.17 get_current_nav_cmd

    /// get_current_nav_cmd - returns the current "navigation" command
    const Mission_Command& get_current_nav_cmd() const
    {
        return _nav_cmd;
    }
1
2
3
4
5

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 command
    uint16_t get_current_nav_index() const
    {
        return _nav_cmd.index==AP_MISSION_CMD_INDEX_NONE?0:_nav_cmd.index;
    }
1
2
3
4
5
6
7

4.19 get_current_nav_id

    /// get_current_nav_id - return the id of the current nav command
    uint16_t get_current_nav_id() const
    {
        return _nav_cmd.id;
    }
1
2
3
4
5

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 RAM
    uint16_t get_prev_nav_cmd_id() const
    {
        return _prev_nav_cmd_id;
    }
1
2
3
4
5
6
7

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 RAM
    uint16_t get_prev_nav_cmd_index() const
    {
        return _prev_nav_cmd_index;
    }
1
2
3
4
5
6
7

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 RAM
    uint16_t get_prev_nav_cmd_with_wp_index() const
    {
        return _prev_nav_cmd_wp_index;
    }
1
2
3
4
5
6
7

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 commands
    bool get_next_nav_cmd(uint16_t start_index, Mission_Command& cmd);
1
2
3
4

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 determined
    int32_t get_next_ground_course_cd(int32_t default_angle);
1
2
3
4

4.25 get_current_do_cmd

    /// get_current_do_cmd - returns active "do" command
    const Mission_Command& get_current_do_cmd() const
    {
        return _do_cmd;
    }
1
2
3
4
5

4.26 get_current_do_cmd_id

    /// get_current_do_cmd_id - returns id of the active "do" command
    uint16_t get_current_do_cmd_id() const
    {
        return _do_cmd.id;
    }
1
2
3
4
5

4.27 set_current_cmd

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

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 deleted
    bool restart_current_nav_cmd();
1
2
3

4.29 read_cmd_from_storage

    /// 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;
1
2
3

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 successful
    bool write_cmd_to_storage(uint16_t index, const Mission_Command& cmd);
1
2
3
4

4.31 write_home_to_storage

    /// write_home_to_storage - writes the special purpose cmd 0 (home) to storage
    ///     home is taken directly from ahrs
    void write_home_to_storage();
1
2
3

4.32 last_change_time_ms

    // return the last time the mission changed in milliseconds
    uint32_t last_change_time_ms(void) const
    {
        return _last_change_time_ms;
    }
1
2
3
4
5

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();
1
2
3
4

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);
1
2
3
4

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 abort
    bool jump_to_abort_landing_sequence(void);
1
2

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 plan
    bool is_best_land_sequence(void);
1
2

4.37 set_in_landing_sequence_flag

    // set in_landing_sequence flag
    void set_in_landing_sequence_flag(bool flag)
    {
        _flags.in_landing_sequence = flag;
    }
1
2
3
4
5

4.38 get_in_landing_sequence_flag

    // get in_landing_sequence flag
    bool get_in_landing_sequence_flag() const {
        return _flags.in_landing_sequence;
    }
1
2
3
4

4.39 set_force_resume

    // force mission to resume when start_or_resume() is called
    void set_force_resume(bool force_resume)
    {
        _force_resume = force_resume;
    }
1
2
3
4
5

4.40 get_semaphore

    // 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;
    }
1
2
3
4
5
6

4.41 contains_item

    // returns true if the mission contains the requested items
    bool contains_item(MAV_CMD command) const;
1
2

4.42 contains_terrain_alt_items

    // returns true if the mission has a terrain relative mission item
    bool contains_terrain_alt_items(void);
1
2

4.43 cmd_has_location

    // returns true if the mission cmd has a location
    static bool cmd_has_location(const uint16_t command);
1
2

4.44 reset_wp_history

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

4.45 continue_after_land_check_for_takeoff/continue_after_land

    /*
      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;
    }
1
2
3
4
5
6
7
8
9
10

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) ;
1
2
3

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;
1
2
3
4
5
6

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 failed
    bool jump_to_tag(const uint16_t tag);
1
2
3

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;
1
2
3

4.50 failed_sdcard_storage

#if AP_SDCARD_STORAGE_ENABLED
    bool failed_sdcard_storage(void) const {
        return _failed_sdcard_storage;
    }
#endif
1
2
3
4
5

5. 参考资料

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