Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Allow simulation of more htan two GPS instances #28578

Merged
merged 6 commits into from
Nov 20, 2024
Merged

Allow simulation of more htan two GPS instances #28578

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
f240dc1
AP_HAL_SITL: add ability to simulate more than 2 GPSs
peterbarker Nov 11, 2024
3822a62
SITL: add ability to simulate more than 2 GPSs
peterbarker Nov 11, 2024
860289c
Tools: add ability to simulate more than 2 GPSs
peterbarker Nov 11, 2024
19067bb
autotest: increase debug when retrying parameter download
peterbarker Nov 12, 2024
4090777
AP_Param: correct maximum-length parameter sanity check
peterbarker Nov 12, 2024
3b295ce
AP_Scripting: adjust for renaming of SIM_GPS_DISABLE to SIM_GPS1_ENABLE
peterbarker Nov 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
100 changes: 50 additions & 50 deletions Tools/autotest/arducopter.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -552,13 +552,13 @@ def ThrottleFailsafe(self, side=60, timeout=360):
self.start_subtest("Radio failsafe RTL fails into land mode due to bad position.")
self.set_parameter('FS_THR_ENABLE', 1)
self.takeoffAndMoveAway()
self.set_parameter('SIM_GPS_DISABLE', 1)
self.set_parameter('SIM_GPS1_ENABLE', 0)
self.delay_sim_time(5)
self.set_parameter("SIM_RC_FAIL", 1)
self.wait_mode("LAND")
self.wait_landed_and_disarmed()
self.set_parameter("SIM_RC_FAIL", 0)
self.set_parameter('SIM_GPS_DISABLE', 0)
self.set_parameter('SIM_GPS1_ENABLE', 1)
self.wait_ekf_happy()
self.end_subtest("Completed Radio failsafe RTL fails into land mode due to bad position.")

Expand All @@ -567,13 +567,13 @@ def ThrottleFailsafe(self, side=60, timeout=360):
self.start_subtest("Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")
self.set_parameter('FS_THR_ENABLE', 4)
self.takeoffAndMoveAway()
self.set_parameter('SIM_GPS_DISABLE', 1)
self.set_parameter('SIM_GPS1_ENABLE', 0)
self.delay_sim_time(5)
self.set_parameter("SIM_RC_FAIL", 1)
self.wait_mode("LAND")
self.wait_landed_and_disarmed()
self.set_parameter("SIM_RC_FAIL", 0)
self.set_parameter('SIM_GPS_DISABLE', 0)
self.set_parameter('SIM_GPS1_ENABLE', 1)
self.wait_ekf_happy()
self.end_subtest("Completed Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")

Expand All @@ -582,13 +582,13 @@ def ThrottleFailsafe(self, side=60, timeout=360):
self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")
self.set_parameter('FS_THR_ENABLE', 5)
self.takeoffAndMoveAway()
self.set_parameter('SIM_GPS_DISABLE', 1)
self.set_parameter('SIM_GPS1_ENABLE', 0)
self.delay_sim_time(5)
self.set_parameter("SIM_RC_FAIL", 1)
self.wait_mode("LAND")
self.wait_landed_and_disarmed()
self.set_parameter("SIM_RC_FAIL", 0)
self.set_parameter('SIM_GPS_DISABLE', 0)
self.set_parameter('SIM_GPS1_ENABLE', 1)
self.wait_ekf_happy()
self.end_subtest("Completed Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")

Expand All @@ -597,9 +597,9 @@ def ThrottleFailsafe(self, side=60, timeout=360):
self.start_subtest("Radio failsafe SmartRTL->RTL fails into RTL mode due to no path.")
self.set_parameter('FS_THR_ENABLE', 4)
self.takeoffAndMoveAway()
self.set_parameter('SIM_GPS_DISABLE', 1)
self.set_parameter('SIM_GPS1_ENABLE', 0)
self.wait_statustext("SmartRTL deactivated: bad position", timeout=60)
self.set_parameter('SIM_GPS_DISABLE', 0)
self.set_parameter('SIM_GPS1_ENABLE', 1)
self.wait_ekf_happy()
self.delay_sim_time(5)
self.set_parameter("SIM_RC_FAIL", 1)
Expand All @@ -613,9 +613,9 @@ def ThrottleFailsafe(self, side=60, timeout=360):
self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to no path.")
self.set_parameter('FS_THR_ENABLE', 5)
self.takeoffAndMoveAway()
self.set_parameter('SIM_GPS_DISABLE', 1)
self.set_parameter('SIM_GPS1_ENABLE', 0)
self.wait_statustext("SmartRTL deactivated: bad position", timeout=60)
self.set_parameter('SIM_GPS_DISABLE', 0)
self.set_parameter('SIM_GPS1_ENABLE', 1)
self.wait_ekf_happy()
self.delay_sim_time(5)
self.set_parameter("SIM_RC_FAIL", 1)
Expand Down Expand Up @@ -1958,8 +1958,8 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20):
glitch_current = 0
self.progress("Apply first glitch")
self.set_parameters({
"SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
"SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
})

