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 _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!")