From 1fd9a6dfcc5b33df7ed6ee9d66279514d3f24ade Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sun, 1 Dec 2024 18:16:09 -0600 Subject: [PATCH] autotest:add AUTOLAND mode test --- ArduPlane/Parameters.h | 4 +++- ArduPlane/mode.h | 6 ------ ArduPlane/mode_autoland.cpp | 1 - Tools/autotest/arduplane.py | 15 +++++++++++++++ 4 files changed, 18 insertions(+), 8 deletions(-) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 39cb0fd56a1e7..9945869250b4b 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -100,7 +100,7 @@ class Parameters { k_param_sonar_old, // unused k_param_log_bitmask, k_param_BoardConfig, - k_param_mode_autoland, // was rssi_range + k_param_rssi_range, // unused, replaced by rssi_ library parameters k_param_flapin_channel_old, // unused, moved to RC_OPTION k_param_flaperon_output, // unused k_param_gps, @@ -363,6 +363,8 @@ class Parameters { k_param_pullup = 270, k_param_quicktune, + k_param_mode_autoland, + }; AP_Int16 format_version; diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index d65dc789f7923..e1c9589472c00 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -890,12 +890,6 @@ class ModeAutoLand: public Mode bool _enter() override; AP_Mission::Mission_Command cmd; bool land_started; - - -private: - - - }; #if HAL_SOARING_ENABLED diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index fde14fd75471e..e9d13858035a2 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -68,7 +68,6 @@ bool ModeAutoLand::_enter() cmd.content.location = plane.next_WP_loc; plane.start_command(cmd); land_started = false; - gcs().send_text(MAV_SEVERITY_WARNING, "AutoLanding"); return true; } diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 8e49b0f2038cc..539841109442d 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4201,6 +4201,20 @@ def FlyEachFrame(self): self.fly_mission(mission_file, strict=False, quadplane=quadplane, mission_timeout=400.0) self.wait_disarmed() + def AutoLandMode(self): + '''Test AUTOLAND mode''' + self.customise_SITL_commandline(["--home", "-35.362938,149.165085,585,173"]) + self.context_collect('STATUSTEXT') + self.takeoff(alt=80, mode='TAKEOFF') + self.wait_text("Takeoff initial direction", check_context=True) + self.change_mode(26) + self.wait_disarmed(120) + self.progress("Check the landed heading matches takeoff") + self.wait_heading(173, accuracy=5, timeout=1) + loc = mavutil.location(-35.362938, 149.165085, 585, 173) + if self.get_distance(loc, self.mav.location()) > 35: + raise NotAchievedException("Did not land close to home") + def RCDisableAirspeedUse(self): '''Test RC DisableAirspeedUse option''' self.set_parameter("RC9_OPTION", 106) @@ -6548,6 +6562,7 @@ def tests1b(self): self.MAV_CMD_DO_AUX_FUNCTION, self.SmartBattery, self.FlyEachFrame, + self.AutoLandMode, self.RCDisableAirspeedUse, self.AHRS_ORIENTATION, self.AHRSTrim,