Tools: random Python cleanups based on lgtm analysis
This commit is contained in:
parent
c6f43e9700
commit
642935fd43
@ -6,7 +6,6 @@
|
|||||||
|
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
import collections
|
import collections
|
||||||
import os
|
|
||||||
import numpy
|
import numpy
|
||||||
import bisect
|
import bisect
|
||||||
import sys
|
import sys
|
||||||
|
@ -38,7 +38,7 @@ def run_replay(logfile):
|
|||||||
|
|
||||||
def get_log_list():
|
def get_log_list():
|
||||||
'''get a list of log files to process'''
|
'''get a list of log files to process'''
|
||||||
import glob, os, sys
|
import glob, sys
|
||||||
pattern = os.path.join(opts.logdir, "*-checked.bin")
|
pattern = os.path.join(opts.logdir, "*-checked.bin")
|
||||||
file_list = glob.glob(pattern)
|
file_list = glob.glob(pattern)
|
||||||
print("Found %u logs to processs" % len(file_list))
|
print("Found %u logs to processs" % len(file_list))
|
||||||
@ -148,7 +148,6 @@ def check_logs():
|
|||||||
os.unlink("replay_results.txt")
|
os.unlink("replay_results.txt")
|
||||||
except Exception as ex:
|
except Exception as ex:
|
||||||
print(ex)
|
print(ex)
|
||||||
pass
|
|
||||||
|
|
||||||
for logfile in log_list:
|
for logfile in log_list:
|
||||||
run_replay(logfile)
|
run_replay(logfile)
|
||||||
@ -157,7 +156,7 @@ def check_logs():
|
|||||||
|
|
||||||
def create_checked_logs():
|
def create_checked_logs():
|
||||||
'''create a set of CHEK logs'''
|
'''create a set of CHEK logs'''
|
||||||
import glob, os, sys
|
import glob, sys
|
||||||
if os.path.isfile(opts.logdir):
|
if os.path.isfile(opts.logdir):
|
||||||
full_file_list = [opts.logdir]
|
full_file_list = [opts.logdir]
|
||||||
else:
|
else:
|
||||||
|
@ -2,13 +2,9 @@
|
|||||||
|
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
import os
|
import os
|
||||||
import pexpect
|
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
|
|
||||||
from common import AutoTest
|
from common import AutoTest
|
||||||
from pysim import util
|
|
||||||
from pysim import vehicleinfo
|
|
||||||
import operator
|
|
||||||
|
|
||||||
# get location of scripts
|
# get location of scripts
|
||||||
testdir = os.path.dirname(os.path.realpath(__file__))
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||||
|
@ -4,7 +4,6 @@
|
|||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
|
|
||||||
import os
|
import os
|
||||||
import pexpect
|
|
||||||
import time
|
import time
|
||||||
|
|
||||||
from common import AutoTest
|
from common import AutoTest
|
||||||
@ -14,8 +13,6 @@ from common import MsgRcvTimeoutException
|
|||||||
from common import NotAchievedException
|
from common import NotAchievedException
|
||||||
from common import PreconditionFailedException
|
from common import PreconditionFailedException
|
||||||
|
|
||||||
from pysim import util
|
|
||||||
|
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
|
|
||||||
# get location of scripts
|
# get location of scripts
|
||||||
@ -961,7 +958,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
success = False
|
success = False
|
||||||
except AutoTestTimeoutException as e:
|
except AutoTestTimeoutException as e:
|
||||||
success = True
|
success = True
|
||||||
pass
|
|
||||||
self.mav.mav.srcSystem = old_srcSystem
|
self.mav.mav.srcSystem = old_srcSystem
|
||||||
if not success:
|
if not success:
|
||||||
raise NotAchievedException(
|
raise NotAchievedException(
|
||||||
@ -976,7 +972,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
self.disarm_vehicle()
|
self.disarm_vehicle()
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
comp_arm_exception = e
|
comp_arm_exception = e
|
||||||
pass
|
|
||||||
self.mav.mav.srcSystem = old_srcSystem
|
self.mav.mav.srcSystem = old_srcSystem
|
||||||
if comp_arm_exception is not None:
|
if comp_arm_exception is not None:
|
||||||
raise comp_arm_exception
|
raise comp_arm_exception
|
||||||
@ -1078,11 +1073,11 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
self.progress("Testing FENCE_POINT protocol")
|
self.progress("Testing FENCE_POINT protocol")
|
||||||
self.set_parameter("FENCE_TOTAL", 1)
|
self.set_parameter("FENCE_TOTAL", 1)
|
||||||
|
|
||||||
self.roundtrip_fencepoint_protocol(0, 1, 1.2345, 5.4321, target_system=target_component, target_component=target_component)
|
self.roundtrip_fencepoint_protocol(0, 1, 1.2345, 5.4321, target_system=target_system, target_component=target_component)
|
||||||
|
|
||||||
lat = 2.345
|
lat = 2.345
|
||||||
lng = 4.321
|
lng = 4.321
|
||||||
self.roundtrip_fencepoint_protocol(0, 1, lat, lng, target_system=target_component, target_component=target_component)
|
self.roundtrip_fencepoint_protocol(0, 1, lat, lng, target_system=target_system, target_component=target_component)
|
||||||
|
|
||||||
def test_offboard(self, timeout=90):
|
def test_offboard(self, timeout=90):
|
||||||
self.load_mission("rover-guided-mission.txt")
|
self.load_mission("rover-guided-mission.txt")
|
||||||
@ -1469,7 +1464,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
m = self.get_mission_item_int_on_link(2, self.mav, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
|
m = self.get_mission_item_int_on_link(2, self.mav, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
|
||||||
if m2.x != m.x:
|
if m2.x != m.x:
|
||||||
raise NotAchievedException("mission items do not match (%d vs %d)" % (m2.x, m.x))
|
raise NotAchievedException("mission items do not match (%d vs %d)" % (m2.x, m.x))
|
||||||
m_nonint = self.get_mission_item_on_link(2, self.mav, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
|
self.get_mission_item_on_link(2, self.mav, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
|
||||||
# ensure we get nacks for bad mission item requests:
|
# ensure we get nacks for bad mission item requests:
|
||||||
self.mav.mav.mission_request_send(target_system,
|
self.mav.mav.mission_request_send(target_system,
|
||||||
target_component,
|
target_component,
|
||||||
@ -1563,7 +1558,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_MISSION, 0)
|
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_MISSION, 0)
|
||||||
|
|
||||||
self.progress("Answering request for mission item 0")
|
self.progress("Answering request for mission item 0")
|
||||||
wp = self.mav.mav.mission_item_int_send(
|
self.mav.mav.mission_item_int_send(
|
||||||
target_system,
|
target_system,
|
||||||
target_component,
|
target_component,
|
||||||
0, # seq
|
0, # seq
|
||||||
@ -1599,7 +1594,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
|
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
|
||||||
|
|
||||||
self.progress("Answering request for waypoints item 1")
|
self.progress("Answering request for waypoints item 1")
|
||||||
wp = self.mav.mav.mission_item_int_send(
|
self.mav.mav.mission_item_int_send(
|
||||||
target_system,
|
target_system,
|
||||||
target_component,
|
target_component,
|
||||||
1, # seq
|
1, # seq
|
||||||
|
@ -7,9 +7,7 @@ import math
|
|||||||
import os
|
import os
|
||||||
import shutil
|
import shutil
|
||||||
import time
|
import time
|
||||||
import traceback
|
|
||||||
|
|
||||||
import pexpect
|
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
from pymavlink import mavextra
|
from pymavlink import mavextra
|
||||||
|
|
||||||
@ -1833,7 +1831,6 @@ class AutoTestCopter(AutoTest):
|
|||||||
self.reboot_sitl()
|
self.reboot_sitl()
|
||||||
|
|
||||||
self.progress("Waiting for location")
|
self.progress("Waiting for location")
|
||||||
old_pos = self.mav.location()
|
|
||||||
self.zero_throttle()
|
self.zero_throttle()
|
||||||
self.takeoff(10, 1800)
|
self.takeoff(10, 1800)
|
||||||
self.change_mode("LAND")
|
self.change_mode("LAND")
|
||||||
@ -3384,8 +3381,6 @@ class AutoTestCopter(AutoTest):
|
|||||||
self.takeoff(10, mode="LOITER")
|
self.takeoff(10, mode="LOITER")
|
||||||
self.set_parameter("SIM_SPEEDUP", 1)
|
self.set_parameter("SIM_SPEEDUP", 1)
|
||||||
self.change_mode("FOLLOW")
|
self.change_mode("FOLLOW")
|
||||||
ex = None
|
|
||||||
tstart = self.get_sim_time_cached()
|
|
||||||
new_loc = self.mav.location()
|
new_loc = self.mav.location()
|
||||||
new_loc_offset_n = 20
|
new_loc_offset_n = 20
|
||||||
new_loc_offset_e = 30
|
new_loc_offset_e = 30
|
||||||
@ -3405,13 +3400,12 @@ class AutoTestCopter(AutoTest):
|
|||||||
while True:
|
while True:
|
||||||
now = self.get_sim_time_cached()
|
now = self.get_sim_time_cached()
|
||||||
if now - last_sent > 0.5:
|
if now - last_sent > 0.5:
|
||||||
last_run = now
|
|
||||||
gpi = self.global_position_int_for_location(new_loc,
|
gpi = self.global_position_int_for_location(new_loc,
|
||||||
now,
|
now,
|
||||||
heading=heading)
|
heading=heading)
|
||||||
gpi.pack(self.mav.mav)
|
gpi.pack(self.mav.mav)
|
||||||
self.mav.mav.send(gpi)
|
self.mav.mav.send(gpi)
|
||||||
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
||||||
pos = self.mav.location()
|
pos = self.mav.location()
|
||||||
delta = self.get_distance(expected_loc, pos)
|
delta = self.get_distance(expected_loc, pos)
|
||||||
max_delta = 2
|
max_delta = 2
|
||||||
@ -3931,13 +3925,13 @@ class AutoTestHeli(AutoTestCopter):
|
|||||||
self.set_rc(8, 1000)
|
self.set_rc(8, 1000)
|
||||||
|
|
||||||
def set_rc_default(self):
|
def set_rc_default(self):
|
||||||
super(AutoTestCopter, self).set_rc_default()
|
super(AutoTestHeli, self).set_rc_default()
|
||||||
self.progress("Lowering rotor speed")
|
self.progress("Lowering rotor speed")
|
||||||
self.set_rc(8, 1000)
|
self.set_rc(8, 1000)
|
||||||
|
|
||||||
def tests(self):
|
def tests(self):
|
||||||
'''return list of all tests'''
|
'''return list of all tests'''
|
||||||
ret = super(AutoTestCopter, self).tests()
|
ret = AutoTest.tests(self)
|
||||||
ret.extend([
|
ret.extend([
|
||||||
("AVCMission", "Fly AVC mission", self.fly_avc_test),
|
("AVCMission", "Fly AVC mission", self.fly_avc_test),
|
||||||
|
|
||||||
|
@ -5,19 +5,14 @@ from __future__ import print_function
|
|||||||
import math
|
import math
|
||||||
import os
|
import os
|
||||||
|
|
||||||
import pexpect
|
|
||||||
from pymavlink import quaternion
|
from pymavlink import quaternion
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
|
|
||||||
from pysim import util
|
|
||||||
|
|
||||||
from common import AutoTest
|
from common import AutoTest
|
||||||
from common import AutoTestTimeoutException
|
from common import AutoTestTimeoutException
|
||||||
from common import NotAchievedException
|
from common import NotAchievedException
|
||||||
from common import PreconditionFailedException
|
from common import PreconditionFailedException
|
||||||
|
|
||||||
from MAVProxy.modules.lib import mp_util
|
|
||||||
|
|
||||||
# get location of scripts
|
# get location of scripts
|
||||||
testdir = os.path.dirname(os.path.realpath(__file__))
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||||
SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 585, 354)
|
SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 585, 354)
|
||||||
@ -693,7 +688,6 @@ class AutoTestPlane(AutoTest):
|
|||||||
self.wait_mode('AUTO')
|
self.wait_mode('AUTO')
|
||||||
self.wait_ready_to_arm()
|
self.wait_ready_to_arm()
|
||||||
self.arm_vehicle()
|
self.arm_vehicle()
|
||||||
tstart = self.get_sim_time_cached()
|
|
||||||
last_mission_current_msg = 0
|
last_mission_current_msg = 0
|
||||||
last_seq = None
|
last_seq = None
|
||||||
while self.armed():
|
while self.armed():
|
||||||
@ -957,7 +951,6 @@ class AutoTestPlane(AutoTest):
|
|||||||
here = self.mav.location()
|
here = self.mav.location()
|
||||||
got_radius = self.get_distance(loc, here)
|
got_radius = self.get_distance(loc, here)
|
||||||
average_radius = 0.95*average_radius + 0.05*got_radius
|
average_radius = 0.95*average_radius + 0.05*got_radius
|
||||||
now = self.get_sim_time()
|
|
||||||
on_radius = abs(got_radius - want_radius) < epsilon
|
on_radius = abs(got_radius - want_radius) < epsilon
|
||||||
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
||||||
heading = m.heading
|
heading = m.heading
|
||||||
@ -1185,7 +1178,7 @@ class AutoTestPlane(AutoTest):
|
|||||||
self.change_mode('MANUAL')
|
self.change_mode('MANUAL')
|
||||||
|
|
||||||
# grab home position:
|
# grab home position:
|
||||||
m = self.mav.recv_match(type='HOME_POSITION', blocking=True)
|
self.mav.recv_match(type='HOME_POSITION', blocking=True)
|
||||||
self.homeloc = self.mav.location()
|
self.homeloc = self.mav.location()
|
||||||
|
|
||||||
self.run_subtest("Takeoff", self.takeoff)
|
self.run_subtest("Takeoff", self.takeoff)
|
||||||
@ -1239,6 +1232,7 @@ class AutoTestPlane(AutoTest):
|
|||||||
ex = None
|
ex = None
|
||||||
self.context_push()
|
self.context_push()
|
||||||
self.progress("Making sure we don't ordinarily get RANGEFINDER")
|
self.progress("Making sure we don't ordinarily get RANGEFINDER")
|
||||||
|
m = None
|
||||||
try:
|
try:
|
||||||
m = self.mav.recv_match(type='RANGEFINDER',
|
m = self.mav.recv_match(type='RANGEFINDER',
|
||||||
blocking=True,
|
blocking=True,
|
||||||
@ -1261,7 +1255,6 @@ class AutoTestPlane(AutoTest):
|
|||||||
self.change_mode('AUTO')
|
self.change_mode('AUTO')
|
||||||
self.wait_ready_to_arm()
|
self.wait_ready_to_arm()
|
||||||
self.arm_vehicle()
|
self.arm_vehicle()
|
||||||
home = self.poll_home_position()
|
|
||||||
self.wait_waypoint(5, 5, max_dist=100)
|
self.wait_waypoint(5, 5, max_dist=100)
|
||||||
rf = self.mav.recv_match(type="RANGEFINDER", timeout=1, blocking=True)
|
rf = self.mav.recv_match(type="RANGEFINDER", timeout=1, blocking=True)
|
||||||
if rf is None:
|
if rf is None:
|
||||||
|
@ -4,11 +4,8 @@
|
|||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
import os
|
import os
|
||||||
|
|
||||||
import pexpect
|
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
|
|
||||||
from pysim import util
|
|
||||||
|
|
||||||
from common import AutoTest
|
from common import AutoTest
|
||||||
from common import NotAchievedException
|
from common import NotAchievedException
|
||||||
|
|
||||||
|
@ -74,7 +74,7 @@ def get_default_params(atype, binary):
|
|||||||
util.pexpect_close(sitl)
|
util.pexpect_close(sitl)
|
||||||
sitl = util.start_SITL(binary, model=frame, home=home, speedup=10)
|
sitl = util.start_SITL(binary, model=frame, home=home, speedup=10)
|
||||||
mavproxy = util.start_MAVProxy_SITL(atype)
|
mavproxy = util.start_MAVProxy_SITL(atype)
|
||||||
idx = mavproxy.expect('Saved [0-9]+ parameters to (\S+)')
|
mavproxy.expect('Saved [0-9]+ parameters to (\S+)')
|
||||||
parmfile = mavproxy.match.group(1)
|
parmfile = mavproxy.match.group(1)
|
||||||
dest = buildlogs_path('%s-defaults.parm' % atype)
|
dest = buildlogs_path('%s-defaults.parm' % atype)
|
||||||
shutil.copy(parmfile, dest)
|
shutil.copy(parmfile, dest)
|
||||||
|
@ -2,7 +2,6 @@ from __future__ import print_function
|
|||||||
|
|
||||||
import abc
|
import abc
|
||||||
import math
|
import math
|
||||||
import itertools
|
|
||||||
import os
|
import os
|
||||||
import re
|
import re
|
||||||
import shutil
|
import shutil
|
||||||
@ -1269,13 +1268,13 @@ class AutoTest(ABC):
|
|||||||
def get_distance(loc1, loc2):
|
def get_distance(loc1, loc2):
|
||||||
"""Get ground distance between two locations."""
|
"""Get ground distance between two locations."""
|
||||||
return AutoTest.get_distance_accurate(loc1, loc2)
|
return AutoTest.get_distance_accurate(loc1, loc2)
|
||||||
dlat = loc2.lat - loc1.lat
|
# dlat = loc2.lat - loc1.lat
|
||||||
try:
|
# try:
|
||||||
dlong = loc2.lng - loc1.lng
|
# dlong = loc2.lng - loc1.lng
|
||||||
except AttributeError:
|
# except AttributeError:
|
||||||
dlong = loc2.lon - loc1.lon
|
# dlong = loc2.lon - loc1.lon
|
||||||
|
|
||||||
return math.sqrt((dlat*dlat) + (dlong*dlong)*AutoTest.longitude_scale(loc2.lat)) * 1.113195e5
|
# return math.sqrt((dlat*dlat) + (dlong*dlong)*AutoTest.longitude_scale(loc2.lat)) * 1.113195e5
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_distance_accurate(loc1, loc2):
|
def get_distance_accurate(loc1, loc2):
|
||||||
@ -1292,7 +1291,6 @@ class AutoTest(ABC):
|
|||||||
@staticmethod
|
@staticmethod
|
||||||
def get_latlon_attr(loc, attrs):
|
def get_latlon_attr(loc, attrs):
|
||||||
'''return any found latitude attribute from loc'''
|
'''return any found latitude attribute from loc'''
|
||||||
latattrs = "lat", "latitude"
|
|
||||||
ret = None
|
ret = None
|
||||||
for attr in attrs:
|
for attr in attrs:
|
||||||
if hasattr(loc, attr):
|
if hasattr(loc, attr):
|
||||||
@ -2062,18 +2060,15 @@ class AutoTest(ABC):
|
|||||||
self.progress("Starting MAVProxy interaction as directed")
|
self.progress("Starting MAVProxy interaction as directed")
|
||||||
self.mavproxy.interact()
|
self.mavproxy.interact()
|
||||||
tee.close()
|
tee.close()
|
||||||
tee = None
|
|
||||||
self.check_sitl_reset()
|
self.check_sitl_reset()
|
||||||
return
|
return
|
||||||
self.test_timings[desc] = time.time() - start_time
|
self.test_timings[desc] = time.time() - start_time
|
||||||
self.context_pop()
|
self.context_pop()
|
||||||
self.progress('PASSED: "%s"' % prettyname)
|
self.progress('PASSED: "%s"' % prettyname)
|
||||||
tee.close()
|
tee.close()
|
||||||
tee = None
|
|
||||||
|
|
||||||
def check_test_syntax(self, test_file):
|
def check_test_syntax(self, test_file):
|
||||||
"""Check mistake on autotest function syntax."""
|
"""Check mistake on autotest function syntax."""
|
||||||
import re
|
|
||||||
self.start_test("Check for syntax mistake in autotest lambda")
|
self.start_test("Check for syntax mistake in autotest lambda")
|
||||||
if not os.path.isfile(test_file):
|
if not os.path.isfile(test_file):
|
||||||
self.progress("File %s does not exist" % test_file)
|
self.progress("File %s does not exist" % test_file)
|
||||||
@ -2656,10 +2651,8 @@ class AutoTest(ABC):
|
|||||||
self.change_mode(mode)
|
self.change_mode(mode)
|
||||||
except AutoTestTimeoutException:
|
except AutoTestTimeoutException:
|
||||||
self.progress("PASS not able to set mode : %s disarmed" % mode)
|
self.progress("PASS not able to set mode : %s disarmed" % mode)
|
||||||
pass
|
|
||||||
except ValueError:
|
except ValueError:
|
||||||
self.progress("PASS not able to set mode : %s disarmed" % mode)
|
self.progress("PASS not able to set mode : %s disarmed" % mode)
|
||||||
pass
|
|
||||||
else:
|
else:
|
||||||
self.progress("Not armable mode : %s" % mode)
|
self.progress("Not armable mode : %s" % mode)
|
||||||
self.change_mode(mode)
|
self.change_mode(mode)
|
||||||
@ -2708,11 +2701,9 @@ class AutoTest(ABC):
|
|||||||
except AutoTestTimeoutException:
|
except AutoTestTimeoutException:
|
||||||
self.set_parameter("SIM_GPS_DISABLE", 0)
|
self.set_parameter("SIM_GPS_DISABLE", 0)
|
||||||
self.progress("PASS not able to set mode without Position : %s" % mode)
|
self.progress("PASS not able to set mode without Position : %s" % mode)
|
||||||
pass
|
|
||||||
except ValueError:
|
except ValueError:
|
||||||
self.set_parameter("SIM_GPS_DISABLE", 0)
|
self.set_parameter("SIM_GPS_DISABLE", 0)
|
||||||
self.progress("PASS not able to set mode without Position : %s" % mode)
|
self.progress("PASS not able to set mode without Position : %s" % mode)
|
||||||
pass
|
|
||||||
if mode == "FOLLOW":
|
if mode == "FOLLOW":
|
||||||
self.set_parameter("FOLL_ENABLE", 0)
|
self.set_parameter("FOLL_ENABLE", 0)
|
||||||
self.change_mode(self.default_mode())
|
self.change_mode(self.default_mode())
|
||||||
|
@ -60,14 +60,8 @@ heading = 0.0
|
|||||||
speedN = 0
|
speedN = 0
|
||||||
speedE = 0.0
|
speedE = 0.0
|
||||||
speedD = 0.0
|
speedD = 0.0
|
||||||
xAccel = 0.0
|
|
||||||
yAccel = 0.0
|
|
||||||
zAccel = 0.0
|
|
||||||
rollRate = 0.0
|
rollRate = 0.0
|
||||||
pitchRate = 0.0
|
|
||||||
yawRate = 0.0
|
yawRate = 0.0
|
||||||
rollDeg = 0.0
|
|
||||||
pitchDeg = 0.0
|
|
||||||
yawDeg = 0.0
|
yawDeg = 0.0
|
||||||
airspeed = 0
|
airspeed = 0
|
||||||
magic = 0x4c56414f
|
magic = 0x4c56414f
|
||||||
|
@ -285,10 +285,8 @@ def main_loop():
|
|||||||
"""Run main loop."""
|
"""Run main loop."""
|
||||||
tnow = time.time()
|
tnow = time.time()
|
||||||
last_report = tnow
|
last_report = tnow
|
||||||
last_sim_input = tnow
|
|
||||||
last_wind_update = tnow
|
last_wind_update = tnow
|
||||||
frame_count = 0
|
frame_count = 0
|
||||||
paused = False
|
|
||||||
simstep = 1.0/opts.rate
|
simstep = 1.0/opts.rate
|
||||||
simtime = simstep
|
simtime = simstep
|
||||||
frame_time = 1.0/opts.rate
|
frame_time = 1.0/opts.rate
|
||||||
@ -349,7 +347,6 @@ def main_loop():
|
|||||||
if new_frame:
|
if new_frame:
|
||||||
now = time.time()
|
now = time.time()
|
||||||
if now < last_wall_time + scaled_frame_time:
|
if now < last_wall_time + scaled_frame_time:
|
||||||
dt = last_wall_time+scaled_frame_time - now
|
|
||||||
time.sleep(last_wall_time+scaled_frame_time - now)
|
time.sleep(last_wall_time+scaled_frame_time - now)
|
||||||
now = time.time()
|
now = time.time()
|
||||||
|
|
||||||
|
@ -11,7 +11,6 @@ import sys
|
|||||||
import tempfile
|
import tempfile
|
||||||
import time
|
import time
|
||||||
from math import acos, atan2, cos, pi, sqrt
|
from math import acos, atan2, cos, pi, sqrt
|
||||||
from subprocess import PIPE, Popen, call, check_call
|
|
||||||
|
|
||||||
import pexpect
|
import pexpect
|
||||||
|
|
||||||
@ -68,11 +67,11 @@ def run_cmd(cmd, directory=".", show=True, output=False, checkfail=True):
|
|||||||
if show:
|
if show:
|
||||||
print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), directory,))
|
print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), directory,))
|
||||||
if output:
|
if output:
|
||||||
return Popen(cmd, shell=shell, stdout=PIPE, cwd=directory).communicate()[0]
|
return subprocess.Popen(cmd, shell=shell, stdout=subprocess.PIPE, cwd=directory).communicate()[0]
|
||||||
elif checkfail:
|
elif checkfail:
|
||||||
return check_call(cmd, shell=shell, cwd=directory)
|
return subprocess.check_call(cmd, shell=shell, cwd=directory)
|
||||||
else:
|
else:
|
||||||
return call(cmd, shell=shell, cwd=directory)
|
return subprocess.call(cmd, shell=shell, cwd=directory)
|
||||||
|
|
||||||
|
|
||||||
def rmfile(path):
|
def rmfile(path):
|
||||||
@ -296,7 +295,7 @@ def start_SITL(binary,
|
|||||||
cmd.extend(["--uartF=sim:vicon:"])
|
cmd.extend(["--uartF=sim:vicon:"])
|
||||||
|
|
||||||
if gdb and not os.getenv('DISPLAY'):
|
if gdb and not os.getenv('DISPLAY'):
|
||||||
p = subprocess.Popen(cmd)
|
subprocess.Popen(cmd)
|
||||||
atexit.register(kill_screen_gdb)
|
atexit.register(kill_screen_gdb)
|
||||||
# we are expected to return a pexpect wrapped around the
|
# we are expected to return a pexpect wrapped around the
|
||||||
# stdout of the ArduPilot binary. Not going to happen until
|
# stdout of the ArduPilot binary. Not going to happen until
|
||||||
@ -360,7 +359,6 @@ def expect_setup_callback(e, callback):
|
|||||||
return ret
|
return ret
|
||||||
except pexpect.TIMEOUT:
|
except pexpect.TIMEOUT:
|
||||||
e.expect_user_callback(e)
|
e.expect_user_callback(e)
|
||||||
pass
|
|
||||||
print("Timed out looking for %s" % pattern)
|
print("Timed out looking for %s" % pattern)
|
||||||
raise pexpect.TIMEOUT(timeout)
|
raise pexpect.TIMEOUT(timeout)
|
||||||
|
|
||||||
|
@ -3,13 +3,11 @@
|
|||||||
# Fly ArduPlane QuadPlane in SITL
|
# Fly ArduPlane QuadPlane in SITL
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
import os
|
import os
|
||||||
import pexpect
|
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
|
|
||||||
from common import AutoTest
|
from common import AutoTest
|
||||||
from common import AutoTestTimeoutException
|
from common import AutoTestTimeoutException
|
||||||
|
|
||||||
from pysim import util
|
|
||||||
from pysim import vehicleinfo
|
from pysim import vehicleinfo
|
||||||
import operator
|
import operator
|
||||||
|
|
||||||
|
@ -488,7 +488,6 @@ def run_in_terminal_window(autotest, name, cmd):
|
|||||||
# on MacOS record the window IDs so we can close them later
|
# on MacOS record the window IDs so we can close them later
|
||||||
out = subprocess.Popen(runme, stdout=subprocess.PIPE).communicate()[0]
|
out = subprocess.Popen(runme, stdout=subprocess.PIPE).communicate()[0]
|
||||||
out = out.decode('utf-8')
|
out = out.decode('utf-8')
|
||||||
import re
|
|
||||||
p = re.compile('tab 1 of window id (.*)')
|
p = re.compile('tab 1 of window id (.*)')
|
||||||
|
|
||||||
tstart = time.time()
|
tstart = time.time()
|
||||||
@ -506,7 +505,7 @@ def run_in_terminal_window(autotest, name, cmd):
|
|||||||
else:
|
else:
|
||||||
progress("Cannot find %s process terminal" % name)
|
progress("Cannot find %s process terminal" % name)
|
||||||
else:
|
else:
|
||||||
p = subprocess.Popen(runme)
|
subprocess.Popen(runme)
|
||||||
|
|
||||||
|
|
||||||
tracker_uarta = None # blemish
|
tracker_uarta = None # blemish
|
||||||
|
@ -137,7 +137,6 @@ is bob we will attempt to checkout bob-AVR'''
|
|||||||
return True
|
return True
|
||||||
except subprocess.CalledProcessError:
|
except subprocess.CalledProcessError:
|
||||||
self.progress("Checkout branch %s failed" % branch)
|
self.progress("Checkout branch %s failed" % branch)
|
||||||
pass
|
|
||||||
|
|
||||||
self.progress("Failed to find tag for %s %s %s %s" %
|
self.progress("Failed to find tag for %s %s %s %s" %
|
||||||
(vehicle, ctag, cboard, cframe))
|
(vehicle, ctag, cboard, cframe))
|
||||||
|
@ -3,7 +3,7 @@
|
|||||||
check firmware-version.txt in binaries directory
|
check firmware-version.txt in binaries directory
|
||||||
'''
|
'''
|
||||||
|
|
||||||
import os, shutil, subprocess
|
import os
|
||||||
|
|
||||||
VEHICLES = ['AntennaTracker', 'Copter', 'Plane', 'Rover', 'Sub']
|
VEHICLES = ['AntennaTracker', 'Copter', 'Plane', 'Rover', 'Sub']
|
||||||
|
|
||||||
@ -27,7 +27,7 @@ def parse_git_version(gfile):
|
|||||||
def check_fw_version(version):
|
def check_fw_version(version):
|
||||||
try:
|
try:
|
||||||
(version_numbers, release_type) = version.split("-")
|
(version_numbers, release_type) = version.split("-")
|
||||||
(major, minor, patch) = version_numbers.split(".")
|
(_, _, _) = version_numbers.split(".")
|
||||||
except Exception:
|
except Exception:
|
||||||
return False
|
return False
|
||||||
return True
|
return True
|
||||||
|
@ -5,7 +5,6 @@ script to run configre for all hwdef.dat, to check for syntax errors
|
|||||||
"""
|
"""
|
||||||
|
|
||||||
import os
|
import os
|
||||||
import shutil
|
|
||||||
import subprocess
|
import subprocess
|
||||||
import sys
|
import sys
|
||||||
import fnmatch
|
import fnmatch
|
||||||
|
@ -284,7 +284,7 @@ class ManifestGenerator():
|
|||||||
try:
|
try:
|
||||||
firmware_version = open(firmware_version_file).read()
|
firmware_version = open(firmware_version_file).read()
|
||||||
firmware_version = firmware_version.strip()
|
firmware_version = firmware_version.strip()
|
||||||
(version_numbers, release_type) = firmware_version.split("-")
|
(_, _) = firmware_version.split("-")
|
||||||
except ValueError:
|
except ValueError:
|
||||||
print("malformed firmware-version.txt at (%s)" % (firmware_version_file,), file=sys.stderr)
|
print("malformed firmware-version.txt at (%s)" % (firmware_version_file,), file=sys.stderr)
|
||||||
continue
|
continue
|
||||||
|
@ -4,7 +4,7 @@
|
|||||||
using the algorithm from Bill Premerlani
|
using the algorithm from Bill Premerlani
|
||||||
'''
|
'''
|
||||||
|
|
||||||
import sys, time, os, math
|
import sys
|
||||||
|
|
||||||
# command line option handling
|
# command line option handling
|
||||||
from optparse import OptionParser
|
from optparse import OptionParser
|
||||||
@ -19,7 +19,7 @@ parser.add_option("--repeat", type='int', default=1, help="number of repeats thr
|
|||||||
|
|
||||||
(opts, args) = parser.parse_args()
|
(opts, args) = parser.parse_args()
|
||||||
|
|
||||||
from rotmat import Vector3, Matrix3
|
from rotmat import Vector3
|
||||||
|
|
||||||
if len(args) < 1:
|
if len(args) < 1:
|
||||||
print("Usage: magfit_flashlog.py [options] <LOGFILE...>")
|
print("Usage: magfit_flashlog.py [options] <LOGFILE...>")
|
||||||
|
Loading…
Reference in New Issue
Block a user