// 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); }
// 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; } } }
// shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centi-degreesa and is added to the current target heading void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd) { _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + yaw_shift_cd); }