mirror of https://github.com/ArduPilot/ardupilot
216 lines
7.3 KiB
Python
216 lines
7.3 KiB
Python
'''
|
|
Test AntennaTracker vehicle in SITL
|
|
|
|
AP_FLAKE8_CLEAN
|
|
|
|
'''
|
|
|
|
from __future__ import print_function
|
|
|
|
import math
|
|
import operator
|
|
import os
|
|
|
|
from pymavlink import mavextra
|
|
from pymavlink import mavutil
|
|
|
|
import vehicle_test_suite
|
|
from vehicle_test_suite import NotAchievedException
|
|
|
|
# get location of scripts
|
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
|
SITL_START_LOCATION = mavutil.location(-27.274439, 151.290064, 343, 8.7)
|
|
|
|
|
|
class AutoTestTracker(vehicle_test_suite.TestSuite):
|
|
|
|
def log_name(self):
|
|
return "AntennaTracker"
|
|
|
|
def default_speedup(self):
|
|
'''Tracker seems to be race-free'''
|
|
return 100
|
|
|
|
def test_filepath(self):
|
|
return os.path.realpath(__file__)
|
|
|
|
def sitl_start_location(self):
|
|
return SITL_START_LOCATION
|
|
|
|
def default_mode(self):
|
|
return "AUTO"
|
|
|
|
def is_tracker(self):
|
|
return True
|
|
|
|
def default_frame(self):
|
|
return "tracker"
|
|
|
|
def set_current_test_name(self, name):
|
|
self.current_test_name_directory = "AntennaTracker_Tests/" + name + "/"
|
|
|
|
def apply_defaultfile_parameters(self):
|
|
# tracker doesn't have a default parameters file
|
|
pass
|
|
|
|
def sysid_thismav(self):
|
|
return 2
|
|
|
|
def achieve_attitude(self, desyaw, despitch, tolerance=1, target_system=2, target_component=1):
|
|
'''use set_attitude_target to achieve desyaw / despitch'''
|
|
tstart = self.get_sim_time()
|
|
last_attitude_target_sent = 0
|
|
last_debug = 0
|
|
self.progress("Using set_attitude_target to achieve attitude")
|
|
while True:
|
|
now = self.get_sim_time()
|
|
if now - tstart > 60:
|
|
raise NotAchievedException("Did not achieve attitude")
|
|
if now - last_attitude_target_sent > 0.5:
|
|
last_attitude_target_sent = now
|
|
type_mask = (
|
|
1 << 0 | # ignore roll rate
|
|
1 << 6 # ignore throttle
|
|
)
|
|
self.mav.mav.set_attitude_target_send(
|
|
0, # time_boot_ms
|
|
target_system, # target sysid
|
|
target_component, # target compid
|
|
type_mask, # bitmask of things to ignore
|
|
mavextra.euler_to_quat([0,
|
|
math.radians(despitch),
|
|
math.radians(desyaw)]), # att
|
|
0, # yaw rate (rad/s)
|
|
0, # pitch rate
|
|
0, # yaw rate
|
|
0) # thrust, 0 to 1, translated to a climb/descent rate
|
|
m = self.assert_receive_message('ATTITUDE', timeout=2)
|
|
if now - last_debug > 1:
|
|
last_debug = now
|
|
self.progress("yaw=%f desyaw=%f pitch=%f despitch=%f" %
|
|
(math.degrees(m.yaw), desyaw,
|
|
math.degrees(m.pitch), despitch))
|
|
yaw_ok = abs(math.degrees(m.yaw) - desyaw) < tolerance
|
|
pitch_ok = abs(math.degrees(m.pitch) - despitch) < tolerance
|
|
if yaw_ok and pitch_ok:
|
|
self.progress("Achieved attitude")
|
|
break
|
|
|
|
def reboot_sitl(self, *args, **kwargs):
|
|
self.disarm_vehicle()
|
|
super(AutoTestTracker, self).reboot_sitl(*args, **kwargs)
|
|
|
|
def GUIDED(self):
|
|
'''Test GUIDED mode'''
|
|
self.reboot_sitl() # temporary hack around control issues
|
|
self.change_mode(4) # "GUIDED"
|
|
self.achieve_attitude(desyaw=10, despitch=30)
|
|
self.achieve_attitude(desyaw=0, despitch=0)
|
|
self.achieve_attitude(desyaw=45, despitch=10)
|
|
|
|
def MANUAL(self):
|
|
'''Test MANUAL mode'''
|
|
self.change_mode(0) # "MANUAL"
|
|
for chan in 1, 2:
|
|
for pwm in 1200, 1600, 1367:
|
|
self.set_rc(chan, pwm)
|
|
self.wait_servo_channel_value(chan, pwm)
|
|
|
|
def MAV_CMD_DO_SET_SERVO(self):
|
|
'''Test SERVOTEST mode'''
|
|
self.change_mode(0) # "MANUAL"
|
|
# magically changes to SERVOTEST (3)
|
|
for method in self.run_cmd, self.run_cmd_int:
|
|
for value in 1900, 1200:
|
|
channel = 1
|
|
method(
|
|
mavutil.mavlink.MAV_CMD_DO_SET_SERVO,
|
|
p1=channel,
|
|
p2=value,
|
|
timeout=1,
|
|
)
|
|
self.wait_servo_channel_value(channel, value)
|
|
for value in 1300, 1670:
|
|
channel = 2
|
|
method(
|
|
mavutil.mavlink.MAV_CMD_DO_SET_SERVO,
|
|
p1=channel,
|
|
p2=value,
|
|
timeout=1,
|
|
)
|
|
self.wait_servo_channel_value(channel, value)
|
|
|
|
def MAV_CMD_MISSION_START(self):
|
|
'''test MAV_CMD_MISSION_START mavlink command'''
|
|
for method in self.run_cmd, self.run_cmd_int:
|
|
self.change_mode(0) # "MANUAL"
|
|
method(mavutil.mavlink.MAV_CMD_MISSION_START)
|
|
self.wait_mode("AUTO")
|
|
|
|
def SCAN(self):
|
|
'''Test SCAN mode'''
|
|
self.change_mode(2) # "SCAN"
|
|
self.set_parameter("SCAN_SPEED_YAW", 20)
|
|
for channel in 1, 2:
|
|
self.wait_servo_channel_value(channel,
|
|
1900,
|
|
timeout=90,
|
|
comparator=operator.ge)
|
|
for channel in 1, 2:
|
|
self.wait_servo_channel_value(channel,
|
|
1200,
|
|
timeout=90,
|
|
comparator=operator.le)
|
|
|
|
def BaseMessageSet(self):
|
|
'''ensure we're getting messages we expect'''
|
|
self.set_parameter('BATT_MONITOR', 4)
|
|
self.reboot_sitl()
|
|
for msg in 'BATTERY_STATUS', :
|
|
self.assert_receive_message(msg)
|
|
|
|
def disabled_tests(self):
|
|
return {
|
|
"ArmFeatures": "See https://github.com/ArduPilot/ardupilot/issues/10652",
|
|
"CPUFailsafe": " tracker doesn't have a CPU failsafe",
|
|
}
|
|
|
|
def GPSForYaw(self):
|
|
'''Moving baseline GPS yaw'''
|
|
self.context_push()
|
|
|
|
self.load_default_params_file("tracker-gps-for-yaw.parm")
|
|
self.reboot_sitl()
|
|
|
|
self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True)
|
|
tstart = self.get_sim_time()
|
|
while True:
|
|
if self.get_sim_time_cached() - tstart > 20:
|
|
break
|
|
m_gps_raw = self.assert_receive_message("GPS2_RAW", verbose=True)
|
|
m_sim = self.assert_receive_message("SIMSTATE", verbose=True)
|
|
gps_raw_hdg = m_gps_raw.yaw * 0.01
|
|
sim_hdg = mavextra.wrap_360(math.degrees(m_sim.yaw))
|
|
if abs(gps_raw_hdg - sim_hdg) > 5:
|
|
raise NotAchievedException("GPS_RAW not tracking simstate yaw")
|
|
self.progress(f"yaw match ({gps_raw_hdg} vs {sim_hdg}")
|
|
|
|
self.context_pop()
|
|
|
|
self.reboot_sitl()
|
|
|
|
def tests(self):
|
|
'''return list of all tests'''
|
|
ret = super(AutoTestTracker, self).tests()
|
|
ret.extend([
|
|
self.GUIDED,
|
|
self.MANUAL,
|
|
self.MAV_CMD_DO_SET_SERVO,
|
|
self.MAV_CMD_MISSION_START,
|
|
self.NMEAOutput,
|
|
self.SCAN,
|
|
self.BaseMessageSet,
|
|
self.GPSForYaw,
|
|
])
|
|
return ret
|