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