feat(pipettes): send most recent sensor data after probe (#784)

gripper/core/CMakeLists.txt

@@ -1,9 +1,9 @@
 function(target_gripper_core TARGET)
     if(${USE_SENSOR_MOVE})
     target_compile_definitions(${TARGET} PUBLIC USE_SENSOR_MOVE)
-    target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=1700)
-    target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true)
     endif()
+    target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500)
+    target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true)
     target_sources(${TARGET} PUBLIC
             ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/can_tasks.cpp
             ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/tasks.cpp

gripper/core/tasks.cpp

@@ -16,9 +16,9 @@
 
 static auto tasks = gripper_tasks::AllTask{};
 static auto queues = gripper_tasks::QueueClient{can::ids::NodeId::gripper};
-static std::array<float, SENSOR_BUFFER_SIZE> p_buff;
+static std::array<float, SENSOR_BUFFER_SIZE> sensor_buffer;
 #ifdef USE_TWO_BUFFERS
-static std::array<float, SENSOR_BUFFER_SIZE> p_buff_front;
+static std::array<float, SENSOR_BUFFER_SIZE> sensor_buffer_front;
 #endif
 
 static auto eeprom_task_builder =
@@ -101,14 +101,14 @@ void gripper_tasks::start_tasks(
             5, "cap sensor S1", i2c2_task_client, i2c2_poll_client,
             sensor_hardware, queues,
 #ifdef USE_TWO_BUFFERS
-            p_buff_front);
+            sensor_buffer_front);
 #else
-            p_buff);
+            sensor_buffer);
 #endif
     auto& capacitive_sensor_task_rear =
         capacitive_sensor_task_builder_rear.start(
             5, "cap sensor S0", i2c3_task_client, i2c3_poll_client,
-            sensor_hardware, queues, p_buff);
+            sensor_hardware, queues, sensor_buffer);
 
     tasks.i2c2_task = &i2c2_task;
     tasks.i2c3_task = &i2c3_task;

include/common/core/hardware_delay.hpp

@@ -0,0 +1,12 @@
+#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
+template <typename T>
+requires std::is_integral_v<T>
+static void vtask_hardware_delay(T ticks) {
+#ifndef INC_TASK_H
+    std::ignore = ticks;
+#else
+    vTaskDelay(ticks);
+#endif
+}

include/common/core/sensor_buffer.hpp

@@ -1,7 +1,5 @@
 #pragma once
-
-#ifdef USE_SENSOR_MOVE
-constexpr size_t SENSOR_BUFFER_SIZE = P_BUFF_SIZE;
-#else
-constexpr size_t SENSOR_BUFFER_SIZE = 0;
+#ifndef SENSOR_BUFF_SIZE
+#define SENSOR_BUFF_SIZE 1
 #endif
+constexpr size_t SENSOR_BUFFER_SIZE = SENSOR_BUFF_SIZE;

include/motor-control/core/tasks/usage_storage_task.hpp

@@ -7,6 +7,7 @@
 #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 "eeprom/core/dev_data.hpp"
 #include "motor-control/core/tasks/messages.hpp"
@@ -15,18 +16,6 @@ namespace usage_storage_task {
 
 using namespace usage_messages;
 
-// Not my favorite way to check this, but if we don't have access
-// to vTaskDelay during host compilation so just dummy the function
-template <typename T>
-requires std::is_integral_v<T>
-static void _hardware_delay(T ticks) {
-#ifndef INC_TASK_H
-    std::ignore = ticks;
-#else
-    vTaskDelay(ticks);
-#endif
-}
-
 using TaskMessage = motor_control_task_messages::UsageStorageTaskMessage;
 
 static constexpr uint16_t distance_data_usage_len = 8;
@@ -237,7 +226,7 @@ class UsageStorageTask {
             } else {
                 // wait for the handler to be ready before sending the next
                 // message
-                _hardware_delay(10);
+                vtask_hardware_delay(10);
             }
         }
     }

include/sensors/core/tasks/capacitive_driver.hpp

@@ -42,14 +42,14 @@ class FDC1004 {
     FDC1004(I2CQueueWriter &writer, I2CQueuePoller &poller,
             CanClient &can_client, OwnQueue &own_queue,
             sensors::hardware::SensorHardwareBase &hardware, bool shared_sensor,
-            std::array<float, SENSOR_BUFFER_SIZE> *p_buff)
+            std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer)
         : writer(writer),
           poller(poller),
           can_client(can_client),
           own_queue(own_queue),
           hardware(hardware),
           shared_sensor(shared_sensor),
-          p_buff(p_buff) {}
+          sensor_buffer(sensor_buffer) {}
 
     [[nodiscard]] auto initialized() const -> bool { return _initialized; }
 
@@ -223,13 +223,13 @@ class FDC1004 {
                     .message_index = message_index,
                     .sensor = can::ids::SensorType::capacitive,
                     .sensor_id = sensor_id,
-                    .sensor_data =
-                        convert_to_fixed_point((*p_buff).at(i), S15Q16_RADIX)});
+                    .sensor_data = convert_to_fixed_point(
+                        (*sensor_buffer).at(i), S15Q16_RADIX)});
             if (i % 10 == 0) {
                 // slow it down so the can buffer doesn't choke
                 vTaskDelay(50);
             }
