feat(sensors): Self baselining during sensor moves (#786)

* Use logic or instead of hardcoded enums

* when filling the bufffer auto baseline after the first 10

* the new method that lets us increase senstivity uncovered an issue with setting the sync line this way, it was turning off before the head node could detect it and stop

* few tweaks to the way this works

* only compute the first 10 elements to self-baselien

* don't immediatly turn of the recording when move completes

* save response pressure

* fix the pressure leveling

* rebase fixups

* fix the hardware delay hack and get rid of another instance of it

* send ack after sending buffer

* fixes to the circular buffer

* don't need this and can cause the process to choke

* send the can messages faster

* format

* make the moving baseline log clearer

* use the real world sensor speed

* change auto baseline slightly to ignore the first N samples

* add a new sensor sync value

* don't trigger before autobaseline is finished

* add new binding type for sensors and use the auto-baselineing during sensor moves

* forgot to change an old reference

* format lint

* preserve old behavior

* reduce the complexity in the handle function to satisfy lint

* format

include/bootloader/core/ids.h

@@ -205,6 +205,7 @@ typedef enum {
     can_sensoroutputbinding_sync = 0x1,
     can_sensoroutputbinding_report = 0x2,
     can_sensoroutputbinding_max_threshold_sync = 0x4,
+    can_sensoroutputbinding_auto_baseline_report = 0x8,
 } CANSensorOutputBinding;
 
 /** How a sensor's threshold should be interpreted. */

include/can/core/ids.hpp

@@ -215,6 +215,7 @@ enum class SensorOutputBinding {
     sync = 0x1,
     report = 0x2,
     max_threshold_sync = 0x4,
+    auto_baseline_report = 0x8,
 };
 
 /** How a sensor's threshold should be interpreted. */

include/common/core/hardware_delay.hpp

@@ -1,12 +1,17 @@
 #pragma once
 // Not my favorite way to check this, but if we don't have access
 // to vTaskDelay during host compilation so just dummy the function
+
+#ifdef ENABLE_CROSS_ONLY_HEADERS
+#include "FreeRTOS.h"
+#endif
+
 template <typename T>
 requires std::is_integral_v<T>
 static void vtask_hardware_delay(T ticks) {
-#ifndef INC_TASK_H
-    std::ignore = ticks;
-#else
+#ifdef ENABLE_CROSS_ONLY_HEADERS
     vTaskDelay(ticks);
+#else
+    std::ignore = ticks;
 #endif
 }

include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp

@@ -408,7 +408,12 @@ class MotorInterruptHandler {
                 hardware.get_encoder_pulses();
 #ifdef USE_SENSOR_MOVE
             if (buffered_move.sensor_id != can::ids::SensorId::UNUSED) {
-                auto binding = static_cast<uint8_t>(0x3);  // sync and report
+                auto binding =
+                    static_cast<uint8_t>(can::ids::SensorOutputBinding::sync) |
+                    static_cast<uint8_t>(
+                        can::ids::SensorOutputBinding::report) |
+                    static_cast<uint8_t>(
+                        can::ids::SensorOutputBinding::auto_baseline_report);
                 if (buffered_move.sensor_id == can::ids::SensorId::BOTH) {
                     send_bind_message(buffered_move.sensor_type,
                                       can::ids::SensorId::S0, binding);
@@ -502,21 +507,6 @@ class MotorInterruptHandler {
         tick_count = 0x0;
         stall_handled = false;
         build_and_send_ack(ack_msg_id);
-#ifdef USE_SENSOR_MOVE
-        if (buffered_move.sensor_id != can::ids::SensorId::UNUSED) {
-            auto binding = static_cast<uint8_t>(
-                can::ids::SensorOutputBinding::sync);  // make none?!
-            if (buffered_move.sensor_id == can::ids::SensorId::BOTH) {
-                send_bind_message(buffered_move.sensor_type,
-                                  can::ids::SensorId::S0, binding);
-                send_bind_message(buffered_move.sensor_type,
-                                  can::ids::SensorId::S1, binding);
-            } else {
-                send_bind_message(buffered_move.sensor_type,
-                                  buffered_move.sensor_id, binding);
-            }
-        }
-#endif
         set_buffered_move(MotorMoveMessage{});
         // update the stall check ideal encoder counts based on
         // last known location

include/sensors/core/tasks/capacitive_driver.hpp

@@ -2,25 +2,11 @@
 
 #include <array>
 
-#ifdef ENABLE_CROSS_ONLY_HEADERS
-// TODO(fps 7/12/2023): This is super hacky and I hate throwing #ifdefs
-// in our nicely host-independent code but for now we really just need
-// the vTaskDelay function and hopefully sometime in the near future I
-// can refactor this file with a nice templated sleep function.
-#include "FreeRTOS.h"
-// NOLINTNEXTLINE(cppcoreguidelines-macro-usage)
-#define HACKY_TASK_SLEEP(___timeout___) vTaskDelay(___timeout___)
-
-#else
-
-// NOLINTNEXTLINE(cppcoreguidelines-macro-usage)
-#define HACKY_TASK_SLEEP(___timeout___) (void)(___timeout___)
-#endif
-
 #include "can/core/can_writer_task.hpp"
 #include "can/core/ids.hpp"
 #include "can/core/messages.hpp"
 #include "common/core/bit_utils.hpp"
+#include "common/core/hardware_delay.hpp"
 #include "common/core/logging.h"
 #include "common/core/message_queue.hpp"
 #include "common/core/sensor_buffer.hpp"
@@ -64,11 +50,11 @@ class FDC1004 {
             // holding off for this PR.
 
             // Initial delay to avoid I2C bus traffic.
-            HACKY_TASK_SLEEP(100);
+            vtask_hardware_delay(100);
             update_capacitance_configuration();
             // Second delay to ensure IC is ready to start
             // readings (and also to avoid I2C bus traffic).
-            HACKY_TASK_SLEEP(100);
+            vtask_hardware_delay(100);
             set_sample_rate();
             _initialized = true;
         }
@@ -232,6 +218,9 @@ class FDC1004 {
             }
             (*sensor_buffer).at(i) = 0;
         }
+        can_client.send_can_message(
+            can::ids::NodeId::host,
+            can::messages::Acknowledgment{.message_index = message_index});
 #else
         std::ignore = message_index;
 #endif

include/sensors/core/tasks/pressure_driver.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cmath>
+#include <numeric>
 
 #include "can/core/can_writer_task.hpp"
 #include "can/core/ids.hpp"
@@ -30,6 +31,9 @@ using namespace can::ids;
  * @tparam CanClient
  */
 
+constexpr auto AUTO_BASELINE_START = 10;
+constexpr auto AUTO_BASELINE_END = 20;
+
 template <class I2CQueueWriter, class I2CQueuePoller,
           can::message_writer_task::TaskClient CanClient, class OwnQueue>
 class MMR920 {
@@ -70,9 +74,18 @@ class MMR920 {
         echoing = should_echo;
         if (should_echo) {
             sensor_buffer_index = 0;  // reset buffer index
+            crossed_buffer_index = false;
+            sensor_buffer->fill(0.0);
         }
     }
 
+    void set_auto_baseline_report(bool should_auto) {
+        enable_auto_baseline = should_auto;
+        // Always set this to 0, we want to clear it if disabled and
+        // reset if if we haven't baselined yet
+        current_moving_pressure_baseline_pa = 0.0;
+    }
+
     void set_bind_sync(bool should_bind) {
         bind_sync = should_bind;
         hardware.reset_sync();
@@ -230,12 +243,12 @@ class MMR920 {
     }
 
     auto sensor_buffer_log(float data) -> void {
+        sensor_buffer->at(sensor_buffer_index) = data;
+        sensor_buffer_index++;
         if (sensor_buffer_index == SENSOR_BUFFER_SIZE) {
             sensor_buffer_index = 0;
+            crossed_buffer_index = true;
         }
-
-        sensor_buffer->at(sensor_buffer_index) = data;
-        sensor_buffer_index++;
     }
 
     auto save_temperature(int32_t data) -> bool {
@@ -290,11 +303,21 @@ class MMR920 {
     }
 
     void send_accumulated_sensor_data(uint32_t message_index) {
-        for (int i = 0; i < static_cast<int>(SENSOR_BUFFER_SIZE); i++) {
+        auto start = 0;
+        auto count = sensor_buffer_index;
+        if (crossed_buffer_index) {
+            start = sensor_buffer_index;
+            count = SENSOR_BUFFER_SIZE;
+        }
+
+        can_client.send_can_message(
+            can::ids::NodeId::host,
+            can::messages::Acknowledgment{.message_index = count});
+        for (int i = 0; i < count; i++) {
             // send over buffer and then clear buffer values
             // NOLINTNEXTLINE(div-by-zero)
-            int current_index = (i + sensor_buffer_index) %
-                                static_cast<int>(SENSOR_BUFFER_SIZE);
+            int current_index =
+                (i + start) % static_cast<int>(SENSOR_BUFFER_SIZE);
 
             can_client.send_can_message(
                 can::ids::NodeId::host,
@@ -306,9 +329,56 @@ class MMR920 {
                         (*sensor_buffer).at(current_index))});
             if (i % 10 == 0) {
                 // slow it down so the can buffer doesn't choke
-                vtask_hardware_delay(50);
+                vtask_hardware_delay(20);
+            }
+        }
+        can_client.send_can_message(
+            can::ids::NodeId::host,
+            can::messages::Acknowledgment{.message_index = message_index});
+    }
+
+    auto compute_auto_baseline() -> void {
+        // this is the auto-base lining during a move.  It requires that
+        // a BaselineSensorRequest is sent prior to a move using the
+        // auto baseline. it works by taking several samples
+        // at the beginning of the move but after noise has stopped.
+        // and we haven't crossed the circular buffer barrier yet) it
+        // then takes the average of those samples to create a new
+        // baseline factor
+        current_moving_pressure_baseline_pa =
+            std::accumulate(sensor_buffer->begin() + AUTO_BASELINE_START,
+                            sensor_buffer->begin() + AUTO_BASELINE_END, 0.0) /
+            float(AUTO_BASELINE_END - AUTO_BASELINE_START);
+        for (auto i = sensor_buffer_index - AUTO_BASELINE_END;
+             i < sensor_buffer_index; i++) {
+            // apply the moving baseline to older samples to so that
+            // data is in the same format as later samples, don't apply
+            // the current_pressure_baseline_pa since it has already
+            // been applied
+            sensor_buffer->at(sensor_buffer_index) =
+                sensor_buffer->at(sensor_buffer_index) -
+                current_moving_pressure_baseline_pa;
+        }
+    }
+
+    auto handle_sync_threshold(float pressure) -> void {
+        if (enable_auto_baseline) {
+            if ((sensor_buffer_index > AUTO_BASELINE_END ||
+                 crossed_buffer_index) &&
+                (std::fabs(pressure - current_pressure_baseline_pa -
+                           current_moving_pressure_baseline_pa) >
+                 threshold_pascals)) {
+                hardware.set_sync();
+            } else {
+                hardware.reset_sync();
+            }
+        } else {
+            if (std::fabs(pressure - current_pressure_baseline_pa) >
+                threshold_pascals) {
+                hardware.set_sync();
+            } else {
+                hardware.reset_sync();
             }
-            (*sensor_buffer).at(current_index) = 0;
         }
     }
 
@@ -355,31 +425,43 @@ class MMR920 {
                         .message_index = m.message_index,
                         .severity = can::ids::ErrorSeverity::unrecoverable,
                         .error_code = can::ids::ErrorCode::over_pressure});
-            } else {
+            } else if (!bind_sync) {
+                // if we're not using bind sync turn off the sync line
+                // we don't do this during bind sync because if it's triggering
+                // the sync line on purpose this causes bouncing on the line
+                // that turns off the sync and then immediately turns it back on
+                // and this can cause disrupt the behavior
                 hardware.reset_sync();
             }
         }
         if (bind_sync) {
-            if (std::fabs(pressure - current_pressure_baseline_pa) >
-                threshold_pascals) {
-                hardware.set_sync();
-            } else {
-                hardware.reset_sync();
-            }
+            handle_sync_threshold(pressure);
         }
 
         if (echo_this_time) {
             auto response_pressure = pressure - current_pressure_baseline_pa;
-            // do we want pressure or response pressure
-            sensor_buffer_log(pressure);
-            can_client.send_can_message(
-                can::ids::NodeId::host,
-                can::messages::ReadFromSensorResponse{
-                    .message_index = m.message_index,
-                    .sensor = can::ids::SensorType::pressure,
-                    .sensor_id = sensor_id,
-                    .sensor_data =
-                        mmr920::reading_to_fixed_point(response_pressure)});
+            if (enable_auto_baseline) {
+                // apply moving baseline if using
+                response_pressure -= current_moving_pressure_baseline_pa;
+            }
+            sensor_buffer_log(response_pressure);
+            if (!enable_auto_baseline) {
+                // This preserves the old way of echoing continuous polls
+                can_client.send_can_message(
+                    can::ids::NodeId::host,
+                    can::messages::ReadFromSensorResponse{
+                        .message_index = 0,
+                        .sensor = can::ids::SensorType::pressure,
+                        .sensor_id = sensor_id,
+                        .sensor_data =
+                            mmr920::reading_to_fixed_point(response_pressure)});
+            }
+
+            if (enable_auto_baseline &&
+                sensor_buffer_index == AUTO_BASELINE_END &&
+                !crossed_buffer_index) {
+                compute_auto_baseline();
+            }
         }
     }
 
@@ -516,16 +598,12 @@ class MMR920 {
      * exceed the threshold for the entirety of this period.
      */
     static constexpr uint16_t MAX_PRESSURE_TIME_MS = 200;
-#ifdef USE_PRESSURE_MOVE
-    mmr920::MeasurementRate measurement_mode_rate =
-        mmr920::MeasurementRate::MEASURE_1;
-#else
     mmr920::MeasurementRate measurement_mode_rate =
         mmr920::MeasurementRate::MEASURE_4;
-#endif
 
     bool _initialized = false;
     bool echoing = false;
+    bool enable_auto_baseline = false;
     bool bind_sync = false;
     bool max_pressure_sync = false;
 
@@ -534,6 +612,7 @@ class MMR920 {
     uint16_t total_baseline_reads = 1;
 
     float current_pressure_baseline_pa = 0;
+    float current_moving_pressure_baseline_pa = 0;
     float current_temperature_baseline = 0;
 
     size_t max_pressure_consecutive_readings = 0;
@@ -565,6 +644,7 @@ class MMR920 {
     }
     std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer;
     uint16_t sensor_buffer_index = 0;
+    bool crossed_buffer_index = false;
 };
 
 }  // namespace tasks

include/sensors/core/tasks/pressure_sensor_task.hpp

@@ -81,7 +81,6 @@ class PressureMessageHandler {
 
     void visit(const can::messages::SendAccumulatedSensorDataRequest &m) {
         LOG("Received request to dump pressure data buffer");
-
         driver.send_accumulated_sensor_data(m.message_index);
     }
 
@@ -137,6 +136,10 @@ class PressureMessageHandler {
         driver.set_max_bind_sync(
             m.binding & static_cast<uint8_t>(
                             can::ids::SensorOutputBinding::max_threshold_sync));
+        driver.set_auto_baseline_report(
+            m.binding &
+            static_cast<uint8_t>(
+                can::ids::SensorOutputBinding::auto_baseline_report));
         std::array tags{utils::ResponseTag::IS_PART_OF_POLL,
                         utils::ResponseTag::POLL_IS_CONTINUOUS};
         auto tags_as_int = utils::byte_from_tags(tags);