def mode_listener(self, name, msg):
util.log_info("Mode switched to %s" % msg.name)
if msg.name != shared.status['manual_mode']: # manual override
if msg.name == 'RTL' or msg.name == 'LAND':
util.log_warning("External %s detected. Abort." % msg.name)
shared.status['abort'] = True
def _broadcast_origin(xbee):
"""
Broadcast HOME_ORIGIN to the drones.
Args:
xbee(xbee.Zigbee): the XBee communication interface.
Returns:
bool: True if success, False if failed.
"""
if not shared.home_origin:
util.log_warning("HOME_ORIGIN invalid!")
return False
package = pack(
'=3s2d5f',
'ORG', # 'TAR' = 'target'
shared.home_origin.lat, # latitude
shared.home_origin.lon, # longitude
shared.home_origin.alt, # msl_altitude
shared.des_alt, # relative altitude, default 15.0 m
0,
0,
0) # only report coordinates, no velocity
util.log_info("Broadcasting HOME_ORIGIN %s" % shared.home_origin)
comm.xbee_broadcast(xbee, package)
return True
def gps_listener(self, name, msg): # monitor satellites
if not shared.status['thread_flag'] & shared.NSATS_TOO_LOW:
if msg.satellites_visible < 6:
util.log_warning("Satellites dropped below 5!")
shared.status['thread_flag'] |= shared.NSATS_TOO_LOW
elif msg.satellites_visible >= 10:
util.log_info("Satellites recovered to %d." %
msg.satellites_visible)
shared.status['thread_flag'] &= ~shared.NSATS_TOO_LOW
def _set_home_origin():
"""
Set the high-level HOME_ORIGIN, manually invoked by a key input.
"""
util.log_info('Setting HOME ORIGIN.')
shared.home_origin = LocationGlobalRelative(
31.2991103, # simulated origin
121.4953190,
9)
util.log_info("HOME_ORIGIN: %s" % shared.home_origin)
def _set_home_origin(gpsd):
"""
Set the high-level HOME_ORIGIN, manually invoked by a key input.
Args:
gpsd(gps.gps): the gps object monitoring gps daemon data.
Returns:
bool: True if success, False if failed.
"""
if (gpsd.valid & gps.LATLON_SET) and (gpsd.satellites_used >= MIN_SAT_NUM):
util.log_info("Setting HOME_ORIGIN.")
shared.home_origin = LocationGlobalRelative(gpsd.fix.latitude,
gpsd.fix.longitude,
gpsd.fix.altitude)
util.log_info("HOME_ORIGIN: %s" % shared.home_origin)
return True
elif (gpsd.valid & gps.LATLON_SET):
util.log_info("<ORG> Satellites low at %d" % gpsd.satellites_used)
else:
util.log_info("<ORG> No valid coordinates!")
return False
def _broadcast_rendezvous(xbee):
"""
Broadcast the rendezvous coordinates to the drones.
Args:
gpsd(gps.gps): the gps object monitoring gps daemon data.
xbee(xbee.Zigbee): the XBee communication interface.
"""
package = pack(
'=3s2d5f',
'TAR', # 'TAR' = 'target'
31.3012010, # latitude # physics building grass
121.4981920, # longitude
9, # msl_altitude
shared.des_alt, # relative altitude, default 15.0 m
0,
0,
0) # only report coordinates, no velocity
util.log_info('Sending rendezvous coordinates.')
comm.xbee_broadcast(xbee, package)
def _update_parameter(xbee, path):
"""
Update algorithm parameters to the drones.
Parameters are stored in a text file with a delimiter, `=` and `,` separation.
Broadcast is not reception-guaranteed, so the drones shall echo an ACK.
Args:
xbee(xbee.Zigbee): the XBee communication interface.
path(str): path to a text file storing the parameters.
"""
fp = open(path, 'r')
param = fp.read().split()
for i in xrange(0, len(param)):
param[i] = dict(item.split('=') for item in param[i].split(','))
param = dict(zip(list(param[i]['ID'] for i in range(0, len(param))),
param))
fp.close()
if shared.CURRENT_ALGORITHM == 'MPC':
util.log_info("Updating MPC param.")
package = pack('=3s3f2i', param['MPC']['ID'],
float(param['MPC']['Ts']), float(param['MPC']['Vmax']),
float(param['MPC']['D0']), int(param['MPC']['Hp']),
int(param['MPC']['Hu']))
shared.param_mpc = mas.ParamMPC(unpack('=3s3f2i', package))
util.log_info("New MPC param sent: %s" % shared.param_mpc)
comm.xbee_broadcast(xbee, 'PRM,%s' % package) # identifier: `PRM`
elif shared.CURRENT_ALGORITHM == 'Vicsek':
pass # reserved
else:
util.log_warning("Err: shared.CURRENT_ALGORITHM is None!")
def _broadcast_rendezvous(gpsd, xbee):
"""
Broadcast the rendezvous coordinates to the drones.
Args:
gpsd(gps.gps): the gps object monitoring gps daemon data.
xbee(xbee.Zigbee): the XBee communication interface.
Returns:
bool: True if success, False if failed.
"""
if (gpsd.valid & gps.LATLON_SET) and (gpsd.satellites_used >= MIN_SAT_NUM):
# 'TAR' delimiter is only for flocking algorithms.
# For other applications, the frames may vary.
package = pack(
'=3s2d5f',
'TAR', # 'TAR' = 'target'
gpsd.fix.latitude, # latitude
gpsd.fix.longitude, # longitude
gpsd.fix.altitude, # msl_altitude
shared.des_alt, # relative altitude, default 15.0 m
0,
0,
0) # only report coordinates, no velocity
# constant 39bytes transmision, similar to normal agents.
util.log_info("Sending rendezvous coordinates.")
comm.xbee_broadcast(xbee, package)
return True
elif (gpsd.valid & gps.LATLON_SET):
util.log_info("<TAR> Satellites low at %d" % gpsd.satellites_used)
else:
util.log_info("<TAR> No valid coordinates!")
return False
def main():
"""
The Main function of this script.
"""
args = _parse_arguments()
util.log_init(
"onboard_A%s_%s.txt" %
(args.id, util.get_latest_log("latest_onboard.txt")),
util.log_level[args.level])
shared.AGENT_ID = 'A%s' % args.id
shared.AGENT_COUNT = args.n
shared.CURRENT_ALGORITHM = args.algorithm
shared.AGENT_CHARACTER = args.character
shared.des_alt = args.alt
util.log_info("AGENT_ID = %s" % shared.AGENT_ID)
util.log_info("Algorithm: %s" % shared.CURRENT_ALGORITHM)
util.log_info("Agent type: %s" % shared.AGENT_CHARACTER)
ser = serial.Serial(args.xbee, 230400)
xbee = comm.xbee_init(ser)
util.log_info("Xbee initialized.")
copter = nav.connect(args.pix, baud=921600, wait_ready=True, rate=20)
util.log_info("Copter connected. Firmware: %s" % copter.version)
info = "IFO,%s connected with firmware %s" % (shared.AGENT_ID,
copter.version)
comm.xbee_broadcast(xbee, info)
_add_listeners(copter)
takeoff_thread = nav.Takeoff(copter, xbee, shared.des_alt, 3)
purge_thread = comm.Purge(shared.neighbors)
broadcast_thread = comm.Broadcast(shared.AGENT_ID, copter, xbee)
flocking_thread = _choose_algorithm(copter, xbee, shared.neighbors)
takeoff_thread.start()
takeoff_thread.join() # wait until takeoff procedure completed
if shared.status['airborne']: # only execute the threads when airborne
util.log_info("Copter is airborne, starting threads.")
broadcast_thread.start()
purge_thread.start()
flocking_thread.start()
# main loop
while True:
try:
time.sleep(.2)
except KeyboardInterrupt:
break
if shared.status['airborne']:
# echo exiting status
if shared.status['exiting']:
info = "IFO,%s %s-ing." % (shared.AGENT_ID,
shared.status['command'])
comm.xbee_broadcast(xbee, info)
util.log_info(info)
# if an rtl or land command is received, kill flocking and set the `exiting` flag
elif shared.status['command'] == 'RTL' or shared.status[
'command'] == 'LAND':
shared.status['thread_flag'] |= shared.FLOCKING_FLAG
nav.set_mode(copter, shared.status['command'])
shared.status['exiting'] = True
if not flocking_thread.is_alive(): # break the loop if finished
break
nav.wait_for_disarm(copter) # wait for disarm
comm.xbee_broadcast(xbee, 'IFO,%s terminated.' % shared.AGENT_ID)
purge_thread.stop()
while purge_thread.is_alive():
util.log_info('Waiting for purge to shutdown')
purge_thread.join(3)
util.log_info('Purge killed.')
broadcast_thread.stop()
while broadcast_thread.is_alive():
util.log_info('Waiting for broadcast to shutdown')
broadcast_thread.join(3)
util.log_info('Broadcast killed.')
xbee.halt()
ser.close()
util.log_info("Xbee and serial closed.")
copter.close()
util.log_info("Copter shutdown.")
def main():
"""
The Main function of this script.
"""
args = _parse_arguments()
util.log_init("gcs_%s.txt" % util.get_latest_log("latest_gcs.txt"),
util.log_level[args.level])
shared.AGENT_ID = 'GCS'
shared.CURRENT_ALGORITHM = args.algorithm
util.log_info("AGENT_ID = %s" % shared.AGENT_ID)
util.log_info("Algorithm: %s" % shared.CURRENT_ALGORITHM)
util.log_info("Agent type: Ground station.")
fparam = args.param
ser = serial.Serial(args.xbee, 230400)
xbee = comm.xbee_init(ser)
util.log_info("Xbee initialized.")
gpsd = gps.gps() # in some cases, the gpsd need to be flushed and reset
gps_thread = GNSS(gpsd)
gps_thread.start()
util.log_info('GNSS running.')
# This dictionary should keep the <offset> consistant with <cmd_list>.
# It is useful for multi-to-one mapping. All the keys are lower-cased,
# the first key is the native one, others are considered alias, and are
# only for conveinience usage.
key_dict = {
# <key> <offset>
'p': -4, # set parameters
's': -3, # set HOME ORIGIN
'o': -2, # send origin
't': -1, # send rendezvous
'r': 0, # RTL command
'l': 1, # land command
'b': 2, # lift command
'e': 3, # exit command
'c': 4, # clear rendezvous
}
cmd_list = [
# <command string> <key> <description>
# -----------------Positive Index-----------------
['CTR,RTL ', 'r', 'Exit algorithms at run and RTL'], # 0
['CTR,LAND', 'l', 'Exit algorithms at run and LAND'], # 1
['CTR,LIFT', 'b', 'Initiate takeoff.'], # 2
['CTR,EXIT', 'e', 'Exit takeoff when on the ground.'], # 3
['CLR_RDV ', 'c', 'Clear rendezvous coordinates'], # 4
# -----------------Negative Index-----------------
['"None" ', 'p', 'Update parameters'], # -4
['"None" ', 's', 'Set HOME ORIGIN coordinates'], # -3
['ORG,<pack>', 'o', 'Send HOME ORIGIN coordinates'], # -2
['TAR,<pack>', 't', 'Send rendezvous coordinates'], # -1
]
# get keyboard file descrpter from stdin and save current terminal attribute
# then turn into cbreak style, without terminal echo
# See: https://docs.python.org/2/faq/library.html?highlight=read
keyboard = sys.stdin
old_attr = termios.tcgetattr(keyboard)
new_attr = termios.tcgetattr(keyboard)
new_attr[3] = new_attr[3] & ~termios.ICANON & ~termios.ECHO
termios.tcsetattr(keyboard, termios.TCSANOW, new_attr)
old_flags = fcntl.fcntl(keyboard, fcntl.F_GETFL)
fcntl.fcntl(keyboard, fcntl.F_SETFL, old_flags) # | os.O_NONBLOCK)
key = None
while True: # main loop
try:
time.sleep(.05)
key = keyboard.read(1) # read only one key
if key in key_dict: # valid key
if key_dict[key] >= 0: # positive value: send command
util.log_info("Sending command: '%s'" %
cmd_list[key_dict[key]][0].strip(' '))
comm.xbee_broadcast(xbee,
cmd_list[key_dict[key]][0].strip(' '))
else: # negative value: send coordinates or parameters
# TODO(Q.Yuan): try to get automated broadcasting
if key_dict[key] == -1:
_broadcast_rendezvous(gpsd, xbee)
elif key_dict[key] == -2:
_broadcast_origin(xbee)
elif key_dict[key] == -3:
_set_home_origin(gpsd)
elif key_dict[key] == -4:
_update_parameter(xbee, fparam)
else: # not a valid key, print help
print "\n---- Command List ----\nCommand:\tKey:\tDescription"
for idx in range(0, len(cmd_list)):
print '%s\t%s\t%s' % (cmd_list[idx][0], cmd_list[idx][1],
cmd_list[idx][2])
except IOError:
pass
except KeyboardInterrupt:
break
# clean up
gps_thread.stop()
while gps_thread.is_alive():
util.log_info('Waiting for GNSS to shutdown')
gps_thread.join(3)
util.log_info('GNSS shutdown.')
xbee.halt()
ser.close()
util.log_info("Xbee and serial closed.")
# restore previous terminal attribute
termios.tcsetattr(keyboard, termios.TCSAFLUSH, old_attr)
fcntl.fcntl(keyboard, fcntl.F_SETFL, old_flags)
def main():
"""
The Main function of this script.
"""
args = _parse_arguments()
util.log_init(
"sitl_A%s_%s.txt" % (args.id, util.get_latest_log("latest_sitl.txt")),
util.log_level[args.level])
shared.AGENT_ID = 'A%s' % args.id
shared.AGENT_COUNT = args.n
shared.CURRENT_ALGORITHM = args.algorithm
shared.AGENT_CHARACTER = args.character
shared.des_alt = args.alt
util.log_info("AGENT_ID = %s" % shared.AGENT_ID)
util.log_info("Algorithm: %s" % shared.CURRENT_ALGORITHM)
util.log_info("Agent type: %s" % shared.AGENT_CHARACTER)
print "Start simulator (SITL)"
sitl = SITL(args.pix) # initialize SITL with firmware path
if shared.AGENT_ID in start_loc:
sitl_args = ['--home=%s' % start_loc[shared.AGENT_ID]]
else:
sitl_args = ['--home=%s' % start_loc['FFF']]
# Pre-recorded coordinates.
#sitl_args = ['-I0', '--model', 'quad', '--home=31.301201,121.498192,9,353']
sitl.launch(sitl_args, await_ready=True, restart=True)
# Connect to the vehicle. (Spawn an instance of Vehicle named "vehicle")
# connection port is coded in the file name of the firmware like "ac3.4.5_port5760"
# use regular expression to search the string and extract port number
port = re.search(r'port\d{4}', args.pix)
port = re.search(r'\d{4}', port.group()).group()
print "Connecting to copter on: TCP: 127.0.0.1:%s" % port
copter = nav.connect('tcp:127.0.0.1:%s' % port, wait_ready=True, rate=20)
util.log_info("Copter connected. Firmware: %s" % copter.version)
if not args.xbee: # simulate XBee using ZeroMQ
[pub, sub] = comm.zmq_init(comm_port_list[shared.AGENT_ID],
comm_port_list)
subscriber_thread = comm.Subscriber(shared.AGENT_ID, sub)
subscriber_thread.start()
xbee = pub # make xbee the publisher
util.log_info("ZeroMQ initialzied.")
else: # use actual xbee ports
ser = serial.Serial(args.xbee, 57600)
xbee = comm.xbee_init(ser)
util.log_info("Xbee initialzed.")
info = "IFO,%s connected with firmware %s" % (shared.AGENT_ID,
copter.version)
comm.xbee_broadcast(xbee, info)
_add_listeners(copter)
takeoff_thread = nav.Takeoff(copter, xbee, shared.des_alt, 3)
purge_thread = comm.Purge(shared.neighbors)
broadcast_thread = comm.Broadcast(shared.AGENT_ID, copter, xbee)
flocking_thread = _choose_algorithm(copter, xbee, shared.neighbors)
takeoff_thread.start()
takeoff_thread.join() # wait until takeoff procedure completed
if shared.status['airborne']: # only execute the threads when airborne
util.log_info("Copter is airborne, starting threads.")
broadcast_thread.start()
purge_thread.start()
flocking_thread.start()
# main loop
while True:
try:
time.sleep(.2)
except KeyboardInterrupt:
break
if shared.status['airborne']:
# echo exiting status
if shared.status['exiting']:
info = "IFO,%s %s-ing." % (shared.AGENT_ID,
shared.status['command'])
comm.xbee_broadcast(xbee, info)
util.log_info(info)
# if an rtl or land command is received, kill flocking and set the `exiting` flag
elif shared.status['command'] == 'RTL' or shared.status[
'command'] == 'LAND':
shared.status['thread_flag'] |= shared.FLOCKING_FLAG
nav.set_mode(copter, shared.status['command'])
shared.status['exiting'] = True
if not flocking_thread.is_alive(): # break the loop if finished
break
nav.wait_for_disarm(copter) # wait for disarm
comm.xbee_broadcast(xbee, 'IFO,%s terminated.' % shared.AGENT_ID)
# clean up
purge_thread.stop()
while purge_thread.is_alive():
util.log_info('Waiting for purge to shutdown')
purge_thread.join(3)
util.log_info('Purge killed.')
broadcast_thread.stop()
while broadcast_thread.is_alive():
util.log_info('Waiting for broadcast to shutdown')
broadcast_thread.join(3)
util.log_info('Broadcast killed.')
copter.close()
util.log_info("Copter shutdown.")
if args.xbee:
xbee.halt()
ser.close()
util.log_info("Xbee and serial closed.")
else:
subscriber_thread.stop()
while subscriber_thread.is_alive():
util.log_info('Waiting for Subscriber to shutdown')
subscriber_thread.join(3)
util.log_info('Subscriber killed.')
sitl.stop()
util.log_info("SITL shutdown.")