fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

baro comp: set hpf optional

Browse Source
This commit is contained in:
bresch 2024-04-22 14:07:17 +02:00 committed by Mathieu Bresciani
parent e202e719b0
commit 4cf51e5833
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py
View File

@ -150,7 +150,7 @@ def baroCorrection(x, v_body):
return correction
def run(logfile):
def run(logfile, w_hpf):
(t, v_body, baro, v_local_z, gnss_h) = getAllData(logfile)
# x[0]: pcoef_xn / g
@ -162,8 +162,10 @@ def run(logfile):
# Remove low ferquency part of the signal as we're only interested in the short-term errors
baro_error -= baro_error[0]
sos = butter(4, 0.01, 'hp', fs=1/(t[1]-t[0]), output='sos')
baro_error = sosfilt(sos, baro_error)
if (w_hpf > 0):
sos = butter(4, w_hpf, 'hp', fs=1/(t[1]-t[0]), output='sos')
baro_error = sosfilt(sos, baro_error)
J = lambda x: np.sum(np.power(baro_error - baroCorrection(x, v_body), 2.0)) # cost function
@ -224,8 +226,10 @@ if __name__ == '__main__':
# Provide parameter file path and name
parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str)
parser.add_argument('--hpf', help='Cuttoff frequency of high-pass filter on baro error (Hz)', type=float)
args = parser.parse_args()
logfile = os.path.abspath(args.logfile) # Convert to absolute path
w_hpf = 2 * np.pi * args.hpf
run(logfile)
run(logfile, w_hpf)