diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp index d5b3a2d48..e0b8b3d73 100644 --- a/include/can/core/ids.hpp +++ b/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, diff --git a/include/can/core/message_writer.hpp b/include/can/core/message_writer.hpp index 92649457f..14dfe81dd 100644 --- a/include/can/core/message_writer.hpp +++ b/include/can/core/message_writer.hpp @@ -29,7 +29,8 @@ class MessageWriter { * @param message The message to send */ template - 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; } diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index 71654f7de..8380ee8f3 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -982,6 +983,34 @@ struct ReadFromSensorResponse : BaseMessage { -> 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 { + uint32_t message_index = 0; + can::ids::SensorType sensor{}; + can::ids::SensorId sensor_id{}; + uint8_t data_length = 0; + std::array sensor_data{}; + + template + 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(sensor), iter, limit); + iter = bit_utils::int_to_bytes(static_cast(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 { 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, diff --git a/include/common/tests/mock_message_writer.hpp b/include/common/tests/mock_message_writer.hpp index d2eddce8e..8eb0db35f 100644 --- a/include/common/tests/mock_message_writer.hpp +++ b/include/common/tests/mock_message_writer.hpp @@ -22,10 +22,10 @@ class MockMessageWriter { * @param message The message to send */ template - 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; } diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index ba3a6e508..1e55cfef3 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/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,21 +210,8 @@ 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, @@ -231,12 +219,58 @@ class FDC1004 { } 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 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(&ret.value())); } std::array *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 diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 142cf821f..c7c15126b 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/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 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(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 *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; }; diff --git a/sensors/tests/CMakeLists.txt b/sensors/tests/CMakeLists.txt index 982b40967..db812d23e 100644 --- a/sensors/tests/CMakeLists.txt +++ b/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 diff --git a/sensors/tests/test_capacitive_sensor.cpp b/sensors/tests/test_capacitive_sensor.cpp index 8e0999ac4..c326ff3f0 100644 --- a/sensors/tests/test_capacitive_sensor.cpp +++ b/sensors/tests/test_capacitive_sensor.cpp @@ -541,8 +541,7 @@ SCENARIO("read capacitance sensor values supporting shared CINs") { WHEN("A response for S1 is received") { auto buffer_a = i2c::messages::MaxMessageBuffer{0, 0, 0, 0, 0}; auto buffer_b = i2c::messages::MaxMessageBuffer{0, 0, 0, 0, 0}; - std::array tags{sensors::utils::ResponseTag::IS_PART_OF_POLL, - sensors::utils::ResponseTag::POLL_IS_CONTINUOUS}; + std::array tags{sensors::utils::ResponseTag::IS_PART_OF_POLL}; i2c::messages::TransactionResponse first{ .id = i2c::messages::TransactionIdentifier{ @@ -558,6 +557,7 @@ SCENARIO("read capacitance sensor values supporting shared CINs") { auto second = first; second.id.transaction_index = 1; second.read_buffer = buffer_b; + second.id.is_completed_poll = 1; auto first_task_msg = sensors::utils::TaskMessage(first); auto second_task_msg = sensors::utils::TaskMessage(second); sensor_shared.handle_message(first_task_msg); @@ -621,8 +621,10 @@ SCENARIO("capacitance driver tests no shared CINs") { auto second = first; second.id.transaction_index = 1; second.read_buffer = buffer_b; - callback_host.handle_ongoing_response(first); - callback_host.handle_ongoing_response(second); + for (auto i = 0; i < 14; i++) { + callback_host.handle_ongoing_response(first); + callback_host.handle_ongoing_response(second); + } THEN("it should forward the converted data via can") { REQUIRE(can_queue.has_message()); @@ -654,17 +656,20 @@ SCENARIO("capacitance driver tests no shared CINs") { second.id.transaction_index = 1; second.read_buffer = buffer_b; - callback_host.handle_ongoing_response(first); - callback_host.handle_ongoing_response(second); + for (auto i = 0; i < 14; i++) { + callback_host.handle_ongoing_response(first); + callback_host.handle_ongoing_response(second); + } THEN("it should forward the converted data via can") { can_queue.try_read(&empty_msg); - auto sent = std::get( - empty_msg.message); + auto sent = + std::get( + empty_msg.message); REQUIRE(sent.sensor == can::ids::SensorType::capacitive); // we're just checking that the data is faithfully represented, // don't really care what it is - REQUIRE(sent.sensor_data == + REQUIRE(sent.sensor_data[0] == convert_to_fixed_point(15, S15Q16_RADIX)); } THEN("it should not touch the sync line") { diff --git a/sensors/tests/test_pressure_driver.cpp b/sensors/tests/test_pressure_driver.cpp index 18793480f..fba0cf794 100644 --- a/sensors/tests/test_pressure_driver.cpp +++ b/sensors/tests/test_pressure_driver.cpp @@ -272,6 +272,50 @@ SCENARIO("Testing the pressure sensor driver") { } } + GIVEN("An unlimited poll with sensor binding set to report") { + driver.set_echoing(true); + driver.set_max_bind_sync(false); + std::array tags{sensors::utils::ResponseTag::IS_PART_OF_POLL, + sensors::utils::ResponseTag::POLL_IS_CONTINUOUS}; + auto tags_as_int = sensors::utils::byte_from_tags(tags); + WHEN("the continuous poll function is called") { + driver.poll_continuous_pressure(tags_as_int); + THEN("a READ_PRESSURE command only") { + REQUIRE(i2c_queue.get_size() == 0); + REQUIRE(i2c_poll_queue.get_size() == 1); + auto read_command = get_message< + i2c::messages::ConfigureSingleRegisterContinuousPolling>( + i2c_poll_queue); + REQUIRE( + read_command.first.write_buffer[0] == + static_cast( + sensors::mmr920::Registers::LOW_PASS_PRESSURE_READ)); + } + } + WHEN("pressure driver receives the requested sensor readings") { + auto id = i2c::messages::TransactionIdentifier{ + .token = sensors::utils::build_id( + sensors::mmr920::ADDRESS, + static_cast( + sensors::mmr920::Registers::LOW_PASS_PRESSURE_READ), + tags_as_int), + .is_completed_poll = false, + .transaction_index = static_cast(0)}; + auto sensor_response = i2c::messages::TransactionResponse{ + .id = id, .bytes_read = 3, .read_buffer = {0x00, 0xC0, 0xDE}}; + + for (size_t i = 0; i <= SENSOR_BUFFER_SIZE + 1; i++) { + driver.handle_ongoing_pressure_response(sensor_response); + if (i > 0 && i % 14 == 0) { + // Can queue here can't handle that many messages so just + // pop them off one at a time. + REQUIRE(can_queue.get_size() == 1); + can_queue.reset(); + } + } + } + } + GIVEN("output binding = report") { driver.set_bind_sync(true); std::array tags{sensors::utils::ResponseTag::IS_PART_OF_POLL, diff --git a/sensors/tests/test_pressure_sensor.cpp b/sensors/tests/test_pressure_sensor.cpp index 618485aaf..30fed1b42 100644 --- a/sensors/tests/test_pressure_sensor.cpp +++ b/sensors/tests/test_pressure_sensor.cpp @@ -98,13 +98,17 @@ SCENARIO("Receiving messages through the pressure sensor message handler") { sensors::mmr920::Registers::LOW_PASS_PRESSURE_READ), sensors::utils::byte_from_tags(tags_for_continuous)); auto response_read = sensors::utils::TaskMessage(response_details); - sensor.handle_message(response_read); - THEN("there should be a ReadFromSensorResponse") { + THEN( + "there should be a Batch read sensor response after 14 " + "messages") { + for (auto i = 0; i < 14; i++) { + sensor.handle_message(response_read); + } REQUIRE(can_queue.has_message()); REQUIRE(can_queue.get_size() == 1); can_queue.try_read(&empty_can_msg); REQUIRE(std::holds_alternative< - can::messages::ReadFromSensorResponse>( + can::messages::BatchReadFromSensorResponse>( empty_can_msg.message)); } }