自学教程：C++ update_simple_mode函数代码示例

// loiter_init - initialise loiter controllerbool Copter::ModeLoiter::init(bool ignore_checks){    if (!copter.failsafe.radio) {        float target_roll, target_pitch;        // apply SIMPLE mode transform to pilot inputs        update_simple_mode();        // convert pilot input to lean angles        get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());        // process pilot's roll and pitch input        loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);    } else {        // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason        loiter_nav->clear_pilot_desired_acceleration();    }    loiter_nav->init_target();    // initialise position and desired velocity    if (!pos_control->is_active_z()) {        pos_control->set_alt_target_to_current_alt();        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());    }    return true;}

// land_nogps_run - runs the land controller//      pilot controls roll and pitch angles//      should be called at 100hz or morevoid Copter::ModeLand::nogps_run(){    float target_roll = 0.0f, target_pitch = 0.0f;    float target_yaw_rate = 0;    // process pilot inputs    if (!copter.failsafe.radio) {        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);            // exit land if throttle is high            copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);        }        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // get pilot desired lean angles            get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());    }    // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately    if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);        attitude_control->set_throttle_out(0,false,g.throttle_filt);#else        motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        // multicopters do not stabilize roll/pitch/yaw when disarmed        attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif        // disarm when the landing detector says we've landed        if (ap.land_complete) {            copter.init_disarm_motors();        }        return;    }    // set motors to full range    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // call attitude controller    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);    // pause before beginning land descent    if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {        land_pause = false;    }    land_run_vertical_control(land_pause);}

// land_nogps_run - runs the land controller//      pilot controls roll and pitch angles//      should be called at 100hz or morevoid Sub::land_nogps_run(){    float target_roll = 0.0f, target_pitch = 0.0f;    float target_yaw_rate = 0;    // process pilot inputs    if (!failsafe.radio) {        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // get pilot desired lean angles            get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    }    // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately    if(!ap.auto_armed || ap.at_surface || !motors.get_interlock()) {    	// multicopters do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED        // disarm when the landing detector says we've landed and throttle is at minimum        if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {            init_disarm_motors();        }#else        // disarm when the landing detector says we've landed        if (ap.at_surface) {        	set_mode(ALT_HOLD);        }#endif        return;    }    // call attitude controller    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());    // pause 4 seconds before beginning land descent    float cmb_rate;    if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) {        cmb_rate = 0;    } else {        land_pause = false;        cmb_rate = get_land_descent_speed();    }    // record desired climb rate for logging    desired_climb_rate = cmb_rate;    // call position controller    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);    pos_control.update_z_controller();}

// auto_land_run - lands in auto mode//      called by auto_run at 100hz or morevoid Copter::auto_land_run(){    int16_t roll_control = 0, pitch_control = 0;    float target_yaw_rate = 0;    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately    if(!ap.auto_armed || ap.land_complete) {#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else   // Multirotors do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif        // set target to current position        wp_nav.init_loiter_target();        return;    }    // relax loiter targets if we might be landed    if (ap.land_complete_maybe) {        wp_nav.loiter_soften_for_landing();    }    // process pilot's input    if (!failsafe.radio) {        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // process pilot's roll and pitch input            roll_control = channel_roll->control_in;            pitch_control = channel_pitch->control_in;        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    }    // process roll, pitch inputs    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);    // run loiter controller    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);    // call z-axis position controller    float cmb_rate = get_land_descent_speed();    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);    pos_control.update_z_controller();    // record desired climb rate for logging    desired_climb_rate = cmb_rate;    // roll & pitch from waypoint controller, yaw rate from pilot    attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);}

// rtl_descent_run - implements the final descent to the RTL_ALT//      called by rtl_run at 100hz or morevoid Copter::rtl_descent_run(){    int16_t roll_control = 0, pitch_control = 0;    float target_yaw_rate = 0;    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately    if(!ap.auto_armed || !motors.get_interlock()) {#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else   // multicopters do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif        // set target to current position        wp_nav.init_loiter_target();        return;    }    // process pilot's input    if (!failsafe.radio) {        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // process pilot's roll and pitch input            roll_control = channel_roll->control_in;            pitch_control = channel_pitch->control_in;        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    }    // process roll, pitch inputs    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);    // run loiter controller    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);    // call z-axis position controller    pos_control.set_alt_target_with_slew(pv_alt_above_origin(g.rtl_alt_final), G_Dt);    pos_control.update_z_controller();    // roll & pitch from waypoint controller, yaw rate from pilot    attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);    // check if we've reached within 20cm of final altitude    rtl_state_complete = fabsf(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f;}

// manual_control - process manual controlvoid Copter::ModeZigZag::manual_control(){    float target_yaw_rate = 0.0f;    float target_climb_rate = 0.0f;    // process pilot inputs unless we are in radio failsafe    if (!copter.failsafe.radio) {        float target_roll, target_pitch;        // apply SIMPLE mode transform to pilot inputs        update_simple_mode();        // convert pilot input to lean angles        get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());        // process pilot's roll and pitch input        loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());        // get pilot desired climb rate        target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());        // make sure the climb rate is in the given range, prevent floating point errors        target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);    } else {        // clear out pilot desired acceleration in case radio failsafe event occurs and we        // do not switch to RTL for some reason        loiter_nav->clear_pilot_desired_acceleration();    }    // set motors to full range    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // run loiter controller    loiter_nav->update();    // call attitude controller    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);    // adjust climb rate using rangefinder    target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);    // get avoidance adjusted climb rate    target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);    // update altitude target and call position controller    pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);    // adjusts target up or down using a climb rate    pos_control->update_z_controller();}