-            (*p_buff).at(i) = 0;
+            (*sensor_buffer).at(i) = 0;
         }
 #else
         std::ignore = message_index;
@@ -306,7 +306,7 @@ class FDC1004 {
             // send a response with 9999 to make an overload of the buffer
             // visible
             if (sensor_buffer_index < SENSOR_BUFFER_SIZE) {
-                (*p_buff).at(sensor_buffer_index) = capacitance;
+                (*sensor_buffer).at(sensor_buffer_index) = capacitance;
                 sensor_buffer_index++;
             }
 #else
@@ -537,7 +537,7 @@ class FDC1004 {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
         return RG(*reinterpret_cast<Reg *>(&ret.value()));
     }
-    std::array<float, SENSOR_BUFFER_SIZE> *p_buff;
+    std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer;
     uint16_t sensor_buffer_index = 0;
 
 };  // end of FDC1004 class

include/sensors/core/tasks/capacitive_sensor_task.hpp

@@ -23,9 +23,9 @@ class CapacitiveMessageHandler {
         I2CQueueWriter &i2c_writer, I2CQueuePoller &i2c_poller,
         sensors::hardware::SensorHardwareBase &hardware, CanClient &can_client,
         OwnQueue &own_queue, bool shared_task,
-        std::array<float, SENSOR_BUFFER_SIZE> *p_buff)
-        : driver{i2c_writer, i2c_poller,  can_client, own_queue,
-                 hardware,   shared_task, p_buff} {}
+        std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer)
+        : driver{i2c_writer, i2c_poller,  can_client,   own_queue,
+                 hardware,   shared_task, sensor_buffer} {}
     CapacitiveMessageHandler(const CapacitiveMessageHandler &) = delete;
     CapacitiveMessageHandler(const CapacitiveMessageHandler &&) = delete;
     auto operator=(const CapacitiveMessageHandler &)
@@ -213,14 +213,14 @@ class CapacitiveSensorTask {
         i2c::writer::Writer<QueueImpl> *writer,
         i2c::poller::Poller<QueueImpl> *poller,
         sensors::hardware::SensorHardwareBase *hardware, CanClient *can_client,
-        std::array<float, SENSOR_BUFFER_SIZE> *p_buff,
+        std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer,
         bool shared_task = false) {
         // On the 8 channel, there is a singular cap sensor but we're using
         // multiple channels. We will thus rely on the sensor id in this case
         // to determine which CIN configuration the sensor should be in.
         auto handler = CapacitiveMessageHandler{
-            *writer,     *poller,     *hardware, *can_client,
-            get_queue(), shared_task, p_buff};
+            *writer,     *poller,     *hardware,    *can_client,
+            get_queue(), shared_task, sensor_buffer};
         handler.initialize();
         utils::TaskMessage message{};
         for (;;) {

include/sensors/core/tasks/pressure_driver.hpp

@@ -6,6 +6,7 @@
 #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"
@@ -38,15 +39,15 @@ class MMR920 {
            sensors::hardware::SensorHardwareBase &hardware,
            const can::ids::SensorId &id,
            const sensors::mmr920::SensorVersion version,
-           std::array<float, SENSOR_BUFFER_SIZE> *p_buff)
+           std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer)
         : writer(writer),
           poller(poller),
           can_client(can_client),
           own_queue(own_queue),
           hardware(hardware),
           sensor_id(id),
           sensor_version(version),
