diff --git a/.gitignore b/.gitignore index 51dfa755..a3c7304b 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,4 @@ test_data.txt .ipynb_checkpoints **/.ipynb_checkpoints **/*.pkl +**/*.log diff --git a/.vscode/settings.json b/.vscode/settings.json index 20a70c5a..73040feb 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -18,6 +18,10 @@ "source.organizeImports": "explicit" }, }, + "isort.args": [ + "--profile", + "black" + ], "flake8.args": [ "--max-line-length=120", "--ignore=E203" diff --git a/data_collection_model_and_results/rover/3D_printed_parts/dagu_tx_pwr.stl b/data_collection_model_and_results/rover/3D_printed_parts/dagu_tx_pwr.stl index 0641cbec..91e388a0 100644 Binary files a/data_collection_model_and_results/rover/3D_printed_parts/dagu_tx_pwr.stl and b/data_collection_model_and_results/rover/3D_printed_parts/dagu_tx_pwr.stl differ diff --git a/data_collection_model_and_results/rover/bambu_prints/front_array_gps_pwr_gps_wire.3mf b/data_collection_model_and_results/rover/bambu_prints/front_array_gps_pwr_gps_wire.3mf index cb22d91f..fddac585 100644 Binary files a/data_collection_model_and_results/rover/bambu_prints/front_array_gps_pwr_gps_wire.3mf and b/data_collection_model_and_results/rover/bambu_prints/front_array_gps_pwr_gps_wire.3mf differ diff --git a/requirements.txt b/requirements.txt index 56dac3f5..d759b55c 100644 --- a/requirements.txt +++ b/requirements.txt @@ -65,6 +65,7 @@ pyparsing==3.0.9 pyserial==3.5 pytest==7.4.3 python-dateutil==2.8.2 +PyYAML==6.0.1 pyzmq==25.1.2 requests==2.31.0 scikit-image==0.22.0 diff --git a/spf/grbl_sdr_collect.py b/spf/grbl_sdr_collect_v1.py similarity index 100% rename from spf/grbl_sdr_collect.py rename to spf/grbl_sdr_collect_v1.py diff --git a/spf/grbl_sdr_collect_v2.py b/spf/grbl_sdr_collect_v2.py new file mode 100644 index 00000000..9ab9bb5f --- /dev/null +++ b/spf/grbl_sdr_collect_v2.py @@ -0,0 +1,320 @@ +import argparse +import faulthandler +import json +import logging +import signal +import threading +import time +from dataclasses import dataclass +from datetime import datetime + +import numpy as np +import yaml +from grbl.grbl_interactive import GRBLManager +from tqdm import tqdm + +from spf.rf import beamformer +from spf.sdrpluto.sdr_controller import ( + EmitterConfig, + ReceiverConfig, + get_avg_phase, + get_pplus, + setup_rxtx, + setup_rxtx_and_phase_calibration, + shutdown_radios, +) + +faulthandler.enable() + +run_collection = True + + +@dataclass +class DataSnapshot: + timestamp: float + rx_theta_in_pis: float + rx_center_pos: np.array + rx_spacing: float + avg_phase_diff: float + beam_sds: np.array + + +def prepare_record_entry(ds: DataSnapshot, rx_pos: np.array, tx_pos: np.array): + # t,rx,ry,rtheta,rspacing,avgphase,sds + return np.hstack( + [ + ds.timestamp, # 1 + tx_pos, # 2 + rx_pos, # 2 + ds.rx_theta_in_pis, # 1 + ds.rx_spacing, # 1 + ds.avg_phase_diff, # 2 + ds.beam_sds, # 65 + ] + ) + + +def signal_handler(sig, frame): + logging.info("Ctrl-c issued -> SHUT IT DOWN!") + global run_collection + run_collection = False + shutdown_radios() + + +signal.signal(signal.SIGINT, signal_handler) + + +class ThreadedRX: + def __init__(self, pplus, time_offset): + self.pplus = pplus + self.read_lock = threading.Lock() + self.ready_lock = threading.Lock() + self.ready_lock.acquire() + self.run = False + self.time_offset = time_offset + + def start_read_thread(self): + self.t = threading.Thread(target=self.read_forever) + self.run = True + self.t.start() + + def read_forever(self): + logging.info(f"{str(self.pplus.rx_config.uri)} PPlus read_forever()") + while self.run: + if self.read_lock.acquire(blocking=True, timeout=0.5): + # got the semaphore, read some data! + tries = 0 + try: + signal_matrix = self.pplus.sdr.rx() + except Exception as e: + logging.error( + f"Failed to receive RX data! removing file : retry {tries}", + e, + ) + time.sleep(0.1) + tries += 1 + if tries > 10: + logging.error("GIVE UP") + return + + # process the data + signal_matrix[1] *= np.exp(1j * self.pplus.phase_calibration) + current_time = time.time() - self.time_offset # timestamp + _, beam_sds, _ = beamformer( + self.pplus.rx_config.rx_pos, + signal_matrix, + self.pplus.rx_config.intermediate, + ) + + avg_phase_diff = get_avg_phase(signal_matrix) + + self.data = DataSnapshot( + timestamp=current_time, + rx_center_pos=self.pplus.rx_config.rx_spacing, + rx_theta_in_pis=self.pplus.rx_config.rx_theta_in_pis, + rx_spacing=self.pplus.rx_config.rx_spacing, + beam_sds=beam_sds, + avg_phase_diff=avg_phase_diff, + ) + + try: + self.ready_lock.release() # tell the parent we are ready to provide + except Exception as e: + logging.error(f"Thread encountered an issue exiting {str(e)}") + self.run = False + # logging.info(f"{self.pplus.rx_config.uri} READY") + + logging.info(f"{str(self.pplus.rx_config.uri)} PPlus read_forever() exit!") + + +def bounce_grbl(gm): + direction = None + while gm.collect: + logging.info("TRY TO BOUNCE") + try: + direction = gm.bounce(100, direction=direction) + except Exception as e: + logging.error(e) + logging.info("TRY TO BOUNCE RET") + time.sleep(10) # cool off the motor + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "-c", + "--yaml-config", + type=str, + help="YAML config file", + required=True, + ) + parser.add_argument( + "-l", + "--logging-level", + type=str, + help="Logging level", + default="INFO", + required=False, + ) + parser.add_argument( + "-s", + "--grbl-serial", + type=str, + help="GRBL serial dev", + default=None, + required=False, + ) + args = parser.parse_args() + + # setup logging + start_logging_at = datetime.now().strftime("%Y_%m_%d_%H_%M_%S") + logging.basicConfig( + handlers=[ + logging.FileHandler(f"{start_logging_at}.log"), + logging.StreamHandler(), + ], + format="%(asctime)s:%(levelname)s:%(message)s", + level=getattr(logging, args.logging_level.upper(), None), + ) + + # read YAML + with open(args.yaml_config, "r") as stream: + yaml_config = yaml.safe_load(stream) + + logging.info(json.dumps(yaml_config, sort_keys=True, indent=4)) + + # lets open all the radios + radio_uris = ["ip:%s" % yaml_config["emitter"]["receiver-ip"]] + for receiver in yaml_config["receivers"]: + radio_uris.append("ip:%s" % receiver["receiver-ip"]) + for radio_uri in radio_uris: + get_pplus(uri=radio_uri) + + time.sleep(0.1) + + # get radios online + receiver_pplus = [] + pplus_rx, pplus_tx = (None, None) + for receiver in yaml_config["receivers"]: + rx_config = ReceiverConfig( + lo=receiver["f-carrier"], + rf_bandwidth=receiver["bandwidth"], + sample_rate=receiver["f-sampling"], + gains=[receiver["rx-gain"], receiver["rx-gain"]], + gain_control_mode=receiver["rx-gain-mode"], + enabled_channels=[0, 1], + buffer_size=receiver["buffer-size"], + intermediate=receiver["f-intermediate"], + uri="ip:%s" % receiver["receiver-ip"], + rx_spacing=receiver["antenna-spacing-m"], + rx_theta_in_pis=receiver["theta-in-pis"], + motor_channel=receiver["motor_channel"], + ) + tx_config = EmitterConfig( + lo=receiver["f-carrier"], + rf_bandwidth=receiver["bandwidth"], + sample_rate=receiver["f-sampling"], + intermediate=receiver["f-intermediate"], + gains=[-30, -80], + enabled_channels=[0], + cyclic=True, + uri="ip:%s" % receiver["emitter-ip"], + ) + pplus_rx, pplus_tx = setup_rxtx_and_phase_calibration( + rx_config=rx_config, + tx_config=tx_config, + n_calibration_frames=80, + # leave_tx_on=False, + # using_tx_already_on=None, + ) + pplus_rx.record_matrix = np.memmap( + receiver["output-file"], + dtype="float32", + mode="w+", + shape=( + yaml_config["n-records-per-receiver"], + 7 + 2 + 65, + ), # t,tx,ty,rx,ry,rtheta,rspacing / avg1,avg2 / sds + ) + logging.info("RX online!") + receiver_pplus.append(pplus_rx) + + # setup the emitter + target_yaml_config = yaml_config["emitter"] + target_rx_config = ReceiverConfig( + lo=target_yaml_config["f-carrier"], + rf_bandwidth=target_yaml_config["bandwidth"], + sample_rate=target_yaml_config["f-sampling"], + gains=[target_yaml_config["rx-gain"], target_yaml_config["rx-gain"]], + gain_control_mode=target_yaml_config["rx-gain-mode"], + enabled_channels=[0, 1], + buffer_size=target_yaml_config["buffer-size"], + intermediate=target_yaml_config["f-intermediate"], + uri="ip:%s" % target_yaml_config["receiver-ip"], + ) + target_tx_config = EmitterConfig( + lo=target_yaml_config["f-carrier"], + rf_bandwidth=target_yaml_config["bandwidth"], + sample_rate=target_yaml_config["f-sampling"], + intermediate=target_yaml_config["f-intermediate"], + gains=[-30, -80], + enabled_channels=[0], + cyclic=True, + uri="ip:%s" % target_yaml_config["emitter-ip"], + motor_channel=target_yaml_config["motor_channel"], + ) + + setup_rxtx(rx_config=target_rx_config, tx_config=target_tx_config) + + # threadA semaphore to produce fresh data + # threadB semaphore to produce fresh data + # thread to bounce + # + + # setup GRBL + gm = None + if args.grbl_serial is not None: + gm = GRBLManager(args.grbl_serial) + gm_thread = threading.Thread(target=bounce_grbl, args=(gm,)) + gm_thread.start() + + # setup read threads + + time_offset = time.time() + read_threads = [] + for pplus_rx in receiver_pplus: + read_thread = ThreadedRX(pplus_rx, time_offset) + read_thread.start_read_thread() + read_threads.append(read_thread) + + record_index = 0 + for record_index in tqdm(range(yaml_config["n-records-per-receiver"])): + if not run_collection: + logging.info("Breaking man loop early") + break + for read_thread in read_threads: + while run_collection and not read_thread.ready_lock.acquire(timeout=0.5): + pass + ### + # copy the data out + + rx_pos = np.array([0, 0]) + tx_pos = np.array([0, 0]) + if gm is not None: + tx_pos = gm.position["xy"][target_tx_config.motor_channel] + rx_pos = gm.position["xy"][read_thread.pplus.rx_config.motor_channel] + + read_thread.pplus.record_matrix[record_index] = prepare_record_entry( + ds=read_thread.data, rx_pos=rx_pos, tx_pos=tx_pos + ) + ### + read_thread.read_lock.release() + + logging.info("Shuttingdown: sending false to threads") + for read_thread in read_threads: + read_thread.run = False + logging.info("Shuttingdown: start thread join!") + for read_thread in read_threads: + read_thread.t.join() + + logging.info("Shuttingdown: done") diff --git a/spf/rf.py b/spf/rf.py index 9a4e83cb..01ee6ac2 100644 --- a/spf/rf.py +++ b/spf/rf.py @@ -227,6 +227,14 @@ def get_signal_matrix(self, start_time, duration, rx_lo=0): return sample_matrix # ,raw_signal,demod_times,base_time_offsets[0] +""" +Spacing is the full distance between each antenna +This zero centers the array, for two elements we get +spacing*([-0.5, 0.5]) for the two X positions +and 0 on the Y positions +""" + + @functools.lru_cache(maxsize=1024) def linear_receiver_positions(n_elements, spacing): receiver_positions = np.zeros((n_elements, 2)) @@ -235,8 +243,10 @@ def linear_receiver_positions(n_elements, spacing): class ULADetector(Detector): - def __init__(self, sampling_frequency, n_elements, spacing, sigma=0.0): - super().__init__(sampling_frequency, sigma=sigma) + def __init__( + self, sampling_frequency, n_elements, spacing, sigma=0.0, orientation=0.0 + ): + super().__init__(sampling_frequency, sigma=sigma, orientation=orientation) self.set_receiver_positions(linear_receiver_positions(n_elements, spacing)) @@ -247,8 +257,10 @@ def circular_receiver_positions(n_elements, radius): class UCADetector(Detector): - def __init__(self, sampling_frequency, n_elements, radius, sigma=0.0): - super().__init__(sampling_frequency, sigma=sigma) + def __init__( + self, sampling_frequency, n_elements, radius, sigma=0.0, orientation=0.0 + ): + super().__init__(sampling_frequency, sigma=sigma, orientation=orientation) self.set_receiver_positions(circular_receiver_positions(n_elements, radius)) diff --git a/spf/sdrpluto/close_sdr.py b/spf/sdrpluto/close_sdr.py index 8d7272e3..f4be1b38 100644 --- a/spf/sdrpluto/close_sdr.py +++ b/spf/sdrpluto/close_sdr.py @@ -2,11 +2,12 @@ import adi -from spf.sdrpluto.sdr_controller import close_tx - if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("--ip", type=str, help="ip", required=True) args = parser.parse_args() sdr = adi.ad9361(uri="ip:%s" % args.ip) - close_tx(sdr) + sdr.tx_enabled_channels = [] + sdr.tx_hardwaregain_chan0 = -80 + sdr.tx_hardwaregain_chan1 = -80 + sdr.tx_destroy_buffer() diff --git a/spf/sdrpluto/sdr_controller.py b/spf/sdrpluto/sdr_controller.py index 6078c5a8..94125824 100644 --- a/spf/sdrpluto/sdr_controller.py +++ b/spf/sdrpluto/sdr_controller.py @@ -1,4 +1,5 @@ import argparse +import logging import sys import time from math import gcd @@ -8,7 +9,7 @@ import matplotlib.pyplot as plt import numpy as np -from spf.rf import beamformer +from spf.rf import ULADetector, beamformer # TODO close SDR on exit # import signal @@ -21,32 +22,170 @@ c = 3e8 - -class AdiWrap(adi.ad9361): - def __init__(self, uri): - print("%s: OPEN" % uri) - close_tx(adi.ad9361(uri=receiver_uri)) # improves stability - super(AdiWrap, self).__init__(uri) +pplus_online = {} + +run_radios = True + + +def shutdown_radios(): + global run_radios + run_radios = False + + +def get_uri(rx_config=None, tx_config=None, uri=None): + assert rx_config is not None or tx_config is not None or uri is not None + if rx_config is not None and tx_config is not None: + assert rx_config.uri == tx_config.uri + if rx_config is not None: + return rx_config.uri + elif tx_config is not None: + return tx_config.uri + return uri + + +# TODO not thread safe +def get_pplus(rx_config=None, tx_config=None, uri=None): + uri = get_uri(rx_config=rx_config, tx_config=tx_config, uri=uri) + global pplus_online + if uri not in pplus_online: + pplus_online[uri] = PPlus(rx_config=rx_config, tx_config=tx_config, uri=uri) + else: + pplus_online[uri].set_config(rx_config=rx_config, tx_config=tx_config) + logging.info(f"{uri}: get_pplus PlutoPlus") + return pplus_online[uri] + + +class PPlus: + def __init__(self, rx_config=None, tx_config=None, uri=None): + super(PPlus, self).__init__() + self.uri = get_uri(rx_config=rx_config, tx_config=tx_config, uri=uri) + logging.info(f"{self.uri}: Open PlutoPlus") + + # try to fix issue with radios coming online + self.sdr = adi.ad9361(uri=self.uri) + self.close_tx() + # self.sdr = None + time.sleep(0.1) + + # open for real + # self.sdr = adi.ad9361(uri=self.uri) + self.tx_config = None + self.rx_config = None + self.set_config(rx_config=rx_config, tx_config=tx_config) + + if ( + self.tx_config is None and self.rx_config is None + ): # this is a fresh open or reset + self.close_tx() + self.sdr.tx_destroy_buffer() + self.sdr.rx_destroy_buffer() + self.sdr.tx_enabled_channels = [] + + def set_config(self, rx_config=None, tx_config=None): + logging.info(f"{self.uri} RX{str(rx_config)} TX{str(tx_config)})") + # RX should be setup like this + if rx_config is not None: + assert self.rx_config is None + self.rx_config = rx_config + + # TX should be setup like this + if tx_config is not None: + assert self.tx_config is None + self.tx_config = tx_config def close(self): - close_tx(self) - print("%s: CLOSE" % self.uri) + logging.info(f"{self.uri}: Start close PlutoPlus") + self.close_tx() + logging.info(f"{self.uri}: Done close PlutoPlus") def __del__(self): - close_tx(self) - print("%s: DELETE!" % self.uri) - - -""" -Close pluto -""" - - -def close_tx(sdr): - sdr.tx_enabled_channels = [] - sdr.tx_hardwaregain_chan0 = -80 - sdr.tx_hardwaregain_chan1 = -80 - sdr.tx_destroy_buffer() + logging.info(f"{self.uri}: Start delete PlutoPlus") + self.close_tx() + self.sdr.tx_destroy_buffer() + self.sdr.rx_destroy_buffer() + self.sdr.tx_enabled_channels = [] + logging.info(f"{self.uri}: Done delete PlutoPlus") + + """ + Setup the Rx part of the pluto + """ + + def setup_rx(self): + self.sdr.sample_rate = self.rx_config.sample_rate + assert self.sdr.sample_rate == self.rx_config.sample_rate + + self.sdr.rx_rf_bandwidth = self.rx_config.rf_bandwidth + self.sdr.rx_lo = self.rx_config.lo + + # setup the gain mode + self.sdr.rx_hardwaregain_chan0 = self.rx_config.gains[0] + self.sdr.rx_hardwaregain_chan1 = self.rx_config.gains[1] + self.sdr.gain_control_mode = self.rx_config.gain_control_mode + + if self.rx_config.buffer_size is not None: + self.sdr.rx_buffer_size = self.rx_config.buffer_size + # sdr._rxadc.set_kernel_buffers_count( + # 1 + # ) # set buffers to 1 (instead of the default 4) to avoid stale data on Pluto + self.sdr.rx_enabled_channels = self.rx_config.enabled_channels + + """ + Setup the Tx side of the pluto + """ + + def setup_tx(self): + logging.info(f"{self.tx_config.uri}: Setup TX") + self.sdr.tx_destroy_buffer() + self.sdr.tx_cyclic_buffer = self.tx_config.cyclic # this keeps repeating! + + self.sdr.sample_rate = self.tx_config.sample_rate + assert self.sdr.sample_rate == self.tx_config.sample_rate + + self.sdr.tx_rf_bandwidth = self.tx_config.rf_bandwidth + self.sdr.tx_lo = self.tx_config.lo + + # setup the gain mode + self.sdr.tx_hardwaregain_chan0 = self.tx_config.gains[0] + self.sdr.tx_hardwaregain_chan1 = self.tx_config.gains[1] + + if self.tx_config.buffer_size is not None: + self.sdr.tx_buffer_size = self.tx_config.buffer_size + # self.sdr._rxadc.set_kernel_buffers_count( + # 1 + # ) # set buffers to 1 (instead of the default 4) to avoid stale data on Pluto + self.sdr.tx_enabled_channels = self.tx_config.enabled_channels + + """ + Close pluto + """ + + def close_tx(self): + self.sdr.tx_hardwaregain_chan0 = -80 + self.sdr.tx_hardwaregain_chan1 = -80 + self.sdr.tx_enabled_channels = [] + self.sdr.tx_destroy_buffer() + self.tx_config = None + time.sleep(0.1) + + def close_rx(self): + self.rx_config = None + + """ + Given an online SDR receiver check if the max power peak is as expected during calibration + """ + + def check_for_freq_peak(self): + freq = np.fft.fftfreq( + self.rx_config.buffer_size, d=1.0 / self.rx_config.sample_rate + ) + signal_matrix = np.vstack(self.sdr.rx()) + sp = np.fft.fft(signal_matrix[0]) + max_freq = freq[np.abs(np.argmax(sp.real))] + if np.abs(max_freq - self.rx_config.intermediate) < ( + self.rx_config.sample_rate / self.rx_config.buffer_size + 1 + ): + return True + return False class ReceiverConfig: @@ -55,10 +194,15 @@ def __init__( lo: int, rf_bandwidth: int, sample_rate: int, + intermediate: int, + uri: str, buffer_size: Optional[int] = None, gains: list[int] = [-30, -30], gain_control_mode: str = "slow_attack", enabled_channels: list[int] = [0, 1], + rx_spacing=None, + rx_theta_in_pis=0.0, + motor_channel=None, ): self.lo = lo self.rf_bandwidth = rf_bandwidth @@ -67,6 +211,27 @@ def __init__( self.gains = gains self.gain_control_mode = gain_control_mode self.enabled_channels = enabled_channels + self.intermediate = intermediate + self.uri = uri + self.rx_spacing = rx_spacing + self.rx_theta_in_pis = rx_theta_in_pis + self.motor_channel = motor_channel + + if self.rx_spacing is not None: + d = ULADetector( + sampling_frequency=None, + n_elements=2, + spacing=self.rx_spacing, + orientation=self.rx_theta_in_pis * np.pi, + ) + self.rx_pos = d.all_receiver_pos() + logging.info( + f"{self.uri}:RX antenna positions (theta_in_pis:{self.rx_theta_in_pis}):" + ) + logging.info(f"{self.uri}:\tRX[0]:{str(self.rx_pos[0])}") + logging.info(f"{self.uri}:\tRX[1]:{str(self.rx_pos[1])}") + else: + self.rx_pos = None class EmitterConfig: @@ -75,10 +240,13 @@ def __init__( lo: int, rf_bandwidth: int, sample_rate: int, + intermediate: int, + uri: str, buffer_size: Optional[int] = None, gains: list = [-30, -80], enabled_channels: list[int] = [0], cyclic: bool = True, + motor_channel: int = None, ): self.lo = lo self.rf_bandwidth = rf_bandwidth @@ -87,9 +255,12 @@ def __init__( self.gains = gains self.enabled_channels = enabled_channels self.cyclic = cyclic + self.intermediate = intermediate + self.uri = uri + self.motor_channel = motor_channel -def args_to_receiver_config(args): +def args_to_rx_config(args): return ReceiverConfig( lo=args.fc, rf_bandwidth=int(3 * args.fi), @@ -98,10 +269,12 @@ def args_to_receiver_config(args): gain_control_mode=args.rx_mode, enabled_channels=[0, 1], buffer_size=int(args.rx_n), + intermediate=args.fi, + uri="ip:%s" % args.receiver_ip, ) -def args_to_emitter_config(args): +def args_to_tx_config(args): return EmitterConfig( lo=args.fc, rf_bandwidth=int(3 * args.fi), @@ -109,68 +282,15 @@ def args_to_emitter_config(args): gains=[-30, -80], enabled_channels=[0], cyclic=True, + intermediate=args.fi, + uri="ip:%s" % args.emitter_ip, ) -""" -Setup the Rx part of the pluto -""" - - -def setup_rx(sdr, receiver_config): - sdr.sample_rate = receiver_config.sample_rate - assert sdr.sample_rate == receiver_config.sample_rate - - sdr.rx_rf_bandwidth = receiver_config.rf_bandwidth - sdr.rx_lo = receiver_config.lo - - # setup the gain mode - sdr.rx_hardwaregain_chan0 = receiver_config.gains[0] - sdr.rx_hardwaregain_chan1 = receiver_config.gains[1] - sdr.gain_control_mode = receiver_config.gain_control_mode - - if receiver_config.buffer_size is not None: - sdr.rx_buffer_size = receiver_config.buffer_size - # sdr._rxadc.set_kernel_buffers_count( - # 1 - # ) # set buffers to 1 (instead of the default 4) to avoid stale data on Pluto - sdr.rx_enabled_channels = receiver_config.enabled_channels - - # turn off TX - sdr.tx_enabled_channels = [] - sdr.tx_destroy_buffer() - - -""" -Setup the Tx side of the pluto -""" - - -def setup_tx(sdr, emitter_config): - sdr.tx_cyclic_buffer = emitter_config.cyclic # this keeps repeating! - - sdr.sample_rate = emitter_config.sample_rate - assert sdr.sample_rate == emitter_config.sample_rate - - sdr.tx_rf_bandwidth = emitter_config.rf_bandwidth - sdr.tx_lo = emitter_config.lo - - # setup the gain mode - sdr.tx_hardwaregain_chan0 = emitter_config.gains[0] - sdr.tx_hardwaregain_chan1 = emitter_config.gains[1] - - if emitter_config.buffer_size is not None: - sdr.tx_buffer_size = emitter_config.buffer_size - # sdr._rxadc.set_kernel_buffers_count( - # 1 - # ) # set buffers to 1 (instead of the default 4) to avoid stale data on Pluto - sdr.tx_enabled_channels = emitter_config.enabled_channels - - -def make_tone(args): +def make_tone(tx_config: EmitterConfig): # create a buffe for the signal - fc0 = int(args.fi) - fs = int(args.fs) # must be <=30.72 MHz if both channels are enabled + fc0 = tx_config.intermediate + fs = tx_config.sample_rate # must be <=30.72 MHz if both channels are enabled tx_n = int(fs / gcd(fs, fc0)) while tx_n < 1024 * 16: tx_n *= 2 @@ -182,52 +302,47 @@ def make_tone(args): return np.exp(1j * 2 * np.pi * fc0 * t) * (2**14) -""" -Given an online SDR receiver check if the max power peak is as expected during calibration -""" - - -def check_for_freq_peak(sdr, args): - freq = np.fft.fftfreq(args.rx_n, d=1.0 / args.fs) - signal_matrix = np.vstack(sdr.rx()) - sp = np.fft.fft(signal_matrix[0]) - max_freq = freq[np.abs(np.argmax(sp.real))] - if np.abs(max_freq - args.fi) < (args.fs / args.rx_n + 1): - return True - return False - - -def setup_rxtx(receiver_uri, receiver_config, emitter_uri, emitter_config): +def setup_rxtx(rx_config, tx_config, leave_tx_on=False): retries = 0 - while retries < 10: + while run_radios and retries < 10: + logging.info(f"setup_rxtx({rx_config.uri}, {tx_config.uri}) retry {retries}") # sdr_rx = adi.ad9361(uri=receiver_uri) - sdr_rx = AdiWrap(uri=receiver_uri) - setup_rx(sdr_rx, receiver_config) - - sdr_tx = sdr_rx - if receiver_uri != emitter_uri: - # sdr_tx = adi.ad9361(uri=emitter_uri) - sdr_tx = AdiWrap(uri=emitter_uri) - - setup_tx(sdr_tx, emitter_config) + if rx_config.uri == tx_config.uri: + logging.info(f"{rx_config.uri} RX TX are same") + pplus_rx = get_pplus(rx_config=rx_config, tx_config=tx_config) + pplus_tx = pplus_rx + else: + logging.info(f"{rx_config.uri}(RX) TX are different") + pplus_rx = get_pplus(rx_config=rx_config) + logging.info(f"{tx_config.uri} RX (TX) are different") + pplus_tx = get_pplus(tx_config=tx_config) + + pplus_rx.setup_rx() + pplus_tx.setup_tx() + time.sleep(0.1) # start TX - sdr_tx.tx(make_tone(args)) + pplus_tx.sdr.tx(make_tone(tx_config)) + time.sleep(0.1) # get RX and drop it for _ in range(40): - sdr_rx.rx() + pplus_rx.sdr.rx() # test to see what frequency we are seeing - if check_for_freq_peak(sdr_rx, args): - return sdr_rx, sdr_tx + if pplus_rx.check_for_freq_peak(): + if not leave_tx_on: + pplus_tx.close_tx() + return pplus_rx, pplus_tx + pplus_rx.close_rx() + pplus_tx.close_tx() retries += 1 # try to reset - sdr_tx.close() - sdr_rx = None - sdr_tx = None - time.sleep(1) + pplus_tx.close_tx() + pplus_rx = None + pplus_tx = None + time.sleep(0.1) return None, None @@ -238,59 +353,78 @@ def setup_rxtx(receiver_uri, receiver_config, emitter_uri, emitter_config): def setup_rxtx_and_phase_calibration( - receiver_uri, receiver_config, emitter_uri, emitter_config, tolerance=0.01 + rx_config: ReceiverConfig, + tx_config: EmitterConfig, + tolerance=0.01, + n_calibration_frames=800, + leave_tx_on=False, + using_tx_already_on=None, ): - print("%s: Starting inter antenna receiver phase calibration" % receiver_uri) + logging.info(f"{rx_config.uri}: Starting inter antenna receiver phase calibration") # its important to not use the emitter uri when calibrating! - sdr_rx, sdr_tx = setup_rxtx( - receiver_uri=receiver_uri, - receiver_config=receiver_config, - emitter_uri=emitter_uri, - emitter_config=emitter_config, - ) + if using_tx_already_on is not None: + logging.info(f"{rx_config.uri}: TX already on!") + pplus_rx = get_pplus(rx_config=rx_config) + pplus_rx.setup_rx() + pplus_tx = using_tx_already_on + else: + logging.info(f"{rx_config.uri}: TX not on!") + pplus_rx, pplus_tx = setup_rxtx( + rx_config=rx_config, tx_config=tx_config, leave_tx_on=True + ) - if sdr_rx is None: - print("Failed to bring rx tx online") - return None - print("%s: Emitter online verified by %s" % (emitter_uri, receiver_uri)) + if pplus_rx is None: + logging.info(f"{rx_config.uri}: Failed to bring rx tx online") + return None, None + logging.info(f"{tx_config.uri}: TX online verified by RX {rx_config.uri}") # sdr_rx.phase_calibration=0 # return sdr_rx,sdr_tx # get some new data - print( - "%s: Starting phase calibration (using emitter: %s)" - % (receiver_uri, emitter_uri) + logging.info( + f"{rx_config.uri}: Starting phase calibration (using emitter: {tx_config.uri})" ) - for retry in range(20): - n_calibration_frames = 800 + retries = 0 + while run_radios and retries < 20: + logging.info(f"{rx_config.uri} RETRY {retries}") phase_calibrations = np.zeros(n_calibration_frames) - phase_calibrations2 = np.zeros(n_calibration_frames) + phase_calibrations_cm = np.zeros(n_calibration_frames) for idx in range(n_calibration_frames): - signal_matrix = np.vstack(sdr_rx.rx()) + signal_matrix = np.vstack(pplus_rx.sdr.rx()) phase_calibrations[idx] = ( (np.angle(signal_matrix[0]) - np.angle(signal_matrix[1])) % (2 * np.pi) ).mean() # TODO THIS BREAKS if diff is near 2*np.pi... - phase_calibrations2[idx], _ = circular_mean( + phase_calibrations_cm[idx], _ = circular_mean( np.angle(signal_matrix[0]) - np.angle(signal_matrix[1]) ) - print( - "%s: Phase calibration mean (%0.4f) std (%0.4f)" - % (args.receiver_ip, phase_calibrations.mean(), phase_calibrations.std()) + phase_calibration_u = phase_calibrations.mean() + phase_calibration_std = phase_calibrations.std() + logging.info( + f"{rx_config.uri}: Phase calibration mean \ + ({phase_calibration_u:0.4f}) std ({phase_calibration_std:0.4f})" + ) + + # TODO this part should also get replaced by circular mean + phase_calibration_cm_u = circular_mean(phase_calibrations_cm)[0] + phase_calibration_cm_std = phase_calibrations_cm.std() + logging.info( + f"{rx_config.uri}: Phase calibration mean CM \ + ({phase_calibration_cm_u:0.4f}) std ({phase_calibration_cm_std:0.4f})" ) - print(phase_calibrations.mean(), phase_calibrations2.mean()) - if phase_calibrations.std() < tolerance: - sdr_tx.close() - print( - "%s: Final phase calibration (radians) is %0.4f" - % (args.receiver_ip, phase_calibrations.mean()), - "(fraction of 2pi) %0.4f" % (phase_calibrations.mean() / (2 * np.pi)), + + if phase_calibration_std < tolerance: + if not leave_tx_on: + pplus_tx.close() + logging.info( + f"{rx_config.uri}: Final phase calibration (radians) is {phase_calibration_u:0.4f}\ + (fraction of 2pi) {(phase_calibration_u / (2 * np.pi)):0.4f}" ) - sdr_rx.phase_calibration = phase_calibrations.mean() - return sdr_rx, sdr_tx - sdr_tx.close() - print("%s: Phase calibration failed" % args.receiver_ip) - return None + pplus_rx.phase_calibration = phase_calibrations.mean() + return pplus_rx, pplus_tx + pplus_tx.close() + logging.info(f"{rx_config.uri}: Phase calibration failed") + return None, None def circular_mean(angles, trim=50.0): @@ -313,19 +447,20 @@ def get_avg_phase(signal_matrix, trim=0.0): return mean, _mean -def plot_recv_signal(sdr_rx): - pos = np.array([[-0.03, 0], [0.03, 0]]) +def plot_recv_signal(pplus_rx): fig, axs = plt.subplots(2, 4, figsize=(16, 6)) - rx_n = sdr_rx.rx_buffer_size + rx_n = pplus_rx.sdr.rx_buffer_size t = np.arange(rx_n) while True: - signal_matrix = np.vstack(sdr_rx.rx()) - signal_matrix[1] *= np.exp(1j * sdr_rx.phase_calibration) + signal_matrix = np.vstack(pplus_rx.sdr.rx()) + signal_matrix[1] *= np.exp(1j * pplus_rx.phase_calibration) - beam_thetas, beam_sds, beam_steer = beamformer(pos, signal_matrix, args.fc) + beam_thetas, beam_sds, _ = beamformer( + pplus_rx.rx_config.rx_pos, signal_matrix, args.fc + ) - freq = np.fft.fftfreq(t.shape[-1], d=1.0 / sdr_rx.sample_rate) + freq = np.fft.fftfreq(t.shape[-1], d=1.0 / pplus_rx.sdr.sample_rate) assert t.shape[-1] == rx_n for idx in [0, 1]: axs[idx][0].clear() @@ -355,12 +490,10 @@ def plot_recv_signal(sdr_rx): axs[idx][0].set_title("Real signal recv (%d)" % idx) axs[idx][1].set_title("Power recv (%d)" % idx) - # print("MAXFREQ",freq[np.abs(np.argmax(sp.real))]) diff = (np.angle(signal_matrix[0]) - np.angle(signal_matrix[1])) % (2 * np.pi) axs[0][3].clear() axs[0][3].scatter(t, diff, s=1) mean, _mean = circular_mean(diff) - # print(mean,_mean) axs[0][3].axhline(y=mean, color="black", label="circular mean") axs[0][3].axhline(y=_mean, color="red", label="trimmed circular mean") axs[0][3].set_ylim([0, 2 * np.pi]) @@ -446,39 +579,36 @@ def plot_recv_signal(sdr_rx): if args.mode == "rxcal": # if we use weaker tx gain then the noise in phase calibration goes up - emitter_config = args_to_emitter_config(args) - emitter_config.gains = [-30, -80] - - sdr_rx, sdr_tx = setup_rxtx_and_phase_calibration( - receiver_uri=receiver_uri, - receiver_config=args_to_receiver_config(args), - emitter_uri=emitter_uri, - emitter_config=emitter_config, + tx_config = args_to_tx_config(args) + tx_config.gains = [-30, -80] + + pplus_rx, pplus_tx = setup_rxtx_and_phase_calibration( + rx_config=args_to_rx_config(args), + tx_config=tx_config, ) - if sdr_rx is None: - print("Failed phase calibration, exiting") + if pplus_rx is None: + logging.error("Failed phase calibration, exiting") sys.exit(1) - plot_recv_signal(sdr_rx) + plot_recv_signal(pplus_rx) elif args.mode == "rx": # sdr_rx = adi.ad9361(uri=receiver_uri) - sdr_rx = AdiWrap(uri=receiver_uri) - receiver_config = args_to_receiver_config(args) - setup_rx(sdr_rx, receiver_config) - sdr_rx.phase_calibration = args.cal0 - plot_recv_signal(sdr_rx) + pplus_rx = get_pplus(rx_config=args_to_rx_config(args)) + pplus_rx.setup_rx() + pplus_rx.phase_calibration = args.cal0 + + plot_recv_signal(pplus_rx) elif args.mode == "tx": - sdr_rx, sdr_tx = setup_rxtx( - receiver_uri=emitter_uri, - receiver_config=args_to_receiver_config(args), - emitter_uri=emitter_uri, - emitter_config=args_to_emitter_config(args), + pplus_rx, pplus_tx = setup_rxtx( + rx_config=args_to_rx_config(args), + tx_config=args_to_tx_config(args), + leave_tx_on=True, ) - if sdr_rx is None: - print("Failed to bring emitter online") + if pplus_rx is None: + logging.error("Failed to bring emitter online") sys.exit(1) - print("%s: Emitter online verified by %s" % (emitter_uri, receiver_uri)) + logging.info(f"{emitter_uri}: Emitter online verified by {receiver_uri}") # apply the previous calibration time.sleep(600) diff --git a/spf/wall_array_v2.yaml b/spf/wall_array_v2.yaml new file mode 100644 index 00000000..d13e954a --- /dev/null +++ b/spf/wall_array_v2.yaml @@ -0,0 +1,58 @@ +# The ip of the emitter +# When the emitter is brought online it is verified +# by a receiver that it actually is broadcasting +emitter: + receiver-ip: 192.168.1.15 + emitter-ip: 192.168.1.15 + tx-gain: -3 + rx-gain-mode: fast_attack + rx-gain: -3 + buffer-size: 4096 # 2**12 + f-intermediate: 100000 #1.0e5 + f-carrier: 2500000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 1 + +# Two receivers each with two antennas +# When a receiver is brought online it performs +# phase calibration using an emitter equidistant from +# both receiver antenna +# The orientation of the receiver is described in +# multiples of pi +receivers: + - receiver-ip: 192.168.1.17 + emitter-ip: 192.168.1.17 + theta-in-pis: 0.25 + antenna-spacing-m: 0.065 # 62.5 mm -> 6.5cm -> 0.065m + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-gain: -3 + buffer-size: 4096 # 2**12 + output-file: recieverA.npy + f-intermediate: 100000 #1.0e5 + f-carrier: 2500000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + - receiver-ip: 192.168.1.18 + emitter-ip: 192.168.1.17 + theta-in-pis: -0.25 + antenna-spacing-m: 0.065 # 62.5 mm -> 6.5cm -> 0.065m + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-gain: -3 + buffer-size: 4096 # 2**12 + output-file: recieverB.npy + f-intermediate: 100000 #1.0e5 + f-carrier: 2500000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + + +n-records-per-receiver: 300000 + +