update_flight_mode（ModeAltHold）

这里只看ModeAltHold。

位置：X:\ardupilot\ArduCopter\mode.cpp

// update_flight_mode - calls the appropriate attitude controllers based on flight mode（根据飞行模式调用适当的姿态控制器）
// called at 100hz or more
void Copter::update_flight_mode()
{// Update EKF speed limit - used to limit speed when we are using optical flow（当我们使用光流时，用来限制速度）ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);//只是重新计算了一下这两值flightmode->run();//阅读  201808181419/*需要这里的flightmode，会随着飞行模式的切换调用不同的代码*/
}

定高模式控制代码，最开始的地方。

位置：X:\ardupilot\ArduCopter\mode_althold.cpp

// althold_run - runs the althold controller
// should be called at 100hz or more
void Copter::ModeAltHold::run()
{AltHoldModeState althold_state;float takeoff_climb_rate = 0.0f;// initialize vertical speeds and acceleration（初始化垂直速度和加速度）pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);//speed_down, speed_uppos_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, -get_pilot_speed_dn(), g.pilot_speed_up);// Alt Hold State Machine Determinationif (!motors->armed() || !motors->get_interlock()) {althold_state = AltHold_MotorStopped;} else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) {althold_state = AltHold_Takeoff;} else if (!ap.auto_armed || ap.land_complete) {althold_state = AltHold_Landed;} else {althold_state = AltHold_Flying;}// Alt Hold State Machineswitch (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());attitude_control->reset_rate_controller_I_terms();attitude_control->set_yaw_target_to_current_heading();
#if FRAME_CONFIG == HELI_FRAME    // force descent rate and call position controllerpos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);heli_flags.init_targets_on_arming=true;
#elsepos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
#endifpos_control->update_z_controller();break;case AltHold_Takeoff:
#if FRAME_CONFIG == HELI_FRAME    if (heli_flags.init_targets_on_arming) {heli_flags.init_targets_on_arming=false;}
#endif// set motors to full rangemotors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);// initiate take-offif (!takeoff_state.running) {takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));// indicate we are taking offset_land_complete(false);// clear i termsset_throttle_takeoff();}// get take-off adjusted pilot and takeoff climb ratestakeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);// get avoidance adjusted climb ratetarget_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);// call attitude controllerattitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());// call position controllerpos_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);}#if FRAME_CONFIG == HELI_FRAME    if (heli_flags.init_targets_on_arming) {attitude_control->reset_rate_controller_I_terms();attitude_control->set_yaw_target_to_current_heading();if (motors->get_interlock()) {heli_flags.init_targets_on_arming=false;}}
#elseattitude_control->reset_rate_controller_I_terms();attitude_control->set_yaw_target_to_current_heading();
#endifattitude_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 zeropos_control->update_z_controller();break;case AltHold_Flying:motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);#if AC_AVOID_ENABLED == ENABLED// apply avoidancecopter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max);
#endif// call attitude controllerattitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());// adjust climb rate using rangefinder（使用测距仪调整爬升率）if (copter.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);}// get avoidance adjusted climb rate（规避调整爬升率）target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);// call position controllerpos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);pos_control->update_z_controller();break;}
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// set_speed_z - sets maximum climb and descent rates
/// To-Do: call this in the main code as part of flight mode initialisation
///     calc_leash_length_z should be called afterwards
///     speed_down should be a negative number
void AC_PosControl::set_speed_z(float speed_down, float speed_up)
{// ensure speed_down is always negativespeed_down = -fabsf(speed_down);if ((fabsf(_speed_down_cms-speed_down) > 1.0f) || (fabsf(_speed_up_cms-speed_up) > 1.0f)) {//向上、向下的速度比较大时_speed_down_cms = speed_down;_speed_up_cms = speed_up;_flags.recalc_leash_z = true;calc_leash_length_z();//重新计算刹车距离}
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
///     called by pos_to_rate_z if z-axis speed or accelerations are changed
void AC_PosControl::calc_leash_length_z()
{if (_flags.recalc_leash_z) {_leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_pos_z.kP());_leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_pos_z.kP());_flags.recalc_leash_z = false;}
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const
{float leash_length;// sanity check acceleration and avoid divide by zeroif (accel_cms <= 0.0f) {accel_cms = POSCONTROL_ACCELERATION_MIN;}// avoid divide by zeroif (kP <= 0.0f) {return POSCONTROL_LEASH_LENGTH_MIN;//100.0f  // minimum leash lengths in cm}// calculate leash length（计算刹车长度）if(speed_cms <= accel_cms / kP) {// linear leash length based on speed close in（基于速度接近的线性皮带长度）leash_length = speed_cms / kP;}else{// leash length grows at sqrt of speed further out（刹车长度以更快的速度增长）leash_length = (accel_cms / (2.0f*kP*kP)) + (speed_cms*speed_cms / (2.0f*accel_cms));}// ensure leash is at least 1m long(确保刹车至少有1米长)if( leash_length < POSCONTROL_LEASH_LENGTH_MIN ) {leash_length = POSCONTROL_LEASH_LENGTH_MIN;}return leash_length;
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// set_accel_z - set vertical acceleration in cm/s/s
void AC_PosControl::set_accel_z(float accel_cmss)
{if (fabsf(_accel_z_cms-accel_cmss) > 1.0f) {//加速度比较大时_accel_z_cms = accel_cmss;_flags.recalc_leash_z = true;calc_leash_length_z();//重新计算刹车距离}
}

/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
///     called by pos_to_rate_z if z-axis speed or accelerations are changed
void AC_PosControl::calc_leash_length_z()
{if (_flags.recalc_leash_z) {_leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_pos_z.kP());_leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_pos_z.kP());_flags.recalc_leash_z = false;}
}

/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const
{float leash_length;// sanity check acceleration and avoid divide by zeroif (accel_cms <= 0.0f) {accel_cms = POSCONTROL_ACCELERATION_MIN;}// avoid divide by zeroif (kP <= 0.0f) {return POSCONTROL_LEASH_LENGTH_MIN;//100.0f  // minimum leash lengths in cm}// calculate leash length（计算刹车长度）if(speed_cms <= accel_cms / kP) {// linear leash length based on speed close in（基于速度接近的线性皮带长度）leash_length = speed_cms / kP;}else{// leash length grows at sqrt of speed further out（刹车长度以更快的速度增长）leash_length = (accel_cms / (2.0f*kP*kP)) + (speed_cms*speed_cms / (2.0f*accel_cms));}// ensure leash is at least 1m long(确保刹车至少有1米长)if( leash_length < POSCONTROL_LEASH_LENGTH_MIN ) {leash_length = POSCONTROL_LEASH_LENGTH_MIN;}return leash_length;
}

位置：X:\ardupilot\ArduCopter\mode.cpp

void Copter::Mode::update_simple_mode(void) {copter.update_simple_mode();
}

位置：X:\ardupilot\ArduCopter\ArduCopter.cpp

// update_simple_mode - rotates pilot input if we are in simple mode（如果我们在简单模式下旋转试验输入）
void Copter::update_simple_mode(void)
{float rollx, pitchx;// exit immediately if no new radio frame or not in simple modeif (ap.simple_mode == 0 || !ap.new_radio_frame) {return;}// mark radio frame as consumed（标记无线电帧）ap.new_radio_frame = false;if (ap.simple_mode == 1) {//简单模式// rotate roll, pitch input by -initial simple heading (i.e. north facing)rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw;pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;}else{//超级简单模式// rotate roll, pitch input by -super simple heading (reverse of heading to home)rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;}// rotate roll, pitch input from north facing to vehicle's perspectivechannel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw());//重新设置control_inchannel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw());
}

位置：X:\ardupilot\ArduCopter\mode.cpp

void Copter::Mode::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{copter.get_pilot_desired_lean_angles(roll_in, pitch_in, roll_out, pitch_out, angle_max);
}

位置：X:\ardupilot\ArduCopter\Attitude.cpp

// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{// sanity check angle max parameter（完备性检查角最大参数）aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);// limit max lean angleangle_max = constrain_float(angle_max, 1000, aparm.angle_max);//这里的angle_max == aparm.angle_max吧// scale roll_in, pitch_in to ANGLE_MAX parameter rangefloat scaler = aparm.angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;//4500roll_in *= scaler;pitch_in *= scaler;// do circular limit（做圆形的限制）float total_in = norm(pitch_in, roll_in);if (total_in > angle_max) {float ratio = angle_max / total_in;roll_in *= ratio;pitch_in *= ratio;}// do lateral tilt to euler roll conversion（横向倾斜到欧拉滚动转换）roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));// returnroll_out = roll_in;//对横滚做了一些处理pitch_out = pitch_in;
}

