def __navigation(self):
watcher = CancelWatcher()
radius = self.radius
frequency = 1
try:
target = self.get_target()
CLocation = self.get_location()
CYaw = self.get_heading()
init_angle = angle_heading_target(CLocation, target, CYaw)
self.condition_yaw(init_angle)
while not watcher.IsCancel():
CLocation = self.get_location()
CYaw = self.get_heading()
distance = get_distance_metres(CLocation, target)
angle = angle_heading_target(CLocation, target, CYaw)
logger.debug('{} {}'.format(distance, angle))
if not self.InAngle(angle, 90) or distance <= radius:
logger.info("Reached Target!")
self.brake()
return True
self.forward()
time.sleep(frequency)
except AssertionError, e:
self.brake()
logger.error(e)
return False
def condition_yaw(self, heading):
"""
0<=heading<360 (anti-clockwise)
"""
if config.debug:
logger.warn('condition_yaw() ...')
return
watcher = CancelWatcher()
assert heading >= 0 and heading < 360, 'Param-heading {} is invalid'.format(
heading)
CYaw = self.get_heading()
target_angle = angle_diff(CYaw, heading)
TurnAngle = angle_diff(CYaw, target_angle)
if TurnAngle >= 0 and TurnAngle <= 180:
is_cw = 1
logger.debug('Turn left {}'.format(TurnAngle))
self.yaw_left()
else:
is_cw = -1
logger.debug('Turn right {}'.format(360 - TurnAngle))
self.yaw_right()
logger.debug("Current Angle:{} Target Angle:{}".format(
CYaw, target_angle))
while not watcher.IsCancel():
CYaw = self.get_heading()
# logger.debug("Current Angle:{} Target Angle:{}".format(
# CYaw, target_angle))
if self.isStop(CYaw, target_angle, is_cw):
break
time.sleep(.01)
self.brake()
logger.debug("Fact Angle: %d" % self.get_heading())
def up_metres(self, altitude, relative=True):
if altitude <= 0:
logger.warn('Altitude({}) is invalid'.format(altitude))
return
try:
CAlt = self.get_altitude(False)
if relative:
TAlt = CAlt + altitude
else:
init_alt = self.ORB.get_init_alt()
TAlt = init_alt + altitude
if TAlt < CAlt:
logger.warn('TAlt({}) is less than CAlt ({}).'.format(
TAlt, CAlt))
return
self.GradualTHR(0, 60)
watcher = CancelWatcher()
while not watcher.IsCancel():
CAlt = self.get_altitude(False)
if CAlt >= TAlt:
break
time.sleep(.1)
self.brake()
except AssertionError, e:
logger.error(e)
def GradualTHR(self, begin, end):
watcher = CancelWatcher()
if begin <= end:
while begin < end and not watcher.IsCancel():
self.control_percent(THR=begin)
begin += 1
time.sleep(0.05)
else:
while begin > end and not watcher.IsCancel():
self.control_percent(THR=begin)
begin -= 1
time.sleep(0.05)
def navigation(self):
watcher = CancelWatcher()
radius = self.radius
frequency = self.frequence
try:
target = self.get_target()
CLocation = self.get_location()
CYaw = self.get_heading()
init_angle = angle_heading_target(CLocation, target, CYaw)
self.condition_yaw(init_angle)
while not watcher.IsCancel():
CLocation = self.get_location()
CYaw = self.get_heading()
distance = get_distance_metres(CLocation, target)
angle = angle_heading_target(CLocation, target, CYaw)
if not self.InAngle(angle, 90) or distance <= radius:
# if distance <= radius:
logger.info("Reached Target Waypoint!")
self.brake()
return True
EAngle = int(math.degrees(math.asin(radius / distance)))
logger.debug('{} {} {}'.format(distance, angle, EAngle))
if self.InAngle(angle, max(EAngle, self.Epsilon)):
self.control_FRU(ELE=1)
else:
if angle > EAngle and angle <= 90:
logger.debug('Roll Left')
self.control_FRU(AIL=-1, ELE=1)
elif angle >= 270 and angle < 360 - EAngle:
logger.debug('Roll Right')
self.control_FRU(AIL=1, ELE=1)
else:
self.brake()
self.condition_yaw(angle)
time.sleep(frequency)
except AssertionError, e:
self.brake()
logger.error(e)
return False
def takeoff(self, alt=5):
watcher = CancelWatcher()
if not self.state('Baro'):
return
logger.info('Takeoff to {} m'.format(alt))
self.escalate(0, 60)
while not watcher.IsCancel():
try:
currentAlt = self.get_altitude(True)
except AssertionError, e:
logger.error(e)
break
if currentAlt > alt * 0.95:
logger.info('Reached Altitude :{}'.format(currentAlt))
break
time.sleep(.01)
def down_metres(self, altitude):
watcher = CancelWatcher()
if altitude <= 0:
logger.warn('Altitude({}) is invalid'.format(altitude))
return
try:
CurAlt = self.get_altitude(False)
TarAlt = CAlt - altitude
InitAlt = self.ORB.get_init_alt()
if TAlt < IAlt + 1:
logger.warn('TAltitude({}) is too low.'.format(TAlt - IAlt))
return
self.control_FRU(THR=-1)
watcher = CancelWatcher()
while not watcher.IsCancel():
CAlt = self.get_altitude(False)
if CAlt <= TAlt:
break
self.brake()
except AssertionError, e:
logger.error(e)
def Auto(self):
logger.debug('Auto(AI) start ...')
flag = True
if self.vehicle.wp.isNull():
logger.warn('Waypoint is Null.Please set Waypoint')
return False
self.publish('Mode', 'AI_Auto')
watcher = CancelWatcher()
for point in self.vehicle.wp.points:
if watcher.IsCancel():
logger.warn('Cancel Auto')
flag = False
break
self.publish('Target', point)
result = self.full_auto()
if not result:
logger.error("Navigation except exit")
flag = False
break
self.wp.add_number()
self.vehicle.Auto_finally()
return flag
def full_auto(self):
watcher = CancelWatcher()
interval = 2
radius = self.vehicle.radius
try:
target = self.vehicle.get_target()
CLocation = self.vehicle.get_location()
CYaw = self.vehicle.get_heading()
angle = angle_heading_target(CLocation, target, CYaw)
self.vehicle.condition_yaw(angle)
except AssertionError, e:
logger.error(e)
self.vehicle.brake()
return False
def Auto(self):
logger.info('Auto...')
flag = True
if self.wp.isNull():
logger.error('Waypoint is None')
return False
self.publish('Mode', 'Auto')
watcher = CancelWatcher()
for point in self.wp.points:
if watcher.IsCancel():
logger.warn('Cancel Auto')
flag = False
break
self.publish('Target', point)
result = self.navigation()
if not result:
logger.error("Navigation except exit")
flag = False
break
self.wp.add_number()
self.Auto_finally()
return flag
def _navigation(self):
watcher = CancelWatcher()
radius = self.radius
frequency = self.frequence
try:
target = self.get_target()
CLocation = self.get_location()
CYaw = self.get_heading()
init_angle = angle_heading_target(CLocation, target, CYaw)
self.condition_yaw(init_angle)
while not watcher.IsCancel():
CLocation = self.get_location()
CYaw = self.get_heading()
distance = get_distance_metres(CLocation, target)
angle = angle_heading_target(CLocation, target, CYaw)
if not self.InAngle(angle, 90) or distance <= radius:
logger.info("Reached Target!")
self.brake()
return True
EAngle = int(math.degrees(math.asin(radius / distance)))
logger.debug('{} {} {}'.format(distance, angle, EAngle))
if not self.InAngle(angle, max(EAngle, self.Epsilon)):
self.brake()
self.condition_yaw(angle)
self.forward()
time.sleep(frequency)
# raw_input('next')
except AssertionError, e:
self.brake()
logger.error(e)
return False