// angle_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw angle (all earth frame)
//  if yaw_slew is true then target yaw movement will be gradually moved to the new target based on the SLEW_YAW parameter
void AC_AttitudeControl::angle_ef_roll_pitch_yaw(float roll_angle_ef, float pitch_angle_ef, float yaw_angle_ef, bool slew_yaw)
{
    Vector3f    angle_ef_error;

    // set earth-frame angle targets
    _angle_ef_target.x = constrain_float(roll_angle_ef, -_aparm.angle_max, _aparm.angle_max);
    _angle_ef_target.y = constrain_float(pitch_angle_ef, -_aparm.angle_max, _aparm.angle_max);
    _angle_ef_target.z = yaw_angle_ef;

    // calculate earth frame errors
    angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
    angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
    angle_ef_error.z = wrap_180_cd_float(_angle_ef_target.z - _ahrs.yaw_sensor);

    // constrain the yaw angle error
    if (slew_yaw) {
        angle_ef_error.z = constrain_float(angle_ef_error.z,-_slew_yaw,_slew_yaw);
    }

    // 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();

    // body-frame to motor outputs should be called separately
}
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request
void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error, float overshoot_max)
{
    // calculate angle error with maximum of +- max angle overshoot
    angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
    angle_ef_error.x  = constrain_float(angle_ef_error.x, -overshoot_max, overshoot_max);

    // update roll angle target to be within max angle overshoot of our roll angle
    _angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;

    // increment the roll angle target
    _angle_ef_target.x += roll_rate_ef * _dt;
    _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x);
}
// update_ef_pitch_angle_and_error - update _angle_ef_target.y using an earth frame pitch rate request
void AC_AttitudeControl::update_ef_pitch_angle_and_error(float pitch_rate_ef, Vector3f &angle_ef_error, float overshoot_max)
{
    // calculate angle error with maximum of +- max angle overshoot
    // To-Do: should we do something better as we cross 90 degrees?
    angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
    angle_ef_error.y  = constrain_float(angle_ef_error.y, -overshoot_max, overshoot_max);

    // update pitch angle target to be within max angle overshoot of our pitch angle
    _angle_ef_target.y = angle_ef_error.y + _ahrs.pitch_sensor;

    // increment the pitch angle target
    _angle_ef_target.y += pitch_rate_ef * _dt;
    _angle_ef_target.y = wrap_180_cd_float(_angle_ef_target.y);
}
// angle_ef_roll_pitch_rate_ef_yaw - attempts to maintain a roll and pitch angle and yaw rate (all earth frame)
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef)
{
    Vector3f    angle_ef_error;         // earth frame angle errors

    // set earth-frame angle targets for roll and pitch and calculate angle error
    _angle_ef_target.x = roll_angle_ef;
    angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);

    _angle_ef_target.y = pitch_angle_ef;
    angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);

    if (_accel_y_max > 0.0f) {
        // set earth-frame feed forward rate for yaw
        float rate_change_limit = _accel_y_max * _dt;

        float rate_change = yaw_rate_ef - _rate_ef_desired.z;
        rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
        _rate_ef_desired.z += rate_change;
        // calculate yaw target angle and angle error
        update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
    } else {
        // set yaw feed forward to zero
        _rate_ef_desired.z = 0;
        // calculate yaw target angle and angle error
        update_ef_yaw_angle_and_error(yaw_rate_ef, angle_ef_error);
    }

    // constrain earth-frame angle targets
    _angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
    _angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);

    // convert earth-frame angle errors to body-frame angle errors
    frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);

    // convert earth-frame feed forward rates to body-frame feed forward rates
    frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);

    // 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) {
        _rate_bf_target.z += _rate_bf_desired.z;
    }

    // body-frame to motor outputs should be called separately
}
Example #5
0
// angle_ef_roll_pitch_rate_ef_yaw - attempts to maintain a roll and pitch angle and yaw rate (all earth frame)
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef)
{
    Vector3f    angle_ef_error;         // earth frame angle errors

    // set earth-frame angle targets for roll and pitch and calculate angle error
    _angle_ef_target.x = constrain_float(roll_angle_ef, -_aparm.angle_max, _aparm.angle_max);
    angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);

    _angle_ef_target.y = constrain_float(pitch_angle_ef, -_aparm.angle_max, _aparm.angle_max);
    angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);

    if (_accel_yaw_max > 0.0f) {//当加速度大于0时,增加所需的速度,小于0时,速度保持不变,同时更新角度及误差
        // set earth-frame feed forward rate for yaw
        float rate_change_limit = _accel_yaw_max * _dt;

        float rate_change = yaw_rate_ef - _rate_ef_desired.z;//当前角速度-所需角速度
        rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
        _rate_ef_desired.z += rate_change;//计算所需要的速度值 
        // 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();//更新目标速度

    // set roll and pitch feed forward to zero
    _rate_ef_desired.x = 0;
    _rate_ef_desired.y = 0;
    // 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;//目标速度等于上一个周期的目标+所需的速度

    // body-frame to motor outputs should be called separately
}
// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf)
{
    // store roll, pitch and passthroughs
    _passthrough_roll = roll_passthrough;
    _passthrough_pitch = pitch_passthrough;
    _passthrough_yaw = yaw_rate_bf;

    // set rate controller to use pass through
    _flags_heli.flybar_passthrough = true;

    // set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)
    _rate_bf_desired.x = _ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100;
    _rate_bf_desired.y = _ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100;

    // accel limit desired yaw rate
    if (_accel_yaw_max > 0.0f) {
        float rate_change_limit = _accel_yaw_max * _dt;
        float rate_change = yaw_rate_bf - _rate_bf_desired.z;
        rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
        _rate_bf_desired.z += rate_change;
    } else {
        _rate_bf_desired.z = yaw_rate_bf;
    }

    integrate_bf_rate_error_to_angle_errors();
    _angle_bf_error.x = 0;
    _angle_bf_error.y = 0;

    // update our earth-frame angle targets
    Vector3f angle_ef_error;
    if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) {
        _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
        _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
        _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
    }

    // handle flipping over pitch axis
    if (_angle_ef_target.y > 9000.0f) {
        _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
        _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x);
        _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
    }
    if (_angle_ef_target.y < -9000.0f) {
        _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
        _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x);
        _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
    }

    // convert body-frame angle errors to body-frame rate targets
    update_rate_bf_targets();

    // set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
    _rate_bf_target.x = _rate_bf_desired.x;
    _rate_bf_target.y = _rate_bf_desired.y;

    // add desired target to yaw
    _rate_bf_target.z += _rate_bf_desired.z;
}
// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate request
void AC_AttitudeControl::update_ef_yaw_angle_and_error(float yaw_rate_ef, Vector3f &angle_ef_error, float overshoot_max)
{
    // calculate angle error with maximum of +- max angle overshoot
    angle_ef_error.z = wrap_180_cd_float(_angle_ef_target.z - _ahrs.get_yaw_for_control_cd());
    angle_ef_error.z  = constrain_float(angle_ef_error.z, -overshoot_max, overshoot_max);

    // update yaw angle target to be within max angle overshoot of our current heading
    _angle_ef_target.z = angle_ef_error.z + _ahrs.get_yaw_for_control_cd();


    // increment the yaw angle target
    _angle_ef_target.z += yaw_rate_ef * _dt;
    _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z);
}
Example #8
0
/*
  check current solution against CHEK message
 */
