// 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;
}
// 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;
}
}
}
// 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_cds)
{
// convert from centidegrees on public interface to radians
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);
// store roll, pitch and passthroughs
// NOTE: this abuses yaw_rate_bf_rads
_passthrough_roll = roll_passthrough;
_passthrough_pitch = pitch_passthrough;
_passthrough_yaw = degrees(yaw_rate_bf_rads)*100.0f;
// 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)
_attitude_target_ang_vel.x = _ahrs.get_gyro().x;
_attitude_target_ang_vel.y = _ahrs.get_gyro().y;
// accel limit desired yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
float rate_change_rads = yaw_rate_bf_rads - _attitude_target_ang_vel.z;
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
_attitude_target_ang_vel.z += rate_change_rads;
} else {
_attitude_target_ang_vel.z = yaw_rate_bf_rads;
}
integrate_bf_rate_error_to_angle_errors();
_att_error_rot_vec_rad.x = 0;
_att_error_rot_vec_rad.y = 0;
// update our earth-frame angle targets
Vector3f att_error_euler_rad;
// convert angle error rotation vector into 321-intrinsic euler angle difference
// NOTE: this results an an approximation linearized about the vehicle's attitude
if (ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) {
_attitude_target_euler_angle.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);
_attitude_target_euler_angle.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);
_attitude_target_euler_angle.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);
}
// handle flipping over pitch axis
if (_attitude_target_euler_angle.y > M_PI/2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
if (_attitude_target_euler_angle.y < -M_PI/2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(-M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
// convert body-frame angle errors to body-frame rate targets
_rate_target_ang_vel = update_ang_vel_target_from_att_error(_att_error_rot_vec_rad);
// set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
_rate_target_ang_vel.x = _attitude_target_ang_vel.x;
_rate_target_ang_vel.y = _attitude_target_ang_vel.y;
// add desired target to yaw
_rate_target_ang_vel.z += _attitude_target_ang_vel.z;
_thrust_error_angle = norm(_att_error_rot_vec_rad.x, _att_error_rot_vec_rad.y);
}
