dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_wind_estimator_tuning: optionally use GNSS velocity 

Sometimes GNSS is logged but not used
This commit is contained in:
bresch 2024-03-14 17:19:34 +01:00 committed by Mathieu Bresciani
parent b2f1122372
commit 7c6ecd95a8
1 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py
View File

@ -45,14 +45,20 @@ import numpy as np
import quaternion
from scipy import optimize
def getAllData(logfile):
def getAllData(logfile, use_gnss):
log = ULog(logfile)
v_local = np.matrix([getData(log, 'vehicle_local_position', 'vx'),
getData(log, 'vehicle_local_position', 'vy'),
getData(log, 'vehicle_local_position', 'vz')])
if use_gnss:
v_local = np.array([getData(log, 'vehicle_gps_position', 'vel_n_m_s'),
getData(log, 'vehicle_gps_position', 'vel_e_m_s'),
getData(log, 'vehicle_gps_position', 'vel_d_m_s')])
t_v_local = ms2s(getData(log, 'vehicle_gps_position', 'timestamp'))
t_v_local = ms2s(getData(log, 'vehicle_local_position', 'timestamp'))
else:
v_local = np.array([getData(log, 'vehicle_local_position', 'vx'),
getData(log, 'vehicle_local_position', 'vy'),
getData(log, 'vehicle_local_position', 'vz')])
t_v_local = ms2s(getData(log, 'vehicle_local_position', 'timestamp'))
accel = np.matrix([getData(log, 'sensor_combined', 'accelerometer_m_s2[0]'),
getData(log, 'sensor_combined', 'accelerometer_m_s2[1]'),
@ -126,8 +132,8 @@ def getData(log, topic_name, variable_name, instance=0):
def ms2s(time_ms):
return time_ms * 1e-6
def run(logfile):
(t, v_body, a_body) = getAllData(logfile)
def run(logfile, use_gnss):
(t, v_body, a_body) = getAllData(logfile, use_gnss)
rho = 1.15 # air densitiy
rho15 = 1.225 # air density at 15 degC
@ -197,8 +203,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('--gnss', help='Use GNSS velocity instead of local velocity estimate',
action='store_true')
args = parser.parse_args()
logfile = os.path.abspath(args.logfile) # Convert to absolute path
run(logfile)
run(logfile, args.gnss)