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