From 3a40673589c7e64ca0cec7524c127b5aa5fc2857 Mon Sep 17 00:00:00 2001 From: misko Date: Wed, 6 Mar 2024 06:31:27 +0000 Subject: [PATCH] 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 | 12 +++-- spf/mavlink/mavlink_controller.py | 21 ++++++-- .../rover_receiver_config_pi.yaml | 54 +++++++++++++++++++ 3 files changed, 81 insertions(+), 6 deletions(-) create mode 100644 spf/rover_configs/rover_receiver_config_pi.yaml diff --git a/spf/data_collector.py b/spf/data_collector.py index 65b98333..84450e82 100644 --- a/spf/data_collector.py +++ b/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 diff --git a/spf/mavlink/mavlink_controller.py b/spf/mavlink/mavlink_controller.py index 68a3edf8..f6e29777 100644 --- a/spf/mavlink/mavlink_controller.py +++ b/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): diff --git a/spf/rover_configs/rover_receiver_config_pi.yaml b/spf/rover_configs/rover_receiver_config_pi.yaml new file mode 100644 index 00000000..5fda1508 --- /dev/null +++ b/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 \ No newline at end of file