/*
update state in _control_monitor
*/
void AC_AttitudeControl::control_monitor_update(void)
{
const DataFlash_Class::PID_Info &iroll = get_rate_roll_pid().get_pid_info();
control_monitor_filter_pid(iroll.P + iroll.FF, _control_monitor.rms_roll_P);
control_monitor_filter_pid(iroll.D, _control_monitor.rms_roll_D);
const DataFlash_Class::PID_Info &ipitch = get_rate_pitch_pid().get_pid_info();
control_monitor_filter_pid(ipitch.P + iroll.FF, _control_monitor.rms_pitch_P);
control_monitor_filter_pid(ipitch.D, _control_monitor.rms_pitch_D);
const DataFlash_Class::PID_Info &iyaw = get_rate_yaw_pid().get_pid_info();
control_monitor_filter_pid(iyaw.P + iyaw.D + iyaw.FF, _control_monitor.rms_yaw);
}
void AC_AttitudeControl::relax_bf_rate_controller()
{
// Set reference angular velocity used in angular velocity controller equal
// to the input angular velocity and reset the angular velocity integrators.
// This zeros the output of the angular velocity controller.
_ang_vel_target_rads = _ahrs.get_gyro();
get_rate_roll_pid().reset_I();
get_rate_pitch_pid().reset_I();
get_rate_yaw_pid().reset_I();
// Write euler derivatives derived from vehicle angular velocity to
// _att_target_euler_rate_rads. This resets the state of the input shapers.
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
}
// Ensure attitude controller have zero errors to relax rate controller output
void AC_AttitudeControl::relax_attitude_controllers()
{
// TODO add _ahrs.get_quaternion()
_attitude_target_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
_attitude_target_ang_vel = _ahrs.get_gyro();
_attitude_target_euler_angle = Vector3f(_ahrs.roll, _ahrs.pitch, _ahrs.yaw);
// Set reference angular velocity used in angular velocity controller equal
// to the input angular velocity and reset the angular velocity integrators.
// This zeros the output of the angular velocity controller.
_rate_target_ang_vel = _ahrs.get_gyro();
get_rate_roll_pid().reset_I();
get_rate_pitch_pid().reset_I();
get_rate_yaw_pid().reset_I();
}
// Run the pitch angular velocity PID controller and return the output
float AC_AttitudeControl::rate_target_to_motor_pitch(float rate_actual_rads, float rate_target_rads)
{
float rate_error_rads = rate_target_rads - rate_actual_rads;
// pass error to PID controller
get_rate_pitch_pid().set_input_filter_d(rate_error_rads);
get_rate_pitch_pid().set_desired_rate(rate_target_rads);
float integrator = get_rate_pitch_pid().get_integrator();
// Ensure that integrator can only be reduced if the output is saturated
if (!_motors.limit.roll_pitch || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) {
integrator = get_rate_pitch_pid().get_i();
}
// Compute output in range -1 ~ +1
float output = get_rate_pitch_pid().get_p() + integrator + get_rate_pitch_pid().get_d() + get_rate_pitch_pid().get_ff(rate_target_rads);
// Constrain output
return constrain_float(output, -1.0f, 1.0f);
}
float AC_AttitudeControl::max_rate_step_bf_pitch()
{
float alpha = get_rate_pitch_pid().get_filt_alpha();
float alpha_remaining = 1-alpha;
return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_pitch_pid().kD())/_dt + get_rate_pitch_pid().kP());
}
void AC_AttitudeControl::reset_rate_controller_I_terms()
{
get_rate_roll_pid().reset_I();
get_rate_pitch_pid().reset_I();
get_rate_yaw_pid().reset_I();
}