PID updates to include additional filters #11673

lthall · 2019-06-27T12:28:48Z

This is the first step in a number of PID object updates to address various control challenges I have been seeing with aircraft of various sizes. These changes do not impact the performance if the parameter is left at zero.

I have also found a couple of alt hold initialisation bugs that needed to be fixed to test this properly.

This update adds filtering to address issues with noise entering the controllers through the unfiltered EKF and noise caused by the EKF itself and things like lower sample rate RC inputs.

These updates also support feed forward natively and will address control issues with aircraft controlled by aerodynamic surfaces like single and coax copter.

Heli pids have been simplified but can be simplified further since the internal target filter can be used instead of the FF filter currently used in Heli. This will need to be something the Heli maintainers look at and decide.

I am not sure if I have correctly handled the antenna tracker and sail boat changes.

lthall · 2019-06-27T13:16:02Z

For some reason the first test on travis didn't complete but it did on my branch. I have found that I need to hit restart from time to time but I can't do it here.

libraries/AC_PID/AC_HELI_PID.cpp

lthall · 2019-06-28T05:52:21Z

I also changed the .param file names to update _FILT to _FLTD or _FLTE.

lthall · 2019-06-28T06:24:32Z

I also noticed that a couple other vehicles were using the input filter option. Fixed that too.

khancyr

Hello,
thanks for your work !
I made some general code comments, some place could benefit from const , but that isn't blocking.
Rover modification will need some careful attentation to check if we have miss a configuration in the changes (rover, skidsteering, boat, omni).
But on overrall it seems good !

khancyr · 2019-06-28T07:33:32Z

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

-    float roll_pd, roll_i, roll_ff;             // used to capture pid values
-    float pitch_pd, pitch_i, pitch_ff;          // used to capture pid values
-    float rate_roll_error_rads, rate_pitch_error_rads;    // simply target_rate - current_rate
+    float roll_pid, roll_ff;             // used to capture pid values


it is better to directly assign value on object creation in cpp as it will use the object constructor to assign the value instead of doing it in two steps : varaible create then assignement !

I am maintaining consistency with the way Heli has been done here already. I would be happy to work with Bill and Chris to do more though.

