AC_PID: correct error calculation to use latest target

[IamPete1]

This fixes a bug that was introduced in #24411.

Currently the error is calculated before the target filter is applied, this means the error calculation is using the target from the last loop.

The fix is to do all the target filtering, then calculate the error and do all of the error filtering.

[IamPete1]

This is the core of the bug. Here _target is the value from the last call. It is only updated using the new target on 231.

As proof, on the line above we have just used it to populate target_last. This line could be written as:

        float error = target_last - measurement;

[andyp1per]

As far as I can tell what is actually required is a one line change moving the calculation of error after the target filter has been applied? For such a subtle issue I would prefer would try and do the absolute minimum. So basically this:

diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp
index 3f6d22e562..08890a18ad 100644
--- a/libraries/AC_PID/AC_PID.cpp
+++ b/libraries/AC_PID/AC_PID.cpp
@@ -202,7 +202,6 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit,
     if (_flags._reset_filter) {
         _flags._reset_filter = false;
         _target = target;
-        _error = _target - measurement;
         _derivative = 0.0f;
         _target_derivative = 0.0f;
 #if AP_FILTER_ENABLED
@@ -210,6 +209,9 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit,
             _target_notch->reset();
             _target = _target_notch->apply(_target);
         }
+#endif
+        _error = _target - measurement;
+#if AP_FILTER_ENABLED
         if (_error_notch != nullptr) {
             _error_notch->reset();
             _error = _error_notch->apply(_error);
@@ -218,12 +220,14 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit,
     } else {
         float error_last = _error;
         float target_last = _target;
-        float error = _target - measurement;
 #if AP_FILTER_ENABLED
         // apply notch filters before FTLD/FLTE to avoid shot noise
         if (_target_notch != nullptr) {
             target = _target_notch->apply(target);
         }
+#endif
+        float error = _target - measurement;
+#if AP_FILTER_ENABLED
         if (_error_notch != nullptr) {
             error = _error_notch->apply(error);
         }

?

[IamPete1]

You proposed fix is only applying the notch filter to the target, it completely removes the low pass. EDIT: infact I think it makes no difference, note that _target and target are not the same.

The goal is not only to fix the bug but also to comment and structure the code such that were less likely to make a similar mistake again.

[andyp1per]

I understand the reasoning, but the restructure is not making it clear to me 😛

[andyp1per]

Or put another way I think your restructure is obfuscating the fix. Note that I am pretty sure Leonard gave me this code, so I don't have an axe to grind here 😉

[lthall]

While I understand where the principle of minimal changes come from I believe the extra effort in ensuring the code is also clear to the future developers can't be understated.

I think this PR has both fixed the bug and also made it clearer to help prevent problems in the future or simply how easy the code is to read and understand.

[lthall]

Nice catch!!

[lthall]

Well done. I am very happy you noticed this!

The PR looks good!!

[andyp1per]

@IamPete1 explained it to me - it was not what I was thinking

[bnsgeyer]

LGTM