# record position for 30 seconds
Expand All @@ -1973,15 +1973,15 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20):
glitch_current = -1
self.progress("Completed Glitches")
self.set_parameters({
"SIM_GPS_GLITCH_X": 0,
"SIM_GPS_GLITCH_Y": 0,
"SIM_GPS1_GLTCH_X": 0,
"SIM_GPS1_GLTCH_Y": 0,
})
else:
self.progress("Applying glitch %u" % glitch_current)
# move onto the next glitch
self.set_parameters({
"SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
"SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
})

# start displaying distance moved after all glitches applied
Expand All @@ -2002,8 +2002,8 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20):
# disable gps glitch
if glitch_current != -1:
self.set_parameters({
"SIM_GPS_GLITCH_X": 0,
"SIM_GPS_GLITCH_Y": 0,
"SIM_GPS1_GLTCH_X": 0,
"SIM_GPS1_GLTCH_Y": 0,
})
if self.use_map:
self.show_gps_and_sim_positions(False)
Expand All @@ -2024,7 +2024,7 @@ def GPSGlitchLoiter2(self):
self.wait_attitude(desroll=0, despitch=0, timeout=10, tolerance=1)

# apply glitch
self.set_parameter("SIM_GPS_GLITCH_X", 0.001)
self.set_parameter("SIM_GPS1_GLTCH_X", 0.001)

# check lean angles remain stable for 20 seconds
tstart = self.get_sim_time()
Expand Down Expand Up @@ -2098,11 +2098,11 @@ def GPSGlitchAuto(self, timeout=180):
# stop and test loss of GPS for a short time - it should resume GPS use without falling back into a non aiding mode
self.change_mode("LOITER")
self.set_parameters({
"SIM_GPS_DISABLE": 1,
"SIM_GPS1_ENABLE": 0,
})
self.delay_sim_time(2)
self.set_parameters({
"SIM_GPS_DISABLE": 0,
"SIM_GPS1_ENABLE": 1,
})
# regaining GPS should not result in it falling back to a non-navigation mode
self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1)
Expand All @@ -2118,8 +2118,8 @@ def GPSGlitchAuto(self, timeout=180):
glitch_current = 0
self.progress("Apply first glitch")
self.set_parameters({
"SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
"SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
})

# record position for 30 seconds
Expand All @@ -2132,15 +2132,15 @@ def GPSGlitchAuto(self, timeout=180):
if glitch_current < glitch_num:
self.progress("Applying glitch %u" % glitch_current)
self.set_parameters({
"SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
"SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
})

# turn off glitching
self.progress("Completed Glitches")
self.set_parameters({
"SIM_GPS_GLITCH_X": 0,
"SIM_GPS_GLITCH_Y": 0,
"SIM_GPS1_GLTCH_X": 0,
"SIM_GPS1_GLTCH_Y": 0,
})

# continue with the mission
Expand Down Expand Up @@ -2526,7 +2526,7 @@ def OpticalFlowLimits(self):
self.set_parameters({
"SIM_FLOW_ENABLE": 1,
"FLOW_TYPE": 10,
"SIM_GPS_DISABLE": 1,
"SIM_GPS1_ENABLE": 0,
"SIM_TERRAIN": 0,
})

Expand Down Expand Up @@ -2980,13 +2980,13 @@ def FarOrigin(self):
'''fly a mission far from the vehicle origin'''
# Fly mission #1
self.set_parameters({
"SIM_GPS_DISABLE": 1,
"SIM_GPS1_ENABLE": 0,
})
self.reboot_sitl()
nz = mavutil.location(-43.730171, 169.983118, 1466.3, 270)
self.set_origin(nz)
self.set_parameters({
"SIM_GPS_DISABLE": 0,
"SIM_GPS1_ENABLE": 1,
})
self.progress("# Load copter_mission")
# load the waypoint count
Expand Down Expand Up @@ -3028,8 +3028,8 @@ def CANGPSCopterMission(self):
"GPS1_TYPE": 9,
"GPS2_TYPE": 9,
# disable simulated GPS, so only via DroneCAN
"SIM_GPS_DISABLE": 1,
"SIM_GPS2_DISABLE": 1,
"SIM_GPS1_ENABLE": 0,
"SIM_GPS2_ENABLE": 0,
# this ensures we use DroneCAN baro and compass
"SIM_BARO_COUNT" : 0,
"SIM_MAG1_DEVID" : 0,
Expand Down Expand Up @@ -3206,7 +3206,7 @@ def GuidedEKFLaneChange(self):
"EK3_SRC1_POSZ": 3,
"EK3_AFFINITY" : 1,
"GPS2_TYPE" : 1,
"SIM_GPS2_DISABLE" : 0,
"SIM_GPS2_ENABLE" : 1,
"SIM_GPS2_GLTCH_Z" : -30
})
self.reboot_sitl()
Expand Down Expand Up @@ -7245,7 +7245,7 @@ def loiter_requires_position(self):
self.context_push()
self.set_parameters({
"GPS1_TYPE": 2,
"SIM_GPS_DISABLE": 1,
"SIM_GPS1_ENABLE": 0,
})
# if there is no GPS at all then we must direct EK3 to not use
# it at all. Otherwise it will never initialise, as it wants
Expand Down Expand Up @@ -8986,7 +8986,7 @@ def test_replay_gps_bit(self):
"RNGFND1_POS_Y": -0.07,
"RNGFND1_POS_Z": -0.005,
"SIM_SONAR_SCALE": 30,
"SIM_GPS2_DISABLE": 0,
"SIM_GPS2_ENABLE": 1,
})
self.reboot_sitl()

