Ardusub源码剖析——mode_guided.cpp

标签：control set sub guided Ardusub rate 源码 yaw

代码

#include "Sub.h"

/*
 * Init and run calls for guided flight mode
 */

#define GUIDED_POSVEL_TIMEOUT_MS    3000    // guided mode's position-velocity controller times out after 3seconds with no new updates
#define GUIDED_ATTITUDE_TIMEOUT_MS  1000    // guided mode's attitude controller times out after 1 second with no new updates

static Vector3p posvel_pos_target_cm;
static Vector3f posvel_vel_target_cms;
static uint32_t update_time_ms;

struct {
    uint32_t update_time_ms;
    float roll_cd;
    float pitch_cd;
    float yaw_cd;
    float climb_rate_cms;
} static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f};

struct Guided_Limit {
    uint32_t timeout_ms;  // timeout (in seconds) from the time that guided is invoked
    float alt_min_cm;   // lower altitude limit in cm above home (0 = no limit)
    float alt_max_cm;   // upper altitude limit in cm above home (0 = no limit)
    float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit)
    uint32_t start_time;// system time in milliseconds that control was handed to the external computer
    Vector3f start_pos; // start position as a distance from home in cm.  used for checking horiz_max limit
} guided_limit;

// guided_init - initialise guided controller
bool ModeGuided::init(bool ignore_checks)
{
    if (!sub.position_ok() && !ignore_checks) {
        return false;
    }

    // start in position control mode
    guided_pos_control_start();
    return true;
}

// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
autopilot_yaw_mode ModeGuided::get_default_auto_yaw_mode(bool rtl) const
{
    switch (g.wp_yaw_behavior) {

    case WP_YAW_BEHAVIOR_NONE:
        return AUTO_YAW_HOLD;
        break;

    case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
        if (rtl) {
            return AUTO_YAW_HOLD;
        } else {
            return AUTO_YAW_LOOK_AT_NEXT_WP;
        }
        break;

    case WP_YAW_BEHAVIOR_LOOK_AHEAD:
        return AUTO_YAW_LOOK_AHEAD;
        break;

    case WP_YAW_BEHAVIOR_CORRECT_XTRACK:
        return AUTO_YAW_CORRECT_XTRACK;
        break;

    case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
    default:
        return AUTO_YAW_LOOK_AT_NEXT_WP;
        break;
    }
}


// initialise guided mode's position controller
void ModeGuided::guided_pos_control_start()
{
    // set to position control mode
    sub.guided_mode = Guided_WP;

    // initialise waypoint controller
    sub.wp_nav.wp_and_spline_init();

    // initialise wpnav to stopping point at current altitude
    // To-Do: set to current location if disarmed?
    // To-Do: set to stopping point altitude?
    Vector3f stopping_point;
    sub.wp_nav.get_wp_stopping_point(stopping_point);

    // no need to check return status because terrain data is not used
    sub.wp_nav.set_wp_destination(stopping_point, false);

    // initialise yaw
    sub.yaw_rate_only = false;
    set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}

// initialise guided mode's velocity controller
void ModeGuided::guided_vel_control_start()
{
    // set guided_mode to velocity controller
    sub.guided_mode = Guided_Velocity;

    // initialize vertical maximum speeds and acceleration
    position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
    position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);

    // initialise velocity controller
    position_control->init_z_controller();
    position_control->init_xy_controller();

    // pilot always controls yaw
    sub.yaw_rate_only = false;
    set_auto_yaw_mode(AUTO_YAW_HOLD);
}

// initialise guided mode's posvel controller
void ModeGuided::guided_posvel_control_start()
{
    // set guided_mode to velocity controller
    sub.guided_mode = Guided_PosVel;

    // set vertical speed and acceleration
    position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());
    position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());

    // initialise velocity controller
    position_control->init_z_controller();
    position_control->init_xy_controller();

    // pilot always controls yaw
    sub.yaw_rate_only = false;
    set_auto_yaw_mode(AUTO_YAW_HOLD);
}

// initialise guided mode's angle controller
void ModeGuided::guided_angle_control_start()
{
    // set guided_mode to velocity controller
    sub.guided_mode = Guided_Angle;

    // set vertical speed and acceleration
    position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());
    position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());

    // initialise velocity controller
    position_control->init_z_controller();

    // initialise targets
    guided_angle_state.update_time_ms = AP_HAL::millis();
    guided_angle_state.roll_cd = ahrs.roll_sensor;
    guided_angle_state.pitch_cd = ahrs.pitch_sensor;
    guided_angle_state.yaw_cd = ahrs.yaw_sensor;
    guided_angle_state.climb_rate_cms = 0.0f;

    // pilot always controls yaw
    sub.yaw_rate_only = false;
    set_auto_yaw_mode(AUTO_YAW_HOLD);
}

// guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool ModeGuided::guided_set_destination(const Vector3f& destination)
{
#if AP_FENCE_ENABLED
    // reject destination if outside the fence
    const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
    if (!sub.fence.check_destination_within_fence(dest_loc)) {
        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
        // failure is propagated to GCS with NAK
        return false;
    }
#endif

    // ensure we are in position control mode
    if (sub.guided_mode != Guided_WP) {
        guided_pos_control_start();
    }

    // no need to check return status because terrain data is not used
    sub.wp_nav.set_wp_destination(destination, false);

#if HAL_LOGGING_ENABLED
    // log target
    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f());
#endif

    return true;
}

// sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data)
// or if the fence is enabled and guided waypoint is outside the fence
bool ModeGuided::guided_set_destination(const Location& dest_loc)
{
#if AP_FENCE_ENABLED
    // reject destination outside the fence.
    // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
    if (!sub.fence.check_destination_within_fence(dest_loc)) {
        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
        // failure is propagated to GCS with NAK
        return false;
    }
#endif

    // ensure we are in position control mode
    if (sub.guided_mode != Guided_WP) {
        guided_pos_control_start();
    }

    if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {
        // failure to set destination can only be because of missing terrain data
        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
        // failure is propagated to GCS with NAK
        return false;
    }

#if HAL_LOGGING_ENABLED
    // log target
    sub.Log_Write_GuidedTarget(sub.guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
#endif

    return true;
}

// guided_set_destination - sets guided mode's target destination and target heading
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool ModeGuided::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
#if AP_FENCE_ENABLED
    // reject destination if outside the fence
    const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
    if (!sub.fence.check_destination_within_fence(dest_loc)) {
        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
        // failure is propagated to GCS with NAK
        return false;
    }
#endif

    // ensure we are in position control mode
    if (sub.guided_mode != Guided_WP) {
        guided_pos_control_start();
    }

    // set yaw state
    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

    update_time_ms = AP_HAL::millis();

    // no need to check return status because terrain data is not used
    sub.wp_nav.set_wp_destination(destination, false);

#if HAL_LOGGING_ENABLED
    // log target
    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f());
#endif

    return true;
}

// guided_set_velocity - sets guided mode's target velocity
void ModeGuided::guided_set_velocity(const Vector3f& velocity)
{
    // check we are in velocity control mode
    if (sub.guided_mode != Guided_Velocity) {
        guided_vel_control_start();
    }

    update_time_ms = AP_HAL::millis();

    // set position controller velocity target
    position_control->set_vel_desired_cms(velocity);
}

// guided_set_velocity - sets guided mode's target velocity
void ModeGuided::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
   // check we are in velocity control mode
    if (sub.guided_mode != Guided_Velocity) {
        guided_vel_control_start();
    }

    // set yaw state
    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

    update_time_ms = AP_HAL::millis();

    // set position controller velocity target
    position_control->set_vel_desired_cms(velocity);

}

// set guided mode posvel target
bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
{
#if AP_FENCE_ENABLED
    // reject destination if outside the fence
    const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
    if (!sub.fence.check_destination_within_fence(dest_loc)) {
        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
        // failure is propagated to GCS with NAK
        return false;
    }
#endif

    // check we are in posvel control mode
    if (sub.guided_mode != Guided_PosVel) {
        guided_posvel_control_start();
    }

    update_time_ms = AP_HAL::millis();
    posvel_pos_target_cm = destination.topostype();
    posvel_vel_target_cms = velocity;

    position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
    float dz = posvel_pos_target_cm.z;
    position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0);
    posvel_pos_target_cm.z = dz;

#if HAL_LOGGING_ENABLED
    // log target
    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity);
#endif

    return true;
}

// set guided mode posvel target
bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
    #if AP_FENCE_ENABLED
    // reject destination if outside the fence
    const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
    if (!sub.fence.check_destination_within_fence(dest_loc)) {
        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
        // failure is propagated to GCS with NAK
        return false;
    }
    #endif

    // check we are in posvel control mode
    if (sub.guided_mode != Guided_PosVel) {
        guided_posvel_control_start();
    }

    // set yaw state
    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

    update_time_ms = AP_HAL::millis();

    posvel_pos_target_cm = destination.topostype();
    posvel_vel_target_cms = velocity;

    position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
    float dz = posvel_pos_target_cm.z;
    position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0);
    posvel_pos_target_cm.z = dz;

#if HAL_LOGGING_ENABLED
    // log target
    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity);
#endif

    return true;
}

// set guided mode angle target
void ModeGuided::guided_set_angle(const Quaternion &q, float climb_rate_cms)
{
    // check we are in angle control mode
    if (sub.guided_mode != Guided_Angle) {
        guided_angle_control_start();
    }

    // convert quaternion to euler angles
    q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
    guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
    guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
    guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);

    guided_angle_state.climb_rate_cms = climb_rate_cms;
    guided_angle_state.update_time_ms = AP_HAL::millis();
}

// helper function to set yaw state and targets
void ModeGuided::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
{    
    float current_yaw = wrap_2PI(AP::ahrs().get_yaw());
    float euler_yaw_angle;
    float yaw_error;

    euler_yaw_angle = wrap_2PI((yaw_cd * 0.01f));
    yaw_error = wrap_PI(euler_yaw_angle - current_yaw);

    int direction = 0;
    if (yaw_error < 0){
        direction = -1;
    } else {
        direction = 1;
    }

    /*
    case 1: target yaw only
    case 2: target yaw and yaw rate
    case 3: target yaw rate only
    case 4: hold current yaw
    */
    if (use_yaw && !use_yaw_rate) {
        sub.yaw_rate_only = false;
        sub.mode_auto.set_auto_yaw_look_at_heading(yaw_cd * 0.01f, 0.0f, direction, relative_angle);
    } else if (use_yaw && use_yaw_rate) { 
        sub.yaw_rate_only = false;
        sub.mode_auto.set_auto_yaw_look_at_heading(yaw_cd * 0.01f, yaw_rate_cds * 0.01f, direction, relative_angle);
    } else if (!use_yaw && use_yaw_rate) {
        sub.yaw_rate_only = true;
        sub.mode_auto.set_yaw_rate(yaw_rate_cds * 0.01f);
    } else{
        sub.yaw_rate_only = false;
        set_auto_yaw_mode(AUTO_YAW_HOLD);
    }
}

// guided_run - runs the guided controller
// should be called at 100hz or more
void ModeGuided::run()
{
    // call the correct auto controller
    switch (sub.guided_mode) {

    case Guided_WP:
        // run position controller
        guided_pos_control_run();
        break;

    case Guided_Velocity:
        // run velocity controller
        guided_vel_control_run();
        break;

    case Guided_PosVel:
        // run position-velocity controller
        guided_posvel_control_run();
        break;

    case Guided_Angle:
        // run angle controller
        guided_angle_control_run();
        break;
    }
}

