fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:48:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

wind_est_replay: allow setting the initial scale factor

Browse Source
This commit is contained in:
bresch
2024-05-17 09:17:08 +02:00
committed by Mathieu Bresciani
parent 440465702e
commit b42799fac2
1 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/wind_estimator/python/wind_estimator_replay.py
+7 -3
View File
@@ -54,7 +54,7 @@ def getData(log, topic_name, variable_name, instance=0):
def us2s(time_ms):
return time_ms * 1e-6
def run(logfile, use_gnss):
def run(logfile, use_gnss, scale_init):
log = ULog(logfile)
if use_gnss:
@@ -75,7 +75,10 @@ def run(logfile, use_gnss):
dist_bottom = getData(log, 'vehicle_local_position', 'dist_bottom')
t_dist_bottom = us2s(getData(log, 'vehicle_local_position', 'timestamp'))
state = np.array([0.0, 0.0, 1.0])
if scale_init is None:
scale_init = 1.0
state = np.array([0.0, 0.0, scale_init])
P = np.diag([1.0, 1.0, 1e-4])
wind_nsd = 1e-2
scale_nsd = 1e-4
@@ -145,8 +148,9 @@ if __name__ == '__main__':
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')
parser.add_argument('--scale_init', help='Initial airsped scale factor (1.0 if not specified)', type=float)
args = parser.parse_args()
logfile = os.path.abspath(args.logfile) # Convert to absolute path
run(logfile, args.gnss)
run(logfile, args.gnss, args.scale_init)