def __init__(self): self._gps_uart = machine.UART(1, 9600) self.cfg = ConfigRepository() self._gps_uart.init(tx=self.cfg.get("gps_uart_tx"), rx=self.cfg.get("gps_uart_rx")) self._gps = MicropyGPS() self._model = None
def __init__(self, antenny_configfile = raspi1): config = ConfigRepository(antenny_configfile) while True: try: self.antenny = api_factory(config) break except ValueError as e: # if 'I2C' in e.args[0]: # if '0x28' in e.args[0] or '0x29' in e.args[0]: # print('Warning: bno055 imu not found.', e.args[0]) # newconfig = antenny_configfile + '_nobno055.json' # print('Reconfiguring without imu as', newconfig) # config.new(newconfig) # config.set('use_imu', False) # continue raise e if self.antenny.is_safemode(): raise Exception('Failed to initialise motor driver') self.controllers = [self.antenny.antenna.elevation, self.antenny.antenna.azimuth] self.min = [] self.max = [] self.last = [] for controller in self.controllers: self.min.append(controller.motor.min_duty) self.max.append(controller.motor.max_duty) self.last.append(controller.get_duty()) self.min = np.array(self.min) self.max = np.array(self.max) self.last = np.array(self.last)
def __init__( self, connection_retries: int = 5, ): """Initialize interfaces and attempt connection.""" self.sta_if = network.WLAN(network.STA_IF) self.ap = network.WLAN(network.AP_IF) self.mode = None self.num_retries = connection_retries self.cfg = ConfigRepository() with open('wifi_config.json', 'r') as f: wifi_dict = ujson.load(f) self.ssid = wifi_dict['ssid'] self.password = wifi_dict['key'] self.do_connect()
def __init__( self, imu: ImuController, motor_controller, ): self.antenna_imu = imu self.imu_lock = _thread.allocate_lock() self.motor_controller = motor_controller self.cfg = ConfigRepository() self._el_moving = False self._az_moving = False self._pinned_mode = False # TODO: these all need to be split into their respective heading control class self._elevation_servo_idx = self.cfg.get("elevation_servo_index") self._azimuth_servo_idx = self.cfg.get("azimuth_servo_index") self.get_heading() self._elevation_target = self._el_last = self._heading.elevation self._azimuth_target = self._az_last = self._heading.azimuth self._actual_elevation = 90.0 self._actual_azimuth = 90.0 self.do_imu_calibration() self._el_max_rate = self.cfg.get("elevation_max_rate") self._az_max_rate = self.cfg.get("azimuth_max_rate") self._calibrated_elevation_offset = None self._calibrated_azimuth_offset = None self._heading = None self._pinned_heading = None self._pinned_servo_pos = None self._orientation_updates = True self._motion_control = True self._orientation_thread = _thread.start_new_thread( self.update_orientation, ()) self._move_thread = _thread.start_new_thread(self.move_loop, ()) time.sleep(6) self.motor_controller.set_position(self._elevation_servo_idx, 90) time.sleep(0.1) self.motor_controller.set_position(self._azimuth_servo_idx, 90) time.sleep(0.1)
def __init__(self): self.cfg = ConfigRepository() self._gps = BasicGPSController() self.imu = Bno055ImuController(machine.I2C( 1, scl=machine.Pin(self.cfg.get("i2c_bno_scl"), Pin.OUT, Pin.PULL_DOWN), sda=machine.Pin(self.cfg.get("i2c_bno_sda"), Pin.OUT, Pin.PULL_DOWN), ), sign=(0, 0, 0)) self._sender = UDPTelemetrySender(self._gps, self.imu) self.antenna = AntennaController( self.imu, Pca9685Controller(machine.I2C( 0, scl=Pin(self.cfg.get("i2c_servo_scl"), Pin.OUT, Pin.PULL_DOWN), sda=Pin(self.cfg.get("i2c_servo_sda"), Pin.OUT, Pin.PULL_DOWN), ), address=self.cfg.get("i2c_servo_address"), min_us=500, max_us=2500, degrees=180)) try: self._i2c_screen = machine.I2C( -1, scl=machine.Pin(self.cfg.get("i2c_screen_scl"), Pin.OUT, Pin.PULL_DOWN), sda=machine.Pin(self.cfg.get("i2c_screen_sda"), Pin.OUT, Pin.PULL_DOWN), ) # on [60] ssd1306 self._screen = Ssd1306ScreenController(self._i2c_screen, width=128, height=32) self._screen_thread = _thread.start_new_thread( self.display_status, ()) except: self._screen = None self._screen_thread = None self._sender.start() self._gps_thread = _thread.start_new_thread(self._gps.run, ())
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
class BasicGPSController(GPSController): def __init__(self): self._gps_uart = machine.UART(1, 9600) self.cfg = ConfigRepository() self._gps_uart.init(tx=self.cfg.get("gps_uart_tx"), rx=self.cfg.get("gps_uart_rx")) self._gps = MicropyGPS() self._model = None def run(self): # TODO: Why do we need to constantly update the GPS values here? while True: try: self._update_gps() except Exception as e: logging.info(e) time.sleep(1) def get_status(self) -> GPSStatus: return self._model def _update_gps(self): g_sentence = self._gps_uart.readline() while g_sentence: g_sentence = g_sentence.decode("ascii") for g_word in g_sentence: self._gps.update(g_word) self._model = GPSStatus( self._gps.valid, self._gps.latitude, self._gps.longitude, self._gps.altitude, self._gps.speed, self._gps.course, self._gps.timestamp, ) g_sentence = self._gps_uart.readline()
class AntKontrol: """Controller for Nyansat setup: integrated servo controls, IMU usage Default components: - BNO055 Absolute orientation sensor - 16-channel pwm breakout servo controller """ def __init__(self): self.cfg = ConfigRepository() self._gps = BasicGPSController() self.imu = Bno055ImuController(machine.I2C( 1, scl=machine.Pin(self.cfg.get("i2c_bno_scl"), Pin.OUT, Pin.PULL_DOWN), sda=machine.Pin(self.cfg.get("i2c_bno_sda"), Pin.OUT, Pin.PULL_DOWN), ), sign=(0, 0, 0)) self._sender = UDPTelemetrySender(self._gps, self.imu) self.antenna = AntennaController( self.imu, Pca9685Controller(machine.I2C( 0, scl=Pin(self.cfg.get("i2c_servo_scl"), Pin.OUT, Pin.PULL_DOWN), sda=Pin(self.cfg.get("i2c_servo_sda"), Pin.OUT, Pin.PULL_DOWN), ), address=self.cfg.get("i2c_servo_address"), min_us=500, max_us=2500, degrees=180)) try: self._i2c_screen = machine.I2C( -1, scl=machine.Pin(self.cfg.get("i2c_screen_scl"), Pin.OUT, Pin.PULL_DOWN), sda=machine.Pin(self.cfg.get("i2c_screen_sda"), Pin.OUT, Pin.PULL_DOWN), ) # on [60] ssd1306 self._screen = Ssd1306ScreenController(self._i2c_screen, width=128, height=32) self._screen_thread = _thread.start_new_thread( self.display_status, ()) except: self._screen = None self._screen_thread = None self._sender.start() self._gps_thread = _thread.start_new_thread(self._gps.run, ()) def display_status(self): while self._screen is not None: try: self._screen.display(self.imu.euler()) pass except Exception as e: logging.info("Status display error: {}".format(str(e))) time.sleep(0.2) def close(self): self.antenna.close()
class AntennaController: """ Control antenna related servos & IMU. """ def __init__( self, imu: ImuController, motor_controller, ): self.antenna_imu = imu self.imu_lock = _thread.allocate_lock() self.motor_controller = motor_controller self.cfg = ConfigRepository() self._el_moving = False self._az_moving = False self._pinned_mode = False # TODO: these all need to be split into their respective heading control class self._elevation_servo_idx = self.cfg.get("elevation_servo_index") self._azimuth_servo_idx = self.cfg.get("azimuth_servo_index") self.get_heading() self._elevation_target = self._el_last = self._heading.elevation self._azimuth_target = self._az_last = self._heading.azimuth self._actual_elevation = 90.0 self._actual_azimuth = 90.0 self.do_imu_calibration() self._el_max_rate = self.cfg.get("elevation_max_rate") self._az_max_rate = self.cfg.get("azimuth_max_rate") self._calibrated_elevation_offset = None self._calibrated_azimuth_offset = None self._heading = None self._pinned_heading = None self._pinned_servo_pos = None self._orientation_updates = True self._motion_control = True self._orientation_thread = _thread.start_new_thread( self.update_orientation, ()) self._move_thread = _thread.start_new_thread(self.move_loop, ()) time.sleep(6) self.motor_controller.set_position(self._elevation_servo_idx, 90) time.sleep(0.1) self.motor_controller.set_position(self._azimuth_servo_idx, 90) time.sleep(0.1) def close(self): self._orientation_updates = False self._motion_control = False def do_imu_calibration(self): heading = self.antenna_imu.heading() self._elevation_target = heading.elevation self._azimuth_target = heading.azimuth self._calibrated_elevation_offset = heading.elevation - self._actual_elevation self._calibrated_azimuth_offset = heading.azimuth - self._actual_azimuth def get_heading(self): with self.imu_lock: self._heading = self.antenna_imu.heading() def pin(self): """ Pin the antenna in place. """ self._pinned_heading = self._heading self._pinned_servo_pos = [self._el_last, self._az_last] self._pinned_mode = True def unpin(self): """ Unpin the antenna from it's heading """ self._pinned_heading = None self._pinned_servo_pos = None self._pinned_mode = False def do_move_mode(self): """ Perform a smooth move to the target elevation & azimuth. """ self.motor_controller.smooth_move(self._elevation_servo_idx, self._elevation_target, 10) self._el_moving = False self.motor_controller.smooth_move(self._azimuth_servo_idx, self._azimuth_target, 10) self._az_moving = False def do_pin_mode(self): """ Pin the antenna heading in place, moving for any deltas sensed by the IMU """ delta_x = self._pinned_heading.elevation - self._heading.elevation delta_y = self._pinned_heading.azimuth - self._heading.azimuth logging.info("d-x {}, d-y {}".format(delta_x, delta_y)) self._elevation_target = self._el_last + delta_x * -1 self._azimuth_target = self._az_last + delta_y self.do_move_mode() def update_orientation(self): """ Acquire a lock on the IMU, update the current heading. """ while self._orientation_updates: try: with self.imu_lock: self._heading = self.antenna_imu.heading() except Exception as e: logging.info("Error in orientation update: {}".format(e)) def move_loop(self): """ Handle motion control, in both fixed heading (pin mode) and single target (move mode). """ while self._motion_control: while self._az_moving or self._el_moving or self._pinned_mode: try: if self._pinned_heading: self.do_pin_mode() else: self.do_move_mode() time.sleep(0.1) except Exception as e: logging.info(e) time.sleep(0.1) def imu_status(self) -> str: return self.antenna_imu.get_status().to_string() def set_elevation_degrees(self, deg): """ Perform a move by setting the target elevation in degrees. """ self._el_moving = True self._elevation_target = deg def set_azimuth_degrees(self, deg): """ Perform a move by setting the target azimuth in degrees. """ self._az_moving = True self._azimuth_target = deg @property def azimuth(self): return self._actual_azimuth @azimuth.setter def azimuth(self, deg): self.set_azimuth_degrees(deg) @property def current_azimuth(self): return self._az_last + self._calibrated_azimuth_offset @current_azimuth.setter def current_azimuth(self, deg): self.set_azimuth_degrees(deg + self._calibrated_azimuth_offset) @property def current_elevation(self): return self._el_last + self._calibrated_elevation_offset @current_elevation.setter def current_elevation(self, deg): self.set_elevation_degrees(deg + self._calibrated_elevation_offset) @property def elevation(self): return self._el_last @elevation.setter def elevation(self, deg): self.set_elevation_degrees(deg) def motor_test(self, index, position): pos = self.motor_controller.smooth_move(index, position, 10) x_angle, y_angle, z_angle = self.antenna_imu.euler() return pos, x_angle, y_angle, z_angle def _measure_az(self, min_angle, max_angle): with self.imu_lock: self.motor_controller.set_position(self._azimuth_servo_idx, min_angle) time.sleep(0.3) a1 = self.antenna_imu.heading().azimuth time.sleep(1) self.motor_controller.set_position(self._azimuth_servo_idx, max_angle) time.sleep(0.3) a2 = self.antenna_imu.heading().azimuth time.sleep(1) return a1, a2 def test_az_axis(self): # measure servo pwm parameters self.current_azimuth = 90 time.sleep(1) self.get_heading() self.current_azimuth = 80 time.sleep(2) self.get_heading() a1 = self._heading.azimuth self.current_azimuth = 100 time.sleep(2) self.get_heading() a2 = self._heading.azimuth # should be 20 degrees. what did we get observed_angle = abs(a1) + a2 angle_factor = observed_angle / 20.0 self.motor_controller._set_degrees( 1, self.motor_controller.degrees(1) * angle_factor) print("Observed angle: {} factor: {}".format(observed_angle, angle_factor)) def test_el_axis(self): # measure servo pwm parameters self.current_azimuth = 90.0 time.sleep(1) self.motor_controller.set_position(0, 90) time.sleep(1) self.get_heading() self.motor_controller.set_position(0, 70) time.sleep(2) a1 = self._heading.elevation self.motor_controller.set_position(0, 110) time.sleep(2) self.get_heading() a2 = self._heading.elevation # should be 20 degrees. what did we get observed_angle = a1 - a2 angle_factor = observed_angle / 4.0 self.motor_controller._set_degrees( 0, self.motor_controller.degrees(0) * angle_factor) print("Observed angle: {} factor: {}".format(observed_angle, angle_factor)) def auto_zero_az(self): # automatically find azimuth offset self.motor_controller.set_position(self._azimuth_servo_idx, 90) self.motor_controller.set_position(self._elevation_servo_idx, 90) time.sleep(1) a1 = 60 a2 = 120 p_center = 100 while abs(p_center) > 0.1: p1, p2 = self._measure_az(a1, a2) p_center = (p1 + p2) / 2 print("a1: {},{} a2: {},{} a-center: {}".format( a1, p1, a2, p2, p_center)) if p_center > 0: a2 = a2 - abs(p_center) else: a1 = a1 + abs(p_center) min_y = 100 min_angle = None cur_angle = avg_angle = (a1 + a2) / 2 - 1.5 while cur_angle < avg_angle + 1.5: self.motor_controller.set_position(self._azimuth_servo_idx, cur_angle) time.sleep(0.2) cur_y = abs(self.antenna_imu.heading().azimuth) if cur_y < min_y: min_y = cur_y min_angle = cur_angle cur_angle += 0.1 time.sleep(1) a_center = min_angle self.motor_controller.set_position(self._azimuth_servo_idx, a_center) print("a-center: {}".format(a_center)) self.get_heading() self._calibrated_azimuth_offset = a_center - 90.0 def auto_calibration(self): # read from BNO055 sensor, move antenna # soft home, etc self.motor_controller.set_position(self._azimuth_servo_idx, 90) self.motor_controller.set_position(self._elevation_servo_idx, 90) time.sleep(1) self.motor_controller.set_position(self._elevation_servo_idx, 180) time.sleep(1) self.motor_controller.set_position(self._elevation_servo_idx, 0) time.sleep(1) self.motor_controller.set_position(self._elevation_servo_idx, 180) time.sleep(1) self.motor_controller.set_position(self._elevation_servo_idx, 0) time.sleep(1) self.motor_controller.set_position(self._azimuth_servo_idx, 180) time.sleep(1) self.motor_controller.set_position(self._azimuth_servo_idx, 0) time.sleep(1) self.motor_controller.set_position(self._azimuth_servo_idx, 180) time.sleep(1) self.motor_controller.set_position(self._azimuth_servo_idx, 0) time.sleep(1) self.motor_controller.set_position(self._azimuth_servo_idx, 90) self.motor_controller.set_position(self._elevation_servo_idx, 90) time.sleep(1) self.motor_controller.set_position(self._elevation_servo_idx, 0) self.get_heading() x1 = self._heading.elevation time.sleep(1) self.motor_controller.set_position(self._elevation_servo_idx, 180) self.get_heading() x2 = self._heading.elevation time.sleep(1) self.motor_controller.set_position(self._azimuth_servo_idx, 0) self.get_heading() y1 = self._heading.azimuth time.sleep(1) self.motor_controller.set_position(self._azimuth_servo_idx, 180) self.get_heading() y2 = self._heading.azimuth return "[{}] - [{}] [{}] - [{}]".format(x1, x2, y1, y2)
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
class Connection(object): AP_SSID = 'ESP32AccessPoint' def __init__( self, connection_retries: int = 5, ): """Initialize interfaces and attempt connection.""" self.sta_if = network.WLAN(network.STA_IF) self.ap = network.WLAN(network.AP_IF) self.mode = None self.num_retries = connection_retries self.cfg = ConfigRepository() with open('wifi_config.json', 'r') as f: wifi_dict = ujson.load(f) self.ssid = wifi_dict['ssid'] self.password = wifi_dict['key'] self.do_connect() def create_ap(self): """Create an access point.""" self.ap.active(True) self.ap.config(essid=Connection.AP_SSID) def do_connect(self): """Try to connect to the SSID, if it fails, create an access point.""" # Attempting STA connection print('connecting to network...') self.sta_if.active(True) self.sta_if.connect(self.ssid, self.password) for retry_count in range(self.num_retries): if self.sta_if.isconnected(): break print("Waiting for connection {}/{}".format( retry_count, self.num_retries)) time.sleep(3) # Success: if self.sta_if.isconnected(): self.mode = STA_MODE print('network config:', self.sta_if.ifconfig()) # Failure, starting access point else: print('Could not connect, creating WiFi access point') self.sta_if.active(False) self.create_ap() self.mode = AP_MODE def ip_display(self): """Show connection status on the SSD1306 display.""" if not self.cfg.get("use_screen"): return i2c = I2C(-1, scl=Pin(self.cfg.get("i2c_screen_scl")), sda=Pin(self.cfg.get("i2c_screen_sda"))) screen = SSD1306_I2C(128, 32, i2c) screen.fill(0) if self.mode == STA_MODE: ip = self.sta_if.ifconfig()[0] screen.text('Normal Mode', 0, 0) screen.text('IP Address:', 0, 8) screen.text(ip, 0, 16) if self.mode == AP_MODE: ip = self.ap.ifconfig()[0] screen.text('Access Point:', 0, 0) screen.text(Connection.AP_SSID, 0, 8) screen.text('IP Address:', 0, 16) screen.text(ip, 0, 24) screen.show() time.sleep(5)
""" if config_version == 1: pins = [4, 14, 15, 16, 17, 19] elif config_version == 2: pins = [23, 25, 26, 27, 2, 4] else: print( "WARNING: antenny board revision == -1, if you have an antenny v1 or v2 board, " "please config.set('antenny_board_version', {1, 2})") pins = [] for pin_idx in pins: pin = machine.Pin(pin_idx, machine.Pin.OUT) pin.value(0) config = ConfigRepository() if not failed_imports: initialize_i2c_bus(config.get('antenny_board_version')) # leave this global so the entire system has access to the AntKontrol instance api = antenny.esp32_antenna_api_factory() else: print( "WARNING: necessary imports failed, please reboot the device after installing the " "necessary dependencies") api = None if config.get('use_webrepl'): webrepl.start() def join_leader(my_id: int):