Skip to content

Commit 7b64263

Browse files
peterbarkerpeterbarker
committed
Tools: add ability to simulate more than 2 GPSs
1 parent fab1ef7 commit 7b64263

11 files changed

Lines changed: 98 additions & 104 deletions

‎Tools/autotest/arducopter.py‎

Lines changed: 50 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -552,13 +552,13 @@ def ThrottleFailsafe(self, side=60, timeout=360):
552552
self.start_subtest("Radio failsafe RTL fails into land mode due to bad position.")
553553
self.set_parameter('FS_THR_ENABLE', 1)
554554
self.takeoffAndMoveAway()
555-
self.set_parameter('SIM_GPS_DISABLE', 1)
555+
self.set_parameter('SIM_GPS1_ENABLE', 0)
556556
self.delay_sim_time(5)
557557
self.set_parameter("SIM_RC_FAIL", 1)
558558
self.wait_mode("LAND")
559559
self.wait_landed_and_disarmed()
560560
self.set_parameter("SIM_RC_FAIL", 0)
561-
self.set_parameter('SIM_GPS_DISABLE', 0)
561+
self.set_parameter('SIM_GPS1_ENABLE', 1)
562562
self.wait_ekf_happy()
563563
self.end_subtest("Completed Radio failsafe RTL fails into land mode due to bad position.")
564564

@@ -567,13 +567,13 @@ def ThrottleFailsafe(self, side=60, timeout=360):
567567
self.start_subtest("Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")
568568
self.set_parameter('FS_THR_ENABLE', 4)
569569
self.takeoffAndMoveAway()
570-
self.set_parameter('SIM_GPS_DISABLE', 1)
570+
self.set_parameter('SIM_GPS1_ENABLE', 0)
571571
self.delay_sim_time(5)
572572
self.set_parameter("SIM_RC_FAIL", 1)
573573
self.wait_mode("LAND")
574574
self.wait_landed_and_disarmed()
575575
self.set_parameter("SIM_RC_FAIL", 0)
576-
self.set_parameter('SIM_GPS_DISABLE', 0)
576+
self.set_parameter('SIM_GPS1_ENABLE', 1)
577577
self.wait_ekf_happy()
578578
self.end_subtest("Completed Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")
579579

@@ -582,13 +582,13 @@ def ThrottleFailsafe(self, side=60, timeout=360):
582582
self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")
583583
self.set_parameter('FS_THR_ENABLE', 5)
584584
self.takeoffAndMoveAway()
585-
self.set_parameter('SIM_GPS_DISABLE', 1)
585+
self.set_parameter('SIM_GPS1_ENABLE', 0)
586586
self.delay_sim_time(5)
587587
self.set_parameter("SIM_RC_FAIL", 1)
588588
self.wait_mode("LAND")
589589
self.wait_landed_and_disarmed()
590590
self.set_parameter("SIM_RC_FAIL", 0)
591-
self.set_parameter('SIM_GPS_DISABLE', 0)
591+
self.set_parameter('SIM_GPS1_ENABLE', 1)
592592
self.wait_ekf_happy()
593593
self.end_subtest("Completed Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")
594594

