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

Jump to bottom

Allow simulation of more htan two GPS instances #28578

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
+431 −426
Open

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
1056b85
AP_HAL_SITL: add ability to simulate more than 2 GPSs
peterbarker Nov 11, 2024
d15b92e
SITL: add ability to simulate more than 2 GPSs
peterbarker Nov 11, 2024
73399a3
Tools: add ability to simulate more than 2 GPSs
peterbarker Nov 11, 2024
2a254b5
autotest: increase debug when retrying parameter download
peterbarker Nov 12, 2024
d1b0072
AP_Param: correct maximum-length parameter sanity check
peterbarker Nov 12, 2024
139cbeb
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
# 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 @@
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 @@
# 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 @@
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 @@
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 @@
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 @@
'''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 @@
"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 @@
"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 @@ -6074,7 +6074,7 @@
raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
else:
if reverse is not None:
raise NotAchievedException(

Check failure on line 6077 in Tools/autotest/arducopter.py

View workflow job for this annotation

GitHub Actions / autotest (sitltest-copter-tests2b) 

vehicle_test_suite.NotAchievedException: Detected motor peak at 96.326956Hz, throttle 36.000000%, 5.164512dB

Check failure on line 6077 in Tools/autotest/arducopter.py

View workflow job for this annotation

GitHub Actions / autotest (sitltest-copter-tests2b) 

vehicle_test_suite.NotAchievedException: Detected motor peak at 96.326956Hz, throttle 36.000000%, 5.164512dB

Check failure on line 6077 in Tools/autotest/arducopter.py

View workflow job for this annotation

GitHub Actions / autotest (sitltest-copter-tests2b) 

vehicle_test_suite.NotAchievedException: Detected motor peak at 134.476245Hz, throttle 36.000000%, 6.607697dB

Check failure on line 6077 in Tools/autotest/arducopter.py

View workflow job for this annotation

GitHub Actions / autotest (sitltest-copter-tests2b) 

vehicle_test_suite.NotAchievedException: Detected motor peak at 134.476245Hz, throttle 36.000000%, 6.607697dB

Check failure on line 6077 in Tools/autotest/arducopter.py

View workflow job for this annotation

GitHub Actions / autotest (sitltest-copter-tests2b) 

vehicle_test_suite.NotAchievedException: Detected motor peak at 131.615048Hz, throttle 36.000000%, 6.664925dB
"Detected motor peak at %fHz, throttle %f%%, %fdB" %
(freq, hover_throttle, peakdb))
else:
Expand Down Expand Up @@ -6570,7 +6570,7 @@
self.reboot_sitl()

if ex is not None:
raise ex

Check failure on line 6573 in Tools/autotest/arducopter.py

View workflow job for this annotation

GitHub Actions / autotest (sitltest-copter-tests2b) 

File "/__w/ardupilot/ardupilot/Tools/autotest/arducopter.py", line 6550, in test_gyro_fft_harmonic

Check failure on line 6573 in Tools/autotest/arducopter.py

View workflow job for this annotation

GitHub Actions / autotest (sitltest-copter-tests2b) 

File "/__w/ardupilot/ardupilot/Tools/autotest/arducopter.py", line 6550, in test_gyro_fft_harmonic

Check failure on line 6573 in Tools/autotest/arducopter.py

View workflow job for this annotation

GitHub Actions / autotest (sitltest-copter-tests2b) 

File "/__w/ardupilot/ardupilot/Tools/autotest/arducopter.py", line 6550, in test_gyro_fft_harmonic

def GyroFFTHarmonic(self):
"""Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic."""
Expand Down Expand Up @@ -7245,7 +7245,7 @@
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 @@
"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 @@
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 @@ -9118,9 +9118,9 @@
"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 @@ -9178,18 +9178,18 @@
"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 @@ -9245,9 +9245,9 @@
"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 @@ -11754,7 +11754,7 @@
'''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 @@ -11941,7 +11941,7 @@
'''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 @@ -12142,7 +12142,7 @@

# 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