Skip to content

Commit

Permalink
autotest: update SIM_ENGINE_FAIL to mask
Browse files Browse the repository at this point in the history
  • Loading branch information
robertlong13 committed Dec 17, 2024
1 parent ccb001f commit bc9e290
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 15 deletions.
18 changes: 7 additions & 11 deletions Tools/autotest/arducopter.py
Original file line number Diff line number Diff line change
Expand Up @@ -1374,7 +1374,8 @@ def StabilityPatch(self,

# cut motor 1's to efficiency
self.progress("Cutting motor 1 to 65% efficiency")
self.set_parameter("SIM_ENGINE_MUL", 0.65)
self.set_parameters({"SIM_ENGINE_MUL": 0.65,
"SIM_ENGINE_FAIL": 1 << 0,}) # motor 1

while self.get_sim_time_cached() < tstart + holdtime:
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
Expand Down Expand Up @@ -3285,8 +3286,8 @@ def MotorFail_test_frame(self, model, servo_count, frame_class, fail_servo=0, fa
self.progress("Killing motor %u (%u%%)" %
(fail_servo+1, fail_mul))
self.set_parameters({
"SIM_ENGINE_FAIL": fail_servo,
"SIM_ENGINE_MUL": fail_mul,
"SIM_ENGINE_FAIL": 1 << fail_servo,
})
failed = True

Expand Down Expand Up @@ -3345,10 +3346,7 @@ def MotorFail_test_frame(self, model, servo_count, frame_class, fail_servo=0, fa
raise NotAchievedException("Vehicle is descending")

self.progress("Fixing motors")
self.set_parameters({
"SIM_ENGINE_FAIL": 0,
"SIM_ENGINE_MUL": 1.0,
})
self.set_parameter("SIM_ENGINE_FAIL", 0)

self.do_RTL()

Expand Down Expand Up @@ -4170,8 +4168,7 @@ def _Parachute(self, command):
self.takeoff(40)
self.set_rc(9, 1500)
self.set_parameters({
"SIM_ENGINE_MUL": 0,
"SIM_ENGINE_FAIL": 1,
"SIM_ENGINE_FAIL": 1 << 1, # motor 2
})
self.wait_statustext('BANG! Parachute deployed', timeout=60)
self.set_rc(9, 1000)
Expand All @@ -4184,8 +4181,7 @@ def _Parachute(self, command):
self.takeoff(loiter_alt, mode='LOITER')
self.set_rc(9, 1100)
self.set_parameters({
"SIM_ENGINE_MUL": 0,
"SIM_ENGINE_FAIL": 1,
"SIM_ENGINE_FAIL": 1 << 1, # motor 2
})
tstart = self.get_sim_time()
while self.get_sim_time_cached() < tstart + 5:
Expand Down Expand Up @@ -12024,8 +12020,8 @@ def GripperReleaseOnThrustLoss(self):
self.context_push()
self.context_collect('STATUSTEXT')
self.set_parameters({
"SIM_ENGINE_FAIL": 1,
"SIM_ENGINE_MUL": 0.5,
"SIM_ENGINE_FAIL": 1 << 1, # motor 2
"FLIGHT_OPTIONS": 4,
})

Expand Down
8 changes: 4 additions & 4 deletions Tools/autotest/quadplane.py
Original file line number Diff line number Diff line change
Expand Up @@ -767,8 +767,7 @@ def FwdThrInVTOL(self):
"Q_WVANE_ENABLE": 1,
"Q_WVANE_GAIN": 1,
"STICK_MIXING": 0,
"Q_FWD_THR_USE": 2,
"SIM_ENGINE_FAIL": 2}) # we want to fail the forward thrust motor only
"Q_FWD_THR_USE": 2})

self.takeoff(10, mode="QLOITER")
self.set_rc(2, 1000)
Expand All @@ -785,7 +784,7 @@ def FwdThrInVTOL(self):
self.set_rc(2, 1500)
self.delay_sim_time(5)
loc1 = self.mav.location()
self.set_parameter("SIM_ENGINE_MUL", 0) # simulate a complete loss of forward motor thrust
self.set_parameter("SIM_ENGINE_FAIL", 1 << 2) # simulate a complete loss of forward motor thrust
self.delay_sim_time(20)
self.change_mode('QLAND')
self.wait_disarmed(timeout=60)
Expand Down Expand Up @@ -1475,7 +1474,8 @@ def VTOLQuicktune_CPP(self):
(p, new_values[p], threshold))

self.progress("ensure we are not overtuned")
self.set_parameter('SIM_ENGINE_MUL', 0.9)
self.set_parameters({'SIM_ENGINE_MUL', 0.9,
'SIM_ENGINE_FAIL', 1 << 0})

self.delay_sim_time(5)

Expand Down

0 comments on commit bc9e290

Please sign in to comment.