Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: Vector tidying

This commit is contained in:
Peter Barker 2022-02-09 10:58:17 +11:00 committed by Randy Mackay
parent 1a84c17fee
commit cade8b36a0
1 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -1031,13 +1031,11 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
} }
// limit rate correction for position hold // limit rate correction for position hold
Vector3f trim_rate_cds; Vector3f trim_rate_cds {
trim_rate_cds.x = att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.get().x) - filt_pit_roll_cd.get().x); constrain_float(att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.get().x) - filt_pit_roll_cd.get().x), -15000.0f, 15000.0f),
trim_rate_cds.y = att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.get().y) - filt_pit_roll_cd.get().y); constrain_float(att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.get().y) - filt_pit_roll_cd.get().y), -15000.0f, 15000.0f),
trim_rate_cds.z = att_hold_gain * filt_heading_error_cd.get(); constrain_float(att_hold_gain * filt_heading_error_cd.get(), -15000.0f, 15000.0f)
trim_rate_cds.x = constrain_float(trim_rate_cds.x, -15000.0f, 15000.0f); };
trim_rate_cds.y = constrain_float(trim_rate_cds.y, -15000.0f, 15000.0f);
trim_rate_cds.z = constrain_float(trim_rate_cds.z, -15000.0f, 15000.0f);
switch (axis) { switch (axis) {
case ROLL: case ROLL:
@ -1268,7 +1266,10 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
dwell_freq = waveform_freq_rads; dwell_freq = waveform_freq_rads;
} }
} }
Vector2f att_fdbk = Vector2f(-5730.0f * vel_hold_gain * velocity_bf.y, 5730.0f * vel_hold_gain * velocity_bf.x); const Vector2f att_fdbk {
-5730.0f * vel_hold_gain * velocity_bf.y,
5730.0f * vel_hold_gain * velocity_bf.x
};
filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s()); filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s());
} else { } else {
target_angle_cd = 0.0f; target_angle_cd = 0.0f;
@ -1279,9 +1280,10 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo
filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f)); filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f));
} }
Vector2f trim_angle_cd; const Vector2f trim_angle_cd {
trim_angle_cd.x = constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f); constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f),
trim_angle_cd.y = constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f); constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f)
};
switch (axis) { switch (axis) {
case ROLL: case ROLL:
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + trim_angle_cd.x, trim_angle_cd.y, 0.0f); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + trim_angle_cd.x, trim_angle_cd.y, 0.0f);