class MockTelemetrySender(Thread): def __init__( self, recipient_hostname: str, recipient_port: int, ): super(MockTelemetrySender, self).__init__() self._socket = socket.socket( socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP ) self._socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self._socket.bind(('', 31337)) self._recipient_address = (recipient_hostname, recipient_port) self._update_queue = Queue() def run(self): while self.running: try: data = self._update_queue.get(timeout=_DEFAULT_TIMEOUT) except Empty: continue print("sending telemetry!!") self._socket.sendto(json.dumps(data).encode('utf-8'), self._recipient_address) def update(self, data: dict): self._update_queue.put(data)
def __init__( self, recipient_hostname: str, recipient_port: int, ): super(MockTelemetrySender, self).__init__() self._socket = socket.socket( socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP ) self._socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self._socket.bind(('', 31337)) self._recipient_address = (recipient_hostname, recipient_port) self._update_queue = Queue()
def recv( self, payload_type, # type: Type[MultiAntennyPayload] ): """ Fetch a packet from the queue, of a specific payload type """ try: recv = self.inbound_queue.get(timeout=_DEFAULT_TIMEOUT) except Empty: return None while recv is not None: curr_payload_type = type(recv.payload) if curr_payload_type not in self._payloads_by_packet_type: self._payloads_by_packet_type[curr_payload_type] = Queue() self._payloads_by_packet_type[curr_payload_type].put(recv) try: recv = self.inbound_queue.get(timeout=_DEFAULT_TIMEOUT) except Empty: break try: return self._payloads_by_packet_type[payload_type].get( timeout=_DEFAULT_TIMEOUT) except (Empty, KeyError): return None
def main(board_id: int): api = mock_antenna_api_factory(False, False) # api = esp32_antenna_api_factory(True, True) udp_client = UDPFollowerClient(Queue(), Queue(), MCAST_PORT) follower = AntennyFollowerNode(board_id, udp_client, api) try: udp_client.start() follower.start() while not follower.follow(0x42): time.sleep(0.25) except Exception as e: print(e) udp_client.stop() follower.stop() return follower.join() udp_client.join()
def join_leader(my_id: int): """ Join a leader. """ udp_client = UDPFollowerClient(Queue(), Queue(), MCAST_PORT) follower = AntennyFollowerNode(my_id, udp_client, api) try: udp_client.start() follower.start() while not follower.follow(0x42): time.sleep(1) except Exception as e: print(e) udp_client.stop() follower.stop() return follower.join() udp_client.join()
def mock_antenna_api_factory( use_screen: bool = False, use_telemetry: bool = False, ): """ Create a new MOCK AntennyAPI object. Useful for local debugging in a desktop python environment. """ config = ConfigRepository() imu = MockImuController() motor = MockMotorController() antenna_controller = AntennaController( AxisController( 1, imu, motor, ), AxisController( 0, imu, motor, ), ) if use_screen and not config.get("use_screen"): config.set("use_screen", True) screen = None if config.get("use_screen"): screen = MockScreenController(Queue()) if use_telemetry and not config.get("use_telemetry"): config.set("use_telemetry", True) telemetry_sender = None if use_telemetry: telemetry_sender = MockTelemetrySender('127.0.0.1', 1337) api = AntennyAPI( antenna_controller, imu, config, screen, telemetry_sender, True, ) api.start() return api
def esp32_antenna_api_factory(): """ Create a new AntennyAPI object. """ import machine from machine import Pin from imu.imu_bno055 import Bno055ImuController from motor.motor_pca9685 import Pca9685Controller config = ConfigRepository() safe_mode = False i2c_bno_scl = config.get("i2c_bno_scl") i2c_bno_sda = config.get("i2c_bno_sda") i2c_servo_scl = config.get("i2c_servo_scl") i2c_servo_sda = config.get("i2c_servo_sda") i2c_ch0 = machine.I2C( 0, scl=Pin(i2c_servo_scl, Pin.OUT, Pin.PULL_DOWN), sda=Pin(i2c_servo_sda, Pin.OUT, Pin.PULL_DOWN), ) if (i2c_bno_scl == i2c_servo_scl) and (i2c_bno_sda == i2c_servo_sda): i2c_ch1 = i2c_ch0 LOG.info("I2C Channel 0 is same as Channel 1; using chained bus") else: i2c_ch1 = machine.I2C( -1, scl=Pin(i2c_bno_scl, Pin.OUT, Pin.PULL_DOWN), sda=Pin(i2c_bno_sda, Pin.OUT, Pin.PULL_DOWN), freq=1000, ) if config.get("use_imu"): try: imu = Bno055ImuController(i2c_ch1, crystal=False, address=config.get("i2c_bno_address"), sign=(0, 0, 0)) except OSError: LOG.warning("Unable to initialize IMU, check configuration") imu = MockImuController() else: LOG.warning( "IMU disabled, please set use_imu=True in the settings and run `antkontrol`" ) imu = MockImuController() try: motor = Pca9685Controller(i2c_ch0, address=config.get("i2c_servo_address"), min_us=500, max_us=2500, degrees=180) except OSError: address = i2c_ch0.scan() if (i2c_ch1 != i2c_ch0) and (len(address) != 0): motor = Pca9685Controller(i2c_ch0, address=address[0], min_us=500, max_us=2500, degrees=180) LOG.info("Using auto address configuration for motor driver") else: LOG.warning( "Unable to initialize motor driver, entering SAFE MODE OPERATION" ) LOG.warning( "Your device may be improperly configured. Use the `setup` command to reconfigure and run " "`antkontrol`") safe_mode = True motor = MockMotorController() try: azimuth_index = config.get("azimuth_servo_index") elevation_index = config.get("elevation_servo_index") except: LOG.warning( "Unable to retrieve servo indices, using default values. Your motor movement may be incorrect" ) azimuth_index = 1 elevation_index = 0 antenna_controller = AntennaController( AxisController( azimuth_index, imu, motor, ), AxisController( elevation_index, imu, motor, ), ) screen = None if config.get("use_screen"): screen = MockScreenController(Queue()) else: LOG.warning( "Screen disabled, please set use_screen=True in the settings and run `antkontrol`" ) gps = None if config.get("use_gps"): gps = BasicGPSController() else: LOG.warning( "GPS disabled, please set use_gps=True in the settings and run `antkontrol`." ) gps = MockGPSController() telemetry_sender = None if config.get("use_telemetry"): if not config.get("use_imu"): LOG.warning( "Telemetry enabled, but IMU disabled in config! Please enable the IMU (" "using the IMU mock)") if not config.get("use_gps"): LOG.warning( "Telemetry enabled, but GPS disabled in config! Please enable the GPS (" "using the GPS mock)") telemetry_sender = UDPTelemetrySender(31337, gps, imu) else: LOG.warning( "Telemetry disabled, please set use_screen=True in the settings and run `antkontrol`" ) api = AntennyAPI( antenna_controller, imu, config, screen, telemetry_sender, safe_mode, ) if config.get("enable_demo"): interrupt_pin = machine.Pin(15, machine.Pin.IN, machine.Pin.PULL_UP) interrupt_pin.irq(trigger=machine.Pin.IRQ_FALLING, handler=api.antenna.pin_motion_test) api.start() return api
def y2k_timestamp(): return time.time() - 946684800.0 def programmed_move_demo(device_ids, moves): for el, az, delay in moves: for device_id in device_ids: leader.move(device_id, az, el, y2k_timestamp() + 1) time.sleep(delay) if __name__ == '__main__': logging.basicConfig(level=logging.DEBUG) board_id = 0x42 listen_port = 44444 udp_client = UDPLeaderClient(Queue(), Queue(), 31337, listen_port) udp_client.start() heartbeat = HeartbeatThread(board_id, listen_port, udp_client) leader = AntennyLeader(board_id, listen_port, udp_client, heartbeat) try: leader.start() leader.wait_for_devices([1, 2]) # leader.wait_for_devices([1]) for _ in range(1000): moves = [] for _ in range(10): moves.append((random.randint(5, 175), random.randint(5, 175), 2)) programmed_move_demo( [1, 2], moves,