// stabilize_run - runs the main stabilize controller// should be called at 100hz or morevoid Copter::heli_stabilize_run(){    float target_roll, target_pitch;    float target_yaw_rate;    int16_t pilot_throttle_scaled;    // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because    // we may be in autorotation flight.  These should be reset only when transitioning from disarmed    // to armed, because the pilot will have placed the helicopter down on the landing pad.  This is so    // that the servos move in a realistic fashion while disarmed for operational checks.    // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move        if(!motors.armed()) {        heli_flags.init_targets_on_arming=true;        attitude_control.set_yaw_target_to_current_heading();    }        if(motors.armed() && heli_flags.init_targets_on_arming) {        attitude_control.relax_bf_rate_controller();        attitude_control.set_yaw_target_to_current_heading();        if (motors.rotor_speed_above_critical()) {            heli_flags.init_targets_on_arming=false;        }    }    // send RC inputs direct into motors library for use during manual passthrough for helicopter setup    heli_radio_passthrough();    // apply SIMPLE mode transform to pilot inputs    update_simple_mode();    // convert pilot input to lean angles    // To-Do: convert get_pilot_desired_lean_angles to return angles as floats    get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);    // get pilot's desired yaw rate    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    // get pilot's desired throttle    pilot_throttle_scaled = get_pilot_desired_collective(channel_throttle->control_in);    // call attitude controller    attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());    // output pilot's throttle - note that TradHeli does not used angle-boost    attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);}

// stabilize_run - runs the main stabilize controller// should be called at 100hz or morevoid Copter::stabilize_run(){    float target_roll, target_pitch;    float target_yaw_rate;    int16_t pilot_throttle_scaled;    // if not armed or throttle at zero, set throttle to zero and exit immediately    if(!motors.armed() || ap.throttle_zero) {        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);        // slow start if landed        if (ap.land_complete) {            motors.slow_start(true);        }        return;    }    // apply SIMPLE mode transform to pilot inputs    update_simple_mode();    // convert pilot input to lean angles    // To-Do: convert get_pilot_desired_lean_angles to return angles as floats    get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);    // get pilot's desired yaw rate    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    // get pilot's desired throttle    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in);    // call attitude controller    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());    // body-frame rate controller is run directly from 100hz loop    // output pilot's throttle    attitude_control.set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);}

// stabilize_run - runs the main stabilize controller// should be called at 100hz or morevoid Copter::ModeStabilize::run(){    float target_roll, target_pitch;    float target_yaw_rate;    float pilot_throttle_scaled;    // if not armed set throttle to zero and exit immediately    if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {        zero_throttle_and_relax_ac();        return;    }    // clear landing flag    set_land_complete(false);    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // apply SIMPLE mode transform to pilot inputs    update_simple_mode();    // convert pilot input to lean angles    get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);    // get pilot's desired yaw rate    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());    // get pilot's desired throttle    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());    // call attitude controller    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);    // body-frame rate controller is run directly from 100hz loop    // output pilot's throttle    attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);}

// sport_run - runs the sport controller// should be called at 100hz or morevoid Copter::sport_run(){    float target_roll_rate, target_pitch_rate, target_yaw_rate;    float target_climb_rate = 0;    float takeoff_climb_rate = 0.0f;    // initialize vertical speed and acceleration    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);    pos_control.set_accel_z(g.pilot_accel_z);    // if not armed or throttle at zero, set throttle to zero and exit immediately    if (!motors.armed() || ap.throttle_zero || !motors.get_interlock()) {        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);        return;    }    // apply SIMPLE mode transform    update_simple_mode();    // get pilot's desired roll and pitch rates    // calculate rate requests    target_roll_rate = channel_roll->control_in * g.acro_rp_p;    target_pitch_rate = channel_pitch->control_in * g.acro_rp_p;    int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);    target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;    // Calculate trainer mode earth frame rate command for pitch    int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);    target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;    if (roll_angle > aparm.angle_max){        target_roll_rate -=  g.acro_rp_p*(roll_angle-aparm.angle_max);    }else if (roll_angle < -aparm.angle_max) {        target_roll_rate -=  g.acro_rp_p*(roll_angle+aparm.angle_max);    }    if (pitch_angle > aparm.angle_max){        target_pitch_rate -=  g.acro_rp_p*(pitch_angle-aparm.angle_max);    }else if (pitch_angle < -aparm.angle_max) {        target_pitch_rate -=  g.acro_rp_p*(pitch_angle+aparm.angle_max);    }    // get pilot's desired yaw rate    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    // get pilot desired climb rate    target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);    // get takeoff adjusted pilot and takeoff climb rates    takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);    // check for take-off    if (ap.land_complete && (takeoff_state.running || (channel_throttle->control_in > get_takeoff_trigger_throttle()))) {        if (!takeoff_state.running) {            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));        }        // indicate we are taking off        set_land_complete(false);        // clear i term when we're taking off        set_throttle_takeoff();    }    // reset target lean angles and heading while landed    if (ap.land_complete) {        if (ap.throttle_zero) {            motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        }else{            motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);        }        // move throttle to between minimum and non-takeoff-throttle to keep us on the ground        attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);    }else{        // set motors to full range        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);        // call attitude controller        attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);        // call throttle controller        if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {            // if sonar is ok, use surface tracking            target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);        }        // call position controller        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);        pos_control.update_z_controller();    }}