void Replay::log_check_solution(void)
{
    const LR_MsgHandler::CheckState &check_state = logreader.get_check_state();
    Vector3f euler;
    Vector3f velocity;
    Location loc {};

    _vehicle.EKF.getEulerAngles(euler);
    _vehicle.EKF.getVelNED(velocity);
    _vehicle.EKF.getLLH(loc);

    float roll_error  = degrees(fabsf(euler.x - check_state.euler.x));
    float pitch_error = degrees(fabsf(euler.y - check_state.euler.y));
    float yaw_error = wrap_180_cd_float(100*degrees(fabsf(euler.z - check_state.euler.z)))*0.01f;
    float vel_error = (velocity - check_state.velocity).length();
    float pos_error = get_distance(check_state.pos, loc);

    check_result.max_roll_error  = max(check_result.max_roll_error,  roll_error);
    check_result.max_pitch_error = max(check_result.max_pitch_error, pitch_error);
    check_result.max_yaw_error   = max(check_result.max_yaw_error,   yaw_error);
    check_result.max_vel_error   = max(check_result.max_vel_error,   vel_error);
    check_result.max_pos_error   = max(check_result.max_pos_error,   pos_error);
}
// 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)
{
    Vector3f angle_ef_error;    // earth frame angle errors
    float rate_change_limit;

    // sanity check smoothing gain
    smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);

    float linear_angle = _accel_rp_max/(smoothing_gain*smoothing_gain);
    rate_change_limit = _accel_rp_max * _dt;
    float rate_ef_desired;
    float angle_to_target;

    if (_accel_rp_max > 0.0f) {

    	// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
    	angle_to_target = roll_angle_ef - _angle_ef_target.x;
    	if (angle_to_target > linear_angle) {
    		rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
    	} else if (angle_to_target < -linear_angle) {
    		rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
    	} else {
    		rate_ef_desired = smoothing_gain*angle_to_target;
    	}
    	_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);

    	// calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away
    	angle_to_target = pitch_angle_ef - _angle_ef_target.y;
    	if (angle_to_target > linear_angle) {
    		rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
    	} else if (angle_to_target < -linear_angle) {
    		rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
    	} else {
    		rate_ef_desired = smoothing_gain*angle_to_target;
    	}
    	_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);
    } 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);

    	_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.x = 0;
    	_rate_ef_desired.y = 0;
    }

    if (_accel_y_max > 0.0f) {
    	// set earth-frame feed forward rate for yaw
        rate_change_limit = _accel_y_max * _dt;

        float rate_change = yaw_rate_ef - _rate_ef_desired.z;
        rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
        _rate_ef_desired.z += rate_change;
        // calculate yaw target angle and angle error
        update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
    } else {
        // set yaw feed forward to zero
    	_rate_ef_desired.z = 0;
        // calculate yaw target angle and angle error
        update_ef_yaw_angle_and_error(yaw_rate_ef, angle_ef_error);
    }

    // constrain earth-frame angle targets
    _angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
    _angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);

    // convert earth-frame angle errors to body-frame angle errors
    frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);

    // convert earth-frame feed forward rates to body-frame feed forward rates
    frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);

    // 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) {
        _rate_bf_target += _rate_bf_desired;
    }

    // body-frame to motor outputs should be called separately
}
// rate_bf_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all body frame)
void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf)
{
    Vector3f    angle_ef_error;

    // Update angle error
    if (labs(_ahrs.pitch_sensor)<_acro_angle_switch) {
        _acro_angle_switch = 6000;
        // convert body-frame rates to earth-frame rates
        frame_conversion_bf_to_ef(_rate_bf_desired, _rate_ef_desired);

        // update earth frame angle targets and errors
        update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error);
        update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error);
        update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);

        // convert earth-frame angle errors to body-frame angle errors
        frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
    } else {
        _acro_angle_switch = 4500;
        integrate_bf_rate_error_to_angle_errors();
        frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error);
        _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
        _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
        _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
        if (_angle_ef_target.y > 9000.0f) {
            _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
            _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x);
            _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
        }
        if (_angle_ef_target.y < -9000.0f) {
            _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
            _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x);
            _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
        }
    }

    // convert body-frame angle errors to body-frame rate targets
    update_rate_bf_targets();

    float rate_change, rate_change_limit;

    // update the rate feed forward with angular acceleration limits 
    if (_accel_rp_max > 0.0f) {
    	rate_change_limit = _accel_rp_max * _dt;

    	rate_change = roll_rate_bf - _rate_bf_desired.x;
    	rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
    	_rate_bf_desired.x += rate_change;
    
    	rate_change = pitch_rate_bf - _rate_bf_desired.y;
    	rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
    	_rate_bf_desired.y += rate_change;
    } else {
    	_rate_bf_desired.x = roll_rate_bf;
    	_rate_bf_desired.y = pitch_rate_bf;
    }

    if (_accel_y_max > 0.0f) {
        rate_change_limit = _accel_y_max * _dt;

        rate_change = yaw_rate_bf - _rate_bf_desired.z;
        rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
        _rate_bf_desired.z += rate_change;
    } else {
    	_rate_bf_desired.z = yaw_rate_bf;
    }

    // body-frame rate commands added
    if (_rate_bf_ff_enabled) {
        if (_accel_rp_max > 0.0f) {
            _rate_bf_target.x += _rate_bf_desired.x;
            _rate_bf_target.y += _rate_bf_desired.y;
        }
        if (_accel_y_max > 0.0f) {
            _rate_bf_target.z += _rate_bf_desired.z;
        }
    }
}
// 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
}
Example #12
0
// 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
}