Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: add nasty mission tests

Browse Source
This commit is contained in:
Peter Barker 2021-03-11 19:30:55 +11:00 committed by Peter Barker
parent 627b9b3034
commit c34c60e852
1 changed files with 229 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

229
Tools/autotest/helicopter.py
View File

@ -12,6 +12,8 @@ from common import NotAchievedException, AutoTestTimeoutException
from pymavlink import mavutil from pymavlink import mavutil
from pysim import vehicleinfo from pysim import vehicleinfo
import copy
class AutoTestHelicopter(AutoTestCopter): class AutoTestHelicopter(AutoTestCopter):
@ -409,6 +411,232 @@ class AutoTestHelicopter(AutoTestCopter):
self.wait_servo_channel_value(8, 1000, timeout=3) self.wait_servo_channel_value(8, 1000, timeout=3)
self.wait_disarmed() self.wait_disarmed()
def mission_item_home(self, target_system, target_component):
'''returns a mission_item_int which can be used as home in a mission'''
return self.mav.mav.mission_item_int_encode(
target_system,
target_component,
0, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
3, # p1
0, # p2
0, # p3
0, # p4
int(1.0000 * 1e7), # latitude
int(2.0000 * 1e7), # longitude
31.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
def mission_item_takeoff(self, target_system, target_component):
'''returns a mission_item_int which can be used as takeoff in a mission'''
return self.mav.mav.mission_item_int_encode(
target_system,
target_component,
1, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0, # current
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
int(1.0000 * 1e7), # latitude
int(1.0000 * 1e7), # longitude
31.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
def mission_item_rtl(self, target_system, target_component):
'''returns a mission_item_int which can be used as takeoff in a mission'''
return self.mav.mav.mission_item_int_encode(
target_system,
target_component,
1, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL,
mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
0, # current
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
0, # latitude
0, # longitude
0.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
def scurve_nasty_mission(self, target_system=1, target_component=1):
'''returns a mission which attempts to give the SCurve library
indigestion. The same destination is given several times.'''
wp2_loc = self.mav.location()
wp2_offset_n = 20
wp2_offset_e = 30
self.location_offset_ne(wp2_loc, wp2_offset_n, wp2_offset_e)
wp2_by_three = self.mav.mav.mission_item_int_encode(
target_system,
target_component,
2, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
3, # p1
0, # p2
0, # p3
0, # p4
int(wp2_loc.lat * 1e7), # latitude
int(wp2_loc.lng * 1e7), # longitude
31.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
wp5_loc = self.mav.location()
wp5_offset_n = -20
wp5_offset_e = 30
self.location_offset_ne(wp5_loc, wp5_offset_n, wp5_offset_e)
wp5_by_three = self.mav.mav.mission_item_int_encode(
target_system,
target_component,
5, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT,
0, # current
0, # autocontinue
3, # p1
0, # p2
0, # p3
0, # p4
int(wp5_loc.lat * 1e7), # latitude
int(wp5_loc.lng * 1e7), # longitude
31.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
ret = copy.copy([
# slot 0 is home
self.mission_item_home(target_system=target_system, target_component=target_component),
# slot 1 is takeoff
self.mission_item_takeoff(target_system=target_system, target_component=target_component),
# now three spline waypoints right on top of one another:
copy.copy(wp2_by_three),
copy.copy(wp2_by_three),
copy.copy(wp2_by_three),
# now three MORE spline waypoints right on top of one another somewhere else:
copy.copy(wp5_by_three),
copy.copy(wp5_by_three),
copy.copy(wp5_by_three),
self.mission_item_rtl(target_system=target_system, target_component=target_component),
])
# renumber the items:
count = 0
for item in ret:
item.seq = count
count += 1
return ret
def scurve_nasty_up_mission(self, target_system=1, target_component=1):
'''returns a mission which attempts to give the SCurve library
indigestion. The same destination is given several times but with differing altitudes.'''
wp2_loc = self.mav.location()
wp2_offset_n = 20
wp2_offset_e = 30
self.location_offset_ne(wp2_loc, wp2_offset_n, wp2_offset_e)
wp2 = self.mav.mav.mission_item_int_encode(
target_system,
target_component,
2, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
3, # p1
0, # p2
0, # p3
0, # p4
int(wp2_loc.lat * 1e7), # latitude
int(wp2_loc.lng * 1e7), # longitude
31.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
wp3 = copy.copy(wp2)
wp3.alt = 40
wp4 = copy.copy(wp2)
wp4.alt = 31
wp5_loc = self.mav.location()
wp5_offset_n = -20
wp5_offset_e = 30
self.location_offset_ne(wp5_loc, wp5_offset_n, wp5_offset_e)
wp5 = self.mav.mav.mission_item_int_encode(
target_system,
target_component,
5, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT,
0, # current
0, # autocontinue
3, # p1
0, # p2
0, # p3
0, # p4
int(wp5_loc.lat * 1e7), # latitude
int(wp5_loc.lng * 1e7), # longitude
31.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
wp6 = copy.copy(wp5)
wp6.alt = 41
wp7 = copy.copy(wp5)
wp7.alt = 51
ret = copy.copy([
# slot 0 is home
self.mission_item_home(target_system=target_system, target_component=target_component),
# slot 1 is takeoff
self.mission_item_takeoff(target_system=target_system, target_component=target_component),
wp2,
wp3,
wp4,
# now three MORE spline waypoints right on top of one another somewhere else:
wp5,
wp6,
wp7,
self.mission_item_rtl(target_system=target_system, target_component=target_component),
])
# renumber the items:
count = 0
for item in ret:
item.seq = count
count += 1
return ret
def fly_mission_points(self, points):
'''takes a list of waypoints and flies them, expecting a disarm at end'''
self.check_mission_upload_download(points)
self.set_parameter("AUTO_OPTIONS", 3)
self.change_mode('AUTO')
self.set_rc(8, 1000)
self.wait_ready_to_arm()
self.arm_vehicle()
self.progress("Raising rotor speed")
self.set_rc(8, 2000)
self.wait_waypoint(0, len(points)-1)
self.wait_disarmed()
self.set_rc(8, 1000)
def NastyMission(self):
'''constructs and runs missions designed to test scurves'''
self.fly_mission_points(self.scurve_nasty_mission())
# hopefully we don't need this step forever:
self.progress("Restting mission state machine by changing into LOITER")
self.change_mode('LOITER')
self.fly_mission_points(self.scurve_nasty_up_mission())
def set_rc_default(self): def set_rc_default(self):
super(AutoTestHelicopter, self).set_rc_default() super(AutoTestHelicopter, self).set_rc_default()
self.progress("Lowering rotor speed") self.progress("Lowering rotor speed")
@ -568,6 +796,7 @@ class AutoTestHelicopter(AutoTestCopter):
self.FlyEachFrame, self.FlyEachFrame,
self.AirspeedDrivers, self.AirspeedDrivers,
self.TurbineStart, self.TurbineStart,
self.NastyMission,
]) ])
return ret return ret