Skip to content

Commit

Permalink
Tools: Autotest: create gps-for-yaw test for Sub
Browse files Browse the repository at this point in the history
  • Loading branch information
Williangalvani committed Jan 31, 2024
1 parent d3a9cd0 commit efbee4d
Show file tree
Hide file tree
Showing 2 changed files with 35 additions and 1 deletion.
29 changes: 28 additions & 1 deletion Tools/autotest/ardusub.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,12 @@
import sys
import time

from pymavlink import mavutil
from pymavlink import mavutil, mavextra

import vehicle_test_suite
from vehicle_test_suite import NotAchievedException
from vehicle_test_suite import AutoTestTimeoutException
from math import degrees

if sys.version_info[0] < 3:
ConnectionResetError = AutoTestTimeoutException
Expand Down Expand Up @@ -514,6 +515,31 @@ def MAV_CMD_DO_CHANGE_SPEED(self):
self.wait_groundspeed(speed-0.2, speed+0.2, minimum_duration=2, timeout=60)
self.disarm_vehicle()

def GPSForYaw(self):
'''Consume heading of NMEA GPS and propagate to ATTITUDE'''

# load parameters with gps for yaw and 10 degrees offset
self.context_push()
self.load_default_params_file("sub-gps-for-yaw-nmea.parm")
self.reboot_sitl()
# make sure we are getting both GPS_RAW_INT and SIMSTATE
simstate_m = self.assert_receive_message("SIMSTATE")
real_yaw_deg = degrees(simstate_m.yaw)
expected_yaw_deg = mavextra.wrap_180(real_yaw_deg + 10) # offset in the param file, in degrees
# wait for GPS_RAW_INT to have a good fix
self.wait_gps_fix_type_gte(3, message_type="GPS_RAW_INT", verbose=True)

gps_m = self.assert_receive_message("ATTITUDE")
achieved_yaw_deg = mavextra.wrap_180(degrees(gps_m.yaw))

# ensure new reading propagated to ATTITUDE
if abs(achieved_yaw_deg - expected_yaw_deg) > 5:
raise NotAchievedException(
"Expected to get yaw consumed and at ATTITUDE (want %f got %f)" % (expected_yaw_deg, achieved_yaw_deg)
)
self.context_pop()
self.reboot_sitl()

def _MAV_CMD_CONDITION_YAW(self, run_cmd):
self.arm_vehicle()
self.change_mode('GUIDED')
Expand Down Expand Up @@ -577,6 +603,7 @@ def tests(self):
self.MAV_CMD_MISSION_START,
self.MAV_CMD_DO_CHANGE_SPEED,
self.MAV_CMD_CONDITION_YAW,
self.GPSForYaw,
])

return ret
7 changes: 7 additions & 0 deletions Tools/autotest/default_params/sub-gps-for-yaw-nmea.parm
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
# SITL GPS-for-yaw using a single simulated NMEA GPS
EK3_SRC1_YAW 3
GPS_TYPE 5
SIM_GPS_TYPE 5
SIM_GPS_HDG 1
GPS_AUTO_CONFIG 0
SIM_GPS_HDG_OFS 10.0

0 comments on commit efbee4d

Please sign in to comment.