Harmonic Notch Filter (ESC, FFT)

Sulaiman_MOH · July 18, 2023, 2:34pm

Hello and thank you in advance for the answer.
I am working with Holybro Kit S500 V2 Pixhawk6C and using LiPo 4S 3800. Also for the control, I am using DroneKit python where the script is sent by the telemetry then I will use raspberry pi.
My tests are done using a simple code to move using the velocity and x,y in the NED frame.
The problem is the drone is not so stable and at the same time if I move for example 2m and then go backward I don’t arrive at all to the same position.
I tried to do some manual tuning and then I moved to set Harmonic Notch Filter, I had weird results, first I used ECS telemetry mode and actually after watching batch data the result looks good but still the performance of the drone is not so good and that can be seen in Roll/DesRoll for example.
Then I set the mode to FFT, I thought it is the best mode for the filter but surprisingly the results that I see in batch samples are not so good also the performance even got worse.
I am not so expert in Ardupilot it is my first time using it, so I don’t know if there is anything wrong with the parameter or if I configured some values wrong, please note that I don’t use RC transmitter at all and for that, I could not do autotune.
Here you can see the log for ECS mode
Here for FFT
And the parameters

Thank you very much for all the help

xfacta · July 18, 2023, 9:35pm

You should use conventional RC for tuning and testing before going Guided only.

You are only using PWM despite wanting to use DSHOT, and there’s no ESC RPM data.
RCOut: PWM:1-16
Let us know exactly what ESCs they are because you need to do a few more settings too.

Assuming you do actually have DSHOT capable ESCs, you would need to change the ESC/Motor wires over to the “FMU PWM Output” connector and logically move the motor functions in the MissionPlanner Servo screen to Servo9, Servo10 and so on. Reboot after that.

Otherwise just change MOT_PWM_TYPE,0

This will fix the Harmonic Notch Filter and improve the attitude control a bit

ATC_ANG_PIT_P,7.0
ATC_ANG_RLL_P,7.0
ATC_ANG_YAW_P,5.5
ATC_RAT_PIT_D,0.003
ATC_RAT_PIT_I,0.12
ATC_RAT_PIT_P,0.12
ATC_RAT_RLL_D,0.003
ATC_RAT_RLL_I,0.12
ATC_RAT_RLL_P,0.12
ATC_RAT_YAW_I,0.050
ATC_RAT_YAW_P,0.50
INS_ACCEL_FILTER,10
INS_HNTCH_FM_RAT,0.7
INS_HNTCH_HMNCS,3
INS_HNTCH_MODE,1
INS_HNTCH_OPTS,0
INS_HNTCH_REF,0.19
PSC_ACCZ_I,0.44
PSC_ACCZ_P,0.22

You can set all these at once and the attitude control params should be quite safe too.
But you should really use an RC transmitter/receiver to take off cautiously to test in Stabilise mode, then switch to AltHold. After that use Loiter mode too.

Sulaiman_MOH · July 19, 2023, 7:04pm

Thank you so much for your replay
Actually, those values achieved really good stability.
Unfortunately, for now, I am not able to have RC so I need to get as close as possible to good values.
Notice: I set MOT_PWM_TYPE=0 to use normal PWM and for the filter I used FFT.
I have two questions if it is possible
1- from the log, I could see better Roll/DesRoll and the same for PItch but Yaw is still not that great, also the altitude is not that stable 100% (oscillate up and down slowly).
Here is the list of parameter
and the log
So I think
ATC_ANG_YAW_P,5.5
ATC_RAT_YAW_I, 0.050
ATC_RAT_YAW_P, 0.50
should be decreased, knowing that setting ATC_ANG_YAW_P to a higher value lead to bad oscillation.
and same
ATC_RAT_PIT_D,0.003
ATC_RAT_PIT_I,0.12
ATC_RAT_PIT_P,0.12
ATC_RAT_RLL_D,0.003
ATC_RAT_RLL_I,0.12
ATC_RAT_RLL_P,0.12
should be decreased slightly
Am I right?
2- my second question, I watched the batch graph, my question why the signal is not good before the Notch filter and whatever is the parameter of the Notch this is not fixed, I tried to decrease the value of the low-pass filter of the gyro but this effect is still there, is there any explanation.

xfacta · July 19, 2023, 10:36pm

Set these before you get caught out and damage your battery

BATT_FS_CRT_ACT,1
BATT_FS_LOW_ACT,3

The FFT is not picking up on the actual noise, the fundamental frequency - it has found and staying with the 2nd harmonic but still not configured correctly to knock it out.

So I would set:

FFT_ENABLE,0
INS_HNTCH_BW,40
INS_HNTCH_FM_RAT,0.7
INS_HNTCH_FREQ,92
INS_HNTCH_HMNCS,7
INS_HNTCH_MODE,1
INS_HNTCH_OPTS,0
INS_HNTCH_REF,0.18

Yaw shouldnt be so bad, the yaw problem is not so much oscillations but just lack of control.
Feel free to lower the Yaw values as you suggest, it will be interesting to see the difference.
You could try lowering ATC_ANG_YAW_P while increasing ATC_RAT_YAW_P and I
or
Increasing ATC_ANG_YAW_P while lowering ATC_RAT_YAW_P and I

Dont do anything further with Pitch and Roll until yaw is under control - then reassess pitch and roll.