2021-02-21 04:45:19 -04:00
|
|
|
'''
|
|
|
|
Drive a BalanceBot in SITL
|
|
|
|
|
|
|
|
AP_FLAKE8_CLEAN
|
|
|
|
|
|
|
|
'''
|
|
|
|
|
2018-07-20 08:53:31 -03:00
|
|
|
from __future__ import print_function
|
|
|
|
|
|
|
|
import os
|
|
|
|
|
2020-04-14 20:37:23 -03:00
|
|
|
from rover import AutoTestRover
|
2018-12-15 03:43:20 -04:00
|
|
|
from common import AutoTest
|
2018-07-20 08:53:31 -03:00
|
|
|
|
2019-10-06 10:46:40 -03:00
|
|
|
from common import NotAchievedException
|
|
|
|
|
2018-07-20 08:53:31 -03:00
|
|
|
# get location of scripts
|
|
|
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
|
|
|
|
2021-02-21 04:45:19 -04:00
|
|
|
|
2018-07-20 08:53:31 -03:00
|
|
|
class AutoTestBalanceBot(AutoTestRover):
|
2018-12-15 03:43:20 -04:00
|
|
|
|
2022-09-08 07:47:50 -03:00
|
|
|
def log_name(self):
|
|
|
|
return "BalanceBot"
|
|
|
|
|
2018-12-15 03:43:20 -04:00
|
|
|
def vehicleinfo_key(self):
|
2020-03-26 20:14:17 -03:00
|
|
|
return "Rover"
|
2018-07-20 08:53:31 -03:00
|
|
|
|
|
|
|
def init(self):
|
|
|
|
if self.frame is None:
|
|
|
|
self.frame = 'balancebot'
|
|
|
|
super(AutoTestBalanceBot, self).init()
|
|
|
|
|
2022-09-09 22:24:28 -03:00
|
|
|
def DO_SET_MODE(self):
|
|
|
|
'''Set mode via MAV_COMMAND_DO_SET_MODE'''
|
2018-12-15 03:43:20 -04:00
|
|
|
self.do_set_mode_via_command_long("HOLD")
|
|
|
|
self.do_set_mode_via_command_long("MANUAL")
|
|
|
|
|
2019-02-03 06:43:07 -04:00
|
|
|
def rc_defaults(self):
|
|
|
|
ret = super(AutoTestBalanceBot, self).rc_defaults()
|
|
|
|
ret[3] = 1500
|
|
|
|
return ret
|
2018-12-15 03:43:20 -04:00
|
|
|
|
2019-03-05 18:28:59 -04:00
|
|
|
def is_balancebot(self):
|
|
|
|
return True
|
|
|
|
|
2019-03-09 19:39:10 -04:00
|
|
|
def drive_rtl_mission_max_distance_from_home(self):
|
|
|
|
'''maximum distance allowed from home at end'''
|
|
|
|
'''balancebot tends to wander backwards, away from the target'''
|
|
|
|
return 8
|
|
|
|
|
2022-09-09 22:24:28 -03:00
|
|
|
def DriveRTL(self):
|
|
|
|
'''Drive an RTL Mission'''
|
2019-03-09 19:39:10 -04:00
|
|
|
# if we Hold then the balancebot continues to wander
|
2019-09-17 14:57:51 -03:00
|
|
|
# indefinitely at ~1m/s, hence we set to Acro
|
|
|
|
self.set_parameter("MIS_DONE_BEHAVE", 2)
|
2022-09-09 22:24:28 -03:00
|
|
|
super(AutoTestBalanceBot, self).DriveRTL()
|
2021-02-21 04:45:19 -04:00
|
|
|
|
2022-09-09 22:24:28 -03:00
|
|
|
def TestWheelEncoder(self):
|
2019-10-06 10:46:40 -03:00
|
|
|
'''make sure wheel encoders are generally working'''
|
|
|
|
ex = None
|
|
|
|
try:
|
|
|
|
self.set_parameter("WENC_TYPE", 10)
|
|
|
|
self.set_parameter("AHRS_EKF_TYPE", 10)
|
|
|
|
self.reboot_sitl()
|
|
|
|
self.set_parameter("WENC2_TYPE", 10)
|
|
|
|
self.set_parameter("WENC_POS_Y", 0.075)
|
|
|
|
self.set_parameter("WENC2_POS_Y", -0.075)
|
|
|
|
self.reboot_sitl()
|
|
|
|
self.change_mode("HOLD")
|
|
|
|
self.wait_ready_to_arm()
|
|
|
|
self.change_mode("ACRO")
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.set_rc(3, 1600)
|
|
|
|
|
2022-02-09 20:59:34 -04:00
|
|
|
m = self.assert_receive_message('WHEEL_DISTANCE', timeout=5)
|
2019-10-06 10:46:40 -03:00
|
|
|
|
|
|
|
tstart = self.get_sim_time()
|
|
|
|
while True:
|
|
|
|
if self.get_sim_time_cached() - tstart > 10:
|
|
|
|
break
|
|
|
|
dist_home = self.distance_to_home(use_cached_home=True)
|
|
|
|
m = self.mav.messages.get("WHEEL_DISTANCE")
|
|
|
|
delta = abs(m.distance[0] - dist_home)
|
|
|
|
self.progress("dist-home=%f wheel-distance=%f delta=%f" %
|
|
|
|
(dist_home, m.distance[0], delta))
|
|
|
|
if delta > 5:
|
|
|
|
raise NotAchievedException("wheel distance incorrect")
|
|
|
|
self.disarm_vehicle()
|
|
|
|
except Exception as e:
|
|
|
|
self.progress("Caught exception: %s" %
|
|
|
|
self.get_exception_stacktrace(e))
|
|
|
|
self.disarm_vehicle()
|
|
|
|
ex = e
|
|
|
|
self.reboot_sitl()
|
|
|
|
if ex is not None:
|
|
|
|
raise ex
|
2019-03-09 19:39:10 -04:00
|
|
|
|
2022-09-09 22:24:28 -03:00
|
|
|
def DriveMission(self):
|
|
|
|
'''Drive Mission rover1.txt'''
|
|
|
|
self.drive_mission("balancebot1.txt", strict=False)
|
|
|
|
|
2018-12-15 03:43:20 -04:00
|
|
|
def tests(self):
|
|
|
|
'''return list of all tests'''
|
|
|
|
|
|
|
|
'''note that while AutoTestBalanceBot inherits from Rover we don't
|
|
|
|
inherit Rover's tests!'''
|
|
|
|
ret = AutoTest.tests(self)
|
|
|
|
|
|
|
|
ret.extend([
|
2022-09-09 22:24:28 -03:00
|
|
|
self.DriveRTL,
|
|
|
|
self.DriveMission,
|
|
|
|
self.TestWheelEncoder,
|
2023-10-16 22:27:16 -03:00
|
|
|
self.MAV_CMD_DO_SEND_BANNER,
|
2022-09-09 22:24:28 -03:00
|
|
|
self.DO_SET_MODE,
|
|
|
|
self.ServoRelayEvents,
|
2019-10-06 10:46:40 -03:00
|
|
|
])
|
2018-12-15 03:43:20 -04:00
|
|
|
return ret
|
|
|
|
|
|
|
|
def default_mode(self):
|
|
|
|
return 'MANUAL'
|