Skip to content

Commit

Permalink
refactor sensor driver to use batch responses succesfully
Browse files Browse the repository at this point in the history
  • Loading branch information
@ryanthecoder
ryanthecoder committed Oct 9, 2024
1 parent ba9d2f7 commit 57f612a
Show file tree
Hide file tree
Showing 3 changed files with 68 additions and 49 deletions.
105 changes: 60 additions & 45 deletions include/sensors/core/tasks/pressure_driver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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<uint32_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[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<uint32_t, 14> 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<int>(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<int>(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,
Expand All @@ -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
Expand All @@ -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) >
Expand Down Expand Up @@ -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();
}
Expand Down Expand Up @@ -652,7 +666,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;
};

Expand Down
2 changes: 1 addition & 1 deletion sensors/tests/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
10 changes: 7 additions & 3 deletions sensors/tests/test_pressure_sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
}
Expand Down

0 comments on commit 57f612a

Please sign in to comment.