mirror of https://github.com/ArduPilot/ardupilot
autotest: test SET_GPS_GLOBAL_ORIGIN mav msg
This commit is contained in:
parent
10f39c3d28
commit
c8fc90cca3
|
@ -204,6 +204,9 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
|
||||||
|
|
||||||
self.context_pop()
|
self.context_pop()
|
||||||
|
|
||||||
|
# restart SITL RF driver
|
||||||
|
self.reboot_sitl()
|
||||||
|
|
||||||
if ex:
|
if ex:
|
||||||
raise ex
|
raise ex
|
||||||
|
|
||||||
|
@ -604,6 +607,67 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
|
||||||
self.disarm_vehicle()
|
self.disarm_vehicle()
|
||||||
self.progress("Mission OK")
|
self.progress("Mission OK")
|
||||||
|
|
||||||
|
def wait_ekf_happy_const_pos(self, timeout=45):
|
||||||
|
# All of these must be set for arming to happen in constant position mode:
|
||||||
|
required_value = (mavutil.mavlink.EKF_ATTITUDE |
|
||||||
|
mavutil.mavlink.EKF_VELOCITY_HORIZ |
|
||||||
|
mavutil.mavlink.EKF_VELOCITY_VERT |
|
||||||
|
mavutil.mavlink.EKF_POS_VERT_ABS |
|
||||||
|
mavutil.mavlink.EKF_CONST_POS_MODE)
|
||||||
|
|
||||||
|
# None of these bits must be set for arming to happen:
|
||||||
|
error_bits = mavutil.mavlink.EKF_UNINITIALIZED
|
||||||
|
|
||||||
|
self.wait_ekf_flags(required_value, error_bits, timeout=timeout)
|
||||||
|
|
||||||
|
def wait_ready_to_arm_const_pos(self, timeout=120):
|
||||||
|
self.progress("Waiting for ready to arm (constant position mode)")
|
||||||
|
start = self.get_sim_time()
|
||||||
|
self.wait_ekf_happy_const_pos(timeout=timeout)
|
||||||
|
armable_time = self.get_sim_time() - start
|
||||||
|
self.progress("Took %u seconds to become armable" % armable_time)
|
||||||
|
self.total_waiting_to_arm_time += armable_time
|
||||||
|
self.waiting_to_arm_count += 1
|
||||||
|
|
||||||
|
def collected_msgs(self, msg_type):
|
||||||
|
c = self.context_get()
|
||||||
|
if msg_type not in c.collections:
|
||||||
|
raise NotAchievedException("Not collecting (%s)" % str(msg_type))
|
||||||
|
return c.collections[msg_type]
|
||||||
|
|
||||||
|
def SetGlobalOrigin(self):
|
||||||
|
"""Test SET_GPS_GLOBAL_ORIGIN mav msg"""
|
||||||
|
self.context_push()
|
||||||
|
self.set_parameters({
|
||||||
|
'GPS1_TYPE': 0, # Disable the GPS
|
||||||
|
'EK3_SRC1_POSXY': 0, # Make sure EK3_SRC parameters do not refer to a GPS
|
||||||
|
})
|
||||||
|
self.reboot_sitl()
|
||||||
|
|
||||||
|
# Wait for the EKF to be happy in constant position mode
|
||||||
|
self.wait_ready_to_arm_const_pos()
|
||||||
|
|
||||||
|
if self.current_onboard_log_contains_message('ORGN'):
|
||||||
|
raise NotAchievedException("Found unexpected ORGN message")
|
||||||
|
|
||||||
|
self.context_collect('GPS_GLOBAL_ORIGIN')
|
||||||
|
|
||||||
|
# This should set the EKF origin, write an ORGN msg to df and a GPS_GLOBAL_ORIGIN msg to MAVLink
|
||||||
|
self.mav.mav.set_gps_global_origin_send(1, int(47.607584 * 1e7), int(-122.343911 * 1e7), 0)
|
||||||
|
self.delay_sim_time(1)
|
||||||
|
|
||||||
|
if not self.current_onboard_log_contains_message('ORGN'):
|
||||||
|
raise NotAchievedException("Did not find expected ORGN message")
|
||||||
|
|
||||||
|
num_mavlink_origin_msgs = len(self.collected_msgs('GPS_GLOBAL_ORIGIN'))
|
||||||
|
if num_mavlink_origin_msgs != 1:
|
||||||
|
raise NotAchievedException("Expected 1 GPS_GLOBAL_ORIGIN message, found %d" % num_mavlink_origin_msgs)
|
||||||
|
|
||||||
|
self.context_pop()
|
||||||
|
|
||||||
|
# restart GPS driver
|
||||||
|
self.reboot_sitl()
|
||||||
|
|
||||||
def tests(self):
|
def tests(self):
|
||||||
'''return list of all tests'''
|
'''return list of all tests'''
|
||||||
ret = super(AutoTestSub, self).tests()
|
ret = super(AutoTestSub, self).tests()
|
||||||
|
@ -628,6 +692,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
|
||||||
self.MAV_CMD_DO_CHANGE_SPEED,
|
self.MAV_CMD_DO_CHANGE_SPEED,
|
||||||
self.MAV_CMD_CONDITION_YAW,
|
self.MAV_CMD_CONDITION_YAW,
|
||||||
self.TerrainMission,
|
self.TerrainMission,
|
||||||
|
self.SetGlobalOrigin,
|
||||||
])
|
])
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
|
Loading…
Reference in New Issue