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,