From 57f612a723eb89315268e956c8fe9fc564bc3b3d Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Wed, 9 Oct 2024 12:30:11 -0400 Subject: [PATCH] refactor sensor driver to use batch responses succesfully --- .../sensors/core/tasks/pressure_driver.hpp | 105 ++++++++++-------- sensors/tests/CMakeLists.txt | 2 +- sensors/tests/test_pressure_sensor.cpp | 10 +- 3 files changed, 68 insertions(+), 49 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index e8191d45a..3f8ed6392 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,43 +319,39 @@ 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[i] = mmr920::reading_to_fixed_point( + (*sensor_buffer).at(sensor_buffer_index_start + i)); + } + 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}); - std::array data{}; - int i = 0; - while (i < count) { - // send over buffer and then clear buffer values - // NOLINTNEXTLINE(div-by-zero) - int current_index = - (i + start) % static_cast(SENSOR_BUFFER_SIZE); - uint8_t j = 0; - while (j < 14 and i < count) { - data[j] = mmr920::reading_to_fixed_point( - (*sensor_buffer).at(current_index)); - i++; - j++; - current_index = - (i + start) % static_cast(SENSOR_BUFFER_SIZE); - } - - auto response = can::messages::BatchReadFromSensorResponse{ - .message_index = message_index, - .sensor = can::ids::SensorType::pressure, - .sensor_id = sensor_id, - .data_length = j, - .sensor_data = data, - }; - can_client.send_can_message(can::ids::NodeId::host, response); - vtask_hardware_delay(10); + 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, @@ -359,8 +370,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 @@ -372,7 +383,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) > @@ -455,9 +466,12 @@ class MMR920 { response_pressure -= current_moving_pressure_baseline_pa; } sensor_buffer_log(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(); } @@ -652,7 +666,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_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)); } }