mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 01:18:29 -04:00
autotest: improve gps-for-yaw test
There are flag values in the mavlink definitions for this message, so make sure we get something approaching the right number.
This commit is contained in:
parent
5cf62e8fb3
commit
754192ff26
@ -7337,6 +7337,7 @@ class AutoTestCopter(AutoTest):
|
|||||||
self.do_RTL()
|
self.do_RTL()
|
||||||
|
|
||||||
def GPSForYaw(self):
|
def GPSForYaw(self):
|
||||||
|
self.context_push()
|
||||||
self.load_default_params_file("copter-gps-for-yaw.parm")
|
self.load_default_params_file("copter-gps-for-yaw.parm")
|
||||||
self.reboot_sitl()
|
self.reboot_sitl()
|
||||||
ex = None
|
ex = None
|
||||||
@ -7344,12 +7345,18 @@ class AutoTestCopter(AutoTest):
|
|||||||
self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True)
|
self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True)
|
||||||
m = self.assert_receive_message("GPS2_RAW")
|
m = self.assert_receive_message("GPS2_RAW")
|
||||||
self.progress(self.dump_message_verbose(m))
|
self.progress(self.dump_message_verbose(m))
|
||||||
if m.yaw == 0:
|
want = 27000
|
||||||
raise NotAchievedException("Expected to get GPS-from-yaw")
|
if abs(m.yaw - want) > 500:
|
||||||
|
raise NotAchievedException("Expected to get GPS-from-yaw (want %f got %f)" % (want, m.yaw))
|
||||||
self.wait_ready_to_arm()
|
self.wait_ready_to_arm()
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
self.print_exception_caught(e)
|
self.print_exception_caught(e)
|
||||||
ex = e
|
ex = e
|
||||||
|
|
||||||
|
self.context_pop()
|
||||||
|
|
||||||
|
self.reboot_sitl()
|
||||||
|
|
||||||
if ex is not None:
|
if ex is not None:
|
||||||
raise ex
|
raise ex
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user