Skip to content

Commit

Permalink
autotest:add AUTOLAND mode test
Browse files Browse the repository at this point in the history
  • Loading branch information
Hwurzburg committed Dec 7, 2024
1 parent ed5c5ee commit 436b498
Show file tree
Hide file tree
Showing 4 changed files with 18 additions and 8 deletions.
4 changes: 3 additions & 1 deletion ArduPlane/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -363,6 +363,8 @@ class Parameters {

k_param_pullup = 270,
k_param_quicktune,
k_param_mode_autoland,

};

AP_Int16 format_version;
Expand Down
6 changes: 0 additions & 6 deletions ArduPlane/mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -890,12 +890,6 @@ class ModeAutoLand: public Mode
bool _enter() override;
AP_Mission::Mission_Command cmd;
bool land_started;


private:



};

#if HAL_SOARING_ENABLED
Expand Down
1 change: 0 additions & 1 deletion ArduPlane/mode_autoland.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
15 changes: 15 additions & 0 deletions Tools/autotest/arduplane.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -6478,6 +6492,7 @@ def tests1b(self):
self.MAV_CMD_DO_AUX_FUNCTION,
self.SmartBattery,
self.FlyEachFrame,
self.AutoLandMode,
self.RCDisableAirspeedUse,
self.AHRS_ORIENTATION,
self.AHRSTrim,
Expand Down

0 comments on commit 436b498

Please sign in to comment.