// guided_pos_control_run - runs the guided position controller
// called from guided_run
void ModeGuided::guided_pos_control_run()
{
    // if motors not enabled set throttle to zero and exit immediately
    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
        attitude_control->set_throttle_out(0,true,g.throttle_filt);
        attitude_control->relax_attitude_controllers();
        sub.wp_nav.wp_and_spline_init();
        return;
    }

    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!sub.failsafe.pilot_input) {
        // get pilot's desired yaw rate
        target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
        if (!is_zero(target_yaw_rate)) {
            set_auto_yaw_mode(AUTO_YAW_HOLD);
        } else{
            if (sub.yaw_rate_only){
                set_auto_yaw_mode(AUTO_YAW_RATE);
            } else{
                set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
            }
        }
    }

    // set motors to full range
    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    // run waypoint controller
    sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav());

    float lateral_out, forward_out;
    sub.translate_wpnav_rp(lateral_out, forward_out);

    // Send to forward/lateral outputs
    motors.set_lateral(lateral_out);
    motors.set_forward(forward_out);

    // WP_Nav has set the vertical position control targets
    // run the vertical position controller and set output throttle
    position_control->update_z_controller();

    // call attitude controller
    if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
        // roll & pitch & yaw rate from pilot
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
    } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) {
        // roll, pitch from pilot, yaw & yaw_rate from auto_control
        target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
        attitude_control->input_euler_angle_roll_pitch_slew_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate);
    } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) {
        // roll, pitch from pilot, yaw_rate from auto_control
        target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
    } else {
        // roll, pitch from pilot, yaw heading from auto_heading()
        attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
    }
}

// guided_vel_control_run - runs the guided velocity controller
// called from guided_run
void ModeGuided::guided_vel_control_run()
{
    // ifmotors not enabled set throttle to zero and exit immediately
    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
        attitude_control->set_throttle_out(0,true,g.throttle_filt);
        attitude_control->relax_attitude_controllers();
        // initialise velocity controller
        position_control->init_z_controller();
        position_control->init_xy_controller();
        return;
    }

    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!sub.failsafe.pilot_input) {
        // get pilot's desired yaw rate
        target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
        if (!is_zero(target_yaw_rate)) {
            set_auto_yaw_mode(AUTO_YAW_HOLD);
        } else{
            if (sub.yaw_rate_only){
                set_auto_yaw_mode(AUTO_YAW_RATE);
            } else{
                set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
            }
        }
    }

    // set motors to full range
    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    // set velocity to zero if no updates received for 3 seconds
    uint32_t tnow = AP_HAL::millis();
    if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !position_control->get_vel_desired_cms().is_zero()) {
        position_control->set_vel_desired_cms(Vector3f(0,0,0));
    }

    position_control->stop_pos_xy_stabilisation();
    // call velocity controller which includes z axis controller
    position_control->update_xy_controller();

    position_control->set_pos_target_z_from_climb_rate_cm(position_control->get_vel_desired_cms().z);
    position_control->update_z_controller();

    float lateral_out, forward_out;
    sub.translate_pos_control_rp(lateral_out, forward_out);

    // Send to forward/lateral outputs
    motors.set_lateral(lateral_out);
    motors.set_forward(forward_out);

    // call attitude controller
    if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
        // roll & pitch & yaw rate from pilot
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
    } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) {
        // roll, pitch from pilot, yaw & yaw_rate from auto_control
        target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
        attitude_control->input_euler_angle_roll_pitch_slew_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate);
    } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) {
        // roll, pitch from pilot, yaw_rate from auto_control
        target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
    } else {
        // roll, pitch from pilot, yaw heading from auto_heading()
        attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
    }
}

// guided_posvel_control_run - runs the guided posvel controller
// called from guided_run
void ModeGuided::guided_posvel_control_run()
{
    // if motors not enabled set throttle to zero and exit immediately
    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
        attitude_control->set_throttle_out(0,true,g.throttle_filt);
        attitude_control->relax_attitude_controllers();
        // initialise velocity controller
        position_control->init_z_controller();
        position_control->init_xy_controller();
        return;
    }

    // process pilot's yaw input
    float target_yaw_rate = 0;

    if (!sub.failsafe.pilot_input) {
        // get pilot's desired yaw rate
        target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
        if (!is_zero(target_yaw_rate)) {
            set_auto_yaw_mode(AUTO_YAW_HOLD);
        } else{
            if (sub.yaw_rate_only){
                set_auto_yaw_mode(AUTO_YAW_RATE);
            } else{
                set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
            }
        }
    }

    // set motors to full range
    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    // set velocity to zero if no updates received for 3 seconds
    uint32_t tnow = AP_HAL::millis();
    if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) {
        posvel_vel_target_cms.zero();
    }

    // advance position target using velocity target
    posvel_pos_target_cm += (posvel_vel_target_cms * position_control->get_dt()).topostype();

    // send position and velocity targets to position controller
    position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
    float pz = posvel_pos_target_cm.z;
    position_control->input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0);
    posvel_pos_target_cm.z = pz;

    // run position controller
    position_control->update_xy_controller();
    position_control->update_z_controller();

    float lateral_out, forward_out;
    sub.translate_pos_control_rp(lateral_out, forward_out);

    // Send to forward/lateral outputs
    motors.set_lateral(lateral_out);
    motors.set_forward(forward_out);

    // call attitude controller
    if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
        // roll & pitch & yaw rate from pilot
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
    } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) {
        // roll, pitch from pilot, yaw & yaw_rate from auto_control
        target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
        attitude_control->input_euler_angle_roll_pitch_slew_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate);
    } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) {
        // roll, pitch from pilot, and yaw_rate from auto_control
        target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
    } else {
        // roll, pitch from pilot, yaw heading from auto_heading()
        attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
    }
}

// guided_angle_control_run - runs the guided angle controller
// called from guided_run
void ModeGuided::guided_angle_control_run()
{
    // if motors not enabled set throttle to zero and exit immediately
    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
        attitude_control->set_throttle_out(0.0f,true,g.throttle_filt);
        attitude_control->relax_attitude_controllers();
        // initialise velocity controller
        position_control->init_z_controller();
        return;
    }

    // constrain desired lean angles
    float roll_in = guided_angle_state.roll_cd;
    float pitch_in = guided_angle_state.pitch_cd;
    float total_in = norm(roll_in, pitch_in);
    float angle_max = MIN(attitude_control->get_althold_lean_angle_max_cd(), sub.aparm.angle_max);
    if (total_in > angle_max) {
        float ratio = angle_max / total_in;
        roll_in *= ratio;
        pitch_in *= ratio;
    }

    // wrap yaw request
    float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);

    // constrain climb rate
    float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up());

    // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
    uint32_t tnow = AP_HAL::millis();
    if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
        roll_in = 0.0f;
        pitch_in = 0.0f;
        climb_rate_cms = 0.0f;
    }

    // set motors to full range
    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    // call attitude controller
    attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);

    // call position controller
    position_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms);
    position_control->update_z_controller();
}

// Guided Limit code

// guided_limit_clear - clear/turn off guided limits
void ModeGuided::guided_limit_clear()
{
    guided_limit.timeout_ms = 0;
    guided_limit.alt_min_cm = 0.0f;
    guided_limit.alt_max_cm = 0.0f;
    guided_limit.horiz_max_cm = 0.0f;
}


// set_auto_yaw_mode - sets the yaw mode for auto
void ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode)
{
    // return immediately if no change
    if (sub.auto_yaw_mode == yaw_mode) {
        return;
    }
    sub.auto_yaw_mode = yaw_mode;

    // perform initialisation
    switch (sub.auto_yaw_mode) {
    
    case AUTO_YAW_HOLD:
        // pilot controls the heading
        break;

    case AUTO_YAW_LOOK_AT_NEXT_WP:
        // wpnav will initialise heading when wpnav's set_destination method is called
        break;

    case AUTO_YAW_ROI:
        // point towards a location held in yaw_look_at_WP
        sub.yaw_look_at_WP_bearing = ahrs.yaw_sensor;
        break;

    case AUTO_YAW_LOOK_AT_HEADING:
        // keep heading pointing in the direction held in yaw_look_at_heading
        // caller should set the yaw_look_at_heading
        break;

    case AUTO_YAW_LOOK_AHEAD:
        // Commanded Yaw to automatically look ahead.
        sub.yaw_look_ahead_bearing = ahrs.yaw_sensor;
        break;

    case AUTO_YAW_RESETTOARMEDYAW:
        // initial_armed_bearing will be set during arming so no init required
        break;
    
    case AUTO_YAW_RATE:
        // set target yaw rate to yaw_look_at_heading_slew
        break;
    }
}

// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float ModeGuided::get_auto_heading()
{
    switch (sub.auto_yaw_mode) {

    case AUTO_YAW_ROI:
        // point towards a location held in roi_WP
        return sub.get_roi_yaw();
        break;

    case AUTO_YAW_LOOK_AT_HEADING:
        // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
        return sub.yaw_look_at_heading;
        break;

    case AUTO_YAW_LOOK_AHEAD:
        // Commanded Yaw to automatically look ahead.
        return sub.get_look_ahead_yaw();
        break;

    case AUTO_YAW_RESETTOARMEDYAW:
        // changes yaw to be same as when quad was armed
        return sub.initial_armed_bearing;
        break;

    case AUTO_YAW_CORRECT_XTRACK: {
        // TODO return current yaw if not in appropriate mode
        // Bearing of current track (centidegrees)
        float track_bearing = get_bearing_cd(sub.wp_nav.get_wp_origin().xy(), sub.wp_nav.get_wp_destination().xy());

        // Bearing from current position towards intermediate position target (centidegrees)
        const Vector2f target_vel_xy = position_control->get_vel_target_cms().xy();
        float angle_error = 0.0f;
        if (target_vel_xy.length() >= position_control->get_max_speed_xy_cms() * 0.1f) {
            const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f;
            angle_error = wrap_180_cd(desired_angle_cd - track_bearing);
        }
        float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f);
        return wrap_360_cd(track_bearing + angle_limited);
    }
    break;

    case AUTO_YAW_LOOK_AT_NEXT_WP:
    default:
        // point towards next waypoint.
        // we don't use wp_bearing because we don't want the vehicle to turn too much during flight
        return sub.wp_nav.get_yaw();
        break;
    }
}
// guided_limit_set - set guided timeout and movement limits
void ModeGuided::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
{
    guided_limit.timeout_ms = timeout_ms;
    guided_limit.alt_min_cm = alt_min_cm;
    guided_limit.alt_max_cm = alt_max_cm;
    guided_limit.horiz_max_cm = horiz_max_cm;
}

// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
//  only called from AUTO mode's auto_nav_guided_start function
void ModeGuided::guided_limit_init_time_and_pos()
{
    // initialise start time
    guided_limit.start_time = AP_HAL::millis();

    // initialise start position from current position
    guided_limit.start_pos = inertial_nav.get_position_neu_cm();
}

// guided_limit_check - returns true if guided mode has breached a limit
//  used when guided is invoked from the NAV_GUIDED_ENABLE mission command
bool ModeGuided::guided_limit_check()
{
    // check if we have passed the timeout
    if ((guided_limit.timeout_ms > 0) && (AP_HAL::millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {
        return true;
    }

    // get current location
    const Vector3f& curr_pos = inertial_nav.get_position_neu_cm();

    // check if we have gone below min alt
    if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) {
        return true;
    }

    // check if we have gone above max alt
    if (!is_zero(guided_limit.alt_max_cm) && (curr_pos.z > guided_limit.alt_max_cm)) {
        return true;
    }

    // check if we have gone beyond horizontal limit
    if (guided_limit.horiz_max_cm > 0.0f) {
        const float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos.xy(), curr_pos.xy());
        if (horiz_move > guided_limit.horiz_max_cm) {
            return true;
        }
    }

    // if we got this far we must be within limits
    return false;
}

剖析

#include "Sub.h"

这行代码包含了名为 “Sub.h” 的头文件。这是一个自定义的头文件，包含了相关的函数、变量和类的声明。

/*
 * Init and run calls for guided flight mode
 */

下面的代码与引导飞行模式的初始化和运行相关。

