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_up
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, -get_pilot_speed_dn(), g.pilot_speed_up);
// Alt Hold State Machine Determination
if (!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 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());
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 controller
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
heli_flags.init_targets_on_arming=true;
#else
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:
#if FRAME_CONFIG == HELI_FRAME
if (heli_flags.init_targets_on_arming) {
heli_flags.init_targets_on_arming=false;
}
#endif
// 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);
// 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);
}
#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;
}
}
#else
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
#endif
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;
case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#if AC_AVOID_ENABLED == ENABLED
// apply avoidance
copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max);
#endif
// call attitude controller
attitude_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 controller
pos_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 negative
speed_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 zero
if (accel_cms <= 0.0f) {
accel_cms = POSCONTROL_ACCELERATION_MIN;
}
// avoid divide by zero
if (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 zero
if (accel_cms <= 0.0f) {
accel_cms = POSCONTROL_ACCELERATION_MIN;
}
// avoid divide by zero
if (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 mode
if (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 perspective
channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw());//重新设置control_in
channel_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 angle
angle_max = constrain_float(angle_max, 1000, aparm.angle_max);//这里的angle_max == aparm.angle_max吧
// scale roll_in, pitch_in to ANGLE_MAX parameter range
float scaler = aparm.angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;//4500
roll_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)));
// return
roll_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 request
if (g2.acro_y_expo <= 0) {
yaw_request = stick_angle * g.acro_yaw_p;
} else {
// expo（exponent指数） variables
float y_in, y_in3, y_out;
// range check expo
if (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f) {
g2.acro_y_expo = 1.0f;
}
// yaw expo
y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;//4500
y_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 rate
return 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 check
if( failsafe.radio ) {
return 0.0f;
}
#if TOY_MODE_ENABLED == ENABLED
if (g2.toy_mode.enabled()) {
// allow throttle to be reduced after throttle arming and for
// slower descent close to the ground
g2.toy_mode.throttle_adjust(throttle_control);
}
#endif
float 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 value
throttle_control = constrain_float(throttle_control,0.0f,1000.0f);
// ensure a reasonable deadzone
g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);
// check throttle is above, below or in the deadband
if (throttle_control < deadband_bottom) {
// below the deadband
desired_rate = get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom;
}else if (throttle_control > deadband_top) {
// above the deadband
desired_rate = g.pilot_speed_up * (throttle_control-deadband_top) / (1000.0f-deadband_top);
}else{
// must be in the deadband
desired_rate = 0.0f;
}
return desired_rate;
}
位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.h
// Sets yaw target to vehicle heading
void 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 cast
uint32_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 reset
check_for_ekf_z_reset();
// check if leash lengths need to be recalculated
calc_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 targets
rate_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 target
if (_flags.reset_rate_to_accel_z) {
_vel_last.z = _vel_target.z;
}
// feed forward desired acceleration calculation
if (_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 p
p = _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 controller
accel_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 acceleration
float 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 engaged
if (_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 logging
p = _pid_accel_z.get_p();
// get i term
i = _pid_accel_z.get_integrator();
// ensure imax is always large enough to overpower hover throttle
if (_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 term
d = _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 disabled
if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) == 0 || !_proximity_enabled) {
return;
}
// exit immediately if angle max is zero
if (_angle_max <= 0.0f || veh_angle_max <= 0.0f) {
return;
}
float roll_positive = 0.0f; // maximum positive roll value
float roll_negative = 0.0f; // minimum negative roll value
float pitch_positive = 0.0f; // maximum position pitch value
float pitch_negative = 0.0f; // minimum negative pitch value
// get maximum positive and negative roll and pitch percentages from proximity sensor
get_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-degrees
Vector2f 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 override
const 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 angles
rp_out.x += roll;
rp_out.y += pitch;
// apply total angular limits
vec_len = rp_out.length();
if (vec_len > veh_angle_max) {
rp_out *= (veh_angle_max / vec_len);
}
// return adjusted roll, pitch
roll = 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 radians
float 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 overshoot
smoothing_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();//返回0
if (_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 controller
attitude_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 error
Vector3f 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 == ENABLED
avoid.adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), target_rate, G_Dt);
return target_rate;
#else
return 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 disabled
if (_enabled == AC_AVOID_DISABLED) {
return;
}
// do not adjust climb_rate if level or descending
if (climb_rate_cms <= 0.0f) {
return;
}
// limit acceleration
float 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 fence
if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && (_fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) {
// calculate distance from vehicle to safe altitude
float 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 metres
float 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 sensor
float 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 rate
if (limit_alt) {
// do not allow climbing if we've breached the safe altitude
if (alt_diff <= 0.0f) {
climb_rate_cms = MIN(climb_rate_cms, 0.0f);
return;
}
// limit climb rate
const 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 speed
float 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 acceleration
float 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 cast
uint32_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 reset
check_for_ekf_z_reset();
// check if leash lengths need to be recalculated
calc_leash_length_z();
// call position controller（调用位置控制器）
pos_to_rate_z();
}
到这里吧，都是水。
当飞机需要有位置移动的时候就需要有位置控制。
定点不需要移动，所以不需要位置控制。
定高不需要Z轴方向移动，所以Z周方向有位置控制。
。。。。。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