void Copter::Mode::land_run_horizontal_control(){    LowPassFilterFloat &rc_throttle_control_in_filter = copter.rc_throttle_control_in_filter;    AP_Vehicle::MultiCopter &aparm = copter.aparm;    float target_roll = 0.0f;    float target_pitch = 0.0f;    float target_yaw_rate = 0;    // relax loiter target if we might be landed    if (ap.land_complete_maybe) {        loiter_nav->soften_for_landing();    }    // process pilot inputs    if (!copter.failsafe.radio) {        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){            copter.Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);            // exit land if throttle is high            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);            }        }        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // convert pilot input to lean angles            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());            // record if pilot has overriden roll or pitch            if (!is_zero(target_roll) || !is_zero(target_pitch)) {                ap.land_repo_active = true;            }        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());    }#if PRECISION_LANDING == ENABLED    AC_PrecLand &precland = copter.precland;    bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired();    // run precision landing    if (doing_precision_landing) {        Vector2f target_pos, target_vel_rel;        if (!precland.get_target_position_cm(target_pos)) {            target_pos.x = inertial_nav.get_position().x;            target_pos.y = inertial_nav.get_position().y;        }        if (!precland.get_target_velocity_relative_cms(target_vel_rel)) {            target_vel_rel.x = -inertial_nav.get_velocity().x;            target_vel_rel.y = -inertial_nav.get_velocity().y;        }        pos_control->set_xy_target(target_pos.x, target_pos.y);        pos_control->override_vehicle_velocity_xy(-target_vel_rel);    }#endif    // process roll, pitch inputs    loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);    // run loiter controller    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);    int32_t nav_roll  = loiter_nav->get_roll();    int32_t nav_pitch = loiter_nav->get_pitch();    if (g2.wp_navalt_min > 0) {        // user has requested an altitude below which navigation        // attitude is limited. This is used to prevent commanded roll        // over on landing, which particularly affects helicopters if        // there is any position estimate drift after touchdown. We        // limit attitude to 7 degrees below this limit and linearly        // interpolate for 1m above that        const int alt_above_ground = get_alt_above_ground();        float attitude_limit_cd = linear_interpolate(700, aparm.angle_max, alt_above_ground,                                                     g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U);        float total_angle_cd = norm(nav_roll, nav_pitch);        if (total_angle_cd > attitude_limit_cd) {            float ratio = attitude_limit_cd / total_angle_cd;            nav_roll *= ratio;            nav_pitch *= ratio;            // tell position controller we are applying an external limit            pos_control->set_limit_accel_xy();        }    }    // call attitude controller    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate);}

// rtl_descent_run - implements the final descent to the RTL_ALT//      called by rtl_run at 100hz or morevoid Copter::ModeRTL::descent_run(){    float target_roll = 0.0f;    float target_pitch = 0.0f;    float target_yaw_rate = 0.0f;    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately    if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {        zero_throttle_and_relax_ac();        // set target to current position        loiter_nav->clear_pilot_desired_acceleration();        loiter_nav->init_target();        return;    }    // process pilot's input    if (!copter.failsafe.radio) {        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);            // exit land if throttle is high            if (!copter.set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {                copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);            }        }        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // convert pilot input to lean angles            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());            // record if pilot has overriden roll or pitch            if (!is_zero(target_roll) || !is_zero(target_pitch)) {                if (!ap.land_repo_active) {                    copter.Log_Write_Event(DATA_LAND_REPO_ACTIVE);                }                ap.land_repo_active = true;            }        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());    }    // set motors to full range    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // process roll, pitch inputs    loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);    // run loiter controller    loiter_nav->update();    // call z-axis position controller    pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);    pos_control->update_z_controller();    // roll & pitch from waypoint controller, yaw rate from pilot    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);    // check if we've reached within 20cm of final altitude    _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;}

// land_run - runs the land controller//      horizontal position controlled with loiter controller//      should be called at 100hz or morevoid Copter::land_gps_run(){    int16_t roll_control = 0, pitch_control = 0;    float target_yaw_rate = 0;    // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately    if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else   // Multirotors do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif        wp_nav.init_loiter_target();#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED        // disarm when the landing detector says we've landed and throttle is at minimum        if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {            init_disarm_motors();        }#else        // disarm when the landing detector says we've landed        if (ap.land_complete) {            init_disarm_motors();        }#endif        return;    }    // relax loiter target if we might be landed    if (ap.land_complete_maybe) {        wp_nav.loiter_soften_for_landing();    }    // process pilot inputs    if (!failsafe.radio) {        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // process pilot's roll and pitch input            roll_control = channel_roll->control_in;            pitch_control = channel_pitch->control_in;        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    }    // process roll, pitch inputs    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);    // run loiter controller    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);    // call attitude controller    attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);    // pause 4 seconds before beginning land descent    float cmb_rate;    if(land_pause && millis()-land_start_time < 4000) {        cmb_rate = 0;    } else {        land_pause = false;        cmb_rate = get_land_descent_speed();    }    // record desired climb rate for logging    desired_climb_rate = cmb_rate;    // update altitude target and call position controller    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);    pos_control.update_z_controller();}

// althold_run - runs the althold controller// should be called at 100hz or morevoid Copter::althold_run(){    AltHoldModeState althold_state;    float takeoff_climb_rate = 0.0f;    // initialize vertical speeds and acceleration    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);    pos_control.set_accel_z(g.pilot_accel_z);    // apply SIMPLE mode transform to pilot inputs    update_simple_mode();    // get pilot desired lean angles    float target_roll, target_pitch;    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());    // get pilot's desired yaw rate    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());    // get pilot desired climb rate    float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);#if FRAME_CONFIG == HELI_FRAME    // helicopters are held on the ground until rotor speed runup has finished    bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete());#else    bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);#endif    // Alt Hold State Machine Determination    if (!motors.armed() || !motors.get_interlock()) {        althold_state = AltHold_MotorStopped;    } else if (takeoff_state.running || takeoff_triggered) {        althold_state = AltHold_Takeoff;    } else if (!ap.auto_armed || ap.land_complete) {        althold_state = AltHold_Landed;    } else {        althold_state = AltHold_Flying;    }    // Alt Hold State Machine    switch (althold_state) {    case AltHold_MotorStopped:        motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());#if FRAME_CONFIG == HELI_FRAME            // force descent rate and call position controller        pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);#else        attitude_control.reset_rate_controller_I_terms();        attitude_control.set_yaw_target_to_current_heading();        pos_control.relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero#endif        pos_control.update_z_controller();        break;    case AltHold_Takeoff:        // set motors to full range        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);        // initiate take-off        if (!takeoff_state.running) {            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));            // indicate we are taking off            set_land_complete(false);            // clear i terms            set_throttle_takeoff();        }        // get take-off adjusted pilot and takeoff climb rates        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);        // call attitude controller        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        // call position controller        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);        pos_control.update_z_controller();        break;    case AltHold_Landed:        // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)        if (target_climb_rate < 0.0f) {            motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        } else {            motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);        }        attitude_control.reset_rate_controller_I_terms();        attitude_control.set_yaw_target_to_current_heading();        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        pos_control.relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero        pos_control.update_z_controller();        break;//.........这里部分代码省略.........

// loiter_run - runs the loiter controller// should be called at 100hz or morevoid Copter::loiter_run(){    LoiterModeState loiter_state;    float target_yaw_rate = 0.0f;    float target_climb_rate = 0.0f;    float takeoff_climb_rate = 0.0f;    // process pilot inputs unless we are in radio failsafe    if (!failsafe.radio) {        // apply SIMPLE mode transform to pilot inputs        update_simple_mode();        // process pilot's roll and pitch input        wp_nav.set_pilot_desired_acceleration(channel_roll->control_in, channel_pitch->control_in);        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);        // get pilot desired climb rate        target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);        target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);    } else {        // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason        wp_nav.clear_pilot_desired_acceleration();    }    // relax loiter target if we might be landed    if (ap.land_complete_maybe) {        wp_nav.loiter_soften_for_landing();    }    // Loiter State Machine Determination    if(!ap.auto_armed || !motors.get_interlock()) {        loiter_state = Loiter_Disarmed;    } else if (takeoff_state.running || (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()))) {        loiter_state = Loiter_Takeoff;    } else if (ap.land_complete) {        loiter_state = Loiter_Landed;    } else {        loiter_state = Loiter_Flying;    }    // Loiter State Machine    switch (loiter_state) {    case Loiter_Disarmed:        wp_nav.init_loiter_target();#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else   // multicopters do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif  // HELI_FRAME        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);        break;    case Loiter_Takeoff:        if (!takeoff_state.running) {            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));            // indicate we are taking off            set_land_complete(false);            // clear i term when we're taking off            set_throttle_takeoff();        }        // get takeoff adjusted pilot and takeoff climb rates        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);        // run loiter controller        wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);        // update altitude target and call position controller        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);        pos_control.update_z_controller();        break;    case Loiter_Landed:        wp_nav.init_loiter_target();#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());        attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);#else   // multicopters do not stabilize roll/pitch/yaw when disarmed        // move throttle to between minimum and non-takeoff-throttle to keep us on the ground        attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);#endif        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);        break;    case Loiter_Flying://.........这里部分代码省略.........

// althold_run - runs the althold controller// should be called at 100hz or morevoid Copter::althold_run(){    float target_roll, target_pitch;    float target_yaw_rate;    float target_climb_rate;    float takeoff_climb_rate = 0.0f;    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately    if(!ap.auto_armed || !motors.get_interlock()) {        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);        return;    }    // apply SIMPLE mode transform to pilot inputs    update_simple_mode();    // get pilot desired lean angles    // To-Do: convert get_pilot_desired_lean_angles to return angles as floats    get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch);    // get pilot's desired yaw rate    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    // get pilot desired climb rate    target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);    // get takeoff adjusted pilot and takeoff climb rates    takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);    // check for take-off    if (ap.land_complete && (takeoff_state.running || (channel_throttle->control_in > get_takeoff_trigger_throttle()))) {        if (!takeoff_state.running) {            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));        }        // indicate we are taking off        set_land_complete(false);        // clear i term when we're taking off        set_throttle_takeoff();    }    // reset target lean angles and heading while landed    if (ap.land_complete) {        attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);    }else{        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        // body-frame rate controller is run directly from 100hz loop        // call throttle controller        if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {            // if sonar is ok, use surface tracking            target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);        }        // call position controller        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);        pos_control.update_z_controller();            }}

// poshold_run - runs the PosHold controller// should be called at 100hz or morevoid Copter::poshold_run(){    float target_roll, target_pitch;  // pilot's roll and pitch angle inputs    float target_yaw_rate = 0;          // pilot desired yaw rate in centi-degrees/sec    float target_climb_rate = 0;      // pilot desired climb rate in centimeters/sec    float takeoff_climb_rate = 0.0f;    // takeoff induced climb rate    float brake_to_loiter_mix;          // mix of brake and loiter controls.  0 = fully brake controls, 1 = fully loiter controls    float controller_to_pilot_roll_mix; // mix of controller and pilot controls.  0 = fully last controller controls, 1 = fully pilot controls    float controller_to_pilot_pitch_mix;    // mix of controller and pilot controls.  0 = fully last controller controls, 1 = fully pilot controls    float vel_fw, vel_right;            // vehicle's current velocity in body-frame forward and right directions    const Vector3f& vel = inertial_nav.get_velocity();    // initialize vertical speeds and acceleration    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);    pos_control.set_accel_z(g.pilot_accel_z);    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately    if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        wp_nav.init_loiter_target();        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);        return;    }    // process pilot inputs    if (!failsafe.radio) {        // apply SIMPLE mode transform to pilot inputs        update_simple_mode();        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());        // get pilot desired climb rate (for alt-hold mode and take-off)        target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());        target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);        // get takeoff adjusted pilot and takeoff climb rates        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);        // check for take-off        if (ap.land_complete && (takeoff_state.running || channel_throttle->get_control_in() > get_takeoff_trigger_throttle())) {            if (!takeoff_state.running) {                takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));            }            // indicate we are taking off            set_land_complete(false);            // clear i term when we're taking off            set_throttle_takeoff();        }    }    // relax loiter target if we might be landed    if (ap.land_complete_maybe) {        wp_nav.loiter_soften_for_landing();    }    // if landed initialise loiter targets, set throttle to zero and exit    if (ap.land_complete) {        // if throttle zero reset attitude and exit immediately        if (ap.throttle_zero) {            motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        }else{            motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);        }        wp_nav.init_loiter_target();        // move throttle to between minimum and non-takeoff-throttle to keep us on the ground        attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);        return;    }else{        // convert pilot input to lean angles        get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);        // convert inertial nav earth-frame velocities to body-frame        // To-Do: move this to AP_Math (or perhaps we already have a function to do this)        vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw();        vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw();                // If not in LOITER, retrieve latest wind compensation lean angles related to current yaw        if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER)        poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch);        // Roll state machine        //  Each state (aka mode) is responsible for:        //      1. dealing with pilot input        //      2. calculating the final roll output to the attitude controller        //      3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state        switch (poshold.roll_mode) {            case POSHOLD_PILOT_OVERRIDE:                // update pilot desired roll angle using latest radio input                //  this filters the input so that it returns to zero no faster than the brake-rate                poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll);                // switch to BRAKE mode for next iteration if no pilot input                if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) {//.........这里部分代码省略.........

// loiter_run - runs the loiter controller// should be called at 100hz or morevoid Copter::ModeLoiter::run(){    LoiterModeState loiter_state;    float target_yaw_rate = 0.0f;    float target_climb_rate = 0.0f;    float takeoff_climb_rate = 0.0f;    // initialize vertical speed and acceleration    pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);    pos_control->set_accel_z(g.pilot_accel_z);    // process pilot inputs unless we are in radio failsafe    if (!copter.failsafe.radio) {        // apply SIMPLE mode transform to pilot inputs        update_simple_mode();        // process pilot's roll and pitch input        wp_nav->set_pilot_desired_acceleration(channel_roll->get_control_in(), channel_pitch->get_control_in());        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());        // get pilot desired climb rate        target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());        target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);    } else {        // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason        wp_nav->clear_pilot_desired_acceleration();    }    // relax loiter target if we might be landed    if (ap.land_complete_maybe) {        wp_nav->loiter_soften_for_landing();    }    // Loiter State Machine Determination    if (!motors->armed() || !motors->get_interlock()) {        loiter_state = Loiter_MotorStopped;    } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {        loiter_state = Loiter_Takeoff;    } else if (!ap.auto_armed || ap.land_complete) {        loiter_state = Loiter_Landed;    } else {        loiter_state = Loiter_Flying;    }    // Loiter State Machine    switch (loiter_state) {    case Loiter_MotorStopped:        motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);#if FRAME_CONFIG == HELI_FRAME        // force descent rate and call position controller        pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);#else        wp_nav->init_loiter_target();        attitude_control->reset_rate_controller_I_terms();        attitude_control->set_yaw_target_to_current_heading();        pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero#endif        wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());        pos_control->update_z_controller();        break;    case Loiter_Takeoff:        // set motors to full range        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);        // initiate take-off        if (!takeoff_state.running) {            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));            // indicate we are taking off            set_land_complete(false);            // clear i term when we're taking off            set_throttle_takeoff();        }        // get takeoff adjusted pilot and takeoff climb rates        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);        // get avoidance adjusted climb rate        target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);        // run loiter controller        wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);        // call attitude controller        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());        // update altitude target and call position controller        pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);        pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);        pos_control->update_z_controller();        break;    case Loiter_Landed://.........这里部分代码省略.........

// land_nogps_run - runs the land controller//      pilot controls roll and pitch angles//      should be called at 100hz or morevoid Copter::land_nogps_run(){    float target_roll = 0.0f, target_pitch = 0.0f;    float target_yaw_rate = 0;    // process pilot inputs    if (!failsafe.radio) {        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);            // exit land if throttle is high            set_mode(ALT_HOLD);        }        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // get pilot desired lean angles            get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    }    // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately    if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        // multicopters do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED        // disarm when the landing detector says we've landed and throttle is at minimum        if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {            init_disarm_motors();        }#else        // disarm when the landing detector says we've landed        if (ap.land_complete) {            init_disarm_motors();        }#endif        return;    }    // set motors to full range    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // call attitude controller    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());    // pause 4 seconds before beginning land descent    float cmb_rate;    if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) {        cmb_rate = 0;    } else {        land_pause = false;        cmb_rate = get_land_descent_speed();    }    // record desired climb rate for logging    desired_climb_rate = cmb_rate;    // call position controller    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);    pos_control.update_z_controller();}

// sport_run - runs the sport controller// should be called at 100hz or morevoid Copter::ModeSport::run(){    SportModeState sport_state;    float takeoff_climb_rate = 0.0f;    // initialize vertical speed and acceleration    pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);    pos_control->set_accel_z(g.pilot_accel_z);    // apply SIMPLE mode transform    update_simple_mode();    // get pilot's desired roll and pitch rates    // calculate rate requests    float target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p;    float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p;    // get attitude targets    const Vector3f att_target = attitude_control->get_att_target_euler_cd();    // Calculate trainer mode earth frame rate command for roll    int32_t roll_angle = wrap_180_cd(att_target.x);    target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;    // Calculate trainer mode earth frame rate command for pitch    int32_t pitch_angle = wrap_180_cd(att_target.y);    target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;    AP_Vehicle::MultiCopter &aparm = copter.aparm;    if (roll_angle > aparm.angle_max){        target_roll_rate -=  g.acro_rp_p*(roll_angle-aparm.angle_max);    }else if (roll_angle < -aparm.angle_max) {        target_roll_rate -=  g.acro_rp_p*(roll_angle+aparm.angle_max);    }    if (pitch_angle > aparm.angle_max){        target_pitch_rate -=  g.acro_rp_p*(pitch_angle-aparm.angle_max);    }else if (pitch_angle < -aparm.angle_max) {        target_pitch_rate -=  g.acro_rp_p*(pitch_angle+aparm.angle_max);    }    // get pilot's desired yaw rate    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());    // get pilot desired climb rate    float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());    target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);    // State Machine Determination    if (!motors->armed() || !motors->get_interlock()) {        sport_state = Sport_MotorStopped;    } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {        sport_state = Sport_Takeoff;    } else if (!ap.auto_armed || ap.land_complete) {        sport_state = Sport_Landed;    } else {        sport_state = Sport_Flying;    }    // State Machine    switch (sport_state) {    case Sport_MotorStopped:        motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);        attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);#if FRAME_CONFIG == HELI_FRAME        // force descent rate and call position controller        pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);#else        attitude_control->relax_attitude_controllers();        attitude_control->reset_rate_controller_I_terms();        attitude_control->set_yaw_target_to_current_heading();        pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero#endif        pos_control->update_z_controller();        break;    case Sport_Takeoff:        // set motors to full range        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);        // initiate take-off        if (!takeoff_state.running) {            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));            // indicate we are taking off            set_land_complete(false);            // clear i terms            set_throttle_takeoff();        }        // get take-off adjusted pilot and takeoff climb rates        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);        // get avoidance adjusted climb rate        target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);//.........这里部分代码省略.........

// land_run - runs the land controller//      horizontal position controlled with loiter controller//      should be called at 100hz or morevoid Copter::land_gps_run(){    int16_t roll_control = 0, pitch_control = 0;    float target_yaw_rate = 0;    // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately    if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        // multicopters do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif        wp_nav.init_loiter_target();#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED        // disarm when the landing detector says we've landed and throttle is at minimum        if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {            init_disarm_motors();        }#else        // disarm when the landing detector says we've landed        if (ap.land_complete) {            init_disarm_motors();        }#endif        return;    }    // relax loiter target if we might be landed    if (ap.land_complete_maybe) {        wp_nav.loiter_soften_for_landing();    }    // process pilot inputs    if (!failsafe.radio) {        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);            // exit land if throttle is high            if (!set_mode(LOITER)) {                set_mode(ALT_HOLD);            }        }        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // process pilot's roll and pitch input            roll_control = channel_roll->control_in;            pitch_control = channel_pitch->control_in;            // record if pilot has overriden roll or pitch            if (roll_control != 0 || pitch_control != 0) {                ap.land_repo_active = true;            }        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    }    // set motors to full range    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // process roll, pitch inputs    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);#if PRECISION_LANDING == ENABLED    // run precision landing    if (!ap.land_repo_active) {        wp_nav.shift_loiter_target(precland.get_target_shift(wp_nav.get_loiter_target()));    }#endif    // run loiter controller    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);    // call attitude controller    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);    // pause 4 seconds before beginning land descent    float cmb_rate;    if(land_pause && millis()-land_start_time < 4000) {        cmb_rate = 0;    } else {        land_pause = false;        cmb_rate = get_land_descent_speed();    }    // record desired climb rate for logging    desired_climb_rate = cmb_rate;    // update altitude target and call position controller//.........这里部分代码省略.........

// rtl_returnhome_run - return home//      called by rtl_run at 100hz or morevoid Copter::rtl_land_run(){    int16_t roll_control = 0, pitch_control = 0;    float target_yaw_rate = 0;    // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately    if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        // multicopters do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif        // set target to current position        wp_nav.init_loiter_target();#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED        // disarm when the landing detector says we've landed and throttle is at minimum        if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {            init_disarm_motors();        }#else        // disarm when the landing detector says we've landed        if (ap.land_complete) {            init_disarm_motors();        }#endif        // check if we've completed this stage of RTL        rtl_state_complete = ap.land_complete;        return;    }    // relax loiter target if we might be landed    if (ap.land_complete_maybe) {        wp_nav.loiter_soften_for_landing();    }    // process pilot's input    if (!failsafe.radio) {        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);            // exit land if throttle is high            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);            }        }        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // process pilot's roll and pitch input            roll_control = channel_roll->control_in;            pitch_control = channel_pitch->control_in;        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    }    // set motors to full range    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);     // process pilot's roll and pitch input    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);    // run loiter controller    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);    // call z-axis position controller    float cmb_rate = get_land_descent_speed();    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);    pos_control.update_z_controller();    // record desired climb rate for logging    desired_climb_rate = cmb_rate;    // roll & pitch from waypoint controller, yaw rate from pilot    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);    // check if we've completed this stage of RTL    rtl_state_complete = ap.land_complete;}

// rtl_descent_run - implements the final descent to the RTL_ALT//      called by rtl_run at 100hz or morevoid Copter::rtl_descent_run(){    int16_t roll_control = 0, pitch_control = 0;    float target_yaw_rate = 0;    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately    if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        // call attitude controller        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        // multicopters do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif        // set target to current position        wp_nav.init_loiter_target();        return;    }    // process pilot's input    if (!failsafe.radio) {        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);            // exit land if throttle is high            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);            }        }        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // process pilot's roll and pitch input            roll_control = channel_roll->get_control_in();            pitch_control = channel_pitch->get_control_in();        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());    }    // set motors to full range    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // process roll, pitch inputs    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);    // run loiter controller    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);    // call z-axis position controller    pos_control.set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);    pos_control.update_z_controller();    // roll & pitch from waypoint controller, yaw rate from pilot    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());    // check if we've reached within 20cm of final altitude    rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f;}

void Copter::land_run_horizontal_control(){    int16_t roll_control = 0, pitch_control = 0;    float target_yaw_rate = 0;        // relax loiter target if we might be landed    if (ap.land_complete_maybe) {        wp_nav.loiter_soften_for_landing();    }        // process pilot inputs    if (!failsafe.radio) {        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);            // exit land if throttle is high            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);            }        }        if (g.land_repositioning) {            // apply SIMPLE mode transform to pilot inputs            update_simple_mode();            // process pilot's roll and pitch input            roll_control = channel_roll->get_control_in();            pitch_control = channel_pitch->get_control_in();            // record if pilot has overriden roll or pitch            if (roll_control != 0 || pitch_control != 0) {                ap.land_repo_active = true;            }        }        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());    }#if PRECISION_LANDING == ENABLED    bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired();    // run precision landing    if (doing_precision_landing && precland_last_update_ms != precland.last_update_ms()) {        Vector3f target_pos;        precland.get_target_position(target_pos);        pos_control.set_xy_target(target_pos.x, target_pos.y);        pos_control.freeze_ff_xy();        precland_last_update_ms = precland.last_update_ms();    }#else    bool doing_precision_landing = false;#endif        // process roll, pitch inputs    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);    // run loiter controller    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);    // call attitude controller    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());}

// althold_run - runs the althold controller// should be called at 100hz or morevoid Copter::althold_run(){    AltHoldModeState althold_state;    float takeoff_climb_rate = 0.0f;    // initialize vertical speeds and acceleration    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);    pos_control.set_accel_z(g.pilot_accel_z);    // apply SIMPLE mode transform to pilot inputs    update_simple_mode();    // get pilot desired lean angles    float target_roll, target_pitch;    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());    // get pilot's desired yaw rate    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());    // get pilot desired climb rate    float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);#if FRAME_CONFIG == HELI_FRAME    // helicopters are held on the ground until rotor speed runup has finished    bool takeoff_triggered = (ap.land_complete && (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()) && motors.rotor_runup_complete());#else    bool takeoff_triggered = (ap.land_complete && (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()) && motors.spool_up_complete());#endif    // Alt Hold State Machine Determination    if (!motors.armed() || !motors.get_interlock()) {        althold_state = AltHold_MotorStopped;    } else if (!ap.auto_armed){        althold_state = AltHold_NotAutoArmed;    } else if (takeoff_state.running || takeoff_triggered){        althold_state = AltHold_Takeoff;    } else if (ap.land_complete){        althold_state = AltHold_Landed;    } else {        althold_state = AltHold_Flying;    }    // Alt Hold State Machine    switch (althold_state) {    case AltHold_MotorStopped:        motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);#if FRAME_CONFIG == HELI_FRAME            // helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying        // call attitude controller        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        // force descent rate and call position controller        pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);        pos_control.update_z_controller();#else        // Multicopters do not stabilize roll/pitch/yaw when motor are stopped        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);#endif        break;    case AltHold_NotAutoArmed:        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);#if FRAME_CONFIG == HELI_FRAME        // Helicopters always stabilize roll/pitch/yaw        attitude_control.set_yaw_target_to_current_heading();        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else        // Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);        break;    case AltHold_Takeoff:        // initiate take-off        if (!takeoff_state.running) {            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));            // indicate we are taking off            set_land_complete(false);            // clear i terms            set_throttle_takeoff();        }        // get take-off adjusted pilot and takeoff climb rates        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);        // set motors to full range        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);        // call attitude controller        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());//.........这里部分代码省略.........

void Copter::heli_stabilize_run_ruas(){    float target_roll, target_pitch;    float target_yaw_rate;    int16_t pilot_throttle_scaled;    // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because    // we may be in autorotation flight.  These should be reset only when transitioning from disarmed    // to armed, because the pilot will have placed the helicopter down on the landing pad.  This is so    // that the servos move in a realistic fashion while disarmed for operational checks.    // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move    if(!motors.armed()) {        heli_flags.init_targets_on_arming=true;        attitude_control.set_yaw_target_to_current_heading();    }    if(motors.armed() && heli_flags.init_targets_on_arming) {        attitude_control.set_yaw_target_to_current_heading();        if (motors.rotor_speed_above_critical()) {            heli_flags.init_targets_on_arming=false;        }    }    // send RC inputs direct into motors library for use during manual passthrough for helicopter setup    heli_radio_passthrough();    // apply SIMPLE mode transform to pilot inputs    update_simple_mode();    // updates avoidnace logic    avoidance_maneuver(); //RUAS    //Allowing the pilot to have presidance in heli attitude control demos! The presidnace order can be changed as needed here:    if(!failsafe.radio && (channel_roll->control_in >= 200 || channel_roll->control_in <= -200 || channel_pitch->control_in >= 200 ||channel_pitch->control_in <= -200)){      get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);      //If no pilot input and need to implement manouver    }else if(do_avoid_maneuver){      get_pilot_desired_lean_angles(avoidance_roll_angle_cd, avoidance_pitch_angle_cd , target_roll, target_pitch, aparm.angle_max);      //If no pilot input and no mpending crash, hold middlestick position    }else{      get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);    }    //the pilot takes presidnace in the yaw direction in the demos!    if(!failsafe.radio && (channel_yaw->control_in >= 200 || channel_yaw->control_in <= -200)){      target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    }else if(do_track_maneuver){      //point to air traffic      target_yaw_rate = _trafic_angle * 100 * g.acro_yaw_p;//check this    }else{      //point to current heading      target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    }    // get pilot's desired throttle    pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in);    // call attitude controller    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());    // output pilot's throttle - note that TradHeli does not used angle-boost    attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);}

// althold_run - runs the althold controller// should be called at 100hz or morevoid Sub::althold_run(){    uint32_t tnow = AP_HAL::millis();    // initialize vertical speeds and acceleration    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);    pos_control.set_accel_z(g.pilot_accel_z);    if (!motors.armed() || !motors.get_interlock()) {        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);        pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());        last_pilot_heading = ahrs.yaw_sensor;        return;    }    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);    // apply SIMPLE mode transform to pilot inputs    update_simple_mode();    // get pilot desired lean angles    float target_roll, target_pitch;    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());    // get pilot's desired yaw rate    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());    // get pilot desired climb rate    float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);    // call attitude controller    if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        last_pilot_heading = ahrs.yaw_sensor;        last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading    } else { // hold current heading        // this check is required to prevent bounce back after very fast yaw maneuvers        // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped        if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading            target_yaw_rate = 0; // Stop rotation on yaw axis            // call attitude controller with target yaw rate = 0 to decelerate on yaw axis            attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());            last_pilot_heading = ahrs.yaw_sensor; // update heading to hold        } else { // call attitude controller holding absolute absolute bearing            attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true, get_smoothing_gain());        }    }    // adjust climb rate using rangefinder    if (rangefinder_alt_ok()) {        // if rangefinder is ok, use surface tracking        target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);    }    // call z axis position controller    if (ap.at_bottom) {        pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator        pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom    } else {        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);    }    pos_control.update_z_controller();    //control_in is range 0-1000    //radio_in is raw pwm value    motors.set_forward(channel_forward->norm_input());    motors.set_lateral(channel_lateral->norm_input());}

// loiter_run - runs the loiter controller// should be called at 100hz or morevoid Copter::loiter_run(){    LoiterModeState loiter_state;    float target_yaw_rate = 0.0f;    float target_climb_rate = 0.0f;    float takeoff_climb_rate = 0.0f;    // initialize vertical speed and acceleration    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);    pos_control.set_accel_z(g.pilot_accel_z);    // process pilot inputs unless we are in radio failsafe    if (!failsafe.radio) {        // apply SIMPLE mode transform to pilot inputs        update_simple_mode();        // process pilot's roll and pitch input        wp_nav.set_pilot_desired_acceleration(channel_roll->control_in, channel_pitch->control_in);        // get pilot's desired yaw rate        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);        // get pilot desired climb rate        target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);        target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);    } else {        // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason        wp_nav.clear_pilot_desired_acceleration();    }    // relax loiter target if we might be landed    if (ap.land_complete_maybe) {        wp_nav.loiter_soften_for_landing();    }    // Loiter State Machine Determination    if (!motors.armed() || !motors.get_interlock()) {        loiter_state = Loiter_MotorStopped;    } else if (!ap.auto_armed) {        loiter_state = Loiter_NotAutoArmed;    } else if (takeoff_state.running || (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()))){        loiter_state = Loiter_Takeoff;    } else if (ap.land_complete){        loiter_state = Loiter_Landed;    } else {        loiter_state = Loiter_Flying;    }    // Loiter State Machine    switch (loiter_state) {    case Loiter_MotorStopped:        motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);#if FRAME_CONFIG == HELI_FRAME        // helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying        // run loiter controller        wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);        // call attitude controller        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);        // force descent rate and call position controller        pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);        pos_control.update_z_controller();#else        wp_nav.init_loiter_target();        // multicopters do not stabilize roll/pitch/yaw when motors are stopped        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);#endif        break;    case Loiter_NotAutoArmed:        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);        wp_nav.init_loiter_target();#if FRAME_CONFIG == HELI_FRAME        // Helicopters always stabilize roll/pitch/yaw        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else        // Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);        break;    case Loiter_Takeoff:        if (!takeoff_state.running) {            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));            // indicate we are taking off            set_land_complete(false);            // clear i term when we're taking off            set_throttle_takeoff();        }//.........这里部分代码省略.........

// althold_run - runs the althold controller// should be called at 100hz or morevoid Copter::althold_run(){    AltHoldModeState althold_state;    float takeoff_climb_rate = 0.0f;    // apply SIMPLE mode transform to pilot inputs    update_simple_mode();    // get pilot desired lean angles    float target_roll, target_pitch;    get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());    // get pilot's desired yaw rate    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);    // get pilot desired climb rate    float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);#if FRAME_CONFIG == HELI_FRAME    // helicopters are held on the ground until rotor speed runup has finished    bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()) && motors.rotor_runup_complete());#else    bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()));#endif    // Alt Hold State Machine Determination    if(!ap.auto_armed || !motors.get_interlock()) {        althold_state = AltHold_Disarmed;    } else if (takeoff_state.running || takeoff_triggered){        althold_state = AltHold_Takeoff;    } else if (ap.land_complete){        althold_state = AltHold_Landed;    } else {        althold_state = AltHold_Flying;    }    // Alt Hold State Machine    switch (althold_state) {    case AltHold_Disarmed:#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        attitude_control.set_yaw_target_to_current_heading();        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        attitude_control.set_throttle_out(0,false,g.throttle_filt);#else   // Multicopter do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);#endif  // HELI_FRAME        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);        break;    case AltHold_Takeoff:        // initiate take-off        if (!takeoff_state.running) {            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));            // indicate we are taking off            set_land_complete(false);            // clear i terms            set_throttle_takeoff();        }        // get take-off adjusted pilot and takeoff climb rates        takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        // call position controller        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);        pos_control.update_z_controller();        break;    case AltHold_Landed:#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw        attitude_control.set_yaw_target_to_current_heading();        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);#else   // Multicopter do not stabilize roll/pitch/yaw when disarmed        attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);#endif        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);        break;    case AltHold_Flying:        // call attitude controller        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());        // call throttle controller        if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {            // if sonar is ok, use surface tracking            target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);        }//.........这里部分代码省略.........