#define GUIDED_POSVEL_TIMEOUT_MS    3000    // guided mode's position-velocity controller times out after 3 seconds with no new updates
#define GUIDED_ATTITUDE_TIMEOUT_MS  1000    // guided mode's attitude controller times out after 1 second with no new updates

这两行代码使用 #define 宏定义了两个常量：GUIDED_POSVEL_TIMEOUT_MS 和 GUIDED_ATTITUDE_TIMEOUT_MS。它们分别表示在引导模式下，位置-速度控制器和姿态控制器的超时时间（以毫秒为单位）。

static Vector3p posvel_pos_target_cm;
static Vector3f posvel_vel_target_cms;
static uint32_t update_time_ms;

这里声明了三个静态全局变量：
- posvel_pos_target_cm：一个 Vector3p 类型的变量，用于存储位置-速度控制器目标位置，单位是厘米。
- posvel_vel_target_cms：一个 Vector3f 类型的变量，用于存储位置-速度控制器目标速度，单位是厘米/秒。
- update_time_ms：一个 uint32_t 类型的变量，用于记录上一次更新的时间，单位是毫秒。

struct {
    uint32_t update_time_ms;
    float roll_cd;
    float pitch_cd;
    float yaw_cd;
    float climb_rate_cms;
} static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f};

这里定义了一个匿名结构体，并声明了一个静态全局变量 guided_angle_state。该结构体用于存储引导模式下姿态控制的状态，包括：
- update_time_ms：上一次更新的时间，单位是毫秒。
- roll_cd：横滚角度，单位是角分。
- pitch_cd：俯仰角度，单位是角分。
- yaw_cd：偏航角度，单位是角分。
- climb_rate_cms：爬升率，单位是厘米/秒。
- 初始化时，所有成员变量都设置为0或0.0f。

struct Guided_Limit {
    uint32_t timeout_ms;  // timeout (in seconds) from the time that guided is invoked
    float alt_min_cm;   // lower altitude limit in cm above home (0 = no limit)
    float alt_max_cm;   // upper altitude limit in cm above home (0 = no limit)
    float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit)
    uint32_t start_time;// system time in milliseconds that control was handed to the external computer
    Vector3f start_pos; // start position as a distance from home in cm.  used for checking horiz_max limit
} guided_limit;

这里定义了一个名为 Guided_Limit 的结构体，并声明了一个名为 guided_limit 的全局变量。该结构体用于存储引导模式下的限制条件，包括：
- timeout_ms：从引导模式激活到超时的时间，单位是毫秒。
- alt_min_cm：相对于家的最低高度限制，单位是厘米。
- alt_max_cm：相对于家的最高高度限制，单位是厘米。
- horiz_max_cm：从引导模式激活点开始的最大水平位置限制，单位是厘米。
- start_time：控制权交给外部计算机的系统时间，单位是毫秒。
- start_pos：引导模式激活时的起始位置，相对于家的距离，单位是厘米。

ModeGuided::init(bool ignore_checks)

bool ModeGuided::init(bool ignore_checks)
{

声明一个返回布尔值的成员函数init，它接收一个名为ignore_checks的布尔参数。该参数用于指定是否忽略某些初始化检查。

    if (!sub.position_ok() && !ignore_checks) {
        return false;
    }

这行代码检查sub对象的position_ok成员函数返回值。如果返回false（意味着位置信息不正确或不可用），并且ignore_checks参数也为false（表示不忽略检查），则函数直接返回false。这表明如果位置信息不正确且检查不被忽略，初始化将失败。

    // start in position control mode
    guided_pos_control_start();

如果位置检查通过，或者忽略了位置检查，这行代码将调用guided_pos_control_start函数。这个函数的目的是启动引导模式下的位置控制

    return true;
}

如果初始化成功（即位置信息是好的，或者忽略了位置检查），函数返回true。

get_default_auto_yaw_mode(bool rtl)

autopilot_yaw_mode ModeGuided::get_default_auto_yaw_mode(bool rtl) const
{

声明一个返回autopilot_yaw_mode类型的成员函数get_default_auto_yaw_mode。autopilot_yaw_mode是一个枚举类型，用于表示自动驾驶系统中的偏航模式。该函数接收一个bool rtl参数，表示是否处于RTL模式，并且被声明为const，因为它不会修改对象的状态。

    switch (g.wp_yaw_behavior) {

使用switch语句来根据全局变量g.wp_yaw_behavior的值选择不同的行为。g.wp_yaw_behavior是一个枚举值，代表不同的偏航行为模式。

    case WP_YAW_BEHAVIOR_NONE:
        return AUTO_YAW_HOLD;
        break;

如果WP_YAW_BEHAVIOR的值是WP_YAW_BEHAVIOR_NONE，表示不需要特殊的偏航行为，函数返回AUTO_YAW_HOLD，意味着保持当前的偏航角度。

    case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
        if (rtl) {
            return AUTO_YAW_HOLD;
        } else {
            return AUTO_YAW_LOOK_AT_NEXT_WP;
        }
        break;

如果WP_YAW_BEHAVIOR的值是WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL，表示除了RTL模式外，无人机应该朝向下一个航点（waypoint）。如果当前是RTL模式(rtl为true)，则返回AUTO_YAW_HOLD；否则返回AUTO_YAW_LOOK_AT_NEXT_WP。

    case WP_YAW_BEHAVIOR_LOOK_AHEAD:
        return AUTO_YAW_LOOK_AHEAD;
        break;

如果WP_YAW_BEHAVIOR的值是WP_YAW_BEHAVIOR_LOOK_AHEAD，表示无人机应该朝向飞行路径的前方，函数返回AUTO_YAW_LOOK_AHEAD。

    case WP_YAW_BEHAVIOR_CORRECT_XTRACK:
        return AUTO_YAW_CORRECT_XTRACK;
        break;

如果WP_YAW_BEHAVIOR的值是WP_YAW_BEHAVIOR_CORRECT_XTRACK，表示无人机应该校正横跨轨迹的偏移，函数返回AUTO_YAW_CORRECT_XTRACK。

    case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
    default:
        return AUTO_YAW_LOOK_AT_NEXT_WP;
        break;

如果WP_YAW_BEHAVIOR的值是WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP，或者不匹配任何已知的枚举值（default情况），函数返回AUTO_YAW_LOOK_AT_NEXT_WP，意味着无人机应该朝向下一个航点。

ModeGuided::guided_pos_control_start()

void ModeGuided::guided_pos_control_start()
{

声明一个没有返回值的成员函数guided_pos_control_start。这个函数不接受任何参数，并且用于设置引导模式下的位置控制。

    // set to position control mode
    sub.guided_mode = Guided_WP;

将sub对象的guided_mode成员变量设置为Guided_WP。这表示引导模式下的无人机将使用航点（waypoint）控制，这里Guided_WP是一个枚举值，代表位置控制模式。

    // initialise waypoint controller
    sub.wp_nav.wp_and_spline_init();

调用sub.wp_nav对象的wp_and_spline_init方法来初始化航点控制器。这个方法负责准备航点导航算法，包括任何必要的初始化和配置。

    // initialise wpnav to stopping point at current altitude
    // To-Do: set to current location if disarmed?
    // To-Do: set to stopping point altitude?
    Vector3f stopping_point;
    sub.wp_nav.get_wp_stopping_point(stopping_point);

这里初始化一个名为stopping_point的Vector3f（三维向量）变量，并通过调用get_wp_stopping_point方法来获取当前的停止点。这个停止点可能在当前高度上，并且是无人机的下一个目标位置。注释中提到了两个待办事项，暗示可能需要根据无人机是否已解锁来设置当前位置，或者设置停止点的高度。

    // no need to check return status because terrain data is not used
    sub.wp_nav.set_wp_destination(stopping_point, false);

使用set_wp_destination方法设置无人机的目标位置为前面计算的停止点。第二个参数false可能表示不需要使用地形数据。这里注释提到因为没有使用地形数据，所以不需要检查返回状态。

    // initialise yaw
    sub.yaw_rate_only = false;

将sub对象的yaw_rate_only成员变量设置为false，表示偏航控制不仅仅依赖于偏航速率，也可能涉及到偏航到特定角度。

    set_auto_yaw_mode(get_default_auto_yaw_mode(false));

调用set_auto_yaw_mode函数，并传递get_default_auto_yaw_mode函数的返回值作为参数。get_default_auto_yaw_mode函数接收一个false的参数，表示当前不是在RTL模式。这个调用设置了自动偏航模式，以确定无人机在引导模式下的偏航行为。

ModeGuided::guided_vel_control_start()

void ModeGuided::guided_vel_control_start()
{

声明一个没有返回值的成员函数guided_vel_control_start。这个函数不接受任何参数，并且用于设置引导模式下的速度控制。

    // set guided_mode to velocity controller
    sub.guided_mode = Guided_Velocity;

将sub对象的guided_mode成员变量设置为Guided_Velocity。这表示引导模式下的无人机将使用速度控制器，而不是位置或航点控制器。Guided_Velocity是一个可能代表速度控制模式的枚举值或常数。

    // initialize vertical maximum speeds and acceleration
    position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);

调用position_control对象的set_max_speed_accel_z方法来设置垂直方向（Z轴）的最大速度和加速度。这里用到了几个参数：
- sub.get_pilot_speed_dn()：获取飞行员设定的下降速度（通常是负值）。
- g.pilot_speed_up：获取飞行员设定的上升速度。
- g.pilot_accel_z：获取飞行员设定的垂直加速度。

    position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);

类似地，调用set_correction_speed_accel_z方法来设置用于修正的垂直方向的最大速度和加速度，参数与上面相同。

    // initialise velocity controller
    position_control->init_z_controller();

初始化Z轴（垂直方向）的速度控制器。这通常涉及到设置必要的PID参数和其他控制逻辑，以便控制器能够正确地调节无人机的垂直速度。

    position_control->init_xy_controller();

初始化XY平面（水平方向）的速度控制器。与上面类似，这一步是为了确保控制器能够正确地调节无人机在水平方向上的速度。

    // pilot always controls yaw
    sub.yaw_rate_only = false;

设置sub对象的yaw_rate_only成员变量为false，这表示偏航（yaw）控制不仅仅受限于偏航速率，飞行员可以控制无人机的具体偏航角度。

    set_auto_yaw_mode(AUTO_YAW_HOLD);

调用set_auto_yaw_mode函数，并将AUTO_YAW_HOLD作为参数传递，设置自动偏航模式为保持当前偏航角度不变。这意味着在速度控制模式下，无人机的偏航角度不会自动改变，而是会保持在当前设置的角度。

ModeGuided::guided_posvel_control_start()

void ModeGuided::guided_posvel_control_start()
{

声明一个没有返回值的成员函数guided_posvel_control_start。这个函数不接受任何参数，并且用于设置引导模式下的位置和速度控制。

    // set guided_mode to velocity controller
    sub.guided_mode = Guided_PosVel;

将sub对象的guided_mode成员变量设置为Guided_PosVel。这表示引导模式下的无人机将使用位置和速度控制器。与之前的速度控制模式不同，Guided_PosVel表明控制器会同时考虑位置和速度的指令。

    // set vertical speed and acceleration
    position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());

调用position_control对象的set_max_speed_accel_z方法来设置垂直方向（Z轴）的最大速度和加速度。这里使用了wp_nav对象来获取默认的下降速度、上升速度和加速度：
- sub.wp_nav.get_default_speed_down()：获取默认的下降速度。
- sub.wp_nav.get_default_speed_up()：获取默认的上升速度。
- sub.wp_nav.get_accel_z()：获取垂直方向的加速度。

    position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());

类似地，调用set_correction_speed_accel_z方法来设置用于修正的垂直方向的最大速度和加速度，参数与上面相同。

    // initialise velocity controller
    position_control->init_z_controller();

初始化Z轴（垂直方向）的速度控制器，为接下来的控制指令做好准备。

    position_control->init_xy_controller();

初始化XY平面（水平方向）的速度控制器，无人机将能够响应水平方向的速度控制指令。

    // pilot always controls yaw
    sub.yaw_rate_only = false;

设置sub对象的yaw_rate_only成员变量为false，表示飞行员可以控制无人机的偏航角度，而不仅仅是偏航速率。

    set_auto_yaw_mode(AUTO_YAW_HOLD);

调用set_auto_yaw_mode函数，并将AUTO_YAW_HOLD作为参数传递，设置自动偏航模式为保持当前偏航角度不变。这意味着在位置速度控制模式下，无人机的偏航角度不会自动改变。

ModeGuided::guided_angle_control_start()

void ModeGuided::guided_angle_control_start()
{

声明一个没有返回值的成员函数guided_angle_control_start。这个函数不接受任何参数，并且用于设置引导模式下的角度控制。

    // set guided_mode to angle controller
    sub.guided_mode = Guided_Angle;

将sub对象的guided_mode成员变量设置为Guided_Angle。这表示引导模式下的无人机将使用角度控制器，而不是速度或位置控制器。在这种模式下，无人机会尝试保持指定的飞行角度。

    // set vertical speed and acceleration
    position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());

设置垂直方向（Z轴）的最大速度和加速度，用于控制无人机的爬升和下降。这里使用wp_nav对象获取默认的速度和加速度值。

    position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());

