Skip to content

Commit

Permalink
AC_AutoTune: fix tracking of maximum angular acceleration
Browse files Browse the repository at this point in the history
Issue introduced in #27370
and partially fixed in #27762,
though evidently not properly tested.

Failing to track the maximum can result in dangerously low values being
calculated for `ATC_ACCEL_[RPY]_MAX` and the vehicle becoming unflyable.

Make the variable a reference so that the maximum value is preserved
between function calls.
  • Loading branch information
tpwrules authored and rmackay9 committed Dec 5, 2024
1 parent 9a5f81a commit d7b26a2
Show file tree
Hide file tree
Showing 2 changed files with 2 additions and 2 deletions.
2 changes: 1 addition & 1 deletion libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -644,7 +644,7 @@ void AC_AutoTune_Multi::twitching_test_angle(float angle, float rate, float angl
}

// twitching_measure_acceleration - measure rate of change of measurement
void AC_AutoTune_Multi::twitching_measure_acceleration(float &accel_average, float rate, float rate_max) const
void AC_AutoTune_Multi::twitching_measure_acceleration(float &accel_average, float rate, float &rate_max) const
{
if (rate_max < rate) {
rate_max = rate;
Expand Down
2 changes: 1 addition & 1 deletion libraries/AC_AutoTune/AC_AutoTune_Multi.h
Original file line number Diff line number Diff line change
Expand Up @@ -156,7 +156,7 @@ class AC_AutoTune_Multi : public AC_AutoTune
void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max);

// measure acceleration during twitch test
void twitching_measure_acceleration(float &accel_average, float rate, float rate_max) const;
void twitching_measure_acceleration(float &accel_average, float rate, float &rate_max) const;

// updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back
// optimize D term while keeping the maximum just below the target by adjusting P
Expand Down

0 comments on commit d7b26a2

Please sign in to comment.