support for grbl v5

misko · Mar 26, 2024 · b6c45fe · b6c45fe
1 parent ef42674
commit b6c45fe
Show file tree

Hide file tree

Showing 12 changed files with 431 additions and 93 deletions.
diff --git 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
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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