aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: support for upgrade to PID object

Browse Source
This commit is contained in:
Leonard Hall 2019-07-16 15:02:58 +09:00 committed by Randy Mackay
parent ee820258ad
commit bbe33e38f3
4 changed files with 182 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -135,7 +135,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
AP_GROUPINFO("RATE_Y_MAX", 19, AC_AttitudeControl, _ang_vel_yaw_max, 0.0f),
// @Param: INPUT_TC
// @DisplayName: Attitude control input time constant (aka smoothing)
// @DisplayName: Attitude control input time constant
// @Description: Attitude control input time constant. Low numbers lead to sharper response, higher numbers to softer response
// @Units: s
// @Range: 0 1
@ -927,75 +927,6 @@ Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f
return rate_target_ang_vel;
}
// Run the roll angular velocity PID controller and return the output
float AC_AttitudeControl::rate_target_to_motor_roll(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_roll_pid().set_input_filter_d(rate_error_rads);
get_rate_roll_pid().set_desired_rate(rate_target_rads);
float integrator = get_rate_roll_pid().get_integrator();
// Ensure that integrator can only be reduced if the output is saturated
if (!_motors.limit.roll_pitch || ((is_positive(integrator) && is_negative(rate_error_rads)) || (is_negative(integrator) && is_positive(rate_error_rads)))) {
integrator = get_rate_roll_pid().get_i();
}
// Compute output in range -1 ~ +1
float output = get_rate_roll_pid().get_p() + integrator + get_rate_roll_pid().get_d() + get_rate_roll_pid().get_ff(rate_target_rads);
// Constrain output
return output;
}
// 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 || ((is_positive(integrator) && is_negative(rate_error_rads)) || (is_negative(integrator) && is_positive(rate_error_rads)))) {
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 output;
}
// Run the yaw angular velocity PID controller and return the output
float AC_AttitudeControl::rate_target_to_motor_yaw(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_yaw_pid().set_input_filter_all(rate_error_rads);
get_rate_yaw_pid().set_desired_rate(rate_target_rads);
float integrator = get_rate_yaw_pid().get_integrator();
// Ensure that integrator can only be reduced if the output is saturated
if (!_motors.limit.yaw || ((is_positive(integrator) && is_negative(rate_error_rads)) || (is_negative(integrator) && is_positive(rate_error_rads)))) {
integrator = get_rate_yaw_pid().get_i();
}
// Compute output in range -1 ~ +1
float output = get_rate_yaw_pid().get_p() + integrator + get_rate_yaw_pid().get_d() + get_rate_yaw_pid().get_ff(rate_target_rads);
// Constrain output
return output;
}
// Enable or disable body-frame feed forward
void AC_AttitudeControl::accel_limiting(bool enable_limits)
{
@ -1090,7 +1021,7 @@ float AC_AttitudeControl::stopping_point(float first_ord_mag, float p, float sec
// Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_roll()
{
float alpha = get_rate_roll_pid().get_filt_alpha();
float alpha = MIN(get_rate_roll_pid().get_filt_E_alpha(), get_rate_roll_pid().get_filt_D_alpha());
float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
@ -1100,7 +1031,7 @@ float AC_AttitudeControl::max_rate_step_bf_roll()
// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_pitch()
{
float alpha = get_rate_pitch_pid().get_filt_alpha();
float alpha = MIN(get_rate_pitch_pid().get_filt_E_alpha(), get_rate_pitch_pid().get_filt_D_alpha());
float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
@ -1110,7 +1041,7 @@ float AC_AttitudeControl::max_rate_step_bf_pitch()
// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_yaw()
{
float alpha = get_rate_yaw_pid().get_filt_alpha();
float alpha = MIN(get_rate_yaw_pid().get_filt_E_alpha(), get_rate_yaw_pid().get_filt_D_alpha());
float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);

9
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -314,15 +314,6 @@ protected:
// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f update_ang_vel_target_from_att_error(const Vector3f &attitude_error_rot_vec_rad);
// Run the roll angular velocity PID controller and return the output
float rate_target_to_motor_roll(float rate_actual_rads, float rate_target_rads);
// Run the pitch angular velocity PID controller and return the output
float rate_target_to_motor_pitch(float rate_actual_rads, float rate_target_rads);
// Run the yaw angular velocity PID controller and return the output
virtual float rate_target_to_motor_yaw(float rate_actual_rads, float rate_target_rads);
// Return angle in radians to be added to roll angle. Used by heli to counteract
// tail rotor thrust in hover. Overloaded by AC_Attitude_Heli to return angle.
virtual float get_roll_trim_rad() { return 0;}

116
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -10,7 +10,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Param: RAT_RLL_P
// @DisplayName: Roll axis rate controller P gain
// @Description: Roll axis rate controller P gain. Converts the difference between desired roll rate and actual roll rate into a motor speed output
// @Range: 0.05 0.5
// @Range: 0.01 0.5
// @Increment: 0.005
// @User: Standard
@ -32,7 +32,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Param: RAT_RLL_D
// @DisplayName: Roll axis rate controller D gain
// @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
// @Range: 0.0 0.02
// @Range: 0.0 0.05
// @Increment: 0.001
// @User: Standard
@ -43,9 +43,25 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Increment: 0.001
// @User: Standard
// @Param: RAT_RLL_FILT
// @DisplayName: Roll axis rate controller input frequency in Hz
// @Description: Roll axis rate controller input frequency in Hz
// @Param: RAT_RLL_FLTT
// @DisplayName: Roll axis rate controller target frequency in Hz
// @Description: Roll axis rate controller target frequency in Hz
// @Range: 1 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_FLTE
// @DisplayName: Roll axis rate controller error frequency in Hz
// @Description: Roll axis rate controller error frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_FLTD
// @DisplayName: Roll axis rate controller derivative frequency in Hz
// @Description: Roll axis rate controller derivative frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
@ -55,7 +71,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Param: RAT_PIT_P
// @DisplayName: Pitch axis rate controller P gain
// @Description: Pitch axis rate controller P gain. Converts the difference between desired pitch rate and actual pitch rate into a motor speed output
// @Range: 0.05 0.50
// @Range: 0.01 0.50
// @Increment: 0.005
// @User: Standard
@ -77,7 +93,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Param: RAT_PIT_D
// @DisplayName: Pitch axis rate controller D gain
// @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate
// @Range: 0.0 0.02
// @Range: 0.0 0.05
// @Increment: 0.001
// @User: Standard
@ -88,9 +104,25 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Increment: 0.001
// @User: Standard
// @Param: RAT_PIT_FILT
// @DisplayName: Pitch axis rate controller input frequency in Hz
// @Description: Pitch axis rate controller input frequency in Hz
// @Param: RAT_PIT_FLTT
// @DisplayName: Pitch axis rate controller target frequency in Hz
// @Description: Pitch axis rate controller target frequency in Hz
// @Range: 1 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FLTE
// @DisplayName: Pitch axis rate controller error frequency in Hz
// @Description: Pitch axis rate controller error frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FLTD
// @DisplayName: Pitch axis rate controller derivative frequency in Hz
// @Description: Pitch axis rate controller derivative frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
@ -133,13 +165,29 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Increment: 0.001
// @User: Standard
// @Param: RAT_YAW_FILT
// @DisplayName: Yaw axis rate controller input frequency in Hz
// @Description: Yaw axis rate controller input frequency in Hz
// @Param: RAT_YAW_FLTT
// @DisplayName: Yaw axis rate controller target frequency in Hz
// @Description: Yaw axis rate controller target frequency in Hz
// @Range: 1 5
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FLTE
// @DisplayName: Yaw axis rate controller error frequency in Hz
// @Description: Yaw axis rate controller error frequency in Hz
// @Range: 1 10
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FLTD
// @DisplayName: Yaw axis rate controller derivative frequency in Hz
// @Description: Yaw axis rate controller derivative frequency in Hz
// @Range: 1 20
// @Increment: 1
// @Units: Hz
// @User: Standard
AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Multi, AC_PID),
// @Param: THR_MIX_MIN
@ -163,15 +211,39 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @User: Advanced
AP_GROUPINFO("THR_MIX_MAN", 6, AC_AttitudeControl_Multi, _thr_mix_man, AC_ATTITUDE_CONTROL_MAN_DEFAULT),
// @Param: RAT_RLL_FILT
// @DisplayName: Roll axis rate controller input frequency in Hz
// @Description: Roll axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FILT
// @DisplayName: Pitch axis rate controller input frequency in Hz
// @Description: Pitch axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FILT
// @DisplayName: Yaw axis rate controller input frequency in Hz
// @Description: Yaw axis rate controller input frequency in Hz
// @Range: 1 10
// @Increment: 1
// @Units: Hz
// @User: Standard
AP_GROUPEND
};
AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) :
AC_AttitudeControl(ahrs, aparm, motors, dt),
_motors_multi(motors),
_pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt),
_pid_rate_pitch(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt),
_pid_rate_yaw(AC_ATC_MULTI_RATE_YAW_P, AC_ATC_MULTI_RATE_YAW_I, AC_ATC_MULTI_RATE_YAW_D, AC_ATC_MULTI_RATE_YAW_IMAX, AC_ATC_MULTI_RATE_YAW_FILT_HZ, dt)
_pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt),
_pid_rate_pitch(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt),
_pid_rate_yaw(AC_ATC_MULTI_RATE_YAW_P, AC_ATC_MULTI_RATE_YAW_I, AC_ATC_MULTI_RATE_YAW_D, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, AC_ATC_MULTI_RATE_YAW_FILT_HZ, 0.0f, dt)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -256,9 +328,15 @@ void AC_AttitudeControl_Multi::rate_controller_run()
update_throttle_rpy_mix();
Vector3f gyro_latest = _ahrs.get_gyro_latest();
_motors.set_roll(rate_target_to_motor_roll(gyro_latest.x, _rate_target_ang_vel.x));
_motors.set_pitch(rate_target_to_motor_pitch(gyro_latest.y, _rate_target_ang_vel.y));
_motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _rate_target_ang_vel.z));
_motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll_pitch));
_motors.set_roll_ff(get_rate_roll_pid().get_ff());
_motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.roll_pitch));
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
_motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw));
_motors.set_yaw_ff(get_rate_yaw_pid().get_ff());
control_monitor_update();
}