-          p_buff(p_buff) {}
+          sensor_buffer(sensor_buffer) {}
 
     /**
      * @brief Check if the MMR92 has been initialized.
@@ -228,6 +229,15 @@ class MMR920 {
         return true;
     }
 
+    auto sensor_buffer_log(float data) -> void {
+        if (sensor_buffer_index == SENSOR_BUFFER_SIZE) {
+            sensor_buffer_index = 0;
+        }
+
+        sensor_buffer->at(sensor_buffer_index) = data;
+        sensor_buffer_index++;
+    }
+
     auto save_temperature(int32_t data) -> bool {
         _registers.temperature_result.reading = data;
         LOG("Updated temperature reading is %u",
@@ -280,26 +290,26 @@ class MMR920 {
     }
 
     void send_accumulated_sensor_data(uint32_t message_index) {
-#ifdef USE_SENSOR_MOVE
-        for (int i = 0; i < sensor_buffer_index; i++) {
-            // send over buffer adn then clear buffer values
+        for (int i = 0; i < static_cast<int>(SENSOR_BUFFER_SIZE); 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);
+
             can_client.send_can_message(
                 can::ids::NodeId::host,
                 can::messages::ReadFromSensorResponse{
                     .message_index = message_index,
                     .sensor = can::ids::SensorType::pressure,
                     .sensor_id = sensor_id,
-                    .sensor_data =
-                        mmr920::reading_to_fixed_point((*p_buff).at(i))});
+                    .sensor_data = mmr920::reading_to_fixed_point(
+                        (*sensor_buffer).at(current_index))});
             if (i % 10 == 0) {
                 // slow it down so the can buffer doesn't choke
-                vTaskDelay(50);
+                vtask_hardware_delay(50);
             }
-            (*p_buff).at(i) = 0;
+            (*sensor_buffer).at(current_index) = 0;
         }
-#else
-        std::ignore = message_index;
-#endif
     }
 
     auto handle_ongoing_pressure_response(i2c::messages::TransactionResponse &m)
@@ -360,12 +370,8 @@ class MMR920 {
 
         if (echo_this_time) {
             auto response_pressure = pressure - current_pressure_baseline_pa;
-#ifdef USE_SENSOR_MOVE
-            if (sensor_buffer_index < SENSOR_BUFFER_SIZE) {
-                (*p_buff).at(sensor_buffer_index) = response_pressure;
-                sensor_buffer_index++;
-            }
-#else
+            // do we want pressure or response pressure
+            sensor_buffer_log(pressure);
             can_client.send_can_message(
                 can::ids::NodeId::host,
                 can::messages::ReadFromSensorResponse{
@@ -374,7 +380,6 @@ class MMR920 {
                     .sensor_id = sensor_id,
                     .sensor_data =
                         mmr920::reading_to_fixed_point(response_pressure)});
-#endif
         }
     }
 
@@ -558,7 +563,7 @@ class MMR920 {
         value &= Reg::value_mask;
         return write(Reg::address, value);
     }
-    std::array<float, SENSOR_BUFFER_SIZE> *p_buff;
+    std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer;
     uint16_t sensor_buffer_index = 0;
 };
 

include/sensors/core/tasks/pressure_sensor_task.hpp

@@ -24,9 +24,9 @@ class PressureMessageHandler {
         sensors::hardware::SensorHardwareBase &hardware,
         const can::ids::SensorId &id,
         const sensors::mmr920::SensorVersion &version,
-        std::array<float, SENSOR_BUFFER_SIZE> *p_buff)
+        std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer)
         : driver{i2c_writer, i2c_poller, can_client, own_queue,
-                 hardware,   id,         version,    p_buff} {}
+                 hardware,   id,         version,    sensor_buffer} {}
     PressureMessageHandler(const PressureMessageHandler &) = delete;
     PressureMessageHandler(const PressureMessageHandler &&) = delete;
     auto operator=(const PressureMessageHandler &)
@@ -204,10 +204,10 @@ class PressureSensorTask {
         i2c::poller::Poller<QueueImpl> *poller, CanClient *can_client,
         sensors::hardware::SensorHardwareBase *hardware,
         sensors::mmr920::SensorVersion *sensor_version,
-        std::array<float, SENSOR_BUFFER_SIZE> *p_buff) {
+        std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer) {
         auto handler = PressureMessageHandler{
             *writer,   *poller,   *can_client,     get_queue(),
-            *hardware, sensor_id, *sensor_version, p_buff};
+            *hardware, sensor_id, *sensor_version, sensor_buffer};
         handler.initialize();
         utils::TaskMessage message{};
         for (;;) {

pipettes/core/CMakeLists.txt

@@ -17,42 +17,34 @@ endfunction()
 function(target_pipettes_core_single TARGET REVISION)
     target_pipettes_core_common(${TARGET} ${REVISION})
     target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=SINGLE_CHANNEL)
-    if(${USE_SENSOR_MOVE})
-    target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=2800)
-    endif()
+    target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500)
     target_sources(${TARGET} PUBLIC
             ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/can_task_low_throughput.cpp)
 endfunction()
 
 function(target_pipettes_core_multi TARGET REVISION)
     target_pipettes_core_common(${TARGET} ${REVISION})
     target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=EIGHT_CHANNEL)
-    if(${USE_SENSOR_MOVE})
-    target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=1700)
+    target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500)
     target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true)
-    endif()
     target_sources(${TARGET} PUBLIC
             ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/can_task_low_throughput.cpp)
 endfunction()
 
 function(target_pipettes_core_96 TARGET REVISION)
     target_pipettes_core_common(${TARGET} ${REVISION})
     target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=NINETY_SIX_CHANNEL)
-    if(${USE_SENSOR_MOVE})
-    target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=800)
+    target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500)
     target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true)
-    endif()
     target_sources(${TARGET} PUBLIC
             ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/can_task_high_throughput.cpp)
 endfunction()
 
 function(target_pipettes_core_384 TARGET REVISION)
     target_pipettes_core_common(${TARGET} ${REVISION})
     target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=THREE_EIGHTY_FOUR_CHANNEL)
-    if(${USE_SENSOR_MOVE})
-    target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=1500)
+    target_compile_definitions(${TARGET} PUBLIC SENSOR_BUFF_SIZE=500)
     target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true)
-    endif()
     target_sources(${TARGET} PUBLIC
             ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/can_task_high_throughput.cpp)
 endfunction()