Autotest: add test for Sub position hold

This commit is contained in:
Willian Galvani 2020-08-03 03:57:38 -03:00 committed by Jacob Walser
parent 385f440d90
commit 0f992ac6a3
3 changed files with 53 additions and 1 deletions

View File

@ -147,6 +147,53 @@ class AutoTestSub(AutoTest):
self.disarm_vehicle() self.disarm_vehicle()
def test_pos_hold(self):
"""Test POSHOLD mode"""
self.wait_ready_to_arm()
self.arm_vehicle()
# point North
self.reach_heading_manual(0)
self.mavproxy.send('mode POSHOLD\n')
self.wait_mode('POSHOLD')
#dive a little
self.set_rc(Joystick.Throttle, 1300)
self.delay_sim_time(3)
self.set_rc(Joystick.Throttle, 1500)
self.delay_sim_time(2)
# Save starting point
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
if msg is None:
raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
start_pos = self.mav.location()
# Hold in perfect conditions
self.progress("Testing position hold in perfect conditions")
self.delay_sim_time(10)
distance_m = self.get_distance(start_pos, self.mav.location())
if distance_m > 1:
raise NotAchievedException("Position Hold was unable to keep position in calm waters within 1 meter after 10 seconds, drifted {} meters".format(distance_m))
# Hold in 1 m/s current
self.progress("Testing position hold in current")
self.set_parameter("SIM_WIND_SPD", 1)
self.set_parameter("SIM_WIND_T", 1)
self.delay_sim_time(10)
distance_m = self.get_distance(start_pos, self.mav.location())
if distance_m > 1:
raise NotAchievedException("Position Hold was unable to keep position in 1m/s current within 1 meter after 10 seconds, drifted {} meters".format(distance_m))
# Move forward slowly in 1 m/s current
start_pos = self.mav.location()
self.progress("Testing moving forward in position hold in 1m/s current")
self.set_rc(Joystick.Forward, 1600)
self.delay_sim_time(10)
distance_m = self.get_distance(start_pos, self.mav.location())
bearing = self.get_bearing(start_pos, self.mav.location())
if distance_m < 2 or (bearing > 30 and bearing < 330):
raise NotAchievedException("Position Hold was unable to move north 2 meters, moved {} at {} degrees instead".format(distance_m, bearing))
self.disarm_vehicle()
def test_mot_thst_hover_ignore(self): def test_mot_thst_hover_ignore(self):
"""Test if we are ignoring MOT_THST_HOVER parameter """Test if we are ignoring MOT_THST_HOVER parameter
""" """
@ -299,6 +346,7 @@ class AutoTestSub(AutoTest):
("DiveManual", "Dive manual", self.dive_manual), ("DiveManual", "Dive manual", self.dive_manual),
("AltitudeHold", "Test altitude holde mode", self.test_alt_hold), ("AltitudeHold", "Test altitude holde mode", self.test_alt_hold),
("PositionHold", "Test position hold mode", self.test_pos_hold),
("DiveMission", ("DiveMission",
"Dive mission", "Dive mission",

View File

@ -3274,7 +3274,7 @@ class AutoTest(ABC):
def reach_heading_manual(self, heading, turn_right=True): def reach_heading_manual(self, heading, turn_right=True):
"""Manually direct the vehicle to the target heading.""" """Manually direct the vehicle to the target heading."""
if self.is_copter(): if self.is_copter() or self.is_sub():
self.mavproxy.send('rc 4 1580\n') self.mavproxy.send('rc 4 1580\n')
self.wait_heading(heading) self.wait_heading(heading)
self.set_rc(4, 1500) self.set_rc(4, 1500)

View File

@ -61,6 +61,10 @@ INS_ACC3SCAL_Z 1.000
PSC_ACCZ_P 4 PSC_ACCZ_P 4
PSC_VECZ_P 8 PSC_VECZ_P 8
PSC_POSZ_P 3 PSC_POSZ_P 3
PSC_POSXY_P 2.5
PSC_VELXY_D 0.8
PSC_VELXY_I 0.5
PSC_VELXY_P 5.0
RNGFND1_MAX_CM 3000 RNGFND1_MAX_CM 3000
RNGFND1_PIN 0 RNGFND1_PIN 0
RNGFND1_SCALING 12.12 RNGFND1_SCALING 12.12