设置用于修正的垂直方向的最大速度和加速度，与上面相同。

    // initialise velocity controller
    position_control->init_z_controller();

初始化Z轴（垂直方向）的速度控制器。虽然在此模式中主要控制的是角度，但垂直速度控制器仍然用于辅助控制。

    // initialise targets
    guided_angle_state.update_time_ms = AP_HAL::millis();

初始化guided_angle_state结构体的update_time_ms字段，记录当前时间（毫秒）。这个时间戳用于后续计算或判断是否需要更新控制指令。

    guided_angle_state.roll_cd = ahrs.roll_sensor;
    guided_angle_state.pitch_cd = ahrs.pitch_sensor;
    guided_angle_state.yaw_cd = ahrs.yaw_sensor;

设置初始的滚转（roll）、俯仰（pitch）和偏航（yaw）目标值为当前的传感器读数。单位通常是千分度（centidegrees），即一度的千分之一。

    guided_angle_state.climb_rate_cms = 0.0f;

设置初始的爬升率（climb rate）为0厘米每秒。这意味着在开始时，无人机不会有意地爬升或下降。

    // pilot always controls yaw
    sub.yaw_rate_only = false;

设置sub对象的yaw_rate_only成员变量为false，表示飞行员可以控制无人机的偏航角度，而不仅仅是偏航速率。

    set_auto_yaw_mode(AUTO_YAW_HOLD);

调用set_auto_yaw_mode函数，并将AUTO_YAW_HOLD作为参数传递，设置自动偏航模式为保持当前偏航角度不变。

ModeGuided::guided_set_destination(const Vector3f& destination)

bool ModeGuided::guided_set_destination(const Vector3f& destination)
{

声明一个返回布尔值的成员函数guided_set_destination，它接受一个常量引用类型的Vector3f参数destination。返回值表示设置目的地是否成功。

#if AP_FENCE_ENABLED

这行代码开始了一个预处理指令，仅当AP_FENCE_ENABLED宏定义被设置时，以下代码块才会编译。

    // reject destination if outside the fence
    const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);

创建一个Location类型的对象dest_loc，使用传入的destination构造，并且指定高度框架为相对于原点的上方。

    if (!sub.fence.check_destination_within_fence(dest_loc)) {

检查目的地是否在围栏内。如果不在（check_destination_within_fence返回false），则执行以下代码块。

        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);

使用日志记录器记录一个错误，指出目的地在围栏外。

        // failure is propagated to GCS with NAK
        return false;
    }
#endif

如果目的地在围栏外，函数返回false，这表示设置目的地失败。这个返回值会被发送到地面控制站（GCS），并且通常会以否定应答（NAK）的形式通知用户。

    // ensure we are in position control mode
    if (sub.guided_mode != Guided_WP) {
        guided_pos_control_start();
    }

确保无人机处于位置控制模式。如果当前的guided_mode不是Guided_WP（通常代表航点模式），则调用guided_pos_control_start函数来启动位置控制。

    // no need to check return status because terrain data is not used
    sub.wp_nav.set_wp_destination(destination, false);

调用wp_nav对象的set_wp_destination方法来设置新的航点目的地。第二个参数false表示不需要使用地形数据。

#if HAL_LOGGING_ENABLED

这行代码开始了一个预处理指令，仅当HAL_LOGGING_ENABLED宏定义被设置时，以下代码块才会编译。

    // log target
    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f());
#endif

记录引导模式的目标信息。调用Log_Write_GuidedTarget方法来记录当前的guided_mode、目标位置destination和一个空的Vector3f对象（可能用于记录速度或其他信息）。

    return true;
}

如果所有步骤都成功完成，函数返回true，表示目的地已成功设置。

ModeGuided::guided_set_destination(const Location& dest_loc)

bool ModeGuided::guided_set_destination(const Location& dest_loc)
{

声明一个返回布尔值的成员函数guided_set_destination，它接受一个常量引用类型的Location参数dest_loc，表示目标位置。

#if AP_FENCE_ENABLED

这行代码检查AP_FENCE_ENABLED宏是否被定义，该宏如果被定义，表示启用了围栏功能。

    // reject destination outside the fence.
    if (!sub.fence.check_destination_within_fence(dest_loc)) {

如果目标位置在围栏外，则执行以下代码块。函数check_destination_within_fence用于检查给定的位置是否在围栏内部。

        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);

如果目标位置在围栏外，使用日志记录器记录一个错误，指出目的地在围栏外。

        // failure is propagated to GCS with NAK
        return false;
    }
#endif

如果目的地在围栏外，函数返回false，并且这个失败信息会以否定应答（NAK）的形式发送到地面控制站（GCS）。

    // ensure we are in position control mode
    if (sub.guided_mode != Guided_WP) {
        guided_pos_control_start();
    }

确保无人机处于位置控制模式。如果当前的guided_mode不是Guided_WP，则调用guided_pos_control_start函数来启动位置控制。

    if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {

调用set_wp_destination_loc方法来设置新的航点目的地。如果设置失败（可能是因为缺少地形数据），则执行以下代码块。

        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);

如果无法设置目的地，则记录一个错误。

        // failure is propagated to GCS with NAK
        return false;
    }

如果设置目的地失败，函数返回false，并且这个失败信息会发送到GCS。

#if HAL_LOGGING_ENABLED

这行代码检查HAL_LOGGING_ENABLED宏是否被定义，该宏如果被定义，表示启用了硬件抽象层（HAL）的日志记录功能。

    // log target
    sub.Log_Write_GuidedTarget(sub.guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), Vector3f());
#endif

如果启用了日志记录，记录引导模式的目标信息，包括当前的guided_mode和目标位置（纬度、经度和高度）。

    return true;
}

如果所有步骤都成功完成，函数返回true，表示目的地已成功设置。

ModeGuided::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)

bool ModeGuided::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{

声明一个返回布尔值的函数，接收以下参数：
- destination: 目的地位置，一个三维向量（Vector3f），通常表示相对于无人机起始点的北、东、下（NED）坐标。
- use_yaw: 一个布尔值，指示是否使用指定的偏航角度。
- yaw_cd: 如果use_yaw为true，则这是一个以千分之一度（centidegrees）为单位的偏航角度。
- use_yaw_rate: 一个布尔值，指示是否使用指定的偏航速率。
- yaw_rate_cds: 如果use_yaw_rate为true，则这是一个以千分之一度每秒（centidegrees per second）为单位的偏航速率。
- relative_yaw: 一个布尔值，指示偏航角度是否相对于当前偏航角度。

#if AP_FENCE_ENABLED

检查是否启用了围栏功能。

    // reject destination if outside the fence
    const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);

创建一个Location对象dest_loc，从destination向量转换而来，表示位置高度是相对于起始点（通常是无人机起飞点）的。

    if (!sub.fence.check_destination_within_fence(dest_loc)) {

如果目的地在围栏外，则执行以下代码块。

        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);

记录一个错误，指出目的地在围栏外。

        // failure is propagated to GCS with NAK
        return false;
    }
#endif

如果目的地在围栏外，则返回false。

    // ensure we are in position control mode
    if (sub.guided_mode != Guided_WP) {
        guided_pos_control_start();
    }

确保无人机处于位置控制模式。如果不是，则启动位置控制。

    // set yaw state
    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

设置无人机的偏航状态，包括偏航角度和偏航速率。

    update_time_ms = AP_HAL::millis();

更新时间戳，记录当前时间（毫秒）。

    // no need to check return status because terrain data is not used
    sub.wp_nav.set_wp_destination(destination, false);

设置新的航点目的地。这里不需要检查返回状态，因为地形数据没有被使用。

#if HAL_LOGGING_ENABLED

检查是否启用了日志记录功能。

    // log target
    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f());
#endif

如果启用了日志记录，记录引导模式的目标信息。

    return true;
}

如果所有步骤都成功完成，则返回true。

ModeGuided::guided_set_velocity(const Vector3f& velocity)

void ModeGuided::guided_set_velocity(const Vector3f& velocity)
{

声明一个没有返回值的函数guided_set_velocity，它接收一个Vector3f类型的常量引用参数velocity，表示期望的速度向量，通常是以厘米/秒为单位的北、东、下（NED）坐标系中的速度。

    // check we are in velocity control mode
    if (sub.guided_mode != Guided_Velocity) {
        guided_vel_control_start();
    }

检查当前的guided_mode是否已经是Guided_Velocity模式。如果不是，则调用guided_vel_control_start()函数来启动速度控制模式。确保了在设置期望速度之前，无人机处于正确的控制模式。

    update_time_ms = AP_HAL::millis();

更新update_time_ms变量，记录当前的时间（毫秒）。这个时间戳可能用于后续的时序计算或者用于确保在正确的时间间隔内更新速度目标。

    // set position controller velocity target
    position_control->set_vel_desired_cms(velocity);

调用position_control对象的set_vel_desired_cms方法，设置期望的速度目标。参数velocity是期望的速度向量，单位通常是厘米/秒。set_vel_desired_cms方法会将这个速度向量传递给底层的位置控制器，位置控制器随后会尝试使无人机以这个速度移动。

ModeGuided::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)

void ModeGuided::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{

声明一个没有返回值的函数guided_set_velocity，它接收以下参数：
- velocity: 一个三维向量（Vector3f），表示期望的速度，通常是以厘米/秒为单位的北、东、下（NED）坐标系中的速度。
- use_yaw: 一个布尔值，指示是否使用指定的偏航角度。
- yaw_cd: 如果use_yaw为true，则这是一个以千分之一度（centidegrees）为单位的偏航角度。
- use_yaw_rate: 一个布尔值，指示是否使用指定的偏航速率。
- yaw_rate_cds: 如果use_yaw_rate为true，则这是一个以千分之一度每秒（centidegrees per second）为单位的偏航速率。
- relative_yaw: 一个布尔值，指示偏航角度是否相对于当前偏航角度。

    // check we are in velocity control mode
    if (sub.guided_mode != Guided_Velocity) {
        guided_vel_control_start();
    }

检查当前的guided_mode是否已经是Guided_Velocity模式。如果不是，则调用guided_vel_control_start()函数来启动速度控制模式，确保无人机可以响应速度指令。

    // set yaw state
    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

调用guided_set_yaw_state函数来设置无人机的偏航状态。这个函数内部会处理偏航角度和偏航速率的设置，根据传入的参数决定是设置一个固定的偏航角度还是按照一定的速率改变偏航。

    update_time_ms = AP_HAL::millis();

更新update_time_ms变量，记录当前的时间（毫秒）。这个时间戳可能用于后续的时序计算或确保在正确的时间间隔内更新速度和偏航状态。

    // set position controller velocity target
    position_control->set_vel_desired_cms(velocity);

调用position_control对象的set_vel_desired_cms方法，设置期望的速度目标。velocity参数定义了无人机应该以多快的速度移动以及移动的方向。

ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)

bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
{

声明一个返回布尔值的函数guided_set_destination_posvel，它接收两个参数：
- destination: 一个三维向量，表示期望到达的位置。
- velocity: 一个三维向量，表示期望的速度。
- 函数返回一个布尔值，指示设置目的地和速度是否成功。

#if AP_FENCE_ENABLED
    // reject destination if outside the fence
    const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
    if (!sub.fence.check_destination_within_fence(dest_loc)) {
        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
        // failure is propagated to GCS with NAK
        return false;
    }
#endif

如果启用了飞行边界（fence）功能，这段代码会检查指定的目的地是否在飞行边界内。如果目的地在边界外，它会记录一个错误，并返回false，表示设置失败。

    // check we are in posvel control mode
    if (sub.guided_mode != Guided_PosVel) {
        guided_posvel_control_start();
    }

检查当前的guided_mode是否为Guided_PosVel模式。如果不是，则调用guided_posvel_control_start()来启动位置和速度控制模式。

    update_time_ms = AP_HAL::millis();

更新update_time_ms变量，记录当前的时间（毫秒）。

    posvel_pos_target_cm = destination.topostype();
    posvel_vel_target_cms = velocity;

将目的地位置和速度分别赋值给posvel_pos_target_cm和posvel_vel_target_cms变量，这些变量存储当前的位置和速度目标。

    position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());

调用position_control对象的input_pos_vel_accel_xy方法，设置期望的XY平面位置、速度和加速度（加速度在此处未设置，传递了一个空的Vector2f()）。

    float dz = posvel_pos_target_cm.z;
    position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0);
    posvel_pos_target_cm.z = dz;

类似地，为Z轴（垂直方向）设置期望的位置、速度和加速度（加速度同样未设置，传递了0）。

#if HAL_LOGGING_ENABLED
    // log target
    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity);
#endif

如果启用了日志记录功能，记录当前的目标位置和速度。

    return true;
}

返回true，表示设置目的地和速度成功。

ModeGuided::guided_set_destination_posvel( const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)

bool ModeGuided::guided_set_destination_posvel(
    const Vector3f& destination, 
    const Vector3f& velocity, 
    bool use_yaw, float yaw_cd, 
    bool use_yaw_rate, float yaw_rate_cds, 
    bool relative_yaw)
{

声明了一个返回布尔值的函数guided_set_destination_posvel，它接受以下参数：
- destination: 一个Vector3f对象，表示目标位置（通常是一个北东地（NED）坐标系中的向量）。
- velocity: 一个Vector3f对象，表示目标速度。
- use_yaw: 一个布尔值，指示是否使用特定的偏航角度。
- yaw_cd: 如果use_yaw为true，则这是一个以千分之一度为单位的目标偏航角度。
- use_yaw_rate: 一个布尔值，指示是否使用特定的偏航速率。
- yaw_rate_cds: 如果use_yaw_rate为true，则这是一个以千分之一度每秒为单位的偏航速率。
- relative_yaw: 一个布尔值，指示偏航角度是否相对于当前偏航角度。

#if AP_FENCE_ENABLED
    // reject destination if outside the fence
    const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
    if (!sub.fence.check_destination_within_fence(dest_loc)) {
        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
        // failure is propagated to GCS with NAK
        return false;
    }
#endif

如果启用了飞行边界（fence），这段代码将检查目标位置是否在边界内。如果目标位置在边界外，则记录一个错误并返回false。

    // check we are in posvel control mode
    if (sub.guided_mode != Guided_PosVel) {
        guided_posvel_control_start();
    }

检查是否已经在位置速度控制模式（Guided_PosVel）下。如果不是，则启动该模式。

    // set yaw state
    guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);

设置偏航状态，包括偏航角度或偏航速率。

    update_time_ms = AP_HAL::millis();

更新时间戳，记录当前时间。

    posvel_pos_target_cm = destination.topostype();
    posvel_vel_target_cms = velocity;

将目标位置和速度分别赋值给相应的成员变量。

    position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
    float dz = posvel_pos_target_cm.z;
    position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0);
    posvel_pos_target_cm.z = dz;

调用position_control对象的函数来设置XY平面和Z轴的位置和速度目标。

#if HAL_LOGGING_ENABLED
    // log target
    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity);
#endif

如果启用了日志记录，记录目标位置和速度。

    return true;
}

返回true，表示设置目标成功。

ModeGuided::guided_set_angle(const Quaternion &q, float climb_rate_cms)

void ModeGuided::guided_set_angle(const Quaternion &q, float climb_rate_cms)
{

声明了一个guided_set_angle函数，它接受两个参数：
- q: 一个Quaternion对象，表示无人机的目标四元数姿态。
- climb_rate_cms: 一个浮点数，表示无人机的目标爬升率，单位是厘米/秒。

    // check we are in angle control mode
    if (sub.guided_mode != Guided_Angle) {
        guided_angle_control_start();
    }

检查当前是否处于角度控制模式（Guided_Angle）。如果不是，则调用guided_angle_control_start()函数来启动角度控制模式。

    // convert quaternion to euler angles
    q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);

将传入的四元数姿态q转换为欧拉角（滚动、俯仰和偏航角度）。转换结果分别存储在guided_angle_state.roll_cd、guided_angle_state.pitch_cd和guided_angle_state.yaw_cd变量中。

    guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
    guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
    guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);

将欧拉角从弧度转换为度，然后乘以100以将单位转换为千分之一度（centidegrees，cd）。同时，偏航角度使用wrap_180_cd函数进行了调整，以确保角度在-180°到+180°之间。

    guided_angle_state.climb_rate_cms = climb_rate_cms;

设置无人机的目标爬升率（单位为厘米/秒）。

    guided_angle_state.update_time_ms = AP_HAL::millis();

更新时间戳，记录当前时间（毫秒）。

ModeGuided::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)

void ModeGuided::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
{

声明guided_set_yaw_state函数，它接受五个参数：
- use_yaw: 布尔值，指示是否使用提供的偏航角度。
- yaw_cd: 浮点数，表示目标偏航角度，单位是千分之一度（centidegrees）。
- use_yaw_rate: 布尔值，指示是否使用提供的偏航速率。
- yaw_rate_cds: 浮点数，表示目标偏航速率，单位是千分之一度每秒（centidegrees per second）。
- relative_angle: 布尔值，指示偏航角度是否相对于当前偏航角度。

    float current_yaw = wrap_2PI(AP::ahrs().get_yaw());

获取当前无人机的偏航角度（单位是弧度），并使用wrap_2PI函数将其标准化到[0, 2π]范围内。

    float euler_yaw_angle;
    float yaw_error;

声明变量来存储欧拉偏航角度和偏航误差。

    euler_yaw_angle = wrap_2PI((yaw_cd * 0.01f));
    yaw_error = wrap_PI(euler_yaw_angle - current_yaw);

将输入的偏航角度yaw_cd从千分之一度转换为度，并标准化到[0, 2π]范围内。
计算偏航误差，即目标偏航角度与当前偏航角度之差，并使用wrap_PI函数将其标准化到[-π, π]范围内。

    int direction = 0;
    if (yaw_error < 0){
        direction = -1;
    } else {
        direction = 1;
    }

确定偏航方向。如果偏航误差为负，则方向为-1；如果为正，则为1。

以下是根据输入参数设置偏航目标的不同情况：

    if (use_yaw && !use_yaw_rate) {
        sub.yaw_rate_only = false;
        sub.mode_auto.set_auto_yaw_look_at_heading(yaw_cd * 0.01f, 0.0f, direction, relative_angle);
    } else if (use_yaw && use_yaw_rate) { 
        sub.yaw_rate_only = false;
        sub.mode_auto.set_auto_yaw_look_at_heading(yaw_cd * 0.01f, yaw_rate_cds * 0.01f, direction, relative_angle);
    } else if (!use_yaw && use_yaw_rate) {
        sub.yaw_rate_only = true;
        sub.mode_auto.set_yaw_rate(yaw_rate_cds * 0.01f);
    } else{
        sub.yaw_rate_only = false;
        set_auto_yaw_mode(AUTO_YAW_HOLD);
    }

如果设置了偏航角度且没有设置偏航速率，则仅使用偏航角度，并调用set_auto_yaw_look_at_heading。
如果同时设置了偏航角度和偏航速率，则同时使用这两个值调用set_auto_yaw_look_at_heading。
如果没有设置偏航角度但设置了偏航速率，则仅使用偏航速率，并调用set_yaw_rate。
如果既没有设置偏航角度也没有设置偏航速率，则保持当前的偏航角度不变，通过调用set_auto_yaw_mode函数设置偏航模式为AUTO_YAW_HOLD。

ModeGuided::run()

void ModeGuided::run()
{

声明run函数，没有参数和返回值。这个函数是ModeGuided类的一部分，当需要执行引导模式下的控制逻辑时会被调用。

    // call the correct auto controller
    switch (sub.guided_mode) {

使用switch语句来选择正确的自动控制函数，基于当前的引导模式sub.guided_mode。sub.guided_mode是一个枚举类型，表示当前引导模式的状态。

以下是对每种模式的处理：

    case Guided_WP:
        // run position controller
        guided_pos_control_run();
        break;

如果当前模式是Guided_WP（即基于航点的位置控制模式），则调用guided_pos_control_run函数来运行位置控制回路。

    case Guided_Velocity:
        // run velocity controller
        guided_vel_control_run();
        break;

如果当前模式是Guided_Velocity（即基于速度的控制模式），则调用guided_vel_control_run函数来运行速度控制回路。

    case Guided_PosVel:
        // run position-velocity controller
        guided_posvel_control_run();
        break;

如果当前模式是Guided_PosVel（即同时控制位置和速度的模式），则调用guided_posvel_control_run函数来运行位置-速度控制回路。

    case Guided_Angle:
        // run angle controller
        guided_angle_control_run();
        break;

如果当前模式是Guided_Angle（即基于飞行角度的控制模式），则调用guided_angle_control_run函数来运行角度控制回路。
结束switch语句。

ModeGuided::guided_pos_control_run()

void ModeGuided::guided_pos_control_run()
{

声明guided_pos_control_run函数，没有参数和返回值。这个函数用于执行位置控制相关的逻辑。

    // if motors not enabled set throttle to zero and exit immediately
    if (!motors.armed()) {

检查电机是否已经启用（即是否已经解锁）。motors.armed()是一个检查电机是否上锁的函数，如果电机未上锁（即未启用），则执行以下代码块。

        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);

设置期望的电机状态为GROUND_IDLE，这意味着电机应该处于地面空闲状态，通常这会关闭电机。

        // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
        attitude_control->set_throttle_out(0,true,g.throttle_filt);

当电机未启用时，无人机的姿态控制器不进行稳定控制。这里，将油门设置为0（set_throttle_out(0, ...)），并且立即应用到输出（第二个参数true），使用g.throttle_filt指定的滤波器。

        attitude_control->relax_attitude_controllers();

放松（即重置）姿态控制器。这会清除PID控制器的积分项和其他相关状态，确保在下次启动时控制器从干净的状态开始。

        sub.wp_nav.wp_and_spline_init();

初始化航点导航和样条插值状态。这通常涉及重置航点导航器内部的状态，以便于下次使用。

        return;

如果电机未启用，则退出guided_pos_control_run函数，不执行后续的位置控制逻辑。

// process pilot's yaw input
float target_yaw_rate = 0;

声明一个浮点变量target_yaw_rate，用于存储飞行员期望的偏航速率，初始值为0。

if (!sub.failsafe.pilot_input) {

检查是否启用了飞行员的输入。sub.failsafe.pilot_input是一个标志，如果设置为true，则表示处于故障保护模式，飞行员的输入被忽略。如果不在故障保护模式（即!sub.failsafe.pilot_input为true），则处理飞行员的偏航输入。

    // get pilot's desired yaw rate
    target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

获取飞行员期望的偏航速率。channel_yaw->get_control_in()获取偏航通道的输入值（通常来自遥控器的偏航摇杆），然后sub.get_pilot_desired_yaw_rate()将该输入值转换为期望的偏航速率。

    if (!is_zero(target_yaw_rate)) {
        set_auto_yaw_mode(AUTO_YAW_HOLD);
    } else {

如果飞行员指定的偏航速率不是零（即飞行员正在请求偏航动作），则设置自动偏航模式为AUTO_YAW_HOLD，这意味着无人机将保持当前的偏航角度不变。
如果target_yaw_rate为零（即飞行员没有请求偏航动作），则检查当前的偏航控制模式。

        if (sub.yaw_rate_only){
            set_auto_yaw_mode(AUTO_YAW_RATE);
        } else {
            set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
        }
    }
}

如果sub.yaw_rate_only为true，表示仅使用偏航速率控制，因此设置自动偏航模式为AUTO_YAW_RATE。
如果sub.yaw_rate_only为false，表示使用偏航角度控制，因此设置自动偏航模式为AUTO_YAW_LOOK_AT_HEADING。

// set motors to full range
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

设置电机的期望油门状态为THROTTLE_UNLIMITED，这意味着允许电机输出全范围的油门值，不受限制。

// run waypoint controller
sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav());

更新航点导航器（wp_nav）的状态，并设置故障保护的地形状态。update_wpnav()函数负责计算当前位置与下一个航点之间的距离和方向，而failsafe_terrain_set_status则用于更新故障保护状态。

float lateral_out, forward_out;
sub.translate_wpnav_rp(lateral_out, forward_out);

将航点导航器的输出转换为侧向（lateral_out）和前进（forward_out）的移动指令。这些值将用于控制无人机在水平面的运动。

// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);

将计算出的侧向和前进指令发送到电机控制器，从而控制无人机在水平面的运动。

// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
position_control->update_z_controller();

更新垂直位置控制器。WP_Nav设置了垂直位置控制的目标，update_z_controller()函数负责根据这些目标计算并输出油门值，以控制无人机的高度。

// call attitude controller
if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {

根据当前的自动偏航模式调用态度控制器。以下是根据不同的偏航模式进行的控制：

    // roll & pitch & yaw rate from pilot
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);

如果偏航模式为AUTO_YAW_HOLD，则使用飞行员输入的横滚（roll）、俯仰（pitch）和偏航速率（yaw rate）来控制无人机的姿态。

} else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) {
    // roll, pitch from pilot, yaw & yaw_rate from auto_control
    target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
    attitude_control->input_euler_angle_roll_pitch_slew_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate);

如果偏航模式为AUTO_YAW_LOOK_AT_HEADING，则无人机会朝向特定的航向，这里使用飞行员的横滚和俯仰输入，而偏航角度和偏航速率由自动控制提供。

} else if (sub.auto_yaw_mode == AUTO_YAW_RATE) {
    // roll, pitch from pilot, yaw_rate from auto_control
    target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);