90
libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp
View File

@ -43,7 +43,23 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
// @Increment: 0.001
// @User: Standard
// @Param: RAT_RLL_FILT
// @Param: RAT_RLL_FLTT
// @DisplayName: Roll axis rate controller input frequency in Hz
// @Description: Roll axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_FLTE
// @DisplayName: Roll axis rate controller input frequency in Hz
// @Description: Roll axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_FLTD
// @DisplayName: Roll axis rate controller input frequency in Hz
// @Description: Roll axis rate controller input frequency in Hz
// @Range: 1 100
@ -88,7 +104,23 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
// @Increment: 0.001
// @User: Standard
// @Param: RAT_PIT_FILT
// @Param: RAT_PIT_FLTT
// @DisplayName: Pitch axis rate controller input frequency in Hz
// @Description: Pitch axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FLTE
// @DisplayName: Pitch axis rate controller input frequency in Hz
// @Description: Pitch axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FLTD
// @DisplayName: Pitch axis rate controller input frequency in Hz
// @Description: Pitch axis rate controller input frequency in Hz
// @Range: 1 100
@ -133,7 +165,23 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
// @Increment: 0.001
// @User: Standard
// @Param: RAT_YAW_FILT
// @Param: RAT_YAW_FLTT
// @DisplayName: Yaw axis rate controller input frequency in Hz
// @Description: Yaw axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FLTE
// @DisplayName: Yaw axis rate controller input frequency in Hz
// @Description: Yaw axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FLTD
// @DisplayName: Yaw axis rate controller input frequency in Hz
// @Description: Yaw axis rate controller input frequency in Hz
// @Range: 1 100
@ -163,15 +211,39 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
// @User: Advanced
AP_GROUPINFO("THR_MIX_MAN", 6, AC_AttitudeControl_Sub, _thr_mix_man, AC_ATTITUDE_CONTROL_MAN_DEFAULT),
// @Param: RAT_RLL_FILT
// @DisplayName: Roll axis rate controller input frequency in Hz
// @Description: Roll axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FILT
// @DisplayName: Pitch axis rate controller input frequency in Hz
// @Description: Pitch axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FILT
// @DisplayName: Yaw axis rate controller input frequency in Hz
// @Description: Yaw axis rate controller input frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
AP_GROUPEND
};
AC_AttitudeControl_Sub::AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) :
AC_AttitudeControl(ahrs, aparm, motors, dt),
_motors_multi(motors),
_pid_rate_roll(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, dt),
_pid_rate_pitch(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, dt),
_pid_rate_yaw(AC_ATC_SUB_RATE_YAW_P, AC_ATC_SUB_RATE_YAW_I, AC_ATC_SUB_RATE_YAW_D, AC_ATC_SUB_RATE_YAW_IMAX, AC_ATC_SUB_RATE_YAW_FILT_HZ, dt)
_pid_rate_roll(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ, dt),
_pid_rate_pitch(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ, dt),
_pid_rate_yaw(AC_ATC_SUB_RATE_YAW_P, AC_ATC_SUB_RATE_YAW_I, AC_ATC_SUB_RATE_YAW_D, 0.0f, AC_ATC_SUB_RATE_YAW_IMAX, AC_ATC_SUB_RATE_YAW_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_YAW_FILT_HZ, dt)
{
AP_Param::setup_object_defaults(this, var_info);
@ -263,9 +335,9 @@ void AC_AttitudeControl_Sub::rate_controller_run()
update_throttle_rpy_mix();
Vector3f gyro_latest = _ahrs.get_gyro_latest();
_motors.set_roll(rate_target_to_motor_roll(gyro_latest.x, _rate_target_ang_vel.x));
_motors.set_pitch(rate_target_to_motor_pitch(gyro_latest.y, _rate_target_ang_vel.y));
_motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _rate_target_ang_vel.z));
_motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll_pitch));
_motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.roll_pitch));
_motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw));
control_monitor_update();
}