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