feat(sensors): Self baselining during sensor moves

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,11 @@ 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);  // sync and
+                                                                 // report
                 if (buffered_move.sensor_id == can::ids::SensorId::BOTH) {
                     send_bind_message(buffered_move.sensor_type,
                                       can::ids::SensorId::S0, binding);
@@ -502,21 +506,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,10 @@ 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"
@@ -70,6 +71,8 @@ class MMR920 {
         echoing = should_echo;
         if (should_echo) {
             sensor_buffer_index = 0;  // reset buffer index
+            crossed_buffer_index=false;
+            sensor_buffer->fill(0.0);
         }
     }
 
@@ -230,12 +233,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,10 +293,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 == true) {
+            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) %
+            int current_index = (i + start) %
                                 static_cast<int>(SENSOR_BUFFER_SIZE);
 
             can_client.send_can_message(
@@ -306,10 +320,13 @@ 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);
             }
-            (*sensor_buffer).at(current_index) = 0;
         }
+        can_client.send_can_message(
+                can::ids::NodeId::host,
+                can::messages::Acknowledgment{
+                    .message_index = message_index});
     }
 
     auto handle_ongoing_pressure_response(i2c::messages::TransactionResponse &m)
@@ -355,31 +372,42 @@ 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();
+                // force this to stay set long enough to debounce on other nodes
             } else {
                 hardware.reset_sync();
             }
         }
 
         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)});
+
+            sensor_buffer_log(response_pressure);
+
+            if (sensor_buffer_index == 10 && !crossed_buffer_index) {
+
+                current_pressure_baseline_pa =
+                    std::accumulate(sensor_buffer->begin(), sensor_buffer->begin()+10, 0) /
+                    10 + current_pressure_baseline_pa;
+                for (auto i = sensor_buffer_index - 10; i < sensor_buffer_index;
+                     i++) {
+                    sensor_buffer->at(sensor_buffer_index) =
+                        sensor_buffer->at(sensor_buffer_index) -
+                        current_pressure_baseline_pa;
+                }
+            }
         }
     }
 
@@ -565,6 +593,7 @@ class MMR920 {
     }
     std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer;
     uint16_t sensor_buffer_index = 0;
+    bool crossed_buffer_index = false;
 };
 
 }  // namespace tasks