Ok, so I have been able to look at this collective output problem. This is in two parts.
The first part is that your high noise levels mean your measured acceleration significantly exceeds your maximum acceleration limit so when the controller initialises the acceleration it does not initialise it to your measured acceleration (±3 g or 30m/s/s) but instead it is limited to 2.5m/s/s. Therefore the following loop sees a large acceleration error and this results in a significant collective command just due to the P term in the acceleration PID loop. This part is not a bug.
The second part is what Bill noticed. The I term relaxed by taking the previous output and reducing it. However this assumes that the previous output had no output due to the P term and is therefore constant apart from the slowly reducing I term. This is a bug in that we should not assume this output has no P term and instead it should be based on the previous I term.
I have a fix for the I term problem here with a cleaned up handling of the reset_I_smoothly using a new relax_integrator function. Here:
Now this does not fix your problem as it is fundamentally caused by stupidly large vibrations getting to the position controller. These changes will help reduce your problem a little but the real fix is to:
- Reduce the vibrations getting to your autopilot
- Reduce the magnitude of the vibrations getting to the controller
I can’t help you with 1 but I can help you with 2. Your current setting of the acceleration filter is very high:
INS_ACCEL_FILTER 20 Hz
It looks like your acceleration noise is approximately 5 Hz. You could reduce your INS_ACCEL_FILTER setting to 0.5 to 1 Hz to greatly reduce this noise at the controller level. However we need to ensure that the lower controller response does not make Alt_Hold unstable. If so we may need to reduce the PSC Z gains.