def _wait_home_origin(xbee):
"""
Wait for the HOME_ORIGIN sent by the GCS.
`HOME_ORIGIN` is not the internal `home` set by the flight controller, it is
a high-level reference coordinate, used in the application algorithms.
Note that this is a blocking wait, so the program is stuck indefinately unless
stopped by `EXIT` command or killed by KeyboardInterrupt.
Args:
xbee(xbee.Zigbee): the XBee communication interface.
Returns:
bool: True if success, False if killed by `EXIT` command.
"""
util.log_info("Waiting HOME_ORIGIN.")
wait_count = 0
while not shared.home_origin:
time.sleep(1)
wait_count = wait_count + 1
if shared.status['command'] == 'EXIT':
comm.xbee_broadcast(xbee,
"IFO,%s abort takeoff." % shared.AGENT_ID)
util.log_info("'EXIT' received. Abort takeoff.")
return False
if wait_count >= 10:
wait_count = 0
comm.xbee_broadcast(
xbee, "IFO,%s awaiting HOME_ORIGIN." % shared.AGENT_ID)
return True
def _log_and_broadcast(xbee, info):
"""
Log some information at `INFO` level and broadcast via XBee.
Args:
xbee(xbee.Zigbee): the XBee communication interface.
info(str): the information string.
"""
comm.xbee_broadcast(xbee, info)
util.log_info(info)
def _wait_lift_cmd(xbee):
"""
Wait for takeoff clearance from the GCS.
This is a blocking check, so the program is stuck indefinately unless
received `LIFT` clearance, stopped by `EXIT` command or killed by
KeyboardInterrupt.
Args:
xbee(xbee.Zigbee): the XBee communication interface.
Returns:
bool: True if success, False if killed by `EXIT` command.
"""
shared.status['command'] = 'STDBY'
util.log_info("%s Standby, awaiting 'LIFT'." % shared.AGENT_ID)
wait_count = 0
while True:
time.sleep(.1)
wait_count = wait_count + 1
if shared.status['command'] == 'LIFT':
comm.xbee_broadcast(
xbee, "IFO,%s cleared for takeoff." % shared.AGENT_ID)
util.log_info("'LIFT' received! Taking off!")
return True
elif shared.status['command'] == 'EXIT':
comm.xbee_broadcast(xbee,
"IFO,%s abort takeoff." % shared.AGENT_ID)
util.log_info("'EXIT' received. Abort takeoff.")
return False
elif wait_count >= 100:
wait_count = 0
comm.xbee_broadcast(
xbee, "IFO,%s standby. Alt: %.2f m." %
(shared.AGENT_ID, shared.des_alt))
def _preflight_check(vehicle, xbee):
"""
Perform preflight check, validate home coordinates and satellites fix.
This is a blocking check, so the program is stuck indefinately unless
stopped by `EXIT` command or killed by KeyboardInterrupt.
Args:
vehicle(dronekit.Vehicle): the vehicle to be controlled.
xbee(xbee.Zigbee): the XBee communication interface.
Returns:
bool: True if success, False if killed by `EXIT` command.
"""
util.log_info("Waiting for home location.")
while not vehicle.home_location:
if shared.status['command'] == 'EXIT':
comm.xbee_broadcast(xbee,
"IFO,%s abort takeoff." % shared.AGENT_ID)
util.log_info("'EXIT' received. Abort takeoff.")
return False
time.sleep(2)
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
comm.xbee_broadcast(xbee, "IFO,%s getting home fix." % shared.AGENT_ID)
# We have a home location now.
comm.xbee_broadcast(
xbee, 'IFO,%s home: %s.' % (shared.AGENT_ID, vehicle.home_location))
util.log_info('Home location: %s' % vehicle.home_location)
# Check satellite condition to ensure 3D-fix first
while vehicle.gps_0.fix_type < 3:
if shared.status['command'] == 'EXIT':
comm.xbee_broadcast(xbee,
"IFO,%s abort takeoff." % shared.AGENT_ID)
util.log_info("'EXIT' received. Abort takeoff.")
return False
comm.xbee_broadcast(xbee, "IFO,%s GNSS No 3D-fix." % shared.AGENT_ID)
util.log_warning("GNSS No 3D Fix.")
time.sleep(3)
# APM:Copter parameter: GPS_HDOP_GOOD
# The value is mutiplied by 100 into a integer, default good HDOP is below 140
while vehicle.gps_0.eph > 140 or vehicle.gps_0.satellites_visible < 9:
if shared.status['command'] == 'EXIT':
comm.xbee_broadcast(xbee,
"IFO,%s abort takeoff." % shared.AGENT_ID)
util.log_info("'EXIT' received. Abort takeoff.")
return False
util.log_info(
"HDOP: %.2f NumSat: %s" %
(vehicle.gps_0.eph / 100.0, vehicle.gps_0.satellites_visible))
comm.xbee_broadcast(
xbee, "IFO,%s HDOP: %.2f NumSat: %s" %
(shared.AGENT_ID, vehicle.gps_0.eph / 100.0,
vehicle.gps_0.satellites_visible))
time.sleep(3)
# --END of while-- Preflight check passed.
comm.xbee_broadcast(xbee,
"IFO,%s Preflight check passed." % shared.AGENT_ID)
util.log_info(
"Preflight check passed. HDOP: %.2f NumSats: %s" %
(vehicle.gps_0.eph / 100.0, vehicle.gps_0.satellites_visible))
util.log_info("Local time %s" % shared.timestamp)
return True
def run(self):
"""
Run `SquareRoute` thread.
"""
info = "IFO,Leader %s engaging <SquareRoute>." % shared.AGENT_ID
comm.xbee_broadcast(self._xbee, info)
util.log_info(info)
EDGE_LEN = 7.0 # square edge length
WP_GSPEED = 1.0 # groundspeed for navigating
edge_count = 0 # a counter holding which edge the copter is on
old_gspeed = self._vehicle.groundspeed # log original gspeed
# flying direction vector
dir = [
# dNorth, dEast
[EDGE_LEN, 0, 'North'], # North
[0, EDGE_LEN, 'East'], # East
[-EDGE_LEN, 0, 'South'], # South
[0, -EDGE_LEN, 'West'] # West
]
# waypoints
wp = [
# WP, description
[None, 'N-W'],
[None, 'N-E'],
[None, 'S-E'],
[None, 'S-W']
]
# extract square-shaped waypoints (hard waypoints to counter drifting)
wp[-1][0] = self._vehicle.location.global_relative_frame
wp[0][0] = nav.get_location_metres(wp[-1][0], dir[0][0], dir[0][1])
wp[1][0] = nav.get_location_metres(wp[0][0], dir[1][0], dir[1][1])
wp[2][0] = nav.get_location_metres(wp[1][0], dir[2][0], dir[2][1])
is_on_hold = False # hold position flag for low Nsats
while not self._stopflag:
[self._stopflag,
exit_number] = _check_terminate_condition(self._vehicle,
'SquareRoute')
is_on_hold = _check_satellite_low(self._xbee, is_on_hold)
if is_on_hold: continue
# hold position if RC_CH6 is at low-level
if self._vehicle.channels['6'] < 1500:
time.sleep(1)
continue
# loiter for 3 second and moving along an edge utilizing
# internal function <simple_goto>
time.sleep(3)
_log_and_broadcast(
self._xbee, "IFO,Leader-%s moving towards %s" %
(shared.AGENT_ID, dir[edge_count][2]))
self._vehicle.simple_goto(wp[edge_count][0], groundspeed=WP_GSPEED)
# check distance until reached
while not self._stopflag:
[self._stopflag, exit_number
] = _check_terminate_condition(self._vehicle, 'SquareRoute')
is_on_hold = _check_satellite_low(self._xbee, is_on_hold)
if is_on_hold: continue
time.sleep(0.2)
current = self._vehicle.location.global_relative_frame
distance = nav.get_distance_metres(current, wp[edge_count][0])
if distance <= shared.WP_RADIUS:
_log_and_broadcast(
self._xbee, "IFO,Leader-%s %s corner reached" %
(shared.AGENT_ID, wp[edge_count][1]))
edge_count = (edge_count + 1) % 4
break
elif self._vehicle.channels['6'] < 1500:
nav.send_ned_velocity(self._vehicle, 0, 0, 0, 0.5) # brake
break
# End of While #
_log_and_broadcast(
self._xbee, "IFO,%s SquareRoute ended with number %d." %
(shared.AGENT_ID, exit_number))
self._vehicle.groundspeed = old_gspeed #restore vehicle gspeed