fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 20:41:36 +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 15:59:50 +02:00
committed by Mathieu Bresciani
parent e202e719b0
commit 4cf51e5833
1 changed files with 8 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py
+8 -4
View File
@@ -150,7 +150,7 @@ def baroCorrection(x, v_body):
return correction return correction
def run(logfile): def run(logfile, w_hpf):
(t, v_body, baro, v_local_z, gnss_h) = getAllData(logfile) (t, v_body, baro, v_local_z, gnss_h) = getAllData(logfile)
# x[0]: pcoef_xn / g # 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 # Remove low ferquency part of the signal as we're only interested in the short-term errors
baro_error -= baro_error[0] 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 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 # Provide parameter file path and name
parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) 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() args = parser.parse_args()
logfile = os.path.abspath(args.logfile) # Convert to absolute path logfile = os.path.abspath(args.logfile) # Convert to absolute path
w_hpf = 2 * np.pi * args.hpf
run(logfile) run(logfile, w_hpf)