khancyr · 2019-06-28T07:34:31Z

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

    }

 }

 // rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second
 float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_yaw_actual_rads, float rate_target_rads)
 {
-    float pd,i,vff;     // used to capture pid values for logging
-    float rate_error_rads;       // simply target_rate - current_rate
+    float pid, vff;     // used to capture pid values for logging


same as previous comment

Same as previous

libraries/AC_PID/AC_PID.cpp

khancyr · 2019-06-28T07:41:54Z

libraries/AC_PID/AC_PID.cpp

 {
-    return get_p() + get_i();
+    _pid_info.FF = _target * _kff;
+    return _target * _kff;


better return _pid_info.FF as you already made the calculus

I discussed this with Randy and as we don't control this object we are not comfortable using it as the primary storage medium. I don't know if this is the right decision or not.

All discussion welcome here!!

sorry, I don't understand where we are not controling this object ? _pid_info is part of AC_PID and we aren't doing multthreading... so it is safe. Otherwise we get a bigger issue and need protect with mutex !
my point was to prevent to do the calculus twice. I am even confident that the compiler will create a temp variable to do that on its own.

My basic concern here is the pid object is accessing the logging object to get it's data rather than the other way round. I am sure you are correct that this isn't going to cause an issue in it's current form I can't see this being the right way to do it. Someone making changes to the logging behaviour could mess with the control of aircraft.

You are also right that I am doing the same calculation twice so I could remove that without depending on the pid_info structure.

khancyr · 2019-06-28T07:42:55Z

libraries/AC_PID/AC_PID.cpp

 {
-    return get_p() + get_i() + get_d();
+    _pid_info.FF = target * _kff;
+    return target * _kff;


better return _pid_info.FF as you already made the calculus

same as above.

khancyr · 2019-06-28T07:46:21Z

libraries/AC_PID/AC_PID.cpp

-    // derivative component
-    _pid_info.D = (_kd * _derivative);
-    return _pid_info.D;
+    return _error * _kp;


can't we just get _pid_info.P ?

same as above.

khancyr · 2019-06-28T07:46:43Z

libraries/AC_PID/AC_PID.cpp

 }

+float AC_PID::get_d() const
+{
+    return _kd * _derivative;


can't we just get _pid_info.D ?

same as above.

lthall · 2019-06-30T00:27:24Z

I have precalculated the values going into the _pid_info so it isn't calculated twice.

OXINARF

@lthall I've tried to very complete in my review. As usual, I tried to follow the logic in the changes and I might have misunderstood some differences (which means some comments might be wrong - but better ask than be sorry later 😉).

Besides the inline comments below, I have a general comment about the first commit:

when doing a commit affecting multiple libraries/vehicles we use _Global: _ as the prefix in the commit message (instead of _ArduPilot: _)
I would have separated the adding of error since that's new functionality
I think that change in the commit message should be rename (otherwise someone might think there is an actual functionality change there when it's just a rename)
I've commented below on a number of lines where a rename happens in a later commit; it would be better if those happened in that first commit (after editing the first commit to make the same renames, it will disappear from later commits so it isn't very hard to change)

libraries/AP_Motors/AP_Motors_Class.h

libraries/AC_PID/AC_PID.cpp

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

ArduCopter/tuning.cpp

ArduCopter/system.cpp

lthall · 2019-07-03T00:19:54Z

when doing a commit affecting multiple libraries/vehicles we use _Global: _ as the prefix in the commit message (instead of _ArduPilot: _)
I changed the commit prefix.

I would have separated the adding of error since that's new functionality

I think that change in the commit message should be rename (otherwise someone might think there is an actual functionality change there when it's just a rename)

I've commented below on a number of lines where a rename happens in a later commit; it would be better if those happened in that first commit (after editing the first commit to make the same renames, it will disappear from later commits so it isn't very hard to change)

With my knowledge of GIT this is very difficult for me to do. Where I can I do try to do this but there is about three months of work here where working out what the names are is the last step. It would be great to learn how to insert a simple change without breaking all my work after that. (Yeh I know for many people I am looking incompetent right now, I am :) )

lthall · 2019-07-03T01:51:29Z

Ok after much GIT mashing I have addressed the changes above and squashed them into the previous commits to minimise the number of commits.

I also separated out the Alt_Hold commit as a separate commit and made it first.

Francisco will do some magic on this in the morning to address one of his comments for me.

bnsgeyer · 2019-07-10T02:44:57Z

@lthall this completes heli changes to bring it in line with your PID changes. Hopefully I caught all of the suggested changes to make the code more efficient.

lthall · 2019-07-10T05:32:59Z

Thanks Bill!

I think we are pretty much ready to pull the trigger here!!

OXINARF · 2019-07-11T02:11:21Z

I've worked with @lthall to fix some stuff (I've also fixed an issue with @bnsgeyer's code - his changes were based on an older code version so it carried an issue that Leonard had already fixed).

That's now pushed on this PR - everything was squashed up (but if you want to see the fixes: OXINARF@ced1d68, OXINARF@1d1820b, OXINARF@0d71af3, OXINARF@634ab55, OXINARF@c2ae440, OXINARF@28cd5ed).

I think this is now ready to go. Unless there is an objection in the next day or so, I'll merge it.

rmackay9 · 2019-07-11T03:20:28Z

It looks like AP_Motor's _roll_in_ff isn't used anywhere. Normally we don't add code that isn't used.

libraries/AC_PID/AC_PID.h

libraries/AC_PID/AC_HELI_PID.cpp

libraries/AP_WheelEncoder/AP_WheelRateControl.cpp

tridge · 2019-07-24T10:44:49Z

@kd0aij Leonard and I have found what we think the issue was with the stryker. It now flies for me about the same as master. Can you please re-test?

lthall · 2019-07-24T10:48:51Z

@kd0aij Sorry about messing you around with that one. I didn't realise you weren't using the same motor mixer as copter there.

You may need to set roll and pitch FF to zero as the FF was not getting passed in before.

kd0aij · 2019-07-24T13:32:57Z

@lthall @tridge Thanks, ef1cb8e (pretty obviously) fixes a problem with copter tailsitters, and I agree that the Stryker_quad is now stable in SITL. But it seems that the bodyframe roll option (Q_TAILSIT_INPUT = 2,3) is also broken.

I'll also need to rebase #11195 on this and test to make sure it doesn't adversely affect @losawing with his fast flying wings.

lthall · 2019-07-24T13:48:11Z

@kd0aij Thanks!!

kd0aij · 2019-07-24T17:03:21Z

@lthall I couldn't find any code changes which would have affected the behavior of the tailsitter "bodyframe roll" options, and tested in RF8 again... now it seems OK. Sorry about the confusion, I must have made a mistake with a parameter value.

I'll rebase my gainscaling PR on this and test again, but this PR looks good to me for tailsitters.

rmackay9 · 2019-07-25T02:04:34Z

I've tested with rover (both ackerman and skid-steering) and the outputs for turn rate and speed are identical.

I've tested with BalanceBot as well (Hold mode only) and it works. I bumped into the issue of the WRC_RATE_FF/FILT and WRC2_RATE_FF/FILT parameters not being copied across so initially it didn't balance as well as master. I think we made a choice to not add the parameter upgrades for the WRC_ object though because there are so few users.

rmackay9 · 2019-07-25T03:56:04Z

travis seems confused because it's reporting that travis is "pending" but it's actually completed successfully.

rmackay9 · 2019-07-25T04:32:36Z

I'm going to merge this within a couple of hours unless anybody would like me to delay the merge..

rmackay9 · 2019-07-25T08:38:23Z

Merged, thanks!

amilcarlucas · 2019-07-25T10:17:25Z

libraries/AP_Logger/LogStructure.h

-#define PID_FMT    "Qffffff"
-#define PID_UNITS  "s------"
-#define PID_MULTS  "F------"
+#define PID_LABELS "TimeUS,Des,Act,Err,P,I,D,FF"


missing s/Des/Tar/ ?

@amilcarlucas Sorry, what are you saying here?

You renamed "desired" to "target" everywhere.
I assume you also want to change:

-#define PID_LABELS "TimeUS,Des,Act,Err,P,I,D,FF"
+#define PID_LABELS "TimeUS,Tar,Act,Err,P,I,D,FF"

Ahh, bingo. Thanks!!

lthall · 2019-07-25T13:02:47Z

Thank you everybody for your help and support to get this work in!!

IamPete1 · 2019-07-29T21:54:38Z

@lthall @rmackay9 We seem to have lost the sailboat changes at some point, they were defiantly here when I checked them, do you have them in a branch somewhere?

edit: and possibly the balance-bot stuff too?

lthall · 2019-07-29T23:30:25Z

Hi @IamPete1 I believe they are in APM_Control.

IamPete1 · 2019-07-29T23:46:37Z

@lthall It is broken on master, that is it runs but causes the sail to be fully out all the time, there should be zero error as the the SITL doesn't model roll, so I don't think it is just a change of defaults. I thought I remembered some changes to sailboat.cpp when I had a look, possibly I miss remembered. I will see if I can fix.

lthall · 2019-07-29T23:50:46Z

Ahh, ok. Sorry, I thought the changes went missing. Sailboat was one of the ones that was implemented in a strange way. I will take a look as well.

IamPete1 · 2019-07-30T00:30:35Z

I thought I had seen something that had gone missing. Looks like it may just be a case of bad defaults. The old defaults seem to cause much larger response than on the old PID's. This is abit like what a saw on tracker, would you expect that?

lthall · 2019-07-30T01:21:24Z

@IamPete1 it looks like your original error calculation was done incorrectly.
heel_error = fabsf(_ahrs.roll) - desired_heel;
This will result in -1 * error.
Convention is to calculate error = desired - measured.

I suspect I saw this when I put it in the first time and used update_error instead. This may then have been changed to update_all.

It would be great if you could continue to use update_all and fix the surrounding code because I want to remove update_error from the PID object.

I am very sorry this messed you around!!

IamPete1 · 2019-07-30T16:25:57Z

no worries, quite a simple fix in the end. 8a62a0d

My fault for the original bad implementation, thanks for the pointers on the fix, have tested this on the sailboat although admittedly on the stand rather than the water.

lthall requested review from rmackay9, tridge and bnsgeyer June 27, 2019 12:28

lthall requested a review from OXINARF June 27, 2019 23:01

tridge reviewed Jun 28, 2019

View reviewed changes

libraries/AC_PID/AC_HELI_PID.cpp Show resolved Hide resolved

lthall force-pushed the 20190625_PIDFilterUpdates branch from cb07ede to 0cef459 Compare June 28, 2019 06:24

khancyr reviewed Jun 28, 2019

View reviewed changes

OXINARF added AllVehicles Library labels Jul 1, 2019

OXINARF requested changes Jul 2, 2019

View reviewed changes

lthall force-pushed the 20190625_PIDFilterUpdates branch from 59aea61 to 3ac14db Compare July 3, 2019 00:14

lthall force-pushed the 20190625_PIDFilterUpdates branch from 3ac14db to 4ad15c4 Compare July 3, 2019 01:48

OXINARF force-pushed the 20190625_PIDFilterUpdates branch from 187bcb9 to 8feb10c Compare July 11, 2019 02:11

OXINARF approved these changes Jul 11, 2019

View reviewed changes