void AC_AttitudeControl::update_ang_vel_target_from_att_error() { // Compute the roll angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_roll_max > 0.0f) { _ang_vel_target_rads.x = sqrt_controller(_att_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.x = _p_angle_roll.kP() * _att_error_rot_vec_rad.x; } // Compute the pitch angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_pitch_max > 0.0f) { _ang_vel_target_rads.y = sqrt_controller(_att_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.y = _p_angle_pitch.kP() * _att_error_rot_vec_rad.y; } // Compute the yaw angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_yaw_max > 0.0f) { _ang_vel_target_rads.z = sqrt_controller(_att_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.z = _p_angle_yaw.kP() * _att_error_rot_vec_rad.z; } // Add feedforward term that attempts to ensure that the copter yaws about the reference // Z axis, rather than the vehicle body Z axis. // NOTE: This is a small-angle approximation. _ang_vel_target_rads.x += _att_error_rot_vec_rad.y * _ahrs.get_gyro().z; _ang_vel_target_rads.y += -_att_error_rot_vec_rad.x * _ahrs.get_gyro().z; }
// update_rate_bf_targets - converts body-frame angle error to body-frame rate targets for roll, pitch and yaw axis // targets rates in centi-degrees taken from _angle_bf_error // results in centi-degrees/sec put into _rate_bf_target void AC_AttitudeControl::update_rate_bf_targets() { // stab roll calculation // constrain roll rate request if (_flags.limit_angle_to_rate_request) { _rate_bf_target.x = sqrt_controller(_angle_bf_error.x, _p_angle_roll.kP(), constrain_float(_accel_roll_max/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX)); }else{ _rate_bf_target.x = _p_angle_roll.kP() * _angle_bf_error.x; } // stab pitch calculation // constrain pitch rate request if (_flags.limit_angle_to_rate_request) { _rate_bf_target.y = sqrt_controller(_angle_bf_error.y, _p_angle_pitch.kP(), constrain_float(_accel_pitch_max/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX)); }else{ _rate_bf_target.y = _p_angle_pitch.kP() * _angle_bf_error.y; } // stab yaw calculation // constrain yaw rate request if (_flags.limit_angle_to_rate_request) { _rate_bf_target.z = sqrt_controller(_angle_bf_error.z, _p_angle_yaw.kP(), constrain_float(_accel_yaw_max/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX)); }else{ _rate_bf_target.z = _p_angle_yaw.kP() * _angle_bf_error.z; } // include roll and pitch rate required to account for precession of the desired attitude about the body frame yaw axes _rate_bf_target.x += _angle_bf_error.y * _ahrs.get_gyro_for_control().z; _rate_bf_target.y += -_angle_bf_error.x * _ahrs.get_gyro_for_control().z; }
// Update rate_target_ang_vel using attitude_error_rot_vec_rad Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(Vector3f attitude_error_rot_vec_rad) { Vector3f rate_target_ang_vel; // Compute the roll angular velocity demand from the roll angle error if (_use_ff_and_input_shaping) { rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt); }else{ rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x; } // Compute the pitch angular velocity demand from the roll angle error if (_use_ff_and_input_shaping) { rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt); }else{ rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y; } // Compute the yaw angular velocity demand from the roll angle error if (_use_ff_and_input_shaping) { rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt); }else{ rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z; } return rate_target_ang_vel; }
void AC_AttitudeControl::update_ang_vel_target_from_att_error() { // Compute the roll angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_roll_max > 0.0f) { _ang_vel_target_rads.x = sqrt_controller(_att_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.x = _p_angle_roll.kP() * _att_error_rot_vec_rad.x; } // Compute the pitch angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_pitch_max > 0.0f) { _ang_vel_target_rads.y = sqrt_controller(_att_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.y = _p_angle_pitch.kP() * _att_error_rot_vec_rad.y; } // Compute the yaw angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_yaw_max > 0.0f) { _ang_vel_target_rads.z = sqrt_controller(_att_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.z = _p_angle_yaw.kP() * _att_error_rot_vec_rad.z; } // Account for precession of desired attitude about the body frame yaw axis _ang_vel_target_rads.x += _att_error_rot_vec_rad.y * _ahrs.get_gyro().z; _ang_vel_target_rads.y += -_att_error_rot_vec_rad.x * _ahrs.get_gyro().z; }
// calculates the velocity correction from an angle error. The angular velocity has acceleration and // deceleration limits including basic jerk limiting using smoothing_gain float AC_AttitudeControl::input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel, float dt) { // Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss float ang_vel = sqrt_controller(error_angle, smoothing_gain, accel_max, dt); // Acceleration is limited directly to smooth the beginning of the curve. if (accel_max > 0) { float delta_ang_vel = accel_max * dt; return constrain_float(ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel); } else { return ang_vel; } }
/// run horizontal position controller correcting position and velocity /// converts position (_pos_target) to target velocity (_vel_target) /// desired velocity (_vel_desired) is combined into final target velocity /// converts desired velocities in lat/lon directions to accelerations in lat/lon frame /// converts desired accelerations provided in lat/lon frame to roll/pitch angles void AC_PosControl::run_xy_controller(float dt) { float ekfGndSpdLimit, ekfNavVelGainScaler; AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); Vector3f curr_pos = _inav.get_position(); float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF // avoid divide by zero if (kP <= 0.0f) { _vel_target.x = 0.0f; _vel_target.y = 0.0f; }else{ // calculate distance error _pos_error.x = _pos_target.x - curr_pos.x; _pos_error.y = _pos_target.y - curr_pos.y; // Constrain _pos_error and target position // Constrain the maximum length of _vel_target to the maximum position correction velocity // TODO: replace the leash length with a user definable maximum position correction if (limit_vector_length(_pos_error.x, _pos_error.y, _leash)) { _pos_target.x = curr_pos.x + _pos_error.x; _pos_target.y = curr_pos.y + _pos_error.y; } _vel_target = sqrt_controller(_pos_error, kP, _accel_cms); } // add velocity feed-forward _vel_target.x += _vel_desired.x; _vel_target.y += _vel_desired.y; // the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d; // check if vehicle velocity is being overridden if (_flags.vehicle_horiz_vel_override) { _flags.vehicle_horiz_vel_override = false; } else { _vehicle_horiz_vel.x = _inav.get_velocity().x; _vehicle_horiz_vel.y = _inav.get_velocity().y; } // calculate velocity error _vel_error.x = _vel_target.x - _vehicle_horiz_vel.x; _vel_error.y = _vel_target.y - _vehicle_horiz_vel.y; // TODO: constrain velocity error and velocity target // call pi controller _pid_vel_xy.set_input(_vel_error); // get p vel_xy_p = _pid_vel_xy.get_p(); // update i term if we have not hit the accel or throttle limits OR the i term will reduce // TODO: move limit handling into the PI and PID controller if (!_limit.accel_xy && !_motors.limit.throttle_upper) { vel_xy_i = _pid_vel_xy.get_i(); } else { vel_xy_i = _pid_vel_xy.get_i_shrink(); } // get d vel_xy_d = _pid_vel_xy.get_d(); // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler; accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler; // reset accel to current desired acceleration if (_flags.reset_accel_to_lean_xy) { _accel_target_filter.reset(Vector2f(accel_target.x, accel_target.y)); _flags.reset_accel_to_lean_xy = false; } // filter correction acceleration _accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler)); _accel_target_filter.apply(accel_target, dt); // pass the correction acceleration to the target acceleration output _accel_target.x = _accel_target_filter.get().x; _accel_target.y = _accel_target_filter.get().y; // Add feed forward into the target acceleration output _accel_target.x += _accel_desired.x; _accel_target.y += _accel_desired.y; // the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles // limit acceleration using maximum lean angles float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd()); float accel_max = MIN(GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)), POSCONTROL_ACCEL_XY_MAX); _limit.accel_xy = limit_vector_length(_accel_target.x, _accel_target.y, accel_max); // update angle targets that will be passed to stabilize controller accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target); }
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter // smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain) { float rate_ef_desired; float rate_change_limit; Vector3f angle_ef_error; // earth frame angle errors // sanity check smoothing gain smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f); // if accel limiting and feed forward enabled if ((_accel_roll_max > 0.0f) && _rate_bf_ff_enabled) { rate_change_limit = _accel_roll_max * _dt; // calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away rate_ef_desired = sqrt_controller(roll_angle_ef-_angle_ef_target.x, smoothing_gain, _accel_roll_max); // apply acceleration limit to feed forward roll rate _rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit); // update earth-frame roll angle target using desired roll rate update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); } else { // target roll and pitch to desired input roll and pitch _angle_ef_target.x = roll_angle_ef; angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor); // set roll and pitch feed forward to zero _rate_ef_desired.x = 0; } // constrain earth-frame angle targets _angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max); // if accel limiting and feed forward enabled if ((_accel_pitch_max > 0.0f) && _rate_bf_ff_enabled) { rate_change_limit = _accel_pitch_max * _dt; // calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away rate_ef_desired = sqrt_controller(pitch_angle_ef-_angle_ef_target.y, smoothing_gain, _accel_pitch_max); // apply acceleration limit to feed forward pitch rate _rate_ef_desired.y = constrain_float(rate_ef_desired, _rate_ef_desired.y-rate_change_limit, _rate_ef_desired.y+rate_change_limit); // update earth-frame pitch angle target using desired pitch rate update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); } else { // target roll and pitch to desired input roll and pitch _angle_ef_target.y = pitch_angle_ef; angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor); // set roll and pitch feed forward to zero _rate_ef_desired.y = 0; } // constrain earth-frame angle targets _angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max); if (_accel_yaw_max > 0.0f) { // set earth-frame feed forward rate for yaw rate_change_limit = _accel_yaw_max * _dt; // update yaw rate target with acceleration limit _rate_ef_desired.z += constrain_float(yaw_rate_ef - _rate_ef_desired.z, -rate_change_limit, rate_change_limit); // calculate yaw target angle and angle error update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); } else { // set yaw feed forward to zero _rate_ef_desired.z = yaw_rate_ef; // calculate yaw target angle and angle error update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); } // convert earth-frame angle errors to body-frame angle errors frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error); // convert body-frame angle errors to body-frame rate targets update_rate_bf_targets(); // add body frame rate feed forward if (_rate_bf_ff_enabled) { // convert earth-frame feed forward rates to body-frame feed forward rates frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired); _rate_bf_target += _rate_bf_desired; } else { // convert earth-frame feed forward rates to body-frame feed forward rates frame_conversion_ef_to_bf(Vector3f(0,0,_rate_ef_desired.z), _rate_bf_desired); _rate_bf_target += _rate_bf_desired; } // body-frame to motor outputs should be called separately }
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_smooth(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_rad = radians(euler_roll_angle_cd*0.01f); float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f); float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f); // Sanity check smoothing gain smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f); // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors) euler_roll_angle_rad += get_roll_trim_rad(); if ((get_accel_roll_max_radss() > 0.0f) && _rate_bf_ff_enabled) { // When roll acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler roll-axis // angular velocity that will cause the euler roll angle to smoothly stop at the input angle with limited deceleration // and an exponential decay specified by smoothing_gain at the end. float euler_rate_desired_rads = sqrt_controller(euler_roll_angle_rad-_att_target_euler_rad.x, smoothing_gain, get_accel_roll_max_radss()); // Acceleration is limited directly to smooth the beginning of the curve. float rate_change_limit_rads = get_accel_roll_max_radss() * _dt; _att_target_euler_rate_rads.x = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.x-rate_change_limit_rads, _att_target_euler_rate_rads.x+rate_change_limit_rads); // The output rate is used to update the attitude target euler angles and is fed forward into the rate controller. update_att_target_roll(_att_target_euler_rate_rads.x, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD); } else { // When acceleration limiting and feedforward are not enabled, the target roll euler angle is simply set to the // input value and the feedforward rate is zeroed. _att_target_euler_rad.x = euler_roll_angle_rad; _att_target_euler_rate_rads.x = 0; } _att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad()); if ((get_accel_pitch_max_radss() > 0.0f) && _rate_bf_ff_enabled) { // When pitch acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler pitch-axis // angular velocity that will cause the euler pitch angle to smoothly stop at the input angle with limited deceleration // and an exponential decay specified by smoothing_gain at the end. float euler_rate_desired_rads = sqrt_controller(euler_pitch_angle_rad-_att_target_euler_rad.y, smoothing_gain, get_accel_pitch_max_radss()); // Acceleration is limited directly to smooth the beginning of the curve. float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt; _att_target_euler_rate_rads.y = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.y-rate_change_limit_rads, _att_target_euler_rate_rads.y+rate_change_limit_rads); // The output rate is used to update the attitude target euler angles and is fed forward into the rate controller. update_att_target_pitch(_att_target_euler_rate_rads.y, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD); } else { _att_target_euler_rad.y = euler_pitch_angle_rad; _att_target_euler_rate_rads.y = 0; } _att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad()); if (get_accel_yaw_max_radss() > 0.0f) { // 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. float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt; _att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads); // The output rate is used to update the attitude target euler angles and is fed forward into the rate controller. update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD); } else { // When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate // is fed forward into the rate controller. _att_target_euler_rate_rads.z = euler_yaw_rate_rads; update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD); } // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward if (_rate_bf_ff_enabled) { euler_rate_to_ang_vel(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads); } else { euler_rate_to_ang_vel(_att_target_euler_rad, Vector3f(0,0,_att_target_euler_rate_rads.z), _att_target_ang_vel_rads); } // Call attitude controller attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads); }
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter // smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp此函数通过目标角度计算出所需要的角速度,并更新了误差值 void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain) { float rate_ef_desired;//地面坐标系下的速度 float rate_change_limit; Vector3f angle_ef_error; // earth frame angle errors地面坐标系下角度误差 // sanity check smoothing gain smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);//把smoothing_gain约束在1到5之间,1的话反应迟钝,50反应迅速,反应慢的话在有风的情况下可能导致坠机 // if accel limiting and feed forward enabled if ((_accel_roll_max > 0.0f) && _rate_bf_ff_enabled) {//如果roll角加速度(地面坐标系)最大值大于0且允许速度前馈? rate_change_limit = _accel_roll_max * _dt;//_dt时间间隔,以秒为单位,此函数代表roll在单位时间内改变速率的最大值?即角加速度最大值? // calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away //计算需要的速度 rate_ef_desired = sqrt_controller(roll_angle_ef-_angle_ef_target.x, smoothing_gain, _accel_roll_max);//target为实时目标点,不断更新,而遥控器的信号也是不断更新 // apply acceleration limit to feed forward roll rate限制角速度大小 _rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit); // update earth-frame roll angle target using desired roll rate回传角度误差,第三个参数是什么 update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); } else { // target roll and pitch to desired input roll and pitch _angle_ef_target.x = roll_angle_ef;//目标角度为滚转角 angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);//角度误差为目标角度减去传感器的角度,限制在180度之内 // set roll and pitch feed forward to zero _rate_ef_desired.x = 0;//所需要的滚转角速度设为0 } // constrain earth-frame angle targets _angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);//约束目标角度 // if accel limiting and feed forward enabled if ((_accel_pitch_max > 0.0f) && _rate_bf_ff_enabled) { rate_change_limit = _accel_pitch_max * _dt; // calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away rate_ef_desired = sqrt_controller(pitch_angle_ef-_angle_ef_target.y, smoothing_gain, _accel_pitch_max); // apply acceleration limit to feed forward pitch rate _rate_ef_desired.y = constrain_float(rate_ef_desired, _rate_ef_desired.y-rate_change_limit, _rate_ef_desired.y+rate_change_limit); // update earth-frame pitch angle target using desired pitch rate update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); } else { // target roll and pitch to desired input roll and pitch _angle_ef_target.y = pitch_angle_ef; angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor); // set roll and pitch feed forward to zero _rate_ef_desired.y = 0; } // constrain earth-frame angle targets _angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max); if (_accel_yaw_max > 0.0f) { // set earth-frame feed forward rate for yaw rate_change_limit = _accel_yaw_max * _dt; // update yaw rate target with acceleration limit _rate_ef_desired.z += constrain_float(yaw_rate_ef - _rate_ef_desired.z, -rate_change_limit, rate_change_limit); // calculate yaw target angle and angle error update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); } else { // set yaw feed forward to zero _rate_ef_desired.z = yaw_rate_ef; // calculate yaw target angle and angle error update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); } // convert earth-frame angle errors to body-frame angle errors frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error); // convert body-frame angle errors to body-frame rate targets update_rate_bf_targets(); // add body frame rate feed forward if (_rate_bf_ff_enabled) { // convert earth-frame feed forward rates to body-frame feed forward rates frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired); _rate_bf_target += _rate_bf_desired; } else { // convert earth-frame feed forward rates to body-frame feed forward rates frame_conversion_ef_to_bf(Vector3f(0,0,_rate_ef_desired.z), _rate_bf_desired); _rate_bf_target += _rate_bf_desired; } // body-frame to motor outputs should be called separately }