Expand Down Expand Up @@ -9045,7 +9045,7 @@ def GPSBlendingLog(self):
self.set_parameters({
"GPS2_TYPE": 1,
"SIM_GPS2_TYPE": 1,
"SIM_GPS2_DISABLE": 0,
"SIM_GPS2_ENABLE": 1,
"GPS_AUTO_SWITCH": 2,
})
self.reboot_sitl()
Expand Down Expand Up @@ -9116,9 +9116,9 @@ def GPSBlending(self):
"WP_YAW_BEHAVIOR": 0, # do not yaw
"GPS2_TYPE": 1,
"SIM_GPS2_TYPE": 1,
"SIM_GPS2_DISABLE": 0,
"SIM_GPS_POS_X": 1.0,
"SIM_GPS_POS_Y": -1.0,
"SIM_GPS2_ENABLE": 1,
"SIM_GPS1_POS_X": 1.0,
"SIM_GPS1_POS_Y": -1.0,
"SIM_GPS2_POS_X": -1.0,
"SIM_GPS2_POS_Y": 1.0,
"GPS_AUTO_SWITCH": 2,
Expand Down Expand Up @@ -9176,18 +9176,18 @@ def GPSWeightedBlending(self):
"WP_YAW_BEHAVIOR": 0, # do not yaw
"GPS2_TYPE": 1,
"SIM_GPS2_TYPE": 1,
"SIM_GPS2_DISABLE": 0,
"SIM_GPS_POS_X": 1.0,
"SIM_GPS_POS_Y": -1.0,
"SIM_GPS2_ENABLE": 1,
"SIM_GPS1_POS_X": 1.0,
"SIM_GPS1_POS_Y": -1.0,
"SIM_GPS2_POS_X": -1.0,
"SIM_GPS2_POS_Y": 1.0,
"GPS_AUTO_SWITCH": 2,
})
# configure velocity errors such that the 1st GPS should be
# 4/5, second GPS 1/5 of result (0.5**2)/((0.5**2)+(1.0**2))
self.set_parameters({
"SIM_GPS_VERR_X": 0.3, # m/s
"SIM_GPS_VERR_Y": 0.4,
"SIM_GPS1_VERR_X": 0.3, # m/s
"SIM_GPS1_VERR_Y": 0.4,
"SIM_GPS2_VERR_X": 0.6, # m/s
"SIM_GPS2_VERR_Y": 0.8,
"GPS_BLEND_MASK": 4, # use only speed for blend calculations
Expand Down Expand Up @@ -9243,9 +9243,9 @@ def GPSBlendingAffinity(self):
"WP_YAW_BEHAVIOR": 0, # do not yaw
"GPS2_TYPE": 1,
"SIM_GPS2_TYPE": 1,
"SIM_GPS2_DISABLE": 0,
"SIM_GPS_POS_X": 1.0,
"SIM_GPS_POS_Y": -1.0,
"SIM_GPS2_ENABLE": 1,
"SIM_GPS1_POS_X": 1.0,
"SIM_GPS1_POS_Y": -1.0,
"SIM_GPS2_POS_X": -1.0,
"SIM_GPS2_POS_Y": 1.0,
"GPS_AUTO_SWITCH": 2,
Expand Down Expand Up @@ -11739,7 +11739,7 @@ def GuidedForceArm(self):
'''ensure Guided acts appropriately when force-armed'''
self.set_parameters({
"EK3_SRC2_VELXY": 5,
"SIM_GPS_DISABLE": 1,
"SIM_GPS1_ENABLE": 0,
})
self.load_default_params_file("copter-optflow.parm")
self.reboot_sitl()
Expand Down Expand Up @@ -11926,7 +11926,7 @@ def REQUIRE_POSITION_FOR_ARMING(self):
'''check FlightOption::REQUIRE_POSITION_FOR_ARMING works'''
self.context_push()
self.set_parameters({
"SIM_GPS_NUMSATS": 3, # EKF does not like < 6
"SIM_GPS1_NUMSATS": 3, # EKF does not like < 6
})
self.reboot_sitl()
self.change_mode('STABILIZE')
Expand Down Expand Up @@ -12127,7 +12127,7 @@ def CommonOrigin(self):

# switch to EKF3
self.set_parameters({
'SIM_GPS_GLITCH_X' : 0.001, # about 100m
'SIM_GPS1_GLTCH_X' : 0.001, # about 100m
'EK3_CHECK_SCALE' : 100,
'AHRS_EKF_TYPE' : 3})

Expand Down
Loading
Loading