位置：X:\ardupilot\ArduCopter\mode.cpp

float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
{return copter.get_pilot_desired_yaw_rate(stick_angle);
}

位置：X:\ardupilot\ArduCopter\Attitude.cpp

// get_pilot_desired_heading - transform pilot's yaw input into a desired yaw rate（将飞行员的偏航输入转换为期望的偏航率）
// returns desired yaw rate in centi-degrees per second（返回所需的偏航率，以每秒钟的速度）
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
{float yaw_request;// calculate yaw rate requestif (g2.acro_y_expo <= 0) {yaw_request = stick_angle * g.acro_yaw_p;} else {// expo（exponent指数） variablesfloat y_in, y_in3, y_out;// range check expoif (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f) {g2.acro_y_expo = 1.0f;}// yaw expoy_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;//4500y_in3 = y_in*y_in*y_in;y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in);//应该是泰勒级数yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * g.acro_yaw_p;}// convert pilot input to the desired yaw ratereturn yaw_request;
}

位置：X:\ardupilot\ArduCopter\mode.cpp

float Copter::Mode::get_pilot_desired_climb_rate(float throttle_control)
{return copter.get_pilot_desired_climb_rate(throttle_control);
}

// get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
// without any deadzone at the bottom
float Copter::get_pilot_desired_climb_rate(float throttle_control)
{// throttle failsafe checkif( failsafe.radio ) {return 0.0f;}#if TOY_MODE_ENABLED == ENABLEDif (g2.toy_mode.enabled()) {// allow throttle to be reduced after throttle arming and for// slower descent close to the groundg2.toy_mode.throttle_adjust(throttle_control);}
#endiffloat desired_rate = 0.0f;float mid_stick = get_throttle_mid();float deadband_top = mid_stick + g.throttle_deadzone;float deadband_bottom = mid_stick - g.throttle_deadzone;// ensure a reasonable throttle valuethrottle_control = constrain_float(throttle_control,0.0f,1000.0f);// ensure a reasonable deadzoneg.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);// check throttle is above, below or in the deadbandif (throttle_control < deadband_bottom) {// below the deadbanddesired_rate = get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom;}else if (throttle_control > deadband_top) {// above the deadbanddesired_rate = g.pilot_speed_up * (throttle_control-deadband_top) / (1000.0f-deadband_top);}else{// must be in the deadbanddesired_rate = 0.0f;}return desired_rate;
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.h

// Sets yaw target to vehicle headingvoid set_yaw_target_to_current_heading() { shift_ef_yaw_target(degrees(_ahrs.yaw - _attitude_target_euler_angle.z)*100.0f); }

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.cpp

// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
{float yaw_shift = radians(yaw_shift_cd*0.01f);Quaternion _attitude_target_update_quat;_attitude_target_update_quat.from_axis_angle(Vector3f(0.0f, 0.0f, yaw_shift));//偏航角->四元数_attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat;
}

AltHold_MotorStopped

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// relax_alt_hold_controllers - set all desired and targets to measured（设定所有想要的目标和目标）
void AC_PosControl::relax_alt_hold_controllers(float throttle_setting)
{_pos_target.z = _inav.get_altitude();_vel_desired.z = 0.0f;_flags.use_desvel_ff_z = false;_vel_target.z = _inav.get_velocity_z();_vel_last.z = _inav.get_velocity_z();_accel_feedforward.z = 0.0f;_accel_last_z_cms = 0.0f;_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;_flags.reset_accel_to_throttle = true;_pid_accel_z.set_integrator((throttle_setting-_motors.get_throttle_hover())*1000.0f);
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{// check time since last castuint32_t now = AP_HAL::millis();if (now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS) {//200 // position controller is considered active if it has been called within the past 0.2 seconds_flags.reset_rate_to_accel_z = true;_flags.reset_accel_to_throttle = true;//超时后，修改标志位}_last_update_z_ms = now;// check for ekf altitude resetcheck_for_ekf_z_reset();// check if leash lengths need to be recalculatedcalc_leash_length_z();// call position controller（调用位置控制器）pos_to_rate_z();
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// initialise ekf z axis reset check
void AC_PosControl::init_ekf_z_reset()
{float alt_shift;_ekf_z_reset_ms = _ahrs.getLastPosDownReset(alt_shift);// system time of last recorded ekf altitude reset
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
///     called by pos_to_rate_z if z-axis speed or accelerations are changed
void AC_PosControl::calc_leash_length_z()
{if (_flags.recalc_leash_z) {_leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_pos_z.kP());_leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_pos_z.kP());_flags.recalc_leash_z = false;}
}

姿态控制只有Z轴有位置控制，这里调用Z轴的位置控制。

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

// pos_to_rate_z - position to rate controller for Z axis
// calculates desired rate in earth-frame z axis and passes to rate controller
// vel_up_max, vel_down_max should have already been set before calling this method
void AC_PosControl::pos_to_rate_z()
{float curr_alt = _inav.get_altitude();//目前的高度// clear position limit flags_limit.pos_up = false;_limit.pos_down = false;// calculate altitude error（计算高度误差）_pos_error.z = _pos_target.z - curr_alt;// do not let target altitude get too far from current altitude（不要让目标高度离当前高度太远）if (_pos_error.z > _leash_up_z) {//判断是否超出刹车距离_pos_target.z = curr_alt + _leash_up_z;_pos_error.z = _leash_up_z;_limit.pos_up = true;}if (_pos_error.z < -_leash_down_z) {//判断是否超出刹车距离_pos_target.z = curr_alt - _leash_down_z;_pos_error.z = -_leash_down_z;_limit.pos_down = true;}// calculate _vel_target.z using from _pos_error.z using sqrt controller_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);//根据位置偏差计算目标速度// check speed limits// To-Do: check these speed limits here or in the pos->rate controller_limit.vel_up = false;_limit.vel_down = false;if (_vel_target.z < _speed_down_cms) {// max descent rate in cm/s_vel_target.z = _speed_down_cms;_limit.vel_down = true;}if (_vel_target.z > _speed_up_cms) {// max climb rate in cm/s_vel_target.z = _speed_up_cms;_limit.vel_up = true;}// add feed forward component(添加前馈组件)if (_flags.use_desvel_ff_z) {_vel_target.z += _vel_desired.z;//目标速度 + 前馈项}// call rate based throttle controller which will update accel based throttle controller targetsrate_to_accel_z();
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

// rate_to_accel_z - calculates desired accel required to achieve the velocity target
// calculates desired acceleration and calls accel throttle controller
void AC_PosControl::rate_to_accel_z()
{const Vector3f& curr_vel = _inav.get_velocity();float p;                                // used to capture pid values for logging// reset last velocity target to current targetif (_flags.reset_rate_to_accel_z) {_vel_last.z = _vel_target.z;}// feed forward desired acceleration calculationif (_dt > 0.0f) {if (!_flags.freeze_ff_z) {_accel_feedforward.z = (_vel_target.z - _vel_last.z)/_dt;} else {// stop the feed forward being calculated during a known discontinuity_flags.freeze_ff_z = false;}} else {_accel_feedforward.z = 0.0f;}// store this iteration's velocities for the next iteration_vel_last.z = _vel_target.z;// reset velocity error and filter if this controller has just been engaged// 如果这个控制器刚刚投入使用，则会出现速度错误和过滤器if (_flags.reset_rate_to_accel_z) {// Reset Filter_vel_error.z = 0;_vel_error_filter.reset(0);_flags.reset_rate_to_accel_z = false;} else {// calculate rate error and filter with cut off frequency of 2 Hz_vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt);}// calculate pp = _p_vel_z.kP() * _vel_error.z;//P项// consolidate and constrain target acceleration_accel_target.z = _accel_feedforward.z + p;//P项 + 限流// set target for accel based throttle controlleraccel_to_throttle(_accel_target.z);
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

// accel_to_throttle - alt hold's acceleration controller
// calculates a desired throttle which is sent directly to the motors
void AC_PosControl::accel_to_throttle(float accel_target_z)
{float z_accel_meas;         // actual accelerationfloat p,i,d;              // used to capture pid values for logging// Calculate Earth Frame Z acceleration（计算地球框架Z加速度）z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;// reset target altitude if this controller has just been engagedif (_flags.reset_accel_to_throttle) {// Reset Filter_accel_error.z = 0;_flags.reset_accel_to_throttle = false;} else {// calculate accel error_accel_error.z = accel_target_z - z_accel_meas;//计算偏差}// set input to PID_pid_accel_z.set_input_filter_all(_accel_error.z);_pid_accel_z.set_desired_rate(accel_target_z);// separately calculate p, i, d values for loggingp = _pid_accel_z.get_p();// get i termi = _pid_accel_z.get_integrator();// ensure imax is always large enough to overpower hover throttleif (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) {_pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f);}// update i term as long as we haven't breached the limits or the I term will certainly reduce// To-Do: should this be replaced with limits check from attitude_controller?if ((!_motors.limit.throttle_lower && !_motors.limit.throttle_upper) || (i>0&&_accel_error.z<0) || (i<0&&_accel_error.z>0)) {i = _pid_accel_z.get_i();}// get d termd = _pid_accel_z.get_d();float thr_out = (p+i+d)*0.001f +_motors.get_throttle_hover();//悬停油门 + PID// send throttle to attitude controller with angle boost_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
}

最终计算出期望的油门值。作用到飞机。

AltHold_Takeoff

AltHold_Landed

AltHold_Flying

位置：X:\ardupilot\libraries\AC_Avoidance\AC_Avoid.cpp

// adjust roll-pitch to push vehicle away from objects
// roll and pitch value are in centi-degrees
void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float veh_angle_max)
{// exit immediately if proximity based avoidance is disabledif ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) == 0 || !_proximity_enabled) {return;}// exit immediately if angle max is zeroif (_angle_max <= 0.0f || veh_angle_max <= 0.0f) {return;}float roll_positive = 0.0f;    // maximum positive roll valuefloat roll_negative = 0.0f;    // minimum negative roll valuefloat pitch_positive = 0.0f;   // maximum position pitch valuefloat pitch_negative = 0.0f;   // minimum negative pitch value// get maximum positive and negative roll and pitch percentages from proximity sensorget_proximity_roll_pitch_pct(roll_positive, roll_negative, pitch_positive, pitch_negative);// add maximum positive and negative percentages together for roll and pitch, convert to centi-degreesVector2f rp_out((roll_positive + roll_negative) * 4500.0f, (pitch_positive + pitch_negative) * 4500.0f);// apply avoidance angular limits// the object avoidance lean angle is never more than 75% of the total angle-limit to allow the pilot to overrideconst float angle_limit = constrain_float(_angle_max, 0.0f, veh_angle_max * AC_AVOID_ANGLE_MAX_PERCENT);float vec_len = rp_out.length();if (vec_len > angle_limit) {rp_out *= (angle_limit / vec_len);}// add passed in roll, pitch anglesrp_out.x += roll;rp_out.y += pitch;// apply total angular limitsvec_len = rp_out.length();if (vec_len > veh_angle_max) {rp_out *= (veh_angle_max / vec_len);}// return adjusted roll, pitchroll = rp_out.x;pitch = rp_out.y;
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.cpp

// Command {an euler roll and pitch angle} and {an euler yaw rate} with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
{// Convert from centidegrees on public interface to radiansfloat euler_roll_angle = radians(euler_roll_angle_cd*0.01f);float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);// calculate the attitude target euler angles（计算姿态目标欧拉角,这是根据上次的期望姿态的四元数计算上次期望的欧拉角）_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);// ensure smoothing gain can not cause overshootsmoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)euler_roll_angle += get_roll_trim_rad();//返回0if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {// translate the roll pitch and yaw acceleration limits to the euler axis（将滚动距和偏航加速度限制转换为欧拉轴）Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration// and an exponential decay specified by smoothing_gain at the end._attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt);_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt);// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing// the output rate towards the input rate._attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z);// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward// 将理想姿态的欧拉角导数转换为前馈线角速度矢量euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);//上次期望姿态的欧拉角、期望的欧拉角速度，期望的角速度} else {// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.// 当前馈没有启用时，目标欧拉角是输入目标，前馈率为零。_attitude_target_euler_angle.x = euler_roll_angle;_attitude_target_euler_angle.y = euler_pitch_angle;_attitude_target_euler_angle.z += euler_yaw_rate*_dt;// Compute quaternion target attitude_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);// Set rate feedforward requests to zero_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);}// Call quaternion attitude controllerattitude_controller_run_quat();
}

// Calculates the body frame angular velocities to follow the target attitude（计算机体框架角速度以跟随目标姿态）
void AC_AttitudeControl::attitude_controller_run_quat()
{// Retrieve quaternion vehicle attitude// TODO add _ahrs.get_quaternion()Quaternion attitude_vehicle_quat;attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());//当前飞机的姿态// Compute attitude errorVector3f attitude_error_vector;//经过第三个参数返回thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);//第一个参数没有找到在哪里更新// Compute the angular velocity target from the attitude error（从姿态误差中计算角速度目标）_rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);//计算目标角速度，这是一个向量// Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.// 添加前馈术语，试图确保滚动和音调错误与主体帧而不是参考帧一起旋转。_rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro().z;_rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro().z;// Add the angular velocity feedforward, rotated into vehicle frame// 加入角速度前馈，将其旋转到汽车框架中Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);//_attitude_target_ang_vel以前已经更新Quaternion attitude_error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;//当前姿态的四元数、目标姿态的四元数 = 两个姿态的四元数误差Quaternion target_ang_vel_quat = attitude_error_quat.inverse()*attitude_target_ang_vel_quat*attitude_error_quat;//目标姿态角速度四元数// Correct the thrust vector and smoothly add feedforward and yaw input// 修正推力矢量并平稳地 增加前馈  和  偏航输入if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){// Thrust angle error above which yaw corrections are limited_rate_target_ang_vel.z = _ahrs.get_gyro().z;}else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){//float flip_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE);//radians(30.0f)，这个角度占30的比例_rate_target_ang_vel.x += target_ang_vel_quat.q2*flip_scalar;_rate_target_ang_vel.y += target_ang_vel_quat.q3*flip_scalar;_rate_target_ang_vel.z += target_ang_vel_quat.q4;_rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-flip_scalar) + _rate_target_ang_vel.z*flip_scalar;} else {_rate_target_ang_vel.x += target_ang_vel_quat.q2;_rate_target_ang_vel.y += target_ang_vel_quat.q3;_rate_target_ang_vel.z += target_ang_vel_quat.q4;}if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {// rotate target and normalize（旋转目标和规范化）Quaternion attitude_target_update_quat;attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));//目标姿态的四元数_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;_attitude_target_quat.normalize();//归一化}
}

位置：X:\ardupilot\ArduCopter\mode.cpp

float Copter::Mode::get_avoidance_adjusted_climbrate(float target_rate)
{return copter.get_avoidance_adjusted_climbrate(target_rate);
}

位置：X:\ardupilot\ArduCopter\Attitude.cpp

// get target climb rate reduced to avoid obstacles and altitude fence
float Copter::get_avoidance_adjusted_climbrate(float target_rate)
{
#if AC_AVOID_ENABLED == ENABLEDavoid.adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), target_rate, G_Dt);return target_rate;
#elsereturn target_rate;
#endif
}

位置：X:\ardupilot\libraries\AC_Avoidance\AC_Avoid.cpp

// adjust vertical climb rate so vehicle does not break the vertical fence
void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt)
{// exit immediately if disabledif (_enabled == AC_AVOID_DISABLED) {return;}// do not adjust climb_rate if level or descendingif (climb_rate_cms <= 0.0f) {return;}// limit accelerationfloat accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);bool limit_alt = false;float alt_diff = 0.0f;   // distance from altitude limit to vehicle in metres (positive means vehicle is below limit)// calculate distance below fenceif ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && (_fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) {// calculate distance from vehicle to safe altitudefloat veh_alt;_ahrs.get_relative_position_D_home(veh_alt);// _fence.get_safe_alt_max() is UP, veh_alt is DOWN:alt_diff = _fence.get_safe_alt_max() + veh_alt;limit_alt = true;}// calculate distance to (e.g.) optical flow altitude limit// AHRS values are always in metresfloat alt_limit;float curr_alt;if (_ahrs.get_hgt_ctrl_limit(alt_limit) &&_ahrs.get_relative_position_D_origin(curr_alt)) {// alt_limit is UP, curr_alt is DOWN:const float ctrl_alt_diff = alt_limit + curr_alt;if (!limit_alt || ctrl_alt_diff < alt_diff) {alt_diff = ctrl_alt_diff;limit_alt = true;}}// get distance from proximity sensorfloat proximity_alt_diff;if (_proximity.get_upward_distance(proximity_alt_diff)) {proximity_alt_diff -= _margin;if (!limit_alt || proximity_alt_diff < alt_diff) {alt_diff = proximity_alt_diff;limit_alt = true;}}// limit climb rateif (limit_alt) {// do not allow climbing if we've breached the safe altitudeif (alt_diff <= 0.0f) {climb_rate_cms = MIN(climb_rate_cms, 0.0f);return;}// limit climb rateconst float max_speed = get_max_speed(kP, accel_cmss_limited, alt_diff*100.0f, dt);climb_rate_cms = MIN(max_speed, climb_rate_cms);}
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward
///     should be called continuously (with dt set to be the expected time between calls)
///     actual position target will be moved no faster than the speed_down and speed_up
///     target will also be stopped if the motors hit their limits or leash length is exceeded
///     set force_descend to true during landing to allow target to move low enough to slow the motors
void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)//第一个参数为0
{// calculated increased maximum acceleration if over speedfloat accel_z_cms = _accel_z_cms;if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms)) {accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms;}if (_vel_desired.z > _speed_up_cms && !is_zero(_speed_up_cms)) {accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms;}accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f);//速度限制// jerk_z is calculated to reach full acceleration in 1000ms.float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;// Defines the time it takes to reach the requested accelerationfloat accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));_accel_last_z_cms += jerk_z * dt;_accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);float vel_change_limit = _accel_last_z_cms * dt;_vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);//修改期望的Z轴速度_flags.use_desvel_ff_z = true;// adjust desired alt if motors have not hit their limits（如果发动机没有达到极限，就调整理想的alt值）// To-Do: add check of _limit.pos_down?if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {_pos_target.z += _vel_desired.z * dt;//修正Z轴目标位置}
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{// check time since last castuint32_t now = AP_HAL::millis();if (now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS) {//200 // position controller is considered active if it has been called within the past 0.2 seconds_flags.reset_rate_to_accel_z = true;_flags.reset_accel_to_throttle = true;//超时后，修改标志位}_last_update_z_ms = now;// check for ekf altitude resetcheck_for_ekf_z_reset();// check if leash lengths need to be recalculatedcalc_leash_length_z();// call position controller（调用位置控制器）pos_to_rate_z();
}

到这里吧，都是水。

当飞机需要有位置移动的时候就需要有位置控制。

定点不需要移动，所以不需要位置控制。

定高不需要Z轴方向移动，所以Z周方向有位置控制。

。。。。。

浅谈APM系列-----update_flight_mode（ModeAltHold）相关推荐

浅谈APM系列-----gcs_check_input
SCHED_TASK(gcs_check_input, 400, 180) 流水文,只是记录一下自己看代码的过程. 位置:X:\ardupilot\ArduCopter\GCS_Mavlink.cpp ...
**浅谈STM32系列单片机的零基础学习方法**
浅谈STM32系列单片机的零基础学习方法 *很多朋友想必在零基础学习STM32F1这系列单片机时会感觉无从下手,或者不知道写程序时到底是用库函数好还是寄存器好.* 我个人认为一个零基础的朋友可以通过以 ...
浅谈ARMv8-A系列CPU的架构
目录 01.重头戏RISC 02.ARMv8诞生的契机 03.ARMv8-A架构的主要特性 04.基于SkyEye的ARMv8-A架构的仿真实现 1978年底,物理学家Herm ...
gif透明背景动画_前端基础系列之bmp、jpg、png、gif、svg常用图片格式浅谈（二）...
IT客栈作者:大腰子 bmp.jpg.png.gif.svg常用图片格式之前为大家介绍了几种WEB前端常用的图片格式,对比了它们的特点,参见<前端基础系列之bmp.jpg.png.gif.s ...
music算法_“要热爱请深爱”系列（5）浅谈模拟退火算法
黄乐天浅谈模拟退火算法背景在实际生活中, 数学问题中,我们常常会遇到(一定范围内)函数求最值的问题.一般可以用数学方式解答,但如果遇到如下恶心的函数: 它的函数图像是这样的: 我们只好用计算机科 ...
浅谈Dynamic 关键字系列之三（下）:ExpandoObject，DynamicObject，DynamicMetaObject
接上文:浅谈Dynamic关键字系列之三(上) 为什么TryXXX方法没有被调用?? 将DynamicProduct 中的name修饰符改为private: private string name; ...
浅谈信息安全等级保护与ISO27000系列标准的异同
摘要:信息安全等级保护和ISO27000系列标准是目前国内主流的两个信息安全标准体系,在党政机关及企事业单位运用非常广泛.在建立单位内部信息安全体系的时候往往会遇到需要同时满足两个标准体系要求的难题. ...
浅谈嵌入式MCU软件开发之S32K1xx系列MCU启动过程及重映射代码到RAM中运行方法详解
内容提要注:本文摘自NXP工程师胡恩伟的微信公众号"汽车电子expert成长之路",大家感兴趣可以关注一下. 引言 1. S32K1xx系列MCU启动过程详解(startup_S ...
excel切片器_浅谈Excel ， PBI 切片器系列之二：重新认识切片器
浅谈Excel , PBI 切片器系列之二:重新认识切片器原创海峰上一期我们聊了切片器在PBI 的交互应用,也提到了切片器在Excel里的使用,这一期我觉得有必要从Excel开始来聊一下&quo ...

浅谈APM系列-----update_flight_mode（ModeAltHold）

update_flight_mode（ModeAltHold）

AltHold_MotorStopped

AltHold_Takeoff

AltHold_Landed

AltHold_Flying

浅谈APM系列-----update_flight_mode（ModeAltHold）相关推荐

最新文章

热门文章