Analyzing D-term noise

andyp1per · May 27, 2019, 3:09pm

I have created a pull request https://github.com/ArduPilot/ardupilot/pull/11422 that allows you to visualize the noise in the D-term of the PID controller, both before and after filtering. @Leonardthall let me know if you think this is useful. Sample output from the fix looks like this

Note that I have applied a local fix to make the output into “gyros” so that mission planner can understand it. Also x == roll, y == pitch and z == yaw.

Leonardthall · May 29, 2019, 12:05am

Hi Andy,

Yeh, I am loving the work you are doing. This is all really useful stuff!!!

We just need to make sure that Tridge has time to go over it. He is really swamped at the moment.

Lets get the Notch moved first then lets look at the logging features that are useful.

When you set FAST logging you get every sample that goes through the PID’s if the PID logging is turned on so that has really helped me design the next set of changes to the PID loops.

Might be a good idea to get on skype for a chat at some point.

andyp1per · May 29, 2019, 4:56pm

Yes indeed, because my aim in doing the D-term logging was to enable this:

https://github.com/ArduPilot/ardupilot/pull/11444

I think it shows that the D-term filtering that betaflight does is probably necessary and useful on these small copters. But I guess you might have done this already from your comment about PID loop changes?

With these changes my copter feels a lot smoother and in control, so keen to get some good weather again and bash a bit more.

guglie · May 31, 2019, 9:03am

Cool work @andyp1per!
If you want to add a separate log message for D-term FFT I can make a simple tool to plot that.

andyp1per · May 31, 2019, 9:09am

Thanks. I really need some input from the dev team on this as I think what we all want is the ability to view all the available FFT’s side by side. I think you need this not only because each flight is different but also because the FFT amplitude is dependent on the flight time so it’s very difficult to compare traces from different flights. So ideally you would be able to view gyro pre/post, D pre/post and P all on the same display. To do this I think the logging needs to be made more general capturing the different types of debug point rather than just a sensor type.

guglie · May 31, 2019, 11:32am

Sure

PRE- AND POST- FILTER FFT

I think we can modify ISBH log message

github.com

ArduPilot/ardupilot/blob/master/libraries/AP_Logger/LogStructure.h#L228-L238


struct PACKED log_ISBH {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    uint16_t seqno;
    uint8_t sensor_type; // e.g. GYRO or ACCEL
    uint8_t instance;
    uint16_t multiplier;
    uint16_t sample_count;
    uint64_t sample_us;
    float sample_rate_hz;
};

to add a new flag post_filter.

Then add _doing_pre_filter_logging
using another bit in BAT_OPT bitmask so we can have both pre- and post- enabled or disabled.
Then changing a little the logic in BatchSampler and splitting here

github.com

ArduPilot/ardupilot/blob/cc7696ee694335446087cc82df15da625e7b4dfd/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp#L340-L344


if (!_imu.batchsampler.doing_post_filter_logging()) {
    log_accel_raw(instance, sample_us, accel);
} else {
    log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]);
}

into two ifs should do the trick.

PID FFT

To have also PIDs we can add a new sensor_type to ISBH, that is of type AP_InertialSensor::IMU_SENSOR_TYPE

github.com

ArduPilot/ardupilot/blob/master/libraries/AP_Logger/AP_Logger.cpp#L1055-L1061


bool AP_Logger::Write_ISBH(const uint16_t seqno,
                                     const AP_InertialSensor::IMU_SENSOR_TYPE sensor_type,
                                     const uint8_t sensor_instance,
                                     const uint16_t mult,
                                     const uint16_t sample_count,
                                     const uint64_t sample_us,
                                     const float sample_rate_hz)

That should allow logging accel, gyro, and PID FFT together.
When you have the logs, plotting is not a problem, I can update mavfft script.

I would stop here, it has more sense to move the BatchSampler to a new library and change sensor_type to something more general and not related to inertial sensors. Then for consistency change the log name to something like FFTH and FFTD, but this is a much bigger change that will need also changes in GCSs.

Sorry, I didn’t tried all this changes myself because your pull is still not in master, as @Leonardthall said I would wait that to be merged before adding things.

andyp1per · May 31, 2019, 11:34am

Yeah, hence the need for developer input I think.

andyp1per · June 7, 2019, 4:25pm

Hi @Leonardthall and @peterbarker - so my notch change is in. This is the next change I would like to get in if appropriate, but would really value some feedback/review. I am using this capability quite a lot in my tuning and it works well. I also have fast PID logging on and that shows part of the picture - in particular its good for seeing P oscillation that D can fix, but it’s not so great for seeing the filtering that D needs. This is where my change helps. I also have these queued up:

and am flying with them all to good effect.

Many thanks for your input!

andy

wicked1 · June 8, 2019, 12:50am

I’ve been flying w/ all your updates, and was able to filter my D term to basically a flat line. It isn’t even taking much filtering… 12db attenuation for my notch. About 30hz bandwidth. Then I was able to increase D, and have much less wobbles while descending. And overall, it’s smooth!
I’d like to do an A/B comparison sometime soon, to really see the difference it’s making. I might do that next week some time.

ekliptiko · June 8, 2019, 1:24am

Forgive my rudimentary question, but what does a D-term (or any PID, for that matter) filter do in practical terms?

What would I notice as the operator?

andyp1per · June 8, 2019, 8:08am

D is the differential part of a “Proportional, Integral, Derivative” control loop.

D resists wobble in P so gives better control, especially in propwash.

But D also amplifies noise - something like the square of the frequency - so unwanted noise in D means a lot of noise going to the motors and that’s really bad for the motors - you can literally melt them. In small copters simply the motor noise is quite high frequency and thus can generate a lot of amplified D noise.

So D filtering removes this noise, allowing you higher D and thus better effect on P.

Filtering introduces latency, but for D this does not seem to matter so much so you can filter harder.

andyp1per · June 8, 2019, 8:00pm

I’m really psyched that its working well!