CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/Tools/autotest/pysim/util.py
Views: 1862
1
from __future__ import print_function
2
3
'''
4
AP_FLAKE8_CLEAN
5
'''
6
7
import atexit
8
import math
9
import os
10
import re
11
import shlex
12
import signal
13
import subprocess
14
import sys
15
import tempfile
16
import time
17
18
19
import pexpect
20
21
if sys.version_info[0] >= 3:
22
ENCODING = 'ascii'
23
else:
24
ENCODING = None
25
26
RADIUS_OF_EARTH = 6378100.0 # in meters
27
28
# List of open terminal windows for macosx
29
windowID = []
30
31
32
def topdir():
33
"""Return top of git tree where autotest is running from."""
34
d = os.path.dirname(os.path.realpath(__file__))
35
assert os.path.basename(d) == 'pysim'
36
d = os.path.dirname(d)
37
assert os.path.basename(d) == 'autotest'
38
d = os.path.dirname(d)
39
assert os.path.basename(d) == 'Tools'
40
d = os.path.dirname(d)
41
return d
42
43
44
def relcurdir(path):
45
"""Return a path relative to current dir"""
46
return os.path.relpath(path, os.getcwd())
47
48
49
def reltopdir(path):
50
"""Returns the normalized ABSOLUTE path for 'path', where path is a path relative to topdir"""
51
return os.path.normpath(os.path.join(topdir(), path))
52
53
54
def run_cmd(cmd, directory=".", show=True, output=False, checkfail=True):
55
"""Run a shell command."""
56
shell = False
57
if not isinstance(cmd, list):
58
cmd = [cmd]
59
shell = True
60
if show:
61
print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), directory,))
62
if output:
63
return subprocess.Popen(cmd, shell=shell, stdout=subprocess.PIPE, cwd=directory).communicate()[0]
64
elif checkfail:
65
return subprocess.check_call(cmd, shell=shell, cwd=directory)
66
else:
67
return subprocess.call(cmd, shell=shell, cwd=directory)
68
69
70
def rmfile(path):
71
"""Remove a file if it exists."""
72
try:
73
os.unlink(path)
74
except (OSError, FileNotFoundError):
75
pass
76
77
78
def deltree(path):
79
"""Delete a tree of files."""
80
run_cmd('rm -rf %s' % path)
81
82
83
def relwaf():
84
return "./modules/waf/waf-light"
85
86
87
def waf_configure(board,
88
j=None,
89
debug=False,
90
math_check_indexes=False,
91
coverage=False,
92
ekf_single=False,
93
postype_single=False,
94
force_32bit=False,
95
extra_args=[],
96
extra_hwdef=None,
97
ubsan=False,
98
ubsan_abort=False,
99
num_aux_imus=0,
100
dronecan_tests=False,
101
extra_defines={}):
102
cmd_configure = [relwaf(), "configure", "--board", board]
103
if debug:
104
cmd_configure.append('--debug')
105
if coverage:
106
cmd_configure.append('--coverage')
107
if math_check_indexes:
108
cmd_configure.append('--enable-math-check-indexes')
109
if ekf_single:
110
cmd_configure.append('--ekf-single')
111
if postype_single:
112
cmd_configure.append('--postype-single')
113
if force_32bit:
114
cmd_configure.append('--force-32bit')
115
if ubsan:
116
cmd_configure.append('--ubsan')
117
if ubsan_abort:
118
cmd_configure.append('--ubsan-abort')
119
if num_aux_imus > 0:
120
cmd_configure.append('--num-aux-imus=%u' % num_aux_imus)
121
if dronecan_tests:
122
cmd_configure.append('--enable-dronecan-tests')
123
if extra_hwdef is not None:
124
cmd_configure.extend(['--extra-hwdef', extra_hwdef])
125
for nv in extra_defines.items():
126
cmd_configure.extend(['--define', "%s=%s" % nv])
127
if j is not None:
128
cmd_configure.extend(['-j', str(j)])
129
pieces = [shlex.split(x) for x in extra_args]
130
for piece in pieces:
131
cmd_configure.extend(piece)
132
run_cmd(cmd_configure, directory=topdir(), checkfail=True)
133
134
135
def waf_clean():
136
run_cmd([relwaf(), "clean"], directory=topdir(), checkfail=True)
137
138
139
def waf_build(target=None):
140
cmd = [relwaf(), "build"]
141
if target is not None:
142
cmd.append(target)
143
run_cmd(cmd, directory=topdir(), checkfail=True)
144
145
146
def build_SITL(
147
build_target,
148
board='sitl',
149
clean=True,
150
configure=True,
151
coverage=False,
152
debug=False,
153
ekf_single=False,
154
extra_configure_args=[],
155
extra_defines={},
156
j=None,
157
math_check_indexes=False,
158
postype_single=False,
159
force_32bit=False,
160
ubsan=False,
161
ubsan_abort=False,
162
num_aux_imus=0,
163
dronecan_tests=False,
164
):
165
166
# first configure
167
if configure:
168
waf_configure(board,
169
j=j,
170
debug=debug,
171
math_check_indexes=math_check_indexes,
172
ekf_single=ekf_single,
173
postype_single=postype_single,
174
coverage=coverage,
175
force_32bit=force_32bit,
176
ubsan=ubsan,
177
ubsan_abort=ubsan_abort,
178
extra_defines=extra_defines,
179
num_aux_imus=num_aux_imus,
180
dronecan_tests=dronecan_tests,
181
extra_args=extra_configure_args,)
182
183
# then clean
184
if clean:
185
waf_clean()
186
187
# then build
188
cmd_make = [relwaf(), "build", "--target", build_target]
189
if j is not None:
190
cmd_make.extend(['-j', str(j)])
191
run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)
192
return True
193
194
195
def build_examples(board, j=None, debug=False, clean=False, configure=True, math_check_indexes=False, coverage=False,
196
ekf_single=False, postype_single=False, force_32bit=False, ubsan=False, ubsan_abort=False,
197
num_aux_imus=0, dronecan_tests=False,
198
extra_configure_args=[]):
199
# first configure
200
if configure:
201
waf_configure(board,
202
j=j,
203
debug=debug,
204
math_check_indexes=math_check_indexes,
205
ekf_single=ekf_single,
206
postype_single=postype_single,
207
coverage=coverage,
208
force_32bit=force_32bit,
209
ubsan=ubsan,
210
ubsan_abort=ubsan_abort,
211
extra_args=extra_configure_args,
212
dronecan_tests=dronecan_tests)
213
214
# then clean
215
if clean:
216
waf_clean()
217
218
# then build
219
cmd_make = [relwaf(), "examples"]
220
run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)
221
return True
222
223
224
def build_replay(board, j=None, debug=False, clean=False):
225
# first configure
226
waf_configure(board, j=j, debug=debug)
227
228
# then clean
229
if clean:
230
waf_clean()
231
232
# then build
233
cmd_make = [relwaf(), "replay"]
234
run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)
235
return True
236
237
238
def build_tests(board,
239
j=None,
240
debug=False,
241
clean=False,
242
configure=True,
243
math_check_indexes=False,
244
coverage=False,
245
ekf_single=False,
246
postype_single=False,
247
force_32bit=False,
248
ubsan=False,
249
ubsan_abort=False,
250
num_aux_imus=0,
251
dronecan_tests=False,
252
extra_configure_args=[]):
253
254
# first configure
255
if configure:
256
waf_configure(board,
257
j=j,
258
debug=debug,
259
math_check_indexes=math_check_indexes,
260
ekf_single=ekf_single,
261
postype_single=postype_single,
262
coverage=coverage,
263
force_32bit=force_32bit,
264
ubsan=ubsan,
265
ubsan_abort=ubsan_abort,
266
num_aux_imus=num_aux_imus,
267
dronecan_tests=dronecan_tests,
268
extra_args=extra_configure_args,)
269
270
# then clean
271
if clean:
272
waf_clean()
273
274
# then build
275
run_cmd([relwaf(), "tests"], directory=topdir(), checkfail=True, show=True)
276
return True
277
278
279
# list of pexpect children to close on exit
280
close_list = []
281
282
283
def pexpect_autoclose(p):
284
"""Mark for autoclosing."""
285
global close_list
286
close_list.append(p)
287
288
289
def pexpect_close(p):
290
"""Close a pexpect child."""
291
global close_list
292
293
ex = None
294
if p is None:
295
print("Nothing to close")
296
return
297
try:
298
p.kill(signal.SIGTERM)
299
except IOError as e:
300
print("Caught exception: %s" % str(e))
301
ex = e
302
pass
303
if ex is None:
304
# give the process some time to go away
305
for i in range(20):
306
if not p.isalive():
307
break
308
time.sleep(0.05)
309
try:
310
p.close()
311
except Exception:
312
pass
313
try:
314
p.close(force=True)
315
except Exception:
316
pass
317
if p in close_list:
318
close_list.remove(p)
319
320
321
def pexpect_close_all():
322
"""Close all pexpect children."""
323
global close_list
324
for p in close_list[:]:
325
pexpect_close(p)
326
327
328
def pexpect_drain(p):
329
"""Drain any pending input."""
330
try:
331
p.read_nonblocking(1000, timeout=0)
332
except Exception:
333
pass
334
335
336
def cmd_as_shell(cmd):
337
return (" ".join(['"%s"' % x for x in cmd]))
338
339
340
def make_safe_filename(text):
341
"""Return a version of text safe for use as a filename."""
342
r = re.compile("([^a-zA-Z0-9_.+-])")
343
text.replace('/', '-')
344
filename = r.sub(lambda m: str(hex(ord(str(m.group(1))))).upper(), text)
345
return filename
346
347
348
def valgrind_log_filepath(binary, model):
349
return make_safe_filename('%s-%s-valgrind.log' % (os.path.basename(binary), model,))
350
351
352
def kill_screen_gdb():
353
cmd = ["screen", "-X", "-S", "ardupilot-gdb", "quit"]
354
subprocess.Popen(cmd)
355
356
357
def kill_mac_terminal():
358
global windowID
359
for window in windowID:
360
cmd = ("osascript -e \'tell application \"Terminal\" to close "
361
"(window(get index of window id %s))\'" % window)
362
os.system(cmd)
363
364
365
class FakeMacOSXSpawn(object):
366
"""something that looks like a pspawn child so we can ignore attempts
367
to pause (and otherwise kill(1) SITL. MacOSX using osascript to
368
start/stop sitl
369
"""
370
def __init__(self):
371
pass
372
373
def progress(self, message):
374
print(message)
375
376
def kill(self, sig):
377
# self.progress("FakeMacOSXSpawn: ignoring kill(%s)" % str(sig))
378
pass
379
380
def isalive(self):
381
self.progress("FakeMacOSXSpawn: assuming process is alive")
382
return True
383
384
385
class PSpawnStdPrettyPrinter(object):
386
'''a fake filehandle-like object which prefixes a string to all lines
387
before printing to stdout/stderr. To be used to pass to
388
pexpect.spawn's logfile argument
389
'''
390
def __init__(self, output=sys.stdout, prefix="stdout"):
391
self.output = output
392
self.prefix = prefix
393
self.buffer = ""
394
395
def close(self):
396
self.print_prefixed_line(self.buffer)
397
398
def write(self, data):
399
self.buffer += data
400
lines = self.buffer.split("\n")
401
self.buffer = lines[-1]
402
lines.pop()
403
for line in lines:
404
self.print_prefixed_line(line)
405
406
def print_prefixed_line(self, line):
407
print("%s: %s" % (self.prefix, line), file=self.output)
408
409
def flush(self):
410
pass
411
412
413
def start_SITL(binary,
414
valgrind=False,
415
callgrind=False,
416
gdb=False,
417
gdb_no_tui=False,
418
wipe=False,
419
home=None,
420
model=None,
421
speedup=1,
422
sim_rate_hz=None,
423
defaults_filepath=[],
424
unhide_parameters=False,
425
gdbserver=False,
426
breakpoints=[],
427
disable_breakpoints=False,
428
customisations=[],
429
lldb=False,
430
enable_fgview=False,
431
supplementary=False,
432
stdout_prefix=None):
433
434
if model is None and not supplementary:
435
raise ValueError("model must not be None")
436
437
"""Launch a SITL instance."""
438
cmd = []
439
if (callgrind or valgrind) and os.path.exists('/usr/bin/valgrind'):
440
# we specify a prefix for vgdb-pipe because on Vagrant virtual
441
# machines the pipes are created on the mountpoint for the
442
# shared directory with the host machine. mmap's,
443
# unsurprisingly, fail on files created on that mountpoint.
444
vgdb_prefix = os.path.join(tempfile.gettempdir(), "vgdb-pipe")
445
log_file = valgrind_log_filepath(binary=binary, model=model)
446
cmd.extend([
447
'valgrind',
448
# adding this option allows valgrind to cope with the overload
449
# of operator new
450
"--soname-synonyms=somalloc=nouserintercepts",
451
'--vgdb-prefix=%s' % vgdb_prefix,
452
'-q',
453
'--log-file=%s' % log_file])
454
if callgrind:
455
cmd.extend(["--tool=callgrind"])
456
if gdbserver:
457
cmd.extend(['gdbserver', 'localhost:3333'])
458
if gdb:
459
# attach gdb to the gdbserver:
460
f = open("/tmp/x.gdb", "w")
461
f.write("target extended-remote localhost:3333\nc\n")
462
for breakingpoint in breakpoints:
463
f.write("b %s\n" % (breakingpoint,))
464
if disable_breakpoints:
465
f.write("disable\n")
466
f.close()
467
run_cmd('screen -d -m -S ardupilot-gdbserver '
468
'bash -c "gdb -x /tmp/x.gdb"')
469
elif gdb:
470
f = open("/tmp/x.gdb", "w")
471
f.write("set pagination off\n")
472
for breakingpoint in breakpoints:
473
f.write("b %s\n" % (breakingpoint,))
474
if disable_breakpoints:
475
f.write("disable\n")
476
if not gdb_no_tui:
477
f.write("tui enable\n")
478
f.write("r\n")
479
f.close()
480
if sys.platform == "darwin" and os.getenv('DISPLAY'):
481
cmd.extend(['gdb', '-x', '/tmp/x.gdb', '--args'])
482
elif os.environ.get('DISPLAY'):
483
cmd.extend(['xterm', '-e', 'gdb', '-x', '/tmp/x.gdb', '--args'])
484
else:
485
cmd.extend(['screen',
486
'-L', '-Logfile', 'gdb.log',
487
'-d',
488
'-m',
489
'-S', 'ardupilot-gdb',
490
'gdb', '--cd', os.getcwd(), '-x', '/tmp/x.gdb', binary, '--args'])
491
elif lldb:
492
f = open("/tmp/x.lldb", "w")
493
for breakingpoint in breakpoints:
494
f.write("b %s\n" % (breakingpoint,))
495
if disable_breakpoints:
496
f.write("disable\n")
497
f.write("settings set target.process.stop-on-exec false\n")
498
f.write("process launch\n")
499
f.close()
500
if sys.platform == "darwin" and os.getenv('DISPLAY'):
501
cmd.extend(['lldb', '-s', '/tmp/x.lldb', '--'])
502
elif os.environ.get('DISPLAY'):
503
cmd.extend(['xterm', '-e', 'lldb', '-s', '/tmp/x.lldb', '--'])
504
else:
505
raise RuntimeError("DISPLAY was not set")
506
507
cmd.append(binary)
508
509
if defaults_filepath is None:
510
defaults_filepath = []
511
if not isinstance(defaults_filepath, list):
512
defaults_filepath = [defaults_filepath]
513
defaults = [reltopdir(path) for path in defaults_filepath]
514
515
if not supplementary:
516
if wipe:
517
cmd.append('-w')
518
if home is not None:
519
cmd.extend(['--home', home])
520
cmd.extend(['--model', model])
521
if speedup is not None and speedup != 1:
522
ntf = tempfile.NamedTemporaryFile(mode="w", delete=False)
523
print(f"SIM_SPEEDUP {speedup}", file=ntf)
524
ntf.close()
525
# prepend it so that a caller can override the speedup in
526
# passed-in defaults:
527
defaults = [ntf.name] + defaults
528
if sim_rate_hz is not None:
529
cmd.extend(['--rate', str(sim_rate_hz)])
530
if unhide_parameters:
531
cmd.extend(['--unhide-groups'])
532
# somewhere for MAVProxy to connect to:
533
cmd.append('--serial1=tcp:2')
534
if enable_fgview:
535
cmd.append("--enable-fgview")
536
537
if len(defaults):
538
cmd.extend(['--defaults', ",".join(defaults)])
539
540
cmd.extend(customisations)
541
542
if "--defaults" in customisations:
543
raise ValueError("--defaults must be passed in via defaults_filepath keyword argument, not as part of customisation list") # noqa
544
545
pexpect_logfile_prefix = stdout_prefix
546
if pexpect_logfile_prefix is None:
547
pexpect_logfile_prefix = os.path.basename(binary)
548
pexpect_logfile = PSpawnStdPrettyPrinter(prefix=pexpect_logfile_prefix)
549
550
if (gdb or lldb) and sys.platform == "darwin" and os.getenv('DISPLAY'):
551
global windowID
552
# on MacOS record the window IDs so we can close them later
553
atexit.register(kill_mac_terminal)
554
child = None
555
mydir = os.path.dirname(os.path.realpath(__file__))
556
autotest_dir = os.path.realpath(os.path.join(mydir, '..'))
557
runme = [os.path.join(autotest_dir, "run_in_terminal_window.sh"), 'mactest']
558
runme.extend(cmd)
559
print(cmd)
560
out = subprocess.Popen(runme, stdout=subprocess.PIPE).communicate()[0]
561
out = out.decode('utf-8')
562
p = re.compile('tab 1 of window id (.*)')
563
564
tstart = time.time()
565
while time.time() - tstart < 5:
566
tabs = p.findall(out)
567
568
if len(tabs) > 0:
569
break
570
571
time.sleep(0.1)
572
# sleep for extra 2 seconds for application to start
573
time.sleep(2)
574
if len(tabs) > 0:
575
windowID.append(tabs[0])
576
else:
577
print("Cannot find %s process terminal" % binary)
578
child = FakeMacOSXSpawn()
579
elif gdb and not os.getenv('DISPLAY'):
580
subprocess.Popen(cmd)
581
atexit.register(kill_screen_gdb)
582
# we are expected to return a pexpect wrapped around the
583
# stdout of the ArduPilot binary. Not going to happen until
584
# AP gets a redirect-stdout-to-filehandle option. So, in the
585
# meantime, return a dummy:
586
return pexpect.spawn("true", ["true"],
587
logfile=pexpect_logfile,
588
encoding=ENCODING,
589
timeout=5)
590
else:
591
print("Running: %s" % cmd_as_shell(cmd))
592
593
first = cmd[0]
594
rest = cmd[1:]
595
child = pexpect.spawn(first, rest, logfile=pexpect_logfile, encoding=ENCODING, timeout=5)
596
pexpect_autoclose(child)
597
if gdb or lldb:
598
# if we run GDB we do so in an xterm. "Waiting for
599
# connection" is never going to appear on xterm's output.
600
# ... so let's give it another magic second.
601
time.sleep(1)
602
# TODO: have a SITL-compiled ardupilot able to have its
603
# console on an output fd.
604
else:
605
child.expect('Waiting for ', timeout=300)
606
return child
607
608
609
def mavproxy_cmd():
610
"""return path to which mavproxy to use"""
611
return os.getenv('MAVPROXY_CMD', 'mavproxy.py')
612
613
614
def MAVProxy_version():
615
"""return the current version of mavproxy as a tuple e.g. (1,8,8)"""
616
command = "%s --version" % mavproxy_cmd()
617
output = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE).communicate()[0]
618
output = output.decode('ascii')
619
match = re.search("MAVProxy Version: ([0-9]+)[.]([0-9]+)[.]([0-9]+)", output)
620
if match is None:
621
raise ValueError("Unable to determine MAVProxy version from (%s)" % output)
622
return int(match.group(1)), int(match.group(2)), int(match.group(3))
623
624
625
def start_MAVProxy_SITL(atype,
626
aircraft=None,
627
setup=False,
628
master=None,
629
options=[],
630
sitl_rcin_port=5501,
631
pexpect_timeout=60,
632
logfile=sys.stdout):
633
"""Launch mavproxy connected to a SITL instance."""
634
if master is None:
635
raise ValueError("Expected a master")
636
637
local_mp_modules_dir = os.path.abspath(
638
os.path.join(__file__, '..', '..', '..', 'mavproxy_modules'))
639
env = dict(os.environ)
640
old = env.get('PYTHONPATH', None)
641
env['PYTHONPATH'] = local_mp_modules_dir
642
if old is not None:
643
env['PYTHONPATH'] += os.path.pathsep + old
644
645
global close_list
646
cmd = []
647
cmd.append(mavproxy_cmd())
648
cmd.extend(['--master', master])
649
cmd.extend(['--sitl', "localhost:%u" % sitl_rcin_port])
650
if setup:
651
cmd.append('--setup')
652
if aircraft is None:
653
aircraft = 'test.%s' % atype
654
cmd.extend(['--aircraft', aircraft])
655
cmd.extend(options)
656
cmd.extend(['--default-modules', 'misc,wp,rally,fence,param,arm,mode,rc,cmdlong,output'])
657
658
print("PYTHONPATH: %s" % str(env['PYTHONPATH']))
659
print("Running: %s" % cmd_as_shell(cmd))
660
661
ret = pexpect.spawn(cmd[0], cmd[1:], logfile=logfile, encoding=ENCODING, timeout=pexpect_timeout, env=env)
662
ret.delaybeforesend = 0
663
pexpect_autoclose(ret)
664
return ret
665
666
667
def start_PPP_daemon(ips, sockaddr):
668
"""Start pppd for networking"""
669
670
global close_list
671
cmd = "sudo pppd socket %s debug noauth nodetach %s" % (sockaddr, ips)
672
cmd = cmd.split()
673
print("Running: %s" % cmd_as_shell(cmd))
674
675
ret = pexpect.spawn(cmd[0], cmd[1:], logfile=sys.stdout, encoding=ENCODING, timeout=30)
676
ret.delaybeforesend = 0
677
pexpect_autoclose(ret)
678
return ret
679
680
681
def expect_setup_callback(e, callback):
682
"""Setup a callback that is called once a second while waiting for
683
patterns."""
684
def _expect_callback(pattern, timeout=e.timeout):
685
tstart = time.time()
686
while time.time() < tstart + timeout:
687
try:
688
ret = e.expect_saved(pattern, timeout=1)
689
return ret
690
except pexpect.TIMEOUT:
691
e.expect_user_callback(e)
692
print("Timed out looking for %s" % pattern)
693
raise pexpect.TIMEOUT(timeout)
694
695
e.expect_user_callback = callback
696
e.expect_saved = e.expect
697
e.expect = _expect_callback
698
699
700
def mkdir_p(directory):
701
"""Like mkdir -p ."""
702
if not directory:
703
return
704
if directory.endswith("/"):
705
mkdir_p(directory[:-1])
706
return
707
if os.path.isdir(directory):
708
return
709
mkdir_p(os.path.dirname(directory))
710
os.mkdir(directory)
711
712
713
def loadfile(fname):
714
"""Load a file as a string."""
715
f = open(fname, mode='r')
716
r = f.read()
717
f.close()
718
return r
719
720
721
def lock_file(fname):
722
"""Lock a file."""
723
import fcntl
724
f = open(fname, mode='w')
725
try:
726
fcntl.lockf(f, fcntl.LOCK_EX | fcntl.LOCK_NB)
727
except OSError:
728
return None
729
return f
730
731
732
def check_parent(parent_pid=None):
733
"""Check our parent process is still alive."""
734
if parent_pid is None:
735
try:
736
parent_pid = os.getppid()
737
except OSError:
738
pass
739
if parent_pid is None:
740
return
741
try:
742
os.kill(parent_pid, 0)
743
except OSError:
744
print("Parent had finished - exiting")
745
sys.exit(1)
746
747
748
def gps_newpos(lat, lon, bearing, distance):
749
"""Extrapolate latitude/longitude given a heading and distance
750
thanks to http://www.movable-type.co.uk/scripts/latlong.html .
751
"""
752
from math import sin, asin, cos, atan2, radians, degrees
753
754
lat1 = radians(lat)
755
lon1 = radians(lon)
756
brng = radians(bearing)
757
dr = distance / RADIUS_OF_EARTH
758
759
lat2 = asin(sin(lat1) * cos(dr) +
760
cos(lat1) * sin(dr) * cos(brng))
761
lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
762
cos(dr) - sin(lat1) * sin(lat2))
763
return degrees(lat2), degrees(lon2)
764
765
766
def gps_distance(lat1, lon1, lat2, lon2):
767
"""Return distance between two points in meters,
768
coordinates are in degrees
769
thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""
770
lat1 = math.radians(lat1)
771
lat2 = math.radians(lat2)
772
lon1 = math.radians(lon1)
773
lon2 = math.radians(lon2)
774
dLat = lat2 - lat1
775
dLon = lon2 - lon1
776
777
a = math.sin(0.5 * dLat)**2 + math.sin(0.5 * dLon)**2 * math.cos(lat1) * math.cos(lat2)
778
c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0 - a))
779
return RADIUS_OF_EARTH * c
780
781
782
def gps_bearing(lat1, lon1, lat2, lon2):
783
"""Return bearing between two points in degrees, in range 0-360
784
thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""
785
lat1 = math.radians(lat1)
786
lat2 = math.radians(lat2)
787
lon1 = math.radians(lon1)
788
lon2 = math.radians(lon2)
789
dLon = lon2 - lon1
790
y = math.sin(dLon) * math.cos(lat2)
791
x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dLon)
792
bearing = math.degrees(math.atan2(y, x))
793
if bearing < 0:
794
bearing += 360.0
795
return bearing
796
797
798
def constrain(value, minv, maxv):
799
"""Constrain a value to a range."""
800
if value < minv:
801
value = minv
802
if value > maxv:
803
value = maxv
804
return value
805
806
807
def load_local_module(fname):
808
"""load a python module from within the ardupilot tree"""
809
fname = os.path.join(topdir(), fname)
810
if sys.version_info.major >= 3:
811
import importlib.util
812
spec = importlib.util.spec_from_file_location("local_module", fname)
813
ret = importlib.util.module_from_spec(spec)
814
spec.loader.exec_module(ret)
815
else:
816
import imp
817
ret = imp.load_source("local_module", fname)
818
return ret
819
820
821
def get_git_hash(short=False):
822
short_v = "--short=8 " if short else ""
823
githash = run_cmd(f'git rev-parse {short_v}HEAD', output=True, directory=reltopdir('.')).strip()
824
if sys.version_info.major >= 3:
825
githash = githash.decode('utf-8')
826
return githash
827
828
829
if __name__ == "__main__":
830
import doctest
831
doctest.testmod()
832
833