如果偏航模式为AUTO_YAW_RATE，则使用飞行员的横滚和俯仰输入，偏航速率由自动控制提供。

} else {
    // roll, pitch from pilot, yaw heading from auto_heading()
    attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}

如果是其他偏航模式，则使用飞行员的横滚和俯仰输入，偏航角度由get_auto_heading()函数提供。

ModeGuided::guided_vel_control_run()

// if motors not enabled set throttle to zero and exit immediately
if (!motors.armed()) {

检查电机是否已经启动（armed）。motors.armed()是一个检查电机是否已经解锁并准备运行的函数。如果没有启动，则执行以下代码。

    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);

设置电机的期望油门状态为GROUND_IDLE，这意味着电机应该处于地面待机状态，通常对应于油门最低，电机不产生推力。

    // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
    attitude_control->set_throttle_out(0,true,g.throttle_filt);

当电机未启动时，无人机的姿态控制器不会尝试稳定 roll/pitch/yaw。这里将油门输出设置为0，关闭推力输出。true表示这是一个直接设置油门输出的请求，不应被其他控制逻辑覆盖。g.throttle_filt是一个滤波器参数，用来平滑油门的变化。

    attitude_control->relax_attitude_controllers();

放松（relax）姿态控制器，这意味着姿态控制器将停止尝试维持当前姿态，允许无人机自由地回到中性位置。

    // initialise velocity controller
    position_control->init_z_controller();

初始化垂直（Z轴）速度控制器。在每次启动时，都需要对控制器进行初始化以确保其从正确的状态开始工作。

    position_control->init_xy_controller();

初始化水平（X轴和Y轴）速度控制器。与垂直控制器类似，这确保了水平速度控制器在开始控制无人机之前已经准备好。

    return;

如果电机未启动，则退出函数。这意味着在电机启动之前，不会执行任何进一步的速度控制逻辑。

// process pilot's yaw input
float target_yaw_rate = 0;

这行代码声明了一个名为target_yaw_rate的浮点变量，用于存储飞行员期望的偏航速率，初始值设为0。

if (!sub.failsafe.pilot_input) {

这是一个条件检查，sub.failsafe.pilot_input是一个标志，用来指示是否启用了飞行员的输入故障保护。如果故障保护未激活（即!sub.failsafe.pilot_input为真），则执行以下代码块。

    // get pilot's desired yaw rate
    target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

获取飞行员期望的偏航速率。channel_yaw->get_control_in()方法从遥控器的偏航通道获取当前的输入值（通常是一个介于-1到1之间的值），然后sub.get_pilot_desired_yaw_rate()函数将该输入值转换为无人机的偏航速率（通常是以度/秒为单位）。

    if (!is_zero(target_yaw_rate)) {
        set_auto_yaw_mode(AUTO_YAW_HOLD);
    } else {

检查飞行员是否请求了非零的偏航速率。如果target_yaw_rate不是零（即飞行员请求了偏航动作），则使用set_auto_yaw_mode(AUTO_YAW_HOLD)将偏航模式设置为AUTO_YAW_HOLD，这意味着无人机将保持当前的偏航角度不变。如果target_yaw_rate是零，则执行以下代码块。

        if (sub.yaw_rate_only){
            set_auto_yaw_mode(AUTO_YAW_RATE);
        } else {
            set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
        }
    }

如果飞行员没有请求偏航动作（target_yaw_rate为零），这段代码将根据sub.yaw_rate_only标志设置偏航模式。如果sub.yaw_rate_only为真，则偏航模式被设置为AUTO_YAW_RATE，无人机将以一个恒定的偏航速率飞行。如果sub.yaw_rate_only为假，则偏航模式被设置为AUTO_YAW_LOOK_AT_HEADING，通常这意味着无人机将尝试保持其朝向一个特定的航向

// set motors to full range
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

将电机的期望油门状态设置为THROTTLE_UNLIMITED，这意味着电机油门可以全范围变化，不受任何限制。这通常是在执行某些需要全速操作的飞行模式时使用。

// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis();
if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !position_control->get_vel_desired_cms().is_zero()) {
    position_control->set_vel_desired_cms(Vector3f(0,0,0));
}

这段代码用来检查是否在3秒内接收到位置或速度更新的时间戳。tnow是当前时间，update_time_ms是上次更新位置或速度的时间戳，GUIDED_POSVEL_TIMEOUT_MS是定义的超时时间（通常是3000毫秒，即3秒）。如果超过3秒没有收到更新并且期望的速度不是零，则将期望的速度设置为XYZ三个方向均为零的向量。

position_control->stop_pos_xy_stabilisation();

停止X轴和Y轴的位置稳定控制。这意味着无人机不会尝试维持或达到任何特定的XY平面位置，而是可能会进行速度控制或执行其他类型的移动。

// call velocity controller which includes z axis controller
position_control->update_xy_controller();

更新XY平面上的速度控制器。这个函数调用将处理水平方向的速度控制逻辑，包括计算必要的控制输出以实现期望的水平速度。

position_control->set_pos_target_z_from_climb_rate_cm(position_control->get_vel_desired_cms().z);

根据期望的垂直速度（爬升率）来设置Z轴的位置目标。这里的get_vel_desired_cms().z获取的是期望的垂直速度（以厘米/秒为单位），然后设置Z轴的位置目标以反映这一速度。

position_control->update_z_controller();

更新Z轴的控制器。这个函数调用处理垂直方向的速度控制逻辑，计算实现期望垂直速度所需的控制输出。

float lateral_out, forward_out;
sub.translate_pos_control_rp(lateral_out, forward_out);

这行代码声明了两个浮点变量lateral_out和forward_out，它们用来存储横向和向前的控制输出。translate_pos_control_rp函数（属于某个sub对象的方法）将位置控制器的输出转换为遥控器输入的形式，这样就可以对无人机的横向（roll）和前进（pitch）运动进行控制。

// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);

这两行代码将之前计算出的横向（lateral_out）和前进（forward_out）控制输出发送到电机。set_lateral和set_forward函数会根据这些值调整电机输出，从而控制无人机在水平面内的运动。

// call attitude controller
if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {

这行代码开始了一个条件判断，检查当前的自动偏航模式是否设置为AUTO_YAW_HOLD。如果是，以下代码块将被执行。

    // roll & pitch & yaw rate from pilot
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);

如果处于AUTO_YAW_HOLD模式，无人机会保持当前的偏航角度。此函数调用将飞行员对横滚（roll）和俯仰（pitch）的输入以及期望的偏航速率（target_yaw_rate）发送给姿态控制器。这意味着无人机会响应飞行员对横滚和俯仰的输入，并维持当前的偏航角度。

} else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) {

如果不是AUTO_YAW_HOLD模式，则检查是否为AUTO_YAW_LOOK_AT_HEADING模式。如果是，以下代码块将被执行。

    // roll, pitch from pilot, yaw & yaw_rate from auto_control
    target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
    attitude_control->input_euler_angle_roll_pitch_slew_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate);

在这种模式下，无人机试图将其偏航角度对准一个特定的航向。target_yaw_rate被设置为基于yaw_look_at_heading_slew的值（每秒允许的最大偏航角度变化率），将飞行员的横滚和俯仰输入以及自动控制计算的航向和偏航速率发送给姿态控制器。

} else if (sub.auto_yaw_mode == AUTO_YAW_RATE) {

如果不是上述两种模式，则检查是否为AUTO_YAW_RATE模式。如果是，以下代码块将被执行。

    // roll, pitch from pilot, yaw_rate from auto_control
    target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);

在这种模式下，无人机将以一个恒定的偏航速率飞行。与AUTO_YAW_LOOK_AT_HEADING类似，使用yaw_look_at_heading_slew计算target_yaw_rate，然后将飞行员的横滚和俯仰输入以及期望的偏航速率发送给姿态控制器。

} else {

如果以上模式都不是，则执行以下代码块。

    // roll, pitch from pilot, yaw heading from auto_heading()
    attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}

如果无人机处于其他偏航模式，此代码使用飞行员的横滚和俯仰输入以及通过get_auto_heading()函数获取的自动航向来控制姿态。最后一个参数true表示立即更新偏航角度。

ModeGuided::guided_posvel_control_run()

// if motors not enabled set throttle to zero and exit immediately
if (!motors.armed()) {

这行代码检查电机的解锁状态。motors.armed()是一个方法，用来检查电机是否已经被解锁（即是否已经准备好启动）。如果电机没有解锁，执行以下代码块。

    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);

将电机的期望油门状态设置为GROUND_IDLE，这意味着电机应该处于地面待机状态，通常对应于油门最低，电机不旋转。

    // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
    attitude_control->set_throttle_out(0,true,g.throttle_filt);

当电机未解锁时，无人机的横滚（roll）、俯仰（pitch）和偏航（yaw）通常不会进行稳定控制。这行代码将油门输出设置为0，并且立即应用这个值（第一个参数为0），true表示立即更新油门输出，g.throttle_filt是一个滤波器参数，用来平滑油门的变化。

    attitude_control->relax_attitude_controllers();

