ardupilot/Tools/autotest/antennatracker.py

194 lines
6.2 KiB
Python
Raw Normal View History

#!/usr/bin/env 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
from common import AutoTest
from common 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(AutoTest):
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.mav.recv_match(type='ATTITUDE', blocking=True, timeout=2)
if m is None:
raise NotAchievedException("Did not get ATTITUDE")
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
2020-03-24 23:39:40 -03:00
def reboot_sitl(self, *args, **kwargs):
self.disarm_vehicle()
super(AutoTestTracker, self).reboot_sitl(*args, **kwargs)
def GUIDED(self):
2020-03-24 23:39:40 -03:00
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):
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 SERVOTEST(self):
self.change_mode(0) # "MANUAL"
# magically changes to SERVOTEST (3)
for value in 1900, 1200:
channel = 1
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO,
channel,
value,
0,
0,
0,
0,
0,
timeout=1)
self.wait_servo_channel_value(channel, value)
for value in 1300, 1670:
channel = 2
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO,
channel,
value,
0,
0,
0,
0,
0,
timeout=1)
self.wait_servo_channel_value(channel, value)
def SCAN(self):
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 disabled_tests(self):
return {
"ArmFeatures": "See https://github.com/ArduPilot/ardupilot/issues/10652",
"CPUFailsafe": " tracker doesn't have a CPU failsafe",
}
def tests(self):
'''return list of all tests'''
ret = super(AutoTestTracker, self).tests()
ret.extend([
("GUIDED",
"Test GUIDED mode",
self.GUIDED),
("MANUAL",
"Test MANUAL mode",
self.MANUAL),
("SERVOTEST",
"Test SERVOTEST mode",
self.SERVOTEST),
("NMEAOutput",
"Test AHRS NMEA Output can be read by out NMEA GPS",
self.nmea_output),
("SCAN",
"Test SCAN mode",
self.SCAN),
])
return ret