feat(sensors): Self baselining during sensor moves

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"
@@ -63,11 +49,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;
         }
@@ -231,6 +217,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);