feat(sensors): add a new can message to batch read sensor data (#805)

* add batch message

* use the batch message instead

* remove the old echo, will be going away anyway

* add a bool return to send can message

* auto computer max data length of a batch repsonse

* refactor sensor driver to use batch responses succesfully

* lint

* copy to capacitance sensor

* the data buffer should be int32

* fix the tests

* fix modulus error during batch send

* fix capacatance test

* add test to handle when buffer filles up

* format

* lint

include/can/core/ids.hpp

@@ -107,6 +107,7 @@ enum class MessageId {
     gear_read_motor_driver_request = 0x507,
     max_sensor_value_request = 0x70,
     max_sensor_value_response = 0x71,
+    batch_read_sensor_response = 0x81,
     read_sensor_request = 0x82,
     write_sensor_request = 0x83,
     baseline_sensor_request = 0x84,

include/can/core/message_writer.hpp

@@ -29,7 +29,8 @@ class MessageWriter {
      * @param message The message to send
      */
     template <message_core::CanResponseMessage ResponseMessage>
-    void send_can_message(can::ids::NodeId node, ResponseMessage&& message) {
+    auto send_can_message(can::ids::NodeId node, ResponseMessage&& message)
+        -> bool {
         auto arbitration_id = can::arbitration_id::ArbitrationId{};
         auto task_message = can::message_writer_task::TaskMessage{};
 
@@ -41,7 +42,7 @@ class MessageWriter {
         arbitration_id.originating_node_id(node_id);
         task_message.arbitration_id = arbitration_id;
         task_message.message = message;
-        queue->try_write(task_message);
+        return queue->try_write(task_message);
     }
 
     void set_queue(QueueType* q) { queue = q; }

include/can/core/messages.hpp

@@ -2,6 +2,7 @@
 
 #include <algorithm>
 #include <array>
+#include <cmath>
 #include <cstdint>
 #include <span>
 #include <vector>
@@ -982,6 +983,34 @@ struct ReadFromSensorResponse : BaseMessage<MessageId::read_sensor_response> {
         -> bool = default;
 };
 
+// Max len = (max size - uint32(message_index) - 3x uint8(sensor_type, sensor_id
+// and data_length))/uint32(data_element_size)
+constexpr size_t BATCH_SENSOR_MAX_LEN =
+    size_t((can::message_core::MaxMessageSize - 4 - 1 - 1 - 1) / 4);
+struct BatchReadFromSensorResponse
+    : BaseMessage<MessageId::batch_read_sensor_response> {
+    uint32_t message_index = 0;
+    can::ids::SensorType sensor{};
+    can::ids::SensorId sensor_id{};
+    uint8_t data_length = 0;
+    std::array<int32_t, BATCH_SENSOR_MAX_LEN> sensor_data{};
+
+    template <bit_utils::ByteIterator Output, typename Limit>
+    auto serialize(Output body, Limit limit) const -> uint8_t {
+        auto iter = bit_utils::int_to_bytes(message_index, body, limit);
+        iter =
+            bit_utils::int_to_bytes(static_cast<uint8_t>(sensor), iter, limit);
+        iter = bit_utils::int_to_bytes(static_cast<uint8_t>(sensor_id), iter,
+                                       limit);
+        for (auto i = 0; i < data_length; i++) {
+            iter = bit_utils::int_to_bytes(sensor_data.at(i), iter, limit);
+        }
+        return iter - body;
+    }
+    auto operator==(const BatchReadFromSensorResponse& other) const
+        -> bool = default;
+};
+
 struct SetSensorThresholdRequest
     : BaseMessage<MessageId::set_sensor_threshold_request> {
     uint32_t message_index;
@@ -1879,10 +1908,10 @@ using ResponseMessageType = std::variant<
     ReadMotorDriverRegisterResponse, ReadFromEEPromResponse, MoveCompleted,
     ReadPresenceSensingVoltageResponse, PushToolsDetectedNotification,
     ReadLimitSwitchResponse, MotorPositionResponse, ReadFromSensorResponse,
-    FirmwareUpdateStatusResponse, SensorThresholdResponse,
-    SensorDiagnosticResponse, TaskInfoResponse, PipetteInfoResponse,
-    BindSensorOutputResponse, GripperInfoResponse, TipActionResponse,
-    PeripheralStatusResponse, BrushedMotorConfResponse,
+    BatchReadFromSensorResponse, FirmwareUpdateStatusResponse,
+    SensorThresholdResponse, SensorDiagnosticResponse, TaskInfoResponse,
+    PipetteInfoResponse, BindSensorOutputResponse, GripperInfoResponse,
+    TipActionResponse, PeripheralStatusResponse, BrushedMotorConfResponse,
     UpdateMotorPositionEstimationResponse, BaselineSensorResponse,
     PushTipPresenceNotification, GetMotorUsageResponse, GripperJawStateResponse,
     GripperJawHoldoffResponse, HepaUVInfoResponse, GetHepaFanStateResponse,

include/common/tests/mock_message_writer.hpp

@@ -22,10 +22,10 @@ class MockMessageWriter {
      * @param message The message to send
      */
     template <can::message_core::CanResponseMessage ResponseMessage>
-    void send_can_message(can::ids::NodeId, ResponseMessage&& message) {
+    auto send_can_message(can::ids::NodeId, ResponseMessage&& message) -> bool {
         can::message_writer_task::TaskMessage task_msg{.arbitration_id = 0x1,
                                                        .message = message};
-        queue->try_write(task_msg);
+        return queue->try_write(task_msg);
     }
 
     void set_queue(QueueType* q) { queue = q; }

include/sensors/core/tasks/capacitive_driver.hpp

@@ -90,7 +90,8 @@ class FDC1004 {
     void set_echoing(bool should_echo) {
         echoing = should_echo;
         if (should_echo) {
-            sensor_buffer_index = 0;  // reset buffer index
+            sensor_buffer_index_start = 0;  // reset buffer index
+            sensor_buffer_index_end = 0;    // reset buffer index
         }
     }
 
@@ -209,34 +210,67 @@ class FDC1004 {
     }
 
     void send_accumulated_sensor_data(uint32_t message_index) {
-        for (int i = 0; i < sensor_buffer_index; i++) {
-            // send over buffer adn then clear buffer values
-            can_client.send_can_message(
-                can::ids::NodeId::host,
-                can::messages::ReadFromSensorResponse{
-                    .message_index = message_index,
-                    .sensor = can::ids::SensorType::capacitive,
-                    .sensor_id = sensor_id,
-                    .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
-                vtask_hardware_delay(50);
-            }
-            (*sensor_buffer).at(i) = 0;
+        while (get_buffer_count() > 0) {
+            try_send_next_chunk(message_index);
         }
         can_client.send_can_message(
             can::ids::NodeId::host,
             can::messages::Acknowledgment{.message_index = message_index});
     }
 
     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;
+        sensor_buffer->at(sensor_buffer_index_end) = data;
+        sensor_buffer_index_end++;
+        if (sensor_buffer_index_end == SENSOR_BUFFER_SIZE) {
+            sensor_buffer_index_end = 0;
             crossed_buffer_index = true;
         }
+        if (sensor_buffer_index_end == sensor_buffer_index_start) {
+            sensor_buffer_index_start =
+                (sensor_buffer_index_end + 1) % SENSOR_BUFFER_SIZE;
+        }
+    }
+
+    auto get_buffer_count() -> uint16_t {
+        auto count = sensor_buffer_index_end;
+        if (sensor_buffer_index_end < sensor_buffer_index_start) {
+            count += (SENSOR_BUFFER_SIZE - sensor_buffer_index_start);
+        } else {
+            count -= sensor_buffer_index_start;
+        }
+        return count;
+    }
+
+    auto try_send_next_chunk(uint32_t message_index) -> void {
+        std::array<int32_t, can::messages::BATCH_SENSOR_MAX_LEN> data{};
+        auto count = get_buffer_count();
+        auto data_len = std::min(uint8_t(count),
+                                 uint8_t(can::messages::BATCH_SENSOR_MAX_LEN));
+        if (data_len == 0) {
+            return;
+        }
+        for (uint8_t i = 0; i < data_len; i++) {
+            data.at(i) = convert_to_fixed_point(
+                (*sensor_buffer)
+                    .at((sensor_buffer_index_start + i) % SENSOR_BUFFER_SIZE),
+                S15Q16_RADIX);
+        }
+        auto response = can::messages::BatchReadFromSensorResponse{
+            .message_index = message_index,
+            .sensor = can::ids::SensorType::capacitive,
+            .sensor_id = sensor_id,
+            .data_length = data_len,
+            .sensor_data = data,
+        };
+        if (can_client.send_can_message(can::ids::NodeId::host, response)) {
+            // if we succesfully queue the can message, mark that data as sent
+            // by incrementing the buffer start pointer
+            sensor_buffer_index_start =
+                (sensor_buffer_index_start + data_len) % SENSOR_BUFFER_SIZE;
+        } else {
+            // if the queue is full release the task for bit
+            vtask_hardware_delay(20);
+        }
     }
 
     void handle_fdc_response(i2c::messages::TransactionResponse &m) {
@@ -307,14 +341,9 @@ class FDC1004 {
 
         if (echoing) {
             sensor_buffer_log(capacitance);
-            can_client.send_can_message(
-                can::ids::NodeId::host,
-                can::messages::ReadFromSensorResponse{
-                    .message_index = m.message_index,
-                    .sensor = can::ids::SensorType::capacitive,
-                    .sensor_id = sensor_id,
-                    .sensor_data =
-                        convert_to_fixed_point(capacitance, S15Q16_RADIX)});
+            if (get_buffer_count() >= can::messages::BATCH_SENSOR_MAX_LEN) {
+                try_send_next_chunk(0);
+            }
         }
     }
 
@@ -534,7 +563,8 @@ class FDC1004 {
         return RG(*reinterpret_cast<Reg *>(&ret.value()));
     }
     std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer;
-    uint16_t sensor_buffer_index = 0;
+    uint16_t sensor_buffer_index_start = 0;
+    uint16_t sensor_buffer_index_end = 0;
     bool crossed_buffer_index = false;
 
 };  // end of FDC1004 class

include/sensors/core/tasks/pressure_driver.hpp

@@ -71,7 +71,8 @@ class MMR920 {
     void set_echoing(bool should_echo) {
         echoing = should_echo;
         if (should_echo) {
-            sensor_buffer_index = 0;  // reset buffer index
+            sensor_buffer_index_start = 0;  // reset buffer index
+            sensor_buffer_index_end = 0;    // reset buffer index
             crossed_buffer_index = false;
             sensor_buffer->fill(0.0);
         }
@@ -244,13 +245,27 @@ class MMR920 {
         return true;
     }
 
+    auto get_buffer_count() -> uint16_t {
+        auto count = sensor_buffer_index_end;
+        if (sensor_buffer_index_end < sensor_buffer_index_start) {
+            count += (SENSOR_BUFFER_SIZE - sensor_buffer_index_start);
+        } else {
+            count -= sensor_buffer_index_start;
+        }
+        return count;
+    }
+
     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;
+        sensor_buffer->at(sensor_buffer_index_end) = data;
+        sensor_buffer_index_end++;
+        if (sensor_buffer_index_end == SENSOR_BUFFER_SIZE) {
+            sensor_buffer_index_end = 0;
             crossed_buffer_index = true;
         }
+        if (sensor_buffer_index_end == sensor_buffer_index_start) {
+            sensor_buffer_index_start =
+                (sensor_buffer_index_end + 1) % SENSOR_BUFFER_SIZE;
+        }
     }
 
     auto save_temperature(int32_t data) -> bool {
@@ -304,35 +319,40 @@ class MMR920 {
             mmr920::ADDRESS, reg_id, 3, STOP_DELAY, own_queue, transaction_id);
     }
 
-    void send_accumulated_sensor_data(uint32_t message_index) {
-        auto start = 0;
-        auto count = sensor_buffer_index;
-        if (crossed_buffer_index) {
-            start = sensor_buffer_index;
-            count = SENSOR_BUFFER_SIZE;
+    auto try_send_next_chunk(uint32_t message_index) -> void {
+        std::array<int32_t, can::messages::BATCH_SENSOR_MAX_LEN> data{};
+        auto count = get_buffer_count();
+        auto data_len = std::min(uint8_t(count),
+                                 uint8_t(can::messages::BATCH_SENSOR_MAX_LEN));
+        if (data_len == 0) {
+            return;
         }
+        for (uint8_t i = 0; i < data_len; i++) {
+            data.at(i) = mmr920::reading_to_fixed_point(
+                (*sensor_buffer)
+                    .at((sensor_buffer_index_start + i) % SENSOR_BUFFER_SIZE));
+        }
+        auto response = can::messages::BatchReadFromSensorResponse{
+            .message_index = message_index,
+            .sensor = can::ids::SensorType::pressure,
+            .sensor_id = sensor_id,
+            .data_length = data_len,
+            .sensor_data = data,
+        };
+        if (can_client.send_can_message(can::ids::NodeId::host, response)) {
+            // if we succesfully queue the can message, mark that data as sent
+            // by incrementing the buffer start pointer
+            sensor_buffer_index_start =
+                (sensor_buffer_index_start + data_len) % SENSOR_BUFFER_SIZE;
+        } else {
+            // if the queue is full release the task for bit
+            vtask_hardware_delay(20);
+        }
+    }
 
-        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 + start) % 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(
-                        (*sensor_buffer).at(current_index))});
-            if (i % 10 == 0) {
-                // slow it down so the can buffer doesn't choke
-                vtask_hardware_delay(20);
-            }
+    void send_accumulated_sensor_data(uint32_t message_index) {
+        while (get_buffer_count() > 0) {
+            try_send_next_chunk(message_index);
         }
         can_client.send_can_message(
             can::ids::NodeId::host,
@@ -351,8 +371,8 @@ class MMR920 {
             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++) {
+        for (auto i = sensor_buffer_index_end - AUTO_BASELINE_END;
+             i < sensor_buffer_index_end; 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
@@ -364,7 +384,7 @@ class MMR920 {
 
     auto handle_sync_threshold(float pressure) -> void {
         if (enable_auto_baseline) {
-            if ((sensor_buffer_index > AUTO_BASELINE_END ||
+            if ((sensor_buffer_index_end > AUTO_BASELINE_END ||
                  crossed_buffer_index) &&
                 (std::fabs(pressure - current_pressure_baseline_pa -
                            current_moving_pressure_baseline_pa) >
@@ -447,20 +467,12 @@ class MMR920 {
                 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 (get_buffer_count() >= can::messages::BATCH_SENSOR_MAX_LEN) {
+                try_send_next_chunk(0);
             }
 
             if (enable_auto_baseline &&
-                sensor_buffer_index == AUTO_BASELINE_END &&
+                sensor_buffer_index_end == AUTO_BASELINE_END &&
                 !crossed_buffer_index) {
                 compute_auto_baseline();
             }
@@ -655,7 +667,8 @@ class MMR920 {
         return write(Reg::address, value);
     }
     std::array<float, SENSOR_BUFFER_SIZE> *sensor_buffer;
-    uint16_t sensor_buffer_index = 0;
+    uint16_t sensor_buffer_index_start = 0;
+    uint16_t sensor_buffer_index_end = 0;
     bool crossed_buffer_index = false;
 };
 

sensors/tests/CMakeLists.txt

@@ -21,7 +21,7 @@ set_target_properties(sensors
         PROPERTIES
         CXX_STANDARD 20
         CXX_STANDARD_REQUIRED TRUE)
-
+target_compile_definitions(sensors PUBLIC SENSOR_BUFF_SIZE=300)
 target_compile_options(sensors
         PUBLIC
         -Wall