diff --git a/Sensor/sensor_sentry.py b/Sensor/sensor_sentry.py index 718167f..982a11e 100644 --- a/Sensor/sensor_sentry.py +++ b/Sensor/sensor_sentry.py @@ -4,6 +4,9 @@ import math import time +import matplotlib.pyplot as plt +import numpy as np + from .create_sensors import rev2_dashboard_dump_sensors from .sensor import Sensor from .util import bytes_to_float, bytes_to_int, process_data_bytes @@ -98,6 +101,82 @@ def poll(self, # Resume poll code serial_connection.send(b"\xEF") + def plot(self, + serial_connection: SerialObj, + timeout: int | None = None, + count: int | None = None, + sensors: Optional[List[Sensor]] = None + ) -> None: + if len(self.sensors) == 0: + print("Sentry has no sensors") + return + + sensors_to_plot = sensors if sensors is not None else self.sensors + + # Sensor opcode + serial_connection.send(b"\x03") + + # Poll subcommand code + serial_connection.send(b"\x02") + + num_sensors = len(self.sensors) + serial_connection.send(num_sensors.to_bytes(1, "big")) + + for sensor in self.sensors: + serial_connection.send(sensor.poll_code) + + # Start poll code + serial_connection.send(b"\xF3") + + time_data: List[float] = [] + sensor_data: Dict[Sensor, List[float | int]] = {s: [] for s in sensors_to_plot} + + start = time.time() + poll_count = 0 + while timeout or count: + serial_connection.send(b"\x51") + + data_bytes = serial_connection.read(self.size) + offset = 0 + for sensor in self.sensors: + sensor_data_bytes = data_bytes[offset:offset + sensor.size] + converted = process_data_bytes(sensor_data_bytes, sensor.data_type, sensor.convert_data) + if sensor in sensor_data: + sensor_data[sensor].append(converted if converted is not None else float("nan")) + offset += sensor.size + + elapsed = time.time() - start + time_data.append(elapsed / 60.0) # minutes + + poll_count += 1 + + stop = False + if timeout and elapsed >= timeout: + serial_connection.send(b"\x74") + stop = True + elif count and poll_count >= count: + serial_connection.send(b"\x74") + stop = True + + if stop: + break + + serial_connection.send(b"\x44") + time.sleep(0.2) + serial_connection.send(b"\xEF") + + time_arr = np.array(time_data) + for sensor in sensors_to_plot: + label = f"{sensor.short_name} ({sensor.unit})" + plt.plot(time_arr, np.array(sensor_data[sensor]), label=label) + + plt.title("Sensor Data") + plt.xlabel("Time, min") + plt.ylabel("Measurement Value") + plt.grid() + plt.legend() + plt.show() + def dump(self, serial_connection: SerialObj) -> Dict[Sensor, float | int | None]: # Verify sentry has configured sensors if len(self.sensors) == 0: diff --git a/Testing/Sensor/test_sentry_plot.py b/Testing/Sensor/test_sentry_plot.py new file mode 100644 index 0000000..a5bac8c --- /dev/null +++ b/Testing/Sensor/test_sentry_plot.py @@ -0,0 +1,43 @@ +from BaseController import Firmware, BaseController +from BaseController import create_controllers +from Sensor import Sensor, SensorSentry +from Sensor import conv_functions, create_sensors +from SerialController import SerialSentry, SerialObj, Comport + +def test_sensor(): + # Create the firmware object + appa_firmware = Firmware( + id=b"\x06", + name="APPA", + preset_frame_size=0, + preset_file="" + ) + + # Use premade functions to create the APPA Rev2 Sensors + flight_computer_rev2_sensors = create_sensors.flight_computer_rev2_sensors() + + # Extract the Sensor objects for the Acc{x,y,z} sensors + sensor_sentry = SensorSentry() + + for sensor in flight_computer_rev2_sensors: + if sensor.poll_code in {b"\x00", b"\x01", b"\x02", b"\x03", b"\x04", b"\x05"}: + sensor_sentry.add_sensor(sensor) + + # Create the serial connection + serial_connection = SerialObj() + serial_connection.init_comport("COM6", 921600, 5) + serial_connection.open_comport() + + # Plot all sensors for 5 seconds + print("Sentry Plot (5 seconds):") + sensor_sentry.plot(serial_connection, timeout=5) + + # Plot a subset of sensors for 10 counts + sensors_to_plot = [s for s in sensor_sentry.sensors if s.poll_code in {b"\x00", b"\x01", b"\x02"}] + print("Sentry Plot (count 10, AccX/Y/Z only):") + sensor_sentry.plot(serial_connection, count=10, sensors=sensors_to_plot) + + serial_connection.close_comport() + +if __name__ == "__main__": + test_sensor()