这行代码放松（或重置）姿态控制器。当无人机不进行飞行时，这可以确保姿态控制器的内部状态不会累积错误或过时的数据。

    // initialise velocity controller
    position_control->init_z_controller();

初始化Z轴（垂直方向）的速度控制器。在每次启动或重置控制器时，都需要进行这一步以确保控制器从一个已知的初始状态开始。

    position_control->init_xy_controller();

初始化XY平面（水平方向）的速度控制器。与初始化Z轴控制器类似，这是为了确保水平方向的速度控制从一个已知的初始状态开始。

    return;

如果电机未解锁，则退出guided_posvel_control_run方法。这意味着在电机未解锁的情况下，不会执行此方法中的后续代码。

// process pilot's yaw input
float target_yaw_rate = 0;

声明一个名为target_yaw_rate的变量，用来存储飞行员期望的偏航速率，初始值为0。

if (!sub.failsafe.pilot_input) {

检查失败保护（failsafe）状态，具体地，检查是否有飞行员的输入。如果没有触发飞行员输入的失败保护，执行以下代码块。

    // get pilot's desired yaw rate
    target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

从偏航通道获取飞行员的输入，并通过get_pilot_desired_yaw_rate函数转换成期望的偏航速率。

    if (!is_zero(target_yaw_rate)) {
        set_auto_yaw_mode(AUTO_YAW_HOLD);
    } else {
        if (sub.yaw_rate_only) {
            set_auto_yaw_mode(AUTO_YAW_RATE);
        } else {
            set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
        }
    }

如果飞行员指定的偏航速率不为零（即飞行员正在请求偏航动作），设置自动偏航模式为AUTO_YAW_HOLD，意味着保持当前的偏航角度。
如果飞行员的偏航输入为零，根据yaw_rate_only标志设置偏航模式。如果yaw_rate_only为真，则设置为AUTO_YAW_RATE模式（仅偏航速率控制），否则设置为AUTO_YAW_LOOK_AT_HEADING模式（朝向特定航向）。

// set motors to full range
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

将电机设置为全油门范围，允许电机根据需要提供全范围的推力。

// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis();

获取当前的时间（以毫秒为单位）。

if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) {
    posvel_vel_target_cms.zero();
}

检查自上次位置或速度更新以来是否已经超过了3秒钟（GUIDED_POSVEL_TIMEOUT_MS定义的超时时间）。如果是这样，并且当前的速度目标（posvel_vel_target_cms）不是零，则将速度目标重置为零。这是为了防止在失去控制信号的情况下无人机继续按最后接收到的速度指令移动。posvel_vel_target_cms.zero()将XYZ三个方向的速度目标都设置为0。

// advance position target using velocity target
posvel_pos_target_cm += (posvel_vel_target_cms * position_control->get_dt()).topostype();

根据速度目标（posvel_vel_target_cms）和位置控制器的时间步长（get_dt()，通常是从上一次更新到现在的毫秒数）来推进位置目标。这里，速度目标乘以时间步长得到在时间步长内移动的距离，然后将其加到当前的位置目标（posvel_pos_target_cm）上。.topostype()是一个转换函数，确保结果的单位正确。

// send position and velocity targets to position controller
position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());

将新的位置目标和速度目标发送给位置控制器的XY平面部分。posvel_pos_target_cm.xy()和posvel_vel_target_cms.xy()分别提供了XY平面上的位置和速度目标。第三个参数Vector2f()是一个空的加速度目标，意味着在此调用中没有指定加速度。

float pz = posvel_pos_target_cm.z;
position_control->input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0);
posvel_pos_target_cm.z = pz;

类似地，将Z轴的位置和速度目标发送给位置控制器的Z轴部分。这里，pz用于存储和传递Z轴的位置目标。第三个参数0表示没有指定Z轴的加速度目标。

// run position controller
position_control->update_xy_controller();

更新位置控制器的XY平面控制逻辑。这将计算所需的控制输入以使无人机移动到目标位置，并保持指定的速度。

position_control->update_z_controller();

更新位置控制器的Z轴控制逻辑。与XY平面更新类似，这将计算Z轴的控制输入。

float lateral_out, forward_out;
sub.translate_pos_control_rp(lateral_out, forward_out);

将位置控制器的输出转换为横向（lateral_out）和前进（forward_out）控制信号。具体的转换逻辑依赖于translate_pos_control_rp函数的实现，但通常涉及将位置控制器的输出映射到无人机的动力系统。

// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);

最后，将计算出的横向和前进控制信号发送到电机。这将实际改变电机的推力输出，使无人机按照位置控制器计算的路径移动。

if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {

检查当前的自动偏航模式是否设置为AUTO_YAW_HOLD。AUTO_YAW_HOLD模式通常意味着无人机会保持当前的偏航角度。

    // roll & pitch & yaw rate from pilot
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);

如果处于AUTO_YAW_HOLD模式，无人机的滚转和俯仰角度以及偏航速率将由飞行员的输入控制。这里，channel_roll->get_control_in()和channel_pitch->get_control_in()分别获取飞行员对滚转和俯仰通道的控制输入，target_yaw_rate是期望的偏航速率。这些值被传递给姿态控制器的input_euler_angle_roll_pitch_euler_rate_yaw方法。

} else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) {

如果当前的自动偏航模式是AUTO_YAW_LOOK_AT_HEADING，这意味着无人机将朝向指定的航向。

    // roll, pitch from pilot, yaw & yaw_rate from auto_control
    target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
    attitude_control->input_euler_angle_roll_pitch_slew_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate);

在此模式下，滚转和俯仰仍然由飞行员的输入控制，而偏航角度和偏航速率则由自动控制系统决定。sub.yaw_look_at_heading_slew是航向变化的速度限制，乘以100.0是单位转换。get_auto_heading()获取期望的航向角度。这些值被传递给input_euler_angle_roll_pitch_slew_yaw方法。

} else if (sub.auto_yaw_mode == AUTO_YAW_RATE) {

如果模式是AUTO_YAW_RATE，无人机将按照指定的偏航速率改变偏航角度。

    // roll, pitch from pilot, and yaw_rate from auto_control
    target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);

与AUTO_YAW_LOOK_AT_HEADING类似，滚转和俯仰由飞行员控制，偏航速率由自动控制系统决定。使用的方法是input_euler_angle_roll_pitch_euler_rate_yaw。

} else {

如果以上模式都不匹配，代码将执行以下默认分支。

    // roll, pitch from pilot, yaw heading from auto_heading()
    attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);

默认情况下，滚转和俯仰由飞行员的输入控制，偏航角度则由get_auto_heading()函数返回的自动航向决定。最后一个参数true表示立即更新偏航角度。

ModeGuided::guided_angle_control_run()

    if (!motors.armed()) {

检查 motors 对象的 armed() 方法返回值，如果返回 false（即电机未解锁），则执行以下代码块。

        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);

将电机的期望螺旋桨状态设置为 GROUND_IDLE（地面空闲），这意味着电机不会产生推力。

        // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
        attitude_control->set_throttle_out(0.0f,true,g.throttle_filt);

将油门输出设置为 0（没有推力），第一个参数是油门值（0.0f 表示零油门），第二个参数 true 表示立即更新油门输出，第三个参数 g.throttle_filt 是一个滤波器参数，用于平滑油门的变化。

        attitude_control->relax_attitude_controllers();

调用 relax_attitude_controllers 方法，放松（或重置）姿态控制器的状态，以便在下次启动时它们可以从一个已知的初始状态开始。

        // initialise velocity controller
        position_control->init_z_controller();

初始化 Z 轴的垂直速度控制器。这是为了在下次无人机启用时，确保垂直速度控制器可以从一个已知的初始状态开始工作。

        return;

立即退出 guided_angle_control_run 函数，因为电机未解锁，不需要执行任何后续的姿态或位置控制。

float roll_in = guided_angle_state.roll_cd;

从guided_angle_state结构体中获取期望的滚转角度，单位是千分度（centidegrees，cd）。roll_cd字段代表期望的滚转角度。

float pitch_in = guided_angle_state.pitch_cd;

同样，从guided_angle_state结构体中获取期望的俯仰角度，单位也是千分度（centidegrees，cd）。pitch_cd字段代表期望的俯仰角度。

float total_in = norm(roll_in, pitch_in);

计算滚转和俯仰角度的合成倾斜角度。这里的norm函数是一个自定义函数，用于计算两个向量（在这个上下文中是滚转和俯仰角度）的欧几里得范数，即它们构成直角三角形的斜边的长度。这个值total_in代表了无人机的总倾斜角度。

float angle_max = MIN(attitude_control->get_althold_lean_angle_max_cd(), sub.aparm.angle_max);

确定最大允许的倾斜角度。这里使用了MIN函数来从两个可能的限制值中选择较小的一个。attitude_control->get_althold_lean_angle_max_cd()获取的是姿态控制系统中定义的最大保持悬停的倾斜角度，而sub.aparm.angle_max代表无人机配置参数中设置的最大倾斜角度。

if (total_in > angle_max) {

如果计算出的总倾斜角度total_in超过了允许的最大倾斜角度angle_max，则需要执行以下代码块。

    float ratio = angle_max / total_in;
    roll_in *= ratio;
    pitch_in *= ratio;

如果总倾斜角度超出了限制，计算一个比例因子ratio，这个因子是最大允许倾斜角度与当前总倾斜角度的比值。然后将当前的滚转和俯仰角度乘以这个比例因子，这样就可以按比例减小这两个角度，使得它们的合成倾斜角度等于最大允许的倾斜角度。通过这种方式，代码确保无人机的操作不会超出安全或性能的倾斜角度限制。

// wrap yaw request
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);

这一行代码将偏航角度（yaw）请求限制在±180度范围内。wrap_180_cd函数用于将角度值转换为在-180度到180度之间的等效角度。避免角度值在360度附近跳变，导致无人机的突然转向。

// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up());

此处限制爬升率（climb rate），确保它在安全范围内。constrain_float函数将guided_angle_state.climb_rate_cms（期望的爬升率）限制在由sub.wp_nav.get_default_speed_down()（默认下降速度）和sub.wp_nav.get_default_speed_up()（默认上升速度）定义的范围内。

// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis();
if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
    roll_in = 0.0f;
    pitch_in = 0.0f;
    climb_rate_cms = 0.0f;
}

这段代码检查是否有来自用户的输入超时。如果自上次更新以来已经超过了预定的超时时间（GUIDED_ATTITUDE_TIMEOUT_MS，通常设置为3秒），则将滚转（roll）、俯仰（pitch）角度和爬升率设置为0。

// set motors to full range
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

设置电机到全范围工作状态。这意味着允许电机使用全范围的油门值，THROTTLE_UNLIMITED表明没有油门限制。

// call attitude controller
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);

调用姿态控制器，传入滚转、俯仰和偏航角度，以及一个布尔值（在这里是true），这通常表示请求立即更新姿态。姿态控制器负责调整飞行器的姿态以匹配这些输入的角度。

// call position controller
position_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms);
position_control->update_z_controller();

最后，调用位置控制器来处理垂直（Z轴）运动。首先设置Z轴位置目标基于爬升率，然后更新Z轴控制器以调整飞行器的高度。

ModeGuided::guided_limit_clear()

void ModeGuided::guided_limit_clear()
{

函数声明，指定了函数的返回类型为void（无返回值），并且它是ModeGuided类的一个成员函数。函数名guided_limit_clear表明其功能是清除某些限制。

    guided_limit.timeout_ms = 0;

将guided_limit结构体中的timeout_ms字段设置为0。timeout_ms字段表示一个时间限制（例如，在多少毫秒后限制将生效），将其设置为0意味着禁用超时限制。

    guided_limit.alt_min_cm = 0.0f;

将guided_limit结构体中的alt_min_cm字段设置为0.0f。alt_min_cm字段代表最小高度限制，单位是厘米。将其设置为0意味着移除最小高度限制。

    guided_limit.alt_max_cm = 0.0f;

将guided_limit结构体中的alt_max_cm字段设置为0.0f。alt_max_cm字段代表最大高度限制，单位也是厘米。将其设置为0意味着移除最大高度限制。

    guided_limit.horiz_max_cm = 0.0f;

将guided_limit结构体中的horiz_max_cm字段设置为0.0f。horiz_max_cm字段代表水平距离的最大限制，单位是厘米。将其设置为0意味着移除水平距离限制。

ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode)

// set_auto_yaw_mode - sets the yaw mode for auto
void ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode)
{

这是函数的声明和注释。它表明函数set_auto_yaw_mode用于设置自动模式下的偏航模式，并且接受一个autopilot_yaw_mode类型的参数yaw_mode，这个参数指定了要设置的偏航模式。

    // return immediately if no change
    if (sub.auto_yaw_mode == yaw_mode) {
        return;
    }

如果当前的偏航模式sub.auto_yaw_mode与请求设置的偏航模式yaw_mode相同，则函数会立即返回，因为没有必要进行更改。

    sub.auto_yaw_mode = yaw_mode;

将当前的偏航模式设置为传入的yaw_mode。

    // perform initialisation
    switch (sub.auto_yaw_mode) {

以下是一个switch语句，用于根据新的偏航模式执行必要的初始化。

    case AUTO_YAW_HOLD:
        // pilot controls the heading
        break;

如果偏航模式是AUTO_YAW_HOLD，则飞行员控制航向。在这种情况下，不需要特殊的初始化，所以代码只是跳出switch语句。

    case AUTO_YAW_LOOK_AT_NEXT_WP:
        // wpnav will initialise heading when wpnav's set_destination method is called
        break;

如果是AUTO_YAW_LOOK_AT_NEXT_WP模式，无人机将朝向下一个航点。初始化将在设置目的地时由航点导航器（wpnav）处理。

    case AUTO_YAW_ROI:
        // point towards a location held in yaw_look_at_WP
        sub.yaw_look_at_WP_bearing = ahrs.yaw_sensor;
        break;

在AUTO_YAW_ROI模式下，无人机将朝向一个特定的位置。这里将yaw_look_at_WP_bearing设置为当前的偏航传感器值。

    case AUTO_YAW_LOOK_AT_HEADING:
        // keep heading pointing in the direction held in yaw_look_at_heading
        // caller should set the yaw_look_at_heading
        break;

如果是AUTO_YAW_LOOK_AT_HEADING模式，则保持航向指向yaw_look_at_heading指定的方向。调用者应负责设置这个值。

    case AUTO_YAW_LOOK_AHEAD:
        // Commanded Yaw to automatically look ahead.
        sub.yaw_look_ahead_bearing = ahrs.yaw_sensor;
        break;

AUTO_YAW_LOOK_AHEAD模式下，无人机将自动朝前看。这里将yaw_look_ahead_bearing设置为当前的偏航传感器值。

    case AUTO_YAW_RESETTOARMEDYAW:
        // initial_armed_bearing will be set during arming so no init required
        break;

如果是AUTO_YAW_RESETTOARMEDYAW模式，当无人机上臂时，将重置偏航到上臂时的偏航角度。因为初始化在上臂时已完成，这里不需要额外的操作。

    case AUTO_YAW_RATE:
        // set target yaw rate to yaw_look_at_heading_slew
        break;

在AUTO_YAW_RATE模式下，将目标偏航速率设置为yaw_look_at_heading_slew。

ModeGuided::get_auto_heading()

// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float ModeGuided::get_auto_heading()
{

这是函数的注释和声明。注释说明了函数的用途和更新频率，声明指出了函数返回一个float类型的值，表示目标航向。

    switch (sub.auto_yaw_mode) {

使用switch语句根据当前的自动偏航模式选择相应的代码块来执行。

    case AUTO_YAW_ROI:
        // point towards a location held in roi_WP
        return sub.get_roi_yaw();
        break;

如果偏航模式是AUTO_YAW_ROI，则函数返回一个朝向在roi_WP中存储的位置的航向。

    case AUTO_YAW_LOOK_AT_HEADING:
        // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
        return sub.yaw_look_at_heading;
        break;

在AUTO_YAW_LOOK_AT_HEADING模式下，返回一个保持指向yaw_look_at_heading方向（一个预先设定的航向）的航向值。

    case AUTO_YAW_LOOK_AHEAD:
        // Commanded Yaw to automatically look ahead.
        return sub.get_look_ahead_yaw();
        break;

对于AUTO_YAW_LOOK_AHEAD模式，返回一个朝向前方的航向值

    case AUTO_YAW_RESETTOARMEDYAW:
        // changes yaw to be same as when quad was armed
        return sub.initial_armed_bearing;
        break;

在AUTO_YAW_RESETTOARMEDYAW模式下，返回一个值，使得偏航角度与无人机上臂时相同。

    case AUTO_YAW_CORRECT_XTRACK: {
    // TODO return current yaw if not in appropriate mode

这是一个待办事项注释，表明如果无人机不在适当模式下，可能需要返回当前的偏航角度。

    // Bearing of current track (centidegrees)
    float track_bearing = get_bearing_cd(sub.wp_nav.get_wp_origin().xy(), sub.wp_nav.get_wp_destination().xy());

计算当前航迹的航向（即从当前航点指向下一个航点的方向），结果以“百分度”（centidegrees，即度的100倍）为单位。get_bearing_cd是一个函数，它接收两个点的坐标（起点和终点），并返回它们之间的航向。

    // Bearing from current position towards intermediate position target (centidegrees)
    const Vector2f target_vel_xy = position_control->get_vel_target_cms().xy();
    float angle_error = 0.0f;

获取期望的速度向量（在水平平面上的分量），这表示了无人机应当朝向的中间目标位置。同时，声明一个变量angle_error用于存储后续计算的角度误差。

    if (target_vel_xy.length() >= position_control->get_max_speed_xy_cms() * 0.1f) {

检查期望速度向量的长度是否至少达到最大水平速度的10%。这是为了确保在进行角度误差计算之前，速度向量有足够的长度，避免在低速时进行不必要的校正。

        const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f;
        angle_error = wrap_180_cd(desired_angle_cd - track_bearing);

如果速度向量足够长，计算期望的角度（基于速度向量的方向），将其转换为百分度，并计算期望角度与当前航迹之间的角度误差。wrap_180_cd函数确保角度误差在±180度范围内。

    }
    float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f);

将计算出的角度误差限制在预定的范围内。constrain_float函数确保角度误差不会超过横向角度限制g.xtrack_angle_limit（同样以百分度为单位）。

    return wrap_360_cd(track_bearing + angle_limited);

最后，将限制后的角度误差加到当前航迹的航向上，并使用wrap_360_cd函数确保结果在0至360度范围内，然后返回这个值作为目标航向。

    case AUTO_YAW_LOOK_AT_NEXT_WP:
    default:
        // point towards next waypoint.
        // we don't use wp_bearing because we don't want the vehicle to turn too much during flight
        return sub.wp_nav.get_yaw();
        break;

对于AUTO_YAW_LOOK_AT_NEXT_WP或任何未明确列出的模式（由于default子句），返回一个指向下一个航点的航向值。这里使用wp_nav.get_yaw()而不是wp_bearing来避免在飞行过程中过度转向。

ModeGuided::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)

void ModeGuided::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
{

这是函数的声明。它接受四个参数：

timeout_ms: 超时时间，以毫秒为单位。
alt_min_cm: 最小高度限制，以厘米为单位。
alt_max_cm: 最大高度限制，以厘米为单位。
horiz_max_cm: 水平移动的最大距离限制，以厘米为单位。

函数没有返回值（void 类型）。

    guided_limit.timeout_ms = timeout_ms;

将传入的 timeout_ms 参数赋值给 guided_limit 结构体的 timeout_ms 成员变量，该变量用于存储超时时间。

    guided_limit.alt_min_cm = alt_min_cm;

将传入的 alt_min_cm 参数赋值给 guided_limit 结构体的 alt_min_cm 成员变量，该变量用于存储最小高度限制。

    guided_limit.alt_max_cm = alt_max_cm;

将传入的 alt_max_cm 参数赋值给 guided_limit 结构体的 alt_max_cm 成员变量，该变量用于存储最大高度限制。

    guided_limit.horiz_max_cm = horiz_max_cm;

将传入的 horiz_max_cm 参数赋值给 guided_limit 结构体的 horiz_max_cm 成员变量，该变量用于存储水平移动的最大距离限制。

ModeGuided::guided_limit_init_time_and_pos()

// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
//  only called from AUTO mode's auto_nav_guided_start function

函数的注释，函数的用途是初始化引导模式下的起始时间和位置，这些值用于后续的运动限制检查。这个函数只会在自动模式（AUTO mode）的 auto_nav_guided_start 函数中被调用。

void ModeGuided::guided_limit_init_time_and_pos()
{

    // initialise start time
    guided_limit.start_time = AP_HAL::millis();

初始化 guided_limit 结构体的 start_time 成员变量。AP_HAL::millis() 是一个函数调用，返回自系统启动以来经过的毫秒数。这设置了引导模式操作的起始时间点。

    // initialise start position from current position
    guided_limit.start_pos = inertial_nav.get_position_neu_cm();

初始化 guided_limit 结构体的 start_pos 成员变量。inertial_nav.get_position_neu_cm() 是一个函数调用，返回无人机的当前位置，包括北向（N）、东向（E）和垂直（U）分量，单位是厘米。这设置了引导模式操作的起始位置，用作后续位置限制检查的参考点。

ModeGuided::guided_limit_check()

// guided_limit_check - returns true if guided mode has breached a limit
//  used when guided is invoked from the NAV_GUIDED_ENABLE mission command

bool ModeGuided::guided_limit_check()
{

这是函数的声明，它不接受任何参数，并返回一个布尔值（bool），表示是否违反了限制。

    // check if we have passed the timeout
    if ((guided_limit.timeout_ms > 0) && (AP_HAL::millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {
        return true;
    }

检查是否已经超过了设定的超时时间。如果 guided_limit.timeout_ms 大于0（表示超时限制已设置），并且当前时间与起始时间的差值大于或等于超时时间，则返回 true 表示超时。

    // get current location
    const Vector3f& curr_pos = inertial_nav.get_position_neu_cm();

获取无人机的当前位置，存储在 curr_pos 变量中。位置包括北向、东向和垂直分量，单位是厘米。

    // check if we have gone below min alt
    if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) {
        return true;
    }

检查是否低于最小高度限制。如果 guided_limit.alt_min_cm 不为0（表示最小高度限制已设置），并且当前高度 curr_pos.z 小于最小高度限制，则返回 true。

    // check if we have gone above max alt
    if (!is_zero(guided_limit.alt_max_cm) && (curr_pos.z > guided_limit.alt_max_cm)) {
        return true;
    }

检查是否超过最大高度限制。如果 guided_limit.alt_max_cm 不为0（表示最大高度限制已设置），并且当前高度 curr_pos.z 大于最大高度限制，则返回 true。

    // check if we have gone beyond horizontal limit
    if (guided_limit.horiz_max_cm > 0.0f) {
        const float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos.xy(), curr_pos.xy());
        if (horiz_move > guided_limit.horiz_max_cm) {
            return true;
        }
    }

检查是否超过水平移动距离限制。如果 guided_limit.horiz_max_cm 大于0（表示水平移动距离限制已设置），则计算从起始位置到当前位置的水平距离，并检查是否超过了限制。如果是，则返回 true。

    // if we got this far we must be within limits
    return false;

如果以上所有的检查都没有返回 true，则表示没有违反任何限制，因此返回 false。

标签：control,set,sub,guided,Ardusub,rate,源码,yaw
From： https://blog.csdn.net/weixin_45813121/article/details/144077190