diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index a80de7973d7ec..797be5639d0c7 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -11552,6 +11552,35 @@ def REQUIRE_POSITION_FOR_ARMING(self): self.context_pop() self.reboot_sitl() + def AutoContinueOnRCFailsafe(self): + '''check LOITER when entered after RC failsafe is ignored in auto''' + self.set_parameters({ + "FS_OPTIONS": 1, # 1 is "RC continue if in auto" + }) + + self.upload_simple_relhome_mission([ + # N E U + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 40, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 60, 0, 10), + ]) + + self.takeoff(mode='LOITER') + self.set_rc(1, 1200) + self.delay_sim_time(1) # build up some pilot desired stuff + self.change_mode('AUTO') + self.wait_waypoint(2, 2) + self.set_parameters({ + 'SIM_RC_FAIL': 1, + }) +# self.set_rc(1, 1500) # note we are still in RC fail! + self.wait_waypoint(3, 3) + self.assert_mode_is('AUTO') + self.change_mode('LOITER') + self.wait_groundspeed(0, 0.1, minimum_duration=30, timeout=450) + self.do_RTL() + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -11605,6 +11634,7 @@ def tests2b(self): # this block currently around 9.5mins here self.WatchAlts, self.GuidedEKFLaneChange, self.Sprayer, + self.AutoContinueOnRCFailsafe, self.EK3_RNG_USE_HGT, self.TerrainDBPreArm, self.ThrottleGainBoost, diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index 02e8085877b1f..81bc70dc80cde 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/libraries/AC_WPNav/AC_Loiter.h @@ -33,7 +33,9 @@ class AC_Loiter Vector2f get_pilot_desired_acceleration() const { return Vector2f{_desired_accel.x, _desired_accel.y}; } /// clear pilot desired acceleration - void clear_pilot_desired_acceleration() { _desired_accel.zero(); } + void clear_pilot_desired_acceleration() { + set_pilot_desired_acceleration(0, 0); + } /// get vector to stopping point based on a horizontal position and velocity void get_stopping_point_xy(Vector2f& stopping_point) const;