From 021b23826d511d11fe306457ae33041ff0e9a7e7 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 12 Aug 2022 11:10:59 +0200 Subject: [PATCH] wind_replay: allow estimated local vel and GNSS vel sources local vel by default, set --gnss to use GNSS vel instead --- .../python/wind_estimator_replay.py | 26 +++++++++++++------ 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/src/lib/wind_estimator/python/wind_estimator_replay.py b/src/lib/wind_estimator/python/wind_estimator_replay.py index 08c868aa2e..efa19eaf0e 100644 --- a/src/lib/wind_estimator/python/wind_estimator_replay.py +++ b/src/lib/wind_estimator/python/wind_estimator_replay.py @@ -54,14 +54,20 @@ def getData(log, topic_name, variable_name, instance=0): def us2s(time_ms): return time_ms * 1e-6 -def run(logfile): +def run(logfile, use_gnss): log = ULog(logfile) - 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')]) + 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 = us2s(getData(log, 'vehicle_gps_position', 'timestamp')) - t_v_local = us2s(getData(log, 'vehicle_gps_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 = us2s(getData(log, 'vehicle_local_position', 'timestamp')) true_airspeed = getData(log, 'airspeed', 'true_airspeed_m_s') t_true_airspeed = us2s(getData(log, 'airspeed', 'timestamp')) @@ -88,15 +94,17 @@ def run(logfile): for i in range(n): dt = t_v_local[i] - t_now t_now = t_v_local[i] # run on local position updates - while t_dist_bottom[i_dist_bottom] <= t_now: + + while i_dist_bottom < len(t_dist_bottom) and t_dist_bottom[i_dist_bottom] <= t_now: i_dist_bottom += 1 i_dist_bottom -= 1 + if dist_bottom[i_dist_bottom] > 20.0: # Don't start too low P += Q * dt if t_true_airspeed[i_airspeed] < t_now: - while t_true_airspeed[i_airspeed] < t_now: + while i_airspeed < len(t_true_airspeed) and t_true_airspeed[i_airspeed] < t_now: i_airspeed += 1 i_airspeed -= 1 @@ -135,8 +143,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)