@@ -597,9 +597,9 @@ def ThrottleFailsafe(self, side=60, timeout=360):
597597
self.start_subtest("Radio failsafe SmartRTL->RTL fails into RTL mode due to no path.")
598598
self.set_parameter('FS_THR_ENABLE', 4)
599599
self.takeoffAndMoveAway()
600-
self.set_parameter('SIM_GPS_DISABLE', 1)
600+
self.set_parameter('SIM_GPS1_ENABLE', 0)
601601
self.wait_statustext("SmartRTL deactivated: bad position", timeout=60)
602-
self.set_parameter('SIM_GPS_DISABLE', 0)
602+
self.set_parameter('SIM_GPS1_ENABLE', 1)
603603
self.wait_ekf_happy()
604604
self.delay_sim_time(5)
605605
self.set_parameter("SIM_RC_FAIL", 1)
@@ -613,9 +613,9 @@ def ThrottleFailsafe(self, side=60, timeout=360):
613613
self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to no path.")
614614
self.set_parameter('FS_THR_ENABLE', 5)
615615
self.takeoffAndMoveAway()
616-
self.set_parameter('SIM_GPS_DISABLE', 1)
616+
self.set_parameter('SIM_GPS1_ENABLE', 0)
617617
self.wait_statustext("SmartRTL deactivated: bad position", timeout=60)
618-
self.set_parameter('SIM_GPS_DISABLE', 0)
618+
self.set_parameter('SIM_GPS1_ENABLE', 1)
619619
self.wait_ekf_happy()
620620
self.delay_sim_time(5)
621621
self.set_parameter("SIM_RC_FAIL", 1)
@@ -1958,8 +1958,8 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20):
19581958
glitch_current = 0
19591959
self.progress("Apply first glitch")
19601960
self.set_parameters({
1961-
"SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
1962-
"SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
1961+
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
1962+
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
19631963
})
19641964

19651965
# record position for 30 seconds
@@ -1973,15 +1973,15 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20):
19731973
glitch_current = -1
19741974
self.progress("Completed Glitches")
19751975
self.set_parameters({
1976-
"SIM_GPS_GLITCH_X": 0,
1977-
"SIM_GPS_GLITCH_Y": 0,
1976+
"SIM_GPS1_GLTCH_X": 0,
1977+
"SIM_GPS1_GLTCH_Y": 0,
19781978
})
19791979
else:
19801980
self.progress("Applying glitch %u" % glitch_current)
19811981
# move onto the next glitch
19821982
self.set_parameters({
1983-
"SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
1984-
"SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
1983+
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
1984+
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
19851985
})
19861986

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

20262026
# apply glitch
2027-
self.set_parameter("SIM_GPS_GLITCH_X", 0.001)
2027+
self.set_parameter("SIM_GPS1_GLTCH_X", 0.001)
20282028

20292029
# check lean angles remain stable for 20 seconds
20302030
tstart = self.get_sim_time()
@@ -2098,11 +2098,11 @@ def GPSGlitchAuto(self, timeout=180):
20982098
# stop and test loss of GPS for a short time - it should resume GPS use without falling back into a non aiding mode
20992099
self.change_mode("LOITER")
21002100
self.set_parameters({
2101-
"SIM_GPS_DISABLE": 1,
2101+
"SIM_GPS1_ENABLE": 0,
21022102
})
21032103
self.delay_sim_time(2)
21042104
self.set_parameters({
2105-
"SIM_GPS_DISABLE": 0,
2105+
"SIM_GPS1_ENABLE": 1,
21062106
})
21072107
# regaining GPS should not result in it falling back to a non-navigation mode
21082108
self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1)
@@ -2118,8 +2118,8 @@ def GPSGlitchAuto(self, timeout=180):
21182118
glitch_current = 0
21192119
self.progress("Apply first glitch")
21202120
self.set_parameters({
2121-
"SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
2122-
"SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
2121+
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
2122+
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
21232123
})
21242124

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

21392139
# turn off glitching
21402140
self.progress("Completed Glitches")
21412141
self.set_parameters({
2142-
"SIM_GPS_GLITCH_X": 0,
2143-
"SIM_GPS_GLITCH_Y": 0,
2142+
"SIM_GPS1_GLTCH_X": 0,
2143+
"SIM_GPS1_GLTCH_Y": 0,
21442144
})
21452145

21462146
# continue with the mission
@@ -2526,7 +2526,7 @@ def OpticalFlowLimits(self):
25262526
self.set_parameters({
25272527
"SIM_FLOW_ENABLE": 1,
25282528
"FLOW_TYPE": 10,
2529-
"SIM_GPS_DISABLE": 1,
2529+
"SIM_GPS1_ENABLE": 0,
25302530
"SIM_TERRAIN": 0,
25312531
})
25322532

@@ -2980,13 +2980,13 @@ def FarOrigin(self):
29802980
'''fly a mission far from the vehicle origin'''
29812981
# Fly mission #1
29822982
self.set_parameters({
2983-
"SIM_GPS_DISABLE": 1,
2983+
"SIM_GPS1_ENABLE": 0,
29842984
})
29852985
self.reboot_sitl()
29862986
nz = mavutil.location(-43.730171, 169.983118, 1466.3, 270)
29872987
self.set_origin(nz)
29882988
self.set_parameters({
2989-
"SIM_GPS_DISABLE": 0,
2989+
"SIM_GPS1_ENABLE": 1,
29902990
})
29912991
self.progress("# Load copter_mission")
29922992
# load the waypoint count
@@ -3028,8 +3028,8 @@ def CANGPSCopterMission(self):
30283028
"GPS1_TYPE": 9,
30293029
"GPS2_TYPE": 9,
30303030
# disable simulated GPS, so only via DroneCAN
3031-
"SIM_GPS_DISABLE": 1,
3032-
"SIM_GPS2_DISABLE": 1,
3031+
"SIM_GPS1_ENABLE": 0,
3032+
"SIM_GPS2_ENABLE": 0,
30333033
# this ensures we use DroneCAN baro and compass
30343034
"SIM_BARO_COUNT" : 0,
30353035
"SIM_MAG1_DEVID" : 0,
@@ -3206,7 +3206,7 @@ def GuidedEKFLaneChange(self):
32063206
"EK3_SRC1_POSZ": 3,
32073207
"EK3_AFFINITY" : 1,
32083208
"GPS2_TYPE" : 1,
3209-
"SIM_GPS2_DISABLE" : 0,
3209+
"SIM_GPS2_ENABLE" : 1,
32103210
"SIM_GPS2_GLTCH_Z" : -30
32113211
})
32123212
self.reboot_sitl()
@@ -7245,7 +7245,7 @@ def loiter_requires_position(self):
72457245
self.context_push()
72467246
self.set_parameters({
72477247
"GPS1_TYPE": 2,
7248-
"SIM_GPS_DISABLE": 1,
7248+
"SIM_GPS1_ENABLE": 0,
72497249
})
72507250
# if there is no GPS at all then we must direct EK3 to not use
72517251
# it at all. Otherwise it will never initialise, as it wants
@@ -8986,7 +8986,7 @@ def test_replay_gps_bit(self):
89868986
"RNGFND1_POS_Y": -0.07,
89878987
"RNGFND1_POS_Z": -0.005,
89888988
"SIM_SONAR_SCALE": 30,
8989-
"SIM_GPS2_DISABLE": 0,
8989+
"SIM_GPS2_ENABLE": 1,
89908990
})
89918991
self.reboot_sitl()
89928992

@@ -9045,7 +9045,7 @@ def GPSBlendingLog(self):
90459045
self.set_parameters({
90469046
"GPS2_TYPE": 1,
90479047
"SIM_GPS2_TYPE": 1,
9048-
"SIM_GPS2_DISABLE": 0,
9048+
"SIM_GPS2_ENABLE": 1,
90499049
"GPS_AUTO_SWITCH": 2,
90509050
})
90519051
self.reboot_sitl()
@@ -9116,9 +9116,9 @@ def GPSBlending(self):
91169116
"WP_YAW_BEHAVIOR": 0, # do not yaw
91179117
"GPS2_TYPE": 1,
91189118
"SIM_GPS2_TYPE": 1,
9119-
"SIM_GPS2_DISABLE": 0,
9120-
"SIM_GPS_POS_X": 1.0,
9121-
"SIM_GPS_POS_Y": -1.0,
9119+
"SIM_GPS2_ENABLE": 1,
9120+
"SIM_GPS1_POS_X": 1.0,
9121+
"SIM_GPS1_POS_Y": -1.0,
91229122
"SIM_GPS2_POS_X": -1.0,
91239123
"SIM_GPS2_POS_Y": 1.0,
91249124
"GPS_AUTO_SWITCH": 2,
@@ -9176,18 +9176,18 @@ def GPSWeightedBlending(self):
91769176
"WP_YAW_BEHAVIOR": 0, # do not yaw
91779177
"GPS2_TYPE": 1,
91789178
"SIM_GPS2_TYPE": 1,
9179-
"SIM_GPS2_DISABLE": 0,
9180-
"SIM_GPS_POS_X": 1.0,
9181-
"SIM_GPS_POS_Y": -1.0,
9179+
"SIM_GPS2_ENABLE": 1,
9180+
"SIM_GPS1_POS_X": 1.0,
9181+
"SIM_GPS1_POS_Y": -1.0,
91829182
"SIM_GPS2_POS_X": -1.0,
91839183
"SIM_GPS2_POS_Y": 1.0,
91849184
"GPS_AUTO_SWITCH": 2,
91859185
})
91869186
# configure velocity errors such that the 1st GPS should be
91879187
# 4/5, second GPS 1/5 of result (0.5**2)/((0.5**2)+(1.0**2))
91889188
self.set_parameters({
9189-
"SIM_GPS_VERR_X": 0.3, # m/s
9190-
"SIM_GPS_VERR_Y": 0.4,
9189+
"SIM_GPS1_VERR_X": 0.3, # m/s
9190+
"SIM_GPS1_VERR_Y": 0.4,
91919191
"SIM_GPS2_VERR_X": 0.6, # m/s
91929192
"SIM_GPS2_VERR_Y": 0.8,
91939193
"GPS_BLEND_MASK": 4, # use only speed for blend calculations
@@ -9243,9 +9243,9 @@ def GPSBlendingAffinity(self):
92439243
"WP_YAW_BEHAVIOR": 0, # do not yaw
92449244
"GPS2_TYPE": 1,
92459245
"SIM_GPS2_TYPE": 1,
9246-
"SIM_GPS2_DISABLE": 0,
9247-
"SIM_GPS_POS_X": 1.0,
9248-
"SIM_GPS_POS_Y": -1.0,
9246+
"SIM_GPS2_ENABLE": 1,
9247+
"SIM_GPS1_POS_X": 1.0,
9248+
"SIM_GPS1_POS_Y": -1.0,
92499249
"SIM_GPS2_POS_X": -1.0,
92509250
"SIM_GPS2_POS_Y": 1.0,
92519251
"GPS_AUTO_SWITCH": 2,
@@ -11739,7 +11739,7 @@ def GuidedForceArm(self):
1173911739
'''ensure Guided acts appropriately when force-armed'''
1174011740
self.set_parameters({
1174111741
"EK3_SRC2_VELXY": 5,
11742-
"SIM_GPS_DISABLE": 1,
11742+
"SIM_GPS1_ENABLE": 0,
1174311743
})
1174411744
self.load_default_params_file("copter-optflow.parm")
1174511745
self.reboot_sitl()
@@ -11926,7 +11926,7 @@ def REQUIRE_POSITION_FOR_ARMING(self):
1192611926
'''check FlightOption::REQUIRE_POSITION_FOR_ARMING works'''
1192711927
self.context_push()
1192811928
self.set_parameters({
11929-
"SIM_GPS_NUMSATS": 3, # EKF does not like < 6
11929+
"SIM_GPS1_NUMSATS": 3, # EKF does not like < 6
1193011930
})
1193111931
self.reboot_sitl()
1193211932
self.change_mode('STABILIZE')
@@ -12127,7 +12127,7 @@ def CommonOrigin(self):
1212712127

1212812128
# switch to EKF3
1212912129
self.set_parameters({
12130-
'SIM_GPS_GLITCH_X' : 0.001, # about 100m
12130+
'SIM_GPS1_GLTCH_X' : 0.001, # about 100m
1213112131
'EK3_CHECK_SCALE' : 100,
1213212132
'AHRS_EKF_TYPE' : 3})
1213312133

0 commit comments

Comments
 (0)