motor channel is optional (wall array) ; added check to see if motors…

… are active; changed timing and logic on distance finder trigger and timeouts

spf/data_collector.py

@@ -72,7 +72,11 @@ def radios_to_online(self):
                 enabled_channels=[0],
                 cyclic=True,
                 uri=target_yaml_config["emitter-uri"],
-                motor_channel=target_yaml_config["motor_channel"],
+                motor_channel=(
+                    target_yaml_config["motor_channel"]
+                    if "motor_channel" in target_yaml_config
+                    else None
+                ),
             )
 
             pplus_rx, _ = setup_rxtx(
@@ -98,7 +102,9 @@ def radios_to_online(self):
                 uri=receiver["receiver-uri"],
                 rx_spacing=receiver["antenna-spacing-m"],
                 rx_theta_in_pis=receiver["theta-in-pis"],
-                motor_channel=receiver["motor_channel"],
+                motor_channel=(
+                    receiver["motor_channel"] if "motor_channel" in receiver else None
+                ),
                 rx_buffers=receiver["rx-buffers"],
             )
             assert "emitter-uri" not in receiver
@@ -121,4 +127,4 @@ def start(self):
         pass
 
     def is_collecting(self):
-        pass
+        return True

spf/mavlink/mavlink_controller.py

@@ -151,6 +151,8 @@ def __init__(
             | EKF_STATUS_STRING_TO_DEC["EKF_POS_HORIZ_REL"]
         )
 
+        self.motor_active = False
+
         self.ekf_healthy = False
 
         self.mav_states = []
@@ -265,13 +267,19 @@ def move_to_point(self, point):
                 f"distance (m) TO TARGET {str(self.distance_to_target(point))}"
             )
             if self.distance_finder is not None:
-                if self.distance_finder.distance < 100:
+                logging.info(f"distance finder {self.distance_finder.distance}")
+                if self.distance_finder.distance < 170:
                     if self.mav_mode == "GUIDED":
                         self.set_mode("HOLD")
-                elif self.mav_mode != "GUIDED":
+                elif self.mav_mode != "GUIDED" and self.mav_mode != "MANUAL":
                     self.set_mode("GUIDED")
+                    time.sleep(0.1)
+                    self.reposition(lat=point[1], long=point[0])
+                elif self.mav_mode == "GUIDED" and not self.motor_active:
+                    self.reposition(lat=point[1], long=point[0])
+                    time.sleep(0.5)
             #
-            time.sleep(0.5)
+            time.sleep(0.1)
         logging.info(f"REACHED TARGET {str(point)} Current {str(self.gps)}")
         return True
 
@@ -573,6 +581,12 @@ def handle_RC_CHANNELS(self, msg):
         if msg.chan9_raw > 1500:
             subprocess.run(["sudo", "shutdown", "0"])
 
+    def handle_SERVO_OUTPUT_RAW(self, msg):
+        if msg.servo1_raw == 1500 and msg.servo3_raw == 1500:
+            self.motor_active = False
+        else:
+            self.motor_active = True
+
     message_handlers = {
         "GLOBAL_POSITION_INT": handle_GLOBAL_POSITION_INT,
         "EKF_STATUS_REPORT": handle_EKF_STATUS_REPORT,
@@ -582,6 +596,7 @@ def handle_RC_CHANNELS(self, msg):
         "SYS_STATUS": handle_SYS_STATUS,
         "STATUSTEXT": handle_STATUSTEXT,
         "RC_CHANNELS": handle_RC_CHANNELS,
+        "SERVO_OUTPUT_RAW": handle_SERVO_OUTPUT_RAW,
     }
 
     def process_message(self, msg):

spf/rover_configs/rover_receiver_config_pi.yaml

@@ -0,0 +1,54 @@
+# 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
+emitter:
+  type: external
+
+distance-finder:
+  trigger: 2
+  echo: 3
+
+#USB PORTS
+#USB  2   (Radio A)  |     USB 1 (Radio B)
+receivers:
+  #radio A
+  - receiver-port: 2
+    theta-in-pis: 0
+    antenna-spacing-m: 0.05075 # 50.75 mm 
+    nelements: 2
+    array-type: linear
+    rx-gain-mode: fast_attack
+    rx-buffers: 4
+    rx-gain: -3
+    buffer-size: 4096 # 2**12
+    f-intermediate: 100000 #1.0e5
+    f-carrier: 2467000000 #2.5e9
+    f-sampling: 16000000 # 16.0e6
+    bandwidth: 300000 #3.0e5
+  #radio B
+  - receiver-port: 1
+    theta-in-pis: 1
+    antenna-spacing-m: 0.05075 # 50.75 mm 
+    nelements: 2
+    array-type: linear
+    rx-gain-mode: fast_attack
+    rx-buffers: 4
+    rx-gain: -3
+    buffer-size: 4096 # 2**12
+    f-intermediate: 100000 #1.0e5
+    f-carrier: 2467000000 #2.5e9
+    f-sampling: 16000000 # 16.0e6
+    bandwidth: 300000 #3.0e5
+
+n-thetas: 65
+n-records-per-receiver: 600000
+width: 4000
+calibration-frames: 800
+routine: null
+
+drone-uri: serial
+
+dry-run: True