diff --git a/commands b/commands index c5dcece2..c70f6799 100644 --- a/commands +++ b/commands @@ -9,3 +9,12 @@ python 12_task2_model_training.py --dataset ./sessions_sigma0.0_sources3_n8192_t #just fly without radio python mavlink_controller.py --ip 192.168.1.140 --port 14532 --planner circle + + +#!/bin/bash +repo_root=/home/pi/spf +export PYTHONPATH=${repo_root} +test -z "$VIRTUAL_ENV" && source ~/spf-virtualenv/bin/activate +routine=circle +config=${repo_root}/spf/rover_configs/rover_receiver_config_pi.yaml +python3 ${repo_root}/spf/mavlink_radio_collection.py -c ${config} -m /home/pi/device_mapping -r ${routine} -t "RO${rover_id}" -n 2000 --fake-drone \ No newline at end of file diff --git a/spf/data_collector.py b/spf/data_collector.py index fa6cd430..fece387d 100644 --- a/spf/data_collector.py +++ b/spf/data_collector.py @@ -11,7 +11,7 @@ from spf.dataset.rover_idxs import v3rx_column_names from spf.dataset.v4_data import v4rx_2xf64_keys, v4rx_f64_keys, v4rx_new_dataset -from spf.dataset.v5_data import v5rx_new_dataset +from spf.dataset.v5_data import v5rx_2xf64_keys, v5rx_f64_keys, v5rx_new_dataset from spf.dataset.wall_array_v2_idxs import v2_column_names from spf.rf import beamformer_given_steering, precompute_steering_vectors from spf.sdrpluto.sdr_controller import ( @@ -46,10 +46,10 @@ class DataSnapshotV4(DataSnapshotRaw): @dataclass class DataSnapshotV5(DataSnapshotRaw): - gps_timestamp: Optional[float] = None - gps_lat: Optional[float] = None - gps_long: Optional[float] = None - heading: Optional[float] = None + tx_pos_x_mm: Optional[float] = None + tx_pos_y_mm: Optional[float] = None + rx_pos_x_mm: Optional[float] = None + rx_pos_y_mm: Optional[float] = None @dataclass @@ -240,7 +240,7 @@ def __init__(self, **kwargs): class ThreadedRXRawV5(ThreadedRXRaw): def __init__(self, **kwargs): self.snapshot_class = DataSnapshotV5 - super(ThreadedRXRawV4, self).__init__( + super(ThreadedRXRawV5, self).__init__( **kwargs, ) @@ -516,30 +516,23 @@ def setup_record_matrix(self): compressor=None, ) - # def write_to_record_matrix(self, thread_idx, record_idx, data): - # tx_pos = self.position_controller.controller.position["xy"][ - # self.yaml_config["emitter"]["motor_channel"] - # ] - # rx_pos = self.position_controller.controller.position["xy"][ - # self.rx_configs[0].motor_channel - # ] - # current_pos_heading_and_time = ( - # self.position_controller.get_position_bearing_and_time() - # ) - # data.heading = current_pos_heading_and_time["heading"] - # data.gps_long = current_pos_heading_and_time["gps"][0] - # data.gps_lat = current_pos_heading_and_time["gps"][1] - - # z = self.zarr[f"receivers/r{thread_idx}"] - # z.signal_matrix[record_idx] = data.signal_matrix - # for k in v4rx_f64_keys + v4rx_2xf64_keys: - # z[k][record_idx] = getattr(data, k) # getattr(data, k) - - # def write_to_record_matrix(self, thread_idx, record_idx, data): - - # self.record_matrix[thread_idx, record_idx] = prepare_record_entry_v2( - # ds=data, rx_pos=rx_pos, tx_pos=tx_pos - # ) + def write_to_record_matrix(self, thread_idx, record_idx, data): + tx_pos = self.position_controller.controller.position["xy"][ + self.yaml_config["emitter"]["motor_channel"] + ] + rx_pos = self.position_controller.controller.position["xy"][ + self.rx_configs[0].motor_channel + ] + + data.tx_pos_x_mm = tx_pos[0] + data.tx_pos_y_mm = tx_pos[1] + data.rx_pos_x_mm = rx_pos[0] + data.rx_pos_y_mm = rx_pos[1] + + z = self.zarr[f"receivers/r{thread_idx}"] + z.signal_matrix[record_idx] = data.signal_matrix + for k in v5rx_f64_keys + v5rx_2xf64_keys: + z[k][record_idx] = getattr(data, k) # V2 data format diff --git a/spf/dataset/v5_data.py b/spf/dataset/v5_data.py index 0fe4ecbb..7073264f 100644 --- a/spf/dataset/v5_data.py +++ b/spf/dataset/v5_data.py @@ -1,13 +1,13 @@ from spf.utils import zarr_new_dataset v5rx_f64_keys = [ - "timestamp", + "system_timestamp", "tx_pos_x_mm", "tx_pos_y_mm", "rx_pos_x_mm", "rx_pos_y_mm", - "rx_theta", - "rx_spacing_m", + "rx_theta_in_pis", + "rx_spacing", ] diff --git a/spf/grbl_radio_collection.py b/spf/grbl_radio_collection.py index a5d4b763..dfc986d8 100644 --- a/spf/grbl_radio_collection.py +++ b/spf/grbl_radio_collection.py @@ -2,14 +2,14 @@ import json import logging import os -import tempfile import time from datetime import datetime import yaml -from spf.data_collector import GrblDataCollector +from spf.data_collector import GrblDataCollector, GrblDataCollectorRaw from spf.grbl.grbl_interactive import get_default_gm +from spf.utils import DataVersionNotImplemented, filenames_from_time_in_seconds if __name__ == "__main__": parser = argparse.ArgumentParser() @@ -49,9 +49,12 @@ required=False, default=None, ) + parser.add_argument( + "--temp", type=str, help="temp dirname", required=False, default="./temp" + ) args = parser.parse_args() - run_started_at = datetime.now().strftime("%Y_%m_%d_%H_%M_%S") + run_started_at = datetime.now().timestamp() # # read YAML with open(args.yaml_config, "r") as stream: yaml_config = yaml.safe_load(stream) @@ -65,32 +68,24 @@ assert yaml_config["emitter"]["type"] == "sdr" yaml_config["emitter"]["tx-gain"] = args.tx_gain - output_files_prefix = f"grbl_{run_started_at}_nRX{len(yaml_config['receivers'])}_{yaml_config['routine']}" - if args.tag != "": - output_files_prefix += f"_tag_{args.tag}" - if "dry-run" not in yaml_config or args.dry_run: yaml_config["dry-run"] = args.dry_run - # setup filename - tmpdir = tempfile.TemporaryDirectory() - temp_dir_name = tmpdir.name - filename_log = f"{temp_dir_name}/{output_files_prefix}.log.tmp" - filename_yaml = f"{temp_dir_name}/{output_files_prefix}.yaml.tmp" - filename_npy = f"{temp_dir_name}/{output_files_prefix}.npy.tmp" - temp_filenames = [filename_log, filename_yaml, filename_npy] - os.makedirs(args.output_dir, exist_ok=True) - final_filenames = [ - args.output_dir + "/" + os.path.basename(x.replace(".tmp", "")) - for x in temp_filenames - ] + temp_filenames, final_filenames = filenames_from_time_in_seconds( + run_started_at, + args.temp, + yaml_config, + data_version=yaml_config["data-version"], + craft="rover", + tag=args.tag, + ) logger = logging.getLogger(__name__) # setup logging handlers = [ logging.StreamHandler(), - logging.FileHandler(filename_log), + logging.FileHandler(temp_filenames["log"]), ] logging.basicConfig( handlers=handlers, @@ -99,7 +94,7 @@ ) # make a copy of the YAML - with open(filename_yaml, "w") as outfile: + with open(temp_filenames["yaml"], "w") as outfile: yaml.dump(yaml_config, outfile, default_flow_style=False) logging.info(json.dumps(yaml_config, sort_keys=True, indent=4)) @@ -109,15 +104,27 @@ gm.start() logging.info("Starting data collector...") - data_collector = GrblDataCollector( - data_filename=filename_npy, yaml_config=yaml_config, position_controller=gm - ) + if yaml_config["data-version"] == 2: + data_collector = GrblDataCollector( + data_filename=temp_filenames["data"], + yaml_config=yaml_config, + position_controller=gm, + ) + elif yaml_config["data-version"] == 5: + data_collector = GrblDataCollectorRaw( + data_filename=temp_filenames["data"], + yaml_config=yaml_config, + position_controller=gm, + ) + else: + raise DataVersionNotImplemented + data_collector.radios_to_online() # blocking while not gm.has_planner_started_moving(): logging.info(f"waiting for grbl to start moving {time.time()}") time.sleep(5) # easy poll this - logging.info("DRONE IS READY!!! LETS GOOO!!!") + logging.info("GRBL IS READY!!! LETS GOOO!!!") data_collector.start() while data_collector.is_collecting(): diff --git a/spf/mavlink_radio_collection.py b/spf/mavlink_radio_collection.py index 352592b9..19ab1e38 100644 --- a/spf/mavlink_radio_collection.py +++ b/spf/mavlink_radio_collection.py @@ -17,39 +17,7 @@ drone_get_planner, get_ardupilot_serial, ) -from spf.utils import is_pi - - -def filenames_from_time_in_seconds( - time_in_seconds, temp_dir_name, yaml_config, data_version -): - os.makedirs(temp_dir_name, exist_ok=True) - dt = datetime.fromtimestamp(time_in_seconds) - date_str = dt.strftime("%Y_%m_%d_%H_%M_%S") - - output_files_prefix = ( - f"rover_{date_str}_nRX{len(yaml_config['receivers'])}_{yaml_config['routine']}" - ) - if args.tag != "": - output_files_prefix += f"_tag_{args.tag}" - - filename_log = f"{temp_dir_name}/{output_files_prefix}.log.tmp" - filename_yaml = f"{temp_dir_name}/{output_files_prefix}.yaml.tmp" - if data_version == 3: - filename_data = f"{temp_dir_name}/{output_files_prefix}.npy.tmp" - elif data_version == 4: - filename_data = f"{temp_dir_name}/{output_files_prefix}.zarr.tmp" - else: - raise NotImplementedError - temp_filenames = { - "log": filename_log, - "yaml": filename_yaml, - "data": filename_data, - } - final_filenames = {k: v.replace(".tmp", "") for k, v in temp_filenames.items()} - - return temp_filenames, final_filenames - +from spf.utils import DataVersionNotImplemented, filenames_from_time_in_seconds, is_pi if __name__ == "__main__": parser = argparse.ArgumentParser() @@ -144,7 +112,12 @@ def filenames_from_time_in_seconds( # temp_dir_name = tmpdir.name temp_filenames, final_filenames = filenames_from_time_in_seconds( - run_started_at, args.temp, yaml_config, data_version=yaml_config["data-version"] + run_started_at, + args.temp, + yaml_config, + data_version=yaml_config["data-version"], + craft="rover", + tag=args.tag, ) logger = logging.getLogger(__name__) @@ -213,7 +186,7 @@ def filenames_from_time_in_seconds( position_controller=drone, ) else: - raise NotImplementedError + raise DataVersionNotImplemented logging.info("MavRadioCollection: Radios online...") data_collector.radios_to_online() # blocking diff --git a/spf/utils.py b/spf/utils.py index b1cb3673..aa7550d5 100644 --- a/spf/utils.py +++ b/spf/utils.py @@ -1,3 +1,6 @@ +import os +from datetime import datetime + import numpy as np import zarr from numcodecs import Blosc @@ -69,3 +72,36 @@ def zarr_new_dataset( dtype="float64", ) return z + + +class DataVersionNotImplemented(NotImplementedError): + pass + + +def filenames_from_time_in_seconds( + time_in_seconds, temp_dir_name, yaml_config, data_version, tag, craft +): + os.makedirs(temp_dir_name, exist_ok=True) + dt = datetime.fromtimestamp(time_in_seconds) + date_str = dt.strftime("%Y_%m_%d_%H_%M_%S") + + output_files_prefix = f"{craft}_{date_str}_nRX{len(yaml_config['receivers'])}_{yaml_config['routine']}" + if tag != "": + output_files_prefix += f"_tag_{tag}" + + filename_log = f"{temp_dir_name}/{output_files_prefix}.log.tmp" + filename_yaml = f"{temp_dir_name}/{output_files_prefix}.yaml.tmp" + if data_version == (2, 3): + filename_data = f"{temp_dir_name}/{output_files_prefix}.npy.tmp" + elif data_version in (4, 5): + filename_data = f"{temp_dir_name}/{output_files_prefix}.zarr.tmp" + else: + raise NotImplementedError + temp_filenames = { + "log": filename_log, + "yaml": filename_yaml, + "data": filename_data, + } + final_filenames = {k: v.replace(".tmp", "") for k, v in temp_filenames.items()} + + return temp_filenames, final_filenames diff --git a/spf/v5_configs/wall_array_v2.yaml b/spf/v5_configs/wall_array_v2.yaml new file mode 100644 index 00000000..6c330576 --- /dev/null +++ b/spf/v5_configs/wall_array_v2.yaml @@ -0,0 +1,63 @@ +# The ip of the emitter +# When the emitter is brought online it is verified +# by a receiver that it actually is broadcasting +emitter: + type: sdr + receiver-uri: ip:192.168.1.17 + emitter-uri: ip:192.168.1.15 + tx-gain: -10 + 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-uri: ip:192.168.1.17 + theta-in-pis: -0.25 + antenna-spacing-m: 0.05075 # 50.75 mm + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-buffers: 2 + rx-gain: -3 + buffer-size: 524288 + f-intermediate: 100000 #1.0e5 + f-carrier: 2500000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + - receiver-uri: ip:192.168.1.18 + theta-in-pis: 1.25 + antenna-spacing-m: 0.05075 # 50.75 mm + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-buffers: 2 + rx-gain: -3 + buffer-size: 524288 + f-intermediate: 100000 #1.0e5 + f-carrier: 2500000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + + +n-thetas: 65 +n-records-per-receiver: 10000 +width: 4000 +calibration-frames: 10 +routine: null +skip_phase_calibration: true + + +data-version: 5 \ No newline at end of file diff --git a/spf/v5_configs/wall_array_v2_external.yaml b/spf/v5_configs/wall_array_v2_external.yaml new file mode 100644 index 00000000..760cc3c2 --- /dev/null +++ b/spf/v5_configs/wall_array_v2_external.yaml @@ -0,0 +1,53 @@ +# The ip of the emitter +# When the emitter is brought online it is verified +# by a receiver that it actually is broadcasting +emitter: + type: esp32 + 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-uri: ip:192.168.1.17 + theta-in-pis: -0.25 + antenna-spacing-m: 0.05075 # 50.75 mm + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-buffers: 2 + rx-gain: -3 + buffer-size: 524288 + f-intermediate: 100000 #1.0e5 + f-carrier: 2467000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + - receiver-uri: ip:192.168.1.18 + theta-in-pis: 1.25 + antenna-spacing-m: 0.05075 # 50.75 mm + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-buffers: 2 + rx-gain: -3 + buffer-size: 524288 + f-intermediate: 100000 #1.0e5 + f-carrier: 2467000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + + +n-thetas: 65 +n-records-per-receiver: 10000 +width: 4000 +calibration-frames: 10 +routine: null +skip_phase_calibration: true + + +data-version: 5 \ No newline at end of file diff --git a/spf/v5_configs/wall_array_v2_external_fake.yaml b/spf/v5_configs/wall_array_v2_external_fake.yaml new file mode 100644 index 00000000..55c1dfe3 --- /dev/null +++ b/spf/v5_configs/wall_array_v2_external_fake.yaml @@ -0,0 +1,53 @@ +# The ip of the emitter +# When the emitter is brought online it is verified +# by a receiver that it actually is broadcasting +emitter: + type: esp32 + 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-uri: fake:A + theta-in-pis: -0.25 + antenna-spacing-m: 0.05075 # 50.75 mm + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-buffers: 2 + rx-gain: -3 + buffer-size: 524288 + f-intermediate: 100000 #1.0e5 + f-carrier: 2467000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + - receiver-uri: fake:B + theta-in-pis: 1.25 + antenna-spacing-m: 0.05075 # 50.75 mm + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-buffers: 2 + rx-gain: -3 + buffer-size: 524288 + f-intermediate: 100000 #1.0e5 + f-carrier: 2467000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + + +n-thetas: 65 +n-records-per-receiver: 10000 +width: 4000 +calibration-frames: 800 +routine: null +skip_phase_calibration: true + + +data-version: 5 diff --git a/spf/v5_configs/wall_array_v2_external_single_usb.yaml b/spf/v5_configs/wall_array_v2_external_single_usb.yaml new file mode 100644 index 00000000..203a3167 --- /dev/null +++ b/spf/v5_configs/wall_array_v2_external_single_usb.yaml @@ -0,0 +1,37 @@ +# The ip of the emitter +# When the emitter is brought online it is verified +# by a receiver that it actually is broadcasting +emitter: + type: esp32 + 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-uri: usb:1.3.5 + theta-in-pis: -0.25 + antenna-spacing-m: 0.05075 # 50.75 mm + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-buffers: 2 + rx-gain: -3 + buffer-size: 524288 + f-intermediate: 100000 #1.0e5 + f-carrier: 2500000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + +n-thetas: 65 +n-records-per-receiver: 10000 +width: 4000 +calibration-frames: 10 +routine: null +skip_phase_calibration: true + +data-version: 5 \ No newline at end of file diff --git a/spf/v5_configs/wall_array_v2_single.yaml b/spf/v5_configs/wall_array_v2_single.yaml new file mode 100644 index 00000000..97eb1c78 --- /dev/null +++ b/spf/v5_configs/wall_array_v2_single.yaml @@ -0,0 +1,49 @@ +# The ip of the emitter +# When the emitter is brought online it is verified +# by a receiver that it actually is broadcasting +emitter: + type: sdr + receiver-uri: ip:192.168.1.17 + emitter-uri: ip:192.168.1.15 + tx-gain: -10 + 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-uri: ip:192.168.1.17 + emitter-uri: ip:192.168.1.15 + theta-in-pis: -0.25 + antenna-spacing-m: 0.05075 # 50.75 mm + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-buffers: 2 + rx-gain: -3 + buffer-size: 524288 + f-intermediate: 100000 #1.0e5 + f-carrier: 2500000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + +n-thetas: 65 +n-records-per-receiver: 10000 +width: 4000 +calibration-frames: 10 +routine: null +skip_phase_calibration: true + + +data-version: 5 \ No newline at end of file diff --git a/spf/v5_configs/wall_array_v2_txgain_n5.yaml b/spf/v5_configs/wall_array_v2_txgain_n5.yaml new file mode 100644 index 00000000..350600da --- /dev/null +++ b/spf/v5_configs/wall_array_v2_txgain_n5.yaml @@ -0,0 +1,65 @@ +# The ip of the emitter +# When the emitter is brought online it is verified +# by a receiver that it actually is broadcasting +emitter: + type: sdr + receiver-uri: ip:192.168.1.17 + emitter-uri: ip:192.168.1.15 + tx-gain: -10 + 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-uri: ip:192.168.1.17 + emitter-uri: ip:192.168.1.15 + theta-in-pis: -0.25 + antenna-spacing-m: 0.05075 # 50.75 mm + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-buffers: 2 + rx-gain: -3 + buffer-size: 524288 + f-intermediate: 100000 #1.0e5 + f-carrier: 2500000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + - receiver-uri: ip:192.168.1.18 + emitter-uri: ip:192.168.1.15 + theta-in-pis: 1.25 + antenna-spacing-m: 0.05075 # 50.75 mm + nelements: 2 + array-type: linear + rx-gain-mode: fast_attack + rx-buffers: 2 + rx-gain: -3 + buffer-size: 524288 + f-intermediate: 100000 #1.0e5 + f-carrier: 2500000000 #2.5e9 + f-sampling: 16000000 # 16.0e6 + bandwidth: 300000 #3.0e5 + motor_channel: 0 + + +n-thetas: 65 +n-records-per-receiver: 10000 +width: 4000 +calibration-frames: 10 +routine: null +skip_phase_calibration: true + + +data-version: 5 \ No newline at end of file