Add ambient pressure

src/float/float_transceiver/float_transceiver.ino

@@ -170,9 +170,9 @@ void loop() {
 
     int intComponent = pressure;
     int fracComponent = trunc((pressure - intComponent) * 10000);
-    char judgePacketBuffer[25];
+    char judgePacketBuffer[30];
     snprintf(
-      judgePacketBuffer, 25, "%d,%lu,%d.%04d\0", TEAM_NUM, previousPressureReadTime, intComponent,
+      judgePacketBuffer, 30, "ROS:SINGLE:%d:%lu,%d.%04d\0", TEAM_NUM, previousPressureReadTime, intComponent,
       fracComponent);
     Serial.println(judgePacketBuffer);
 

src/float/surface_transceiver/surface_transceiver.ino

@@ -120,11 +120,18 @@ bool receivePacket() {
   if (len < MAX_RESPONSE_LEN) {
     // This packet is probably an ACK/NACK, but could be a simple packet for judges
     byteBuffer[len] = '\0';
-    serialPrintf("Got pkt w/len %d, str '%s' & vals: ", len, reinterpret_cast<char*>(byteBuffer));
-    for (int i = 0; i < len; i++) {
-      serialPrintf("%d, ", byteBuffer[i]);
+    char *charBuffer = reinterpret_cast<char*>(byteBuffer);
+    // If this is a "single" judge/calibration packet
+    if (strncmp(byteBuffer, "ROS:SINGLE:", 11) == 0) {
+      Serial.println(charBuffer);
+    }
+    else {
+      serialPrintf("Got pkt w/len %d, str '%s' & vals: ", len, charBuffer);
+      for (int i = 0; i < len; i++) {
+        serialPrintf("%d, ", byteBuffer[i]);
+      }
+      Serial.println();
     }
-    Serial.println();
 
     bool isACK = (strncmp(byteBuffer, "ACK", 3) == 0);
     bool isNACK = (strncmp(byteBuffer, "NACK", 4) == 0);

src/surface/transceiver/test/test_parser.py

@@ -1,3 +1,5 @@
+from queue import Queue
+
 import pytest
 from transceiver.serial_reader import SerialReader
 
@@ -10,8 +12,15 @@
 HEADER_TWO_ELEMENTS = "ROS:11,1:313901,988.53;314843,988.57;315785,988.99;316727,991.56;317669,996.21;318611,1002.36;319553,1010.36;320495,1021.11;321437,1034.42;322379,1050.23;323321,1051.86;324263,1053.20;325206,1053.32;326146,1053.46;327088,1053.52;328030,1053.58;328972,1053.61;329914,1053.64;330856,1053.61;331798,1053.60;332740,1053.65;333682,1053.58;334624,1053.53;335566,1053.52;336508,1053.39;337453,1053.41;338395,1053.46;339337,1053.37;340279,1053.42;341221,1053.49;342163,1053.54"  # noqa: E501
 
 
-def test_parser() -> None:
-    msg = SerialReader._message_parser(PACKET)
+ROS_SINGLE_ONE = "ROS:SINGLE:25:5552,992.4500"
+ROS_SINGLE_TWO = "ROS:SINGLE:25:11071,994.4299"
+ROS_SINGLE_THREE = "ROS:SINGLE:25:16592,992.9600"
+ROS_SINGLE_FOUR = "ROS:SINGLE:25:22112,993.3699"
+ROS_SINGLE_FIVE = "ROS:SINGLE:25:27631,993.2600"
+
+
+def test_message_parser() -> None:
+    msg = SerialReader()._message_parser(PACKET)
 
     assert msg == FloatData(
         team_number=11,
@@ -22,7 +31,46 @@ def test_parser() -> None:
     )
 
     with pytest.raises(ValueError, match="Packet expected 3 sections, found 2 sections"):
-        SerialReader._message_parser(NOT_THREE_SECTIONS)
+        SerialReader()._message_parser(NOT_THREE_SECTIONS)
 
     with pytest.raises(ValueError, match="Packet header length of 3 expected found 2 instead"):
-        SerialReader._message_parser(HEADER_TWO_ELEMENTS)
+        SerialReader()._message_parser(HEADER_TWO_ELEMENTS)
+
+
+def test_handle_ros_single() -> None:
+    s = SerialReader()
+    s._handle_ros_single(ROS_SINGLE_ONE)
+    test_queue: Queue[float] = Queue(5)
+    test_queue.put(992.4500)
+
+    assert s.surface_pressures == test_queue
+    assert s.surface_pressure == 992.4500
+
+    s._handle_ros_single(ROS_SINGLE_TWO)
+    test_queue.put(994.4299)
+
+    assert s.surface_pressures == test_queue
+    assert s.surface_pressures == 992.43995
+
+    s._handle_ros_single(ROS_SINGLE_THREE)
+    test_queue.put(992.9600)
+
+    assert s.surface_pressures == test_queue
+    assert s.surface_pressures == 992.6133
+
+    s._handle_ros_single(ROS_SINGLE_FOUR)
+    test_queue.put(993.3699)
+
+    assert s.surface_pressures == test_queue
+    assert s.surface_pressures == 992.80245
+
+    s._handle_ros_single(ROS_SINGLE_FIVE)
+    test_queue.put(993.26)
+
+    assert s.surface_pressures == test_queue
+    assert s.surface_pressures == 992.89396
+
+    # Test no more get added
+    s._handle_ros_single(ROS_SINGLE_FIVE)
+    assert s.surface_pressures == test_queue
+

src/surface/transceiver/transceiver/serial_reader.py

@@ -1,6 +1,6 @@
 import time
 from threading import Thread
-
+from queue import Queue
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSPresetProfiles
@@ -12,6 +12,9 @@
 SECONDS_TO_MINUTES = 1/60
 MBAR_TO_METER_OF_HEAD = 0.010199773339984
 
+
+AMBIENT_PRESSURE_DEFAULT = 1013.25  # in (mbar)
+
 # AKA the length of the float in (m)
 FLOAT_CONVERSION_FACTOR = 0.635
 
@@ -39,6 +42,10 @@ def __init__(self) -> None:
 
         self.serial = Serial("/dev/serial/by-id/usb-Adafruit_Feather_32u4-if00", 115200)
 
+        self.surface_pressure = AMBIENT_PRESSURE_DEFAULT
+
+        self.surface_pressures: Queue[float] = Queue(5)
+
         Thread(target=self.read_serial, daemon=True,
                name="Serial Reader").start()
 
@@ -63,14 +70,34 @@ def ros_publish(self, packet: str) -> None:
             return
 
         try:
-            msg = SerialReader._message_parser(packet)
+            if SerialReader.is_ros_single_message(packet):
+                self._handle_ros_single(packet)
+            else:
+                msg = self._message_parser(packet)
+                self.data_publisher.publish(msg)
         except Exception as e:
             self.get_logger().error(f"Error {e} caught dropping packet")
-            return
-        self.data_publisher.publish(msg)
 
     @staticmethod
-    def _message_parser(packet: str) -> FloatData:
+    def is_ros_single_message(packet: str) -> bool:
+        ros_single = ROS_PACKET + "SINGLE"
+        return packet[:len(ros_single)] != ros_single
+
+    def _handle_ros_single(self, packet: str) -> None:
+        if self.surface_pressures.full():
+            return
+
+        packet_sections = packet.split(SECTION_SEPARATOR)
+        data = packet_sections[3]
+        pressure = float(data.split(COMMA_SEPARATOR)[1])
+
+        self.surface_pressures.put(pressure)
+
+        q = self.surface_pressures.queue
+        avg_pressure = sum(q) / len(q)
+        self.surface_pressure = avg_pressure
+
+    def _message_parser(self, packet: str) -> FloatData:
         msg = FloatData()
 
         packet_sections = packet.split(SECTION_SEPARATOR)
@@ -102,7 +129,7 @@ def _message_parser(packet: str) -> FloatData:
             time_data_list.append(int(time_reading) * MILLISECONDS_TO_SECONDS * SECONDS_TO_MINUTES)
 
             # Starts out as float
-            depth_data_list.append(float(depth_reading) * MBAR_TO_METER_OF_HEAD)
+            depth_data_list.append((float(depth_reading) - self.surface_pressure) * MBAR_TO_METER_OF_HEAD)
         msg.time_data = time_data_list
         msg.depth_data = depth_data_list