implement rolling average derivative pressure sensing

Opentrons · Feb 20, 2024 · ca9b08b · ca9b08b
1 parent 5a833f1
commit ca9b08b
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 7 deletions.
diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp
@@ -20,6 +20,7 @@
 #define PRESSURE_SENSOR_BUFFER_SIZE 3000
 #endif
 
+#define IMPOSSIBLE_PRESSURE -500.0
 namespace sensors {
 
 namespace tasks {
@@ -93,6 +94,8 @@ class MMR920C04 {
                        uint32_t message_index, bool send_threshold = true)
         -> void {
         threshold_pascals = threshold_pa;
+        pressure_running_average = IMPOSSIBLE_PRESSURE;
+        pressure_derivative = 0.0;
         if (send_threshold) {
             auto message = can::messages::SensorThresholdResponse{
                 .message_index = message_index,
@@ -327,6 +330,12 @@ class MMR920C04 {
         auto pressure = mmr920C04::PressureResult::to_pressure(
             _registers.pressure_result.reading);
 
+        if (pressure_running_average == IMPOSSIBLE_PRESSURE) {
+            pressure_running_average = pressure;
+        }
+        auto new_pressure_running_average = (pressure_running_average + pressure) / 2;
+        pressure_derivative = new_pressure_running_average - pressure_running_average;
+        pressure_running_average = new_pressure_running_average;
         if (max_pressure_sync) {
             bool this_tick_over_threshold =
                 std::fabs(pressure - current_pressure_baseline_pa) >=
@@ -355,8 +364,8 @@ class MMR920C04 {
             }
         }
         if (bind_sync) {
-            if (std::fabs(pressure) - std::fabs(current_pressure_baseline_pa) >
-                threshold_pascals) {
+            if ((threshold_pascals > 0 and pressure_derivative > threshold_pascals) or
+                (threshold_pascals <= 0 and pressure_derivative < threshold_pascals)) {
                 hardware.set_sync();
             } else {
                 hardware.reset_sync();
@@ -527,6 +536,9 @@ class MMR920C04 {
     float temperature_running_total = 0;
     uint16_t total_baseline_reads = 1;
 
+    float pressure_running_average = IMPOSSIBLE_PRESSURE;
+    float pressure_derivative = 0.0;
+
     float current_pressure_baseline_pa = 0;
     float current_temperature_baseline = 0;
 

diff --git a/pipettes/core/sensor_tasks.cpp b/pipettes/core/sensor_tasks.cpp
@@ -7,6 +7,9 @@
 static auto tasks = sensor_tasks::Tasks{};
 static auto queue_client = sensor_tasks::QueueClient{};
 static std::array<float, PRESSURE_SENSOR_BUFFER_SIZE> p_buff;
+#if PIPETTE_TYPE_DEFINE != SINGLE_CHANNEL
+static std::array<float, PRESSURE_SENSOR_BUFFER_SIZE> p_buff_secondary;
+#endif
 static auto eeprom_task_builder =
     freertos_task::TaskStarter<512, eeprom::task::EEPromTask>{};
 
@@ -92,7 +95,6 @@ void sensor_tasks::start_tasks(
     queues.tip_notification_queue_rear =
         &tip_notification_task_rear.get_queue();
 }
-
 void sensor_tasks::start_tasks(
     sensor_tasks::CanWriterTask& can_writer,
     sensor_tasks::I2CClient& i2c3_task_client,
@@ -114,12 +116,14 @@ void sensor_tasks::start_tasks(
         PIPETTE_TYPE == EIGHT_CHANNEL ? i2c2_task_client : i2c3_task_client;
     auto& primary_pressure_i2c_poller =
         PIPETTE_TYPE == EIGHT_CHANNEL ? i2c2_poller_client : i2c3_poller_client;
-
+#if PIPETTE_TYPE_DEFINE != SINGLE_CHANNEL
     auto& secondary_pressure_i2c_client =
         PIPETTE_TYPE == EIGHT_CHANNEL ? i2c3_task_client : i2c2_task_client;
+#endif
+#if PIPETTE_TYPE_DEFINE != SINGLE_CHANNEL
     auto& secondary_pressure_i2c_poller =
         PIPETTE_TYPE == EIGHT_CHANNEL ? i2c3_poller_client : i2c2_poller_client;
-
+#endif
     auto& eeprom_i2c_client = PIPETTE_TYPE == NINETY_SIX_CHANNEL
                                   ? i2c3_task_client
                                   : i2c2_task_client;
@@ -134,10 +138,12 @@ void sensor_tasks::start_tasks(
     auto& pressure_sensor_task_rear = pressure_sensor_task_builder_rear.start(
         5, "pressure sensor s0", primary_pressure_i2c_client,
         primary_pressure_i2c_poller, queues, sensor_hardware_primary, p_buff);
+#if PIPETTE_TYPE_DEFINE != SINGLE_CHANNEL
     auto& pressure_sensor_task_front = pressure_sensor_task_builder_front.start(
         5, "pressure sensor s1", secondary_pressure_i2c_client,
         secondary_pressure_i2c_poller, queues, sensor_hardware_secondary,
-        p_buff);
+        p_buff_secondary);
+#endif
     auto& capacitive_sensor_task_rear =
         capacitive_sensor_task_builder_rear.start(
             5, "capacitive sensor s0", i2c3_task_client, i2c3_poller_client,
@@ -148,7 +154,9 @@ void sensor_tasks::start_tasks(
     tasks.eeprom_task = &eeprom_task;
     tasks.environment_sensor_task = &environment_sensor_task;
     tasks.pressure_sensor_task_rear = &pressure_sensor_task_rear;
+#if PIPETTE_TYPE_DEFINE != SINGLE_CHANNEL
     tasks.pressure_sensor_task_front = &pressure_sensor_task_front;
+#endif
     tasks.tip_notification_task_rear = &tip_notification_task_rear;
     tasks.capacitive_sensor_task_rear = &capacitive_sensor_task_rear;
 
@@ -158,8 +166,10 @@ void sensor_tasks::start_tasks(
     queues.capacitive_sensor_queue_rear =
         &capacitive_sensor_task_rear.get_queue();
     queues.pressure_sensor_queue_rear = &pressure_sensor_task_rear.get_queue();
+#if PIPETTE_TYPE_DEFINE != SINGLE_CHANNEL
     queues.pressure_sensor_queue_front =
         &pressure_sensor_task_front.get_queue();
+#endif
     queues.tip_notification_queue_rear =
         &tip_notification_task_rear.get_queue();
 
@@ -191,7 +201,6 @@ void sensor_tasks::start_tasks(
             &tip_notification_task_front.get_queue();
     }
 }
-
 sensor_tasks::QueueClient::QueueClient()
     // This gets overridden in start_tasks, needs to be static here since this
     // is free-store allocated