From 70e8734a3cb82d69c68253bc87e6bcc87d8f2430 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Mon, 17 Jun 2024 11:58:01 -0400 Subject: [PATCH 01/26] Use logic or instead of hardcoded enums --- .../core/stepper_motor/motor_interrupt_handler.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp index 577d1cb41..9d8839b58 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -408,7 +408,11 @@ class MotorInterruptHandler { hardware.get_encoder_pulses(); #ifdef USE_SENSOR_MOVE if (buffered_move.sensor_id != can::ids::SensorId::UNUSED) { - auto binding = static_cast(0x3); // sync and report + auto binding = + static_cast(can::ids::SensorOutputBinding::sync) | + static_cast( + can::ids::SensorOutputBinding::report); // sync and + // report if (buffered_move.sensor_id == can::ids::SensorId::BOTH) { send_bind_message(buffered_move.sensor_type, can::ids::SensorId::S0, binding); From d6c137200652a869ff8ea578c275782957e07dd0 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Mon, 17 Jun 2024 11:58:59 -0400 Subject: [PATCH 02/26] when filling the bufffer auto baseline after the first 10 --- include/sensors/core/tasks/pressure_driver.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index a57867b49..cdaf45510 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include "can/core/can_writer_task.hpp" #include "can/core/ids.hpp" @@ -372,6 +373,13 @@ class MMR920 { auto response_pressure = pressure - current_pressure_baseline_pa; // do we want pressure or response pressure sensor_buffer_log(pressure); + + if (sensor_buffer_index == 10) { + current_pressure_baseline_pa = + std::accumulate(std::begin(*p_buff), std::end(*p_buff), 0) / + 10; + } + can_client.send_can_message( can::ids::NodeId::host, can::messages::ReadFromSensorResponse{ From c00db250aa357df19293cf6c1a3fd9af1e6ff38b Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Mon, 17 Jun 2024 11:59:56 -0400 Subject: [PATCH 03/26] the new method that lets us increase senstivity uncovered an issue with setting the sync line this way, it was turning off before the head node could detect it and stop --- include/sensors/core/tasks/pressure_driver.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index cdaf45510..c876231d0 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -364,6 +364,8 @@ class MMR920 { if (std::fabs(pressure - current_pressure_baseline_pa) > threshold_pascals) { hardware.set_sync(); + // force this to stay set long enough to debounce on other nodes + vTaskDelay(10); } else { hardware.reset_sync(); } From 4e177865d43065c7b12ed621bb8c59311bea7b53 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Mon, 17 Jun 2024 16:32:10 -0400 Subject: [PATCH 04/26] few tweaks to the way this works --- include/sensors/core/tasks/pressure_driver.hpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index c876231d0..12ee8b63d 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -356,7 +356,12 @@ class MMR920 { .message_index = m.message_index, .severity = can::ids::ErrorSeverity::unrecoverable, .error_code = can::ids::ErrorCode::over_pressure}); - } else { + } else if (!bind_sync) { + // if we're not using bind sync turn off the sync line + // we don't do this during bind sync because if it's triggering + // the sync line on purpose this causes bouncing on the line + // that turns off the sync and then immediately turns it back on + // and this can cause disrupt the behavior hardware.reset_sync(); } } @@ -380,6 +385,12 @@ class MMR920 { current_pressure_baseline_pa = std::accumulate(std::begin(*p_buff), std::end(*p_buff), 0) / 10; + for (auto i = sensor_buffer_index - 10; i < sensor_buffer_index; + i++) { + p_buff->at(sensor_buffer_index) = + p_buff->at(sensor_buffer_index) - + current_pressure_baseline_pa; + } } can_client.send_can_message( From d44cfa7bcf36dd71215fdf3e0220a24954455abf Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 18 Jun 2024 11:45:33 -0400 Subject: [PATCH 05/26] only compute the first 10 elements to self-baselien --- include/sensors/core/tasks/pressure_driver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 12ee8b63d..2f96f3cbb 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -383,7 +383,7 @@ class MMR920 { if (sensor_buffer_index == 10) { current_pressure_baseline_pa = - std::accumulate(std::begin(*p_buff), std::end(*p_buff), 0) / + std::accumulate(p_buff->begin(), p_buff->begin()+10, 0) / 10; for (auto i = sensor_buffer_index - 10; i < sensor_buffer_index; i++) { From ac0c76eb0d4b34e1d3f190af593a7120067bac6e Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 18 Jun 2024 11:45:55 -0400 Subject: [PATCH 06/26] don't immediatly turn of the recording when move completes --- .../stepper_motor/motor_interrupt_handler.hpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp index 9d8839b58..c8205a6bb 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -506,21 +506,6 @@ class MotorInterruptHandler { tick_count = 0x0; stall_handled = false; build_and_send_ack(ack_msg_id); -#ifdef USE_SENSOR_MOVE - if (buffered_move.sensor_id != can::ids::SensorId::UNUSED) { - auto binding = static_cast( - can::ids::SensorOutputBinding::sync); // make none?! - if (buffered_move.sensor_id == can::ids::SensorId::BOTH) { - send_bind_message(buffered_move.sensor_type, - can::ids::SensorId::S0, binding); - send_bind_message(buffered_move.sensor_type, - can::ids::SensorId::S1, binding); - } else { - send_bind_message(buffered_move.sensor_type, - buffered_move.sensor_id, binding); - } - } -#endif set_buffered_move(MotorMoveMessage{}); // update the stall check ideal encoder counts based on // last known location From 095fc71e41cd0ac8d028b8edd60167e5163cd8da Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 18 Jun 2024 11:52:48 -0400 Subject: [PATCH 07/26] save response pressure --- include/sensors/core/tasks/pressure_driver.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 2f96f3cbb..415298c55 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -378,8 +378,8 @@ class MMR920 { if (echo_this_time) { auto response_pressure = pressure - current_pressure_baseline_pa; - // do we want pressure or response pressure - sensor_buffer_log(pressure); + + sensor_buffer_log(response_pressure); if (sensor_buffer_index == 10) { current_pressure_baseline_pa = From fb63bf484576c89a3e5a941dea5d1376784b54cb Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 18 Jun 2024 11:53:44 -0400 Subject: [PATCH 08/26] fix the pressure leveling --- include/sensors/core/tasks/pressure_driver.hpp | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 415298c55..640d7a924 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -382,9 +382,11 @@ class MMR920 { sensor_buffer_log(response_pressure); if (sensor_buffer_index == 10) { + // we always start a read at sensor_buffer_index = 0 so we don't need to + // account for it being a circular buffer here current_pressure_baseline_pa = std::accumulate(p_buff->begin(), p_buff->begin()+10, 0) / - 10; + 10 + current_pressure_baseline_pa; for (auto i = sensor_buffer_index - 10; i < sensor_buffer_index; i++) { p_buff->at(sensor_buffer_index) = @@ -392,16 +394,6 @@ class MMR920 { current_pressure_baseline_pa; } } - - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::ReadFromSensorResponse{ - .message_index = m.message_index, - .sensor = can::ids::SensorType::pressure, - .sensor_id = sensor_id, - .sensor_data = - mmr920::reading_to_fixed_point(response_pressure)}); - } } auto handle_ongoing_temperature_response( From 7b0e300bd6f910db7724b0497f6e27a5c057560a Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 18 Jun 2024 12:00:16 -0400 Subject: [PATCH 09/26] rebase fixups --- include/sensors/core/tasks/pressure_driver.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 640d7a924..3299e11ef 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -385,15 +385,16 @@ class MMR920 { // we always start a read at sensor_buffer_index = 0 so we don't need to // account for it being a circular buffer here current_pressure_baseline_pa = - std::accumulate(p_buff->begin(), p_buff->begin()+10, 0) / + std::accumulate(sensor_buffer->begin(), sensor_buffer->begin()+10, 0) / 10 + current_pressure_baseline_pa; for (auto i = sensor_buffer_index - 10; i < sensor_buffer_index; i++) { - p_buff->at(sensor_buffer_index) = - p_buff->at(sensor_buffer_index) - + sensor_buffer->at(sensor_buffer_index) = + sensor_buffer->at(sensor_buffer_index) - current_pressure_baseline_pa; } } + } } auto handle_ongoing_temperature_response( From 920b861db9bb326eda7e978954596c5b8cedeb8a Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Thu, 20 Jun 2024 10:41:18 -0400 Subject: [PATCH 10/26] fix the hardware delay hack and get rid of another instance of it --- include/common/core/hardware_delay.hpp | 11 +++++++--- .../sensors/core/tasks/capacitive_driver.hpp | 20 +++---------------- 2 files changed, 11 insertions(+), 20 deletions(-) diff --git a/include/common/core/hardware_delay.hpp b/include/common/core/hardware_delay.hpp index d6c34ce45..7e236cdaf 100644 --- a/include/common/core/hardware_delay.hpp +++ b/include/common/core/hardware_delay.hpp @@ -1,12 +1,17 @@ #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 + +#ifdef ENABLE_CROSS_ONLY_HEADERS +#include "FreeRTOS.h" +#endif + template requires std::is_integral_v static void vtask_hardware_delay(T ticks) { -#ifndef INC_TASK_H - std::ignore = ticks; -#else +#ifdef ENABLE_CROSS_ONLY_HEADERS vTaskDelay(ticks); +#else + std::ignore = ticks; #endif } diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index 7e890a16b..38485ceab 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/include/sensors/core/tasks/capacitive_driver.hpp @@ -2,25 +2,11 @@ #include -#ifdef ENABLE_CROSS_ONLY_HEADERS -// TODO(fps 7/12/2023): This is super hacky and I hate throwing #ifdefs -// in our nicely host-independent code but for now we really just need -// the vTaskDelay function and hopefully sometime in the near future I -// can refactor this file with a nice templated sleep function. -#include "FreeRTOS.h" -// NOLINTNEXTLINE(cppcoreguidelines-macro-usage) -#define HACKY_TASK_SLEEP(___timeout___) vTaskDelay(___timeout___) - -#else - -// NOLINTNEXTLINE(cppcoreguidelines-macro-usage) -#define HACKY_TASK_SLEEP(___timeout___) (void)(___timeout___) -#endif - #include "can/core/can_writer_task.hpp" #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" @@ -63,11 +49,11 @@ class FDC1004 { // holding off for this PR. // Initial delay to avoid I2C bus traffic. - HACKY_TASK_SLEEP(100); + vtask_hardware_delay(100); update_capacitance_configuration(); // Second delay to ensure IC is ready to start // readings (and also to avoid I2C bus traffic). - HACKY_TASK_SLEEP(100); + vtask_hardware_delay(100); set_sample_rate(); _initialized = true; } From 544d9c08e6af50bc6052e9af35a11fb30e544c12 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Fri, 21 Jun 2024 11:56:25 -0400 Subject: [PATCH 11/26] send ack after sending buffer --- include/sensors/core/tasks/capacitive_driver.hpp | 4 ++++ include/sensors/core/tasks/pressure_driver.hpp | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index 38485ceab..df60ac907 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/include/sensors/core/tasks/capacitive_driver.hpp @@ -217,6 +217,10 @@ class FDC1004 { } (*sensor_buffer).at(i) = 0; } + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::Acknowledgment{ + .message_index = message_index}); #else std::ignore = message_index; #endif diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 3299e11ef..fe5d5ac6b 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -311,6 +311,10 @@ class MMR920 { } (*sensor_buffer).at(current_index) = 0; } + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::Acknowledgment{ + .message_index = message_index}); } auto handle_ongoing_pressure_response(i2c::messages::TransactionResponse &m) From 6c30b6e2ba1a03750ee91160225c598cbb4c112e Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Fri, 21 Jun 2024 16:19:01 -0400 Subject: [PATCH 12/26] fixes to the circular buffer --- .../sensors/core/tasks/pressure_driver.hpp | 30 +++++++++++++------ 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index fe5d5ac6b..01e367c4e 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -71,6 +71,8 @@ class MMR920 { echoing = should_echo; if (should_echo) { sensor_buffer_index = 0; // reset buffer index + crossed_buffer_index=false; + sensor_buffer->fill(0.0); } } @@ -231,12 +233,12 @@ class MMR920 { } 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; + crossed_buffer_index=true; } - - sensor_buffer->at(sensor_buffer_index) = data; - sensor_buffer_index++; } auto save_temperature(int32_t data) -> bool { @@ -291,10 +293,21 @@ class MMR920 { } void send_accumulated_sensor_data(uint32_t message_index) { - for (int i = 0; i < static_cast(SENSOR_BUFFER_SIZE); i++) { + auto start = 0; + auto count = sensor_buffer_index; + if (crossed_buffer_index == true) { + start = sensor_buffer_index; + count = SENSOR_BUFFER_SIZE; + } + + 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 + sensor_buffer_index) % + int current_index = (i + start) % static_cast(SENSOR_BUFFER_SIZE); can_client.send_can_message( @@ -309,7 +322,6 @@ class MMR920 { // slow it down so the can buffer doesn't choke vtask_hardware_delay(50); } - (*sensor_buffer).at(current_index) = 0; } can_client.send_can_message( can::ids::NodeId::host, @@ -385,9 +397,8 @@ class MMR920 { sensor_buffer_log(response_pressure); - if (sensor_buffer_index == 10) { - // we always start a read at sensor_buffer_index = 0 so we don't need to - // account for it being a circular buffer here + if (sensor_buffer_index == 10 && !crossed_buffer_index) { + current_pressure_baseline_pa = std::accumulate(sensor_buffer->begin(), sensor_buffer->begin()+10, 0) / 10 + current_pressure_baseline_pa; @@ -583,6 +594,7 @@ class MMR920 { } std::array *sensor_buffer; uint16_t sensor_buffer_index = 0; + bool crossed_buffer_index = false; }; } // namespace tasks From 372383bc7d7bc0143e47f5be36cb73720a9b6826 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Fri, 21 Jun 2024 16:19:19 -0400 Subject: [PATCH 13/26] don't need this and can cause the process to choke --- include/sensors/core/tasks/pressure_driver.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 01e367c4e..f83d203a1 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -386,7 +386,6 @@ class MMR920 { threshold_pascals) { hardware.set_sync(); // force this to stay set long enough to debounce on other nodes - vTaskDelay(10); } else { hardware.reset_sync(); } From 4b7b24dc54a856c3f70be55e19d87d4a9efdf3bd Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Fri, 21 Jun 2024 16:19:31 -0400 Subject: [PATCH 14/26] send the can messages faster --- include/sensors/core/tasks/pressure_driver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index f83d203a1..6772ba542 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -320,7 +320,7 @@ class MMR920 { (*sensor_buffer).at(current_index))}); if (i % 10 == 0) { // slow it down so the can buffer doesn't choke - vtask_hardware_delay(50); + vtask_hardware_delay(20); } } can_client.send_can_message( From 994d6ef44f7818a7f291cdc27ee84f2b45ffc8fc Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Fri, 21 Jun 2024 16:33:33 -0400 Subject: [PATCH 15/26] format --- .../sensors/core/tasks/capacitive_driver.hpp | 5 ++-- .../sensors/core/tasks/pressure_driver.hpp | 27 +++++++++---------- .../core/tasks/pressure_sensor_task.hpp | 1 - 3 files changed, 15 insertions(+), 18 deletions(-) diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index df60ac907..c59d71de4 100644 --- a/include/sensors/core/tasks/capacitive_driver.hpp +++ b/include/sensors/core/tasks/capacitive_driver.hpp @@ -218,9 +218,8 @@ class FDC1004 { (*sensor_buffer).at(i) = 0; } can_client.send_can_message( - can::ids::NodeId::host, - can::messages::Acknowledgment{ - .message_index = message_index}); + can::ids::NodeId::host, + can::messages::Acknowledgment{.message_index = message_index}); #else std::ignore = message_index; #endif diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 6772ba542..5c448e7ea 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -71,7 +71,7 @@ class MMR920 { echoing = should_echo; if (should_echo) { sensor_buffer_index = 0; // reset buffer index - crossed_buffer_index=false; + crossed_buffer_index = false; sensor_buffer->fill(0.0); } } @@ -237,7 +237,7 @@ class MMR920 { sensor_buffer_index++; if (sensor_buffer_index == SENSOR_BUFFER_SIZE) { sensor_buffer_index = 0; - crossed_buffer_index=true; + crossed_buffer_index = true; } } @@ -301,14 +301,13 @@ class MMR920 { } can_client.send_can_message( - can::ids::NodeId::host, - can::messages::Acknowledgment{ - .message_index = count}); + 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); + int current_index = + (i + start) % static_cast(SENSOR_BUFFER_SIZE); can_client.send_can_message( can::ids::NodeId::host, @@ -324,9 +323,8 @@ class MMR920 { } } can_client.send_can_message( - can::ids::NodeId::host, - can::messages::Acknowledgment{ - .message_index = message_index}); + can::ids::NodeId::host, + can::messages::Acknowledgment{.message_index = message_index}); } auto handle_ongoing_pressure_response(i2c::messages::TransactionResponse &m) @@ -397,10 +395,11 @@ class MMR920 { sensor_buffer_log(response_pressure); if (sensor_buffer_index == 10 && !crossed_buffer_index) { - current_pressure_baseline_pa = - std::accumulate(sensor_buffer->begin(), sensor_buffer->begin()+10, 0) / - 10 + current_pressure_baseline_pa; + std::accumulate(sensor_buffer->begin(), + sensor_buffer->begin() + 10, 0) / + 10 + + current_pressure_baseline_pa; for (auto i = sensor_buffer_index - 10; i < sensor_buffer_index; i++) { sensor_buffer->at(sensor_buffer_index) = @@ -546,7 +545,7 @@ class MMR920 { static constexpr uint16_t MAX_PRESSURE_TIME_MS = 200; #ifdef USE_PRESSURE_MOVE mmr920::MeasurementRate measurement_mode_rate = - mmr920::MeasurementRate::MEASURE_1; + mmr920::MeasurementRate::MEASURE_2; #else mmr920::MeasurementRate measurement_mode_rate = mmr920::MeasurementRate::MEASURE_4; diff --git a/include/sensors/core/tasks/pressure_sensor_task.hpp b/include/sensors/core/tasks/pressure_sensor_task.hpp index 39b158c9d..60b0f45a2 100644 --- a/include/sensors/core/tasks/pressure_sensor_task.hpp +++ b/include/sensors/core/tasks/pressure_sensor_task.hpp @@ -81,7 +81,6 @@ class PressureMessageHandler { void visit(const can::messages::SendAccumulatedSensorDataRequest &m) { LOG("Received request to dump pressure data buffer"); - driver.send_accumulated_sensor_data(m.message_index); } From 92bc6793f0c1e6b24fb407bc184767e4a7ee26a4 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Mon, 24 Jun 2024 10:52:21 -0400 Subject: [PATCH 16/26] make the moving baseline log clearer --- .../sensors/core/tasks/pressure_driver.hpp | 40 ++++++++++++++----- 1 file changed, 29 insertions(+), 11 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 5c448e7ea..f81cee5a3 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -31,6 +31,8 @@ using namespace can::ids; * @tparam CanClient */ +constexpr auto AUTO_BASELINE_SAMPLES = 10; + template class MMR920 { @@ -73,6 +75,7 @@ class MMR920 { sensor_buffer_index = 0; // reset buffer index crossed_buffer_index = false; sensor_buffer->fill(0.0); + current_moving_pressure_baseline_pa = 0.0; } } @@ -380,7 +383,8 @@ class MMR920 { } } if (bind_sync) { - if (std::fabs(pressure - current_pressure_baseline_pa) > + if (std::fabs(pressure - current_pressure_baseline_pa - + current_moving_pressure_baseline_pa) > threshold_pascals) { hardware.set_sync(); // force this to stay set long enough to debounce on other nodes @@ -390,21 +394,34 @@ class MMR920 { } if (echo_this_time) { - auto response_pressure = pressure - current_pressure_baseline_pa; + auto response_pressure = pressure - current_pressure_baseline_pa - + current_moving_pressure_baseline_pa; sensor_buffer_log(response_pressure); - if (sensor_buffer_index == 10 && !crossed_buffer_index) { - current_pressure_baseline_pa = - std::accumulate(sensor_buffer->begin(), - sensor_buffer->begin() + 10, 0) / - 10 + - current_pressure_baseline_pa; - for (auto i = sensor_buffer_index - 10; i < sensor_buffer_index; - i++) { + if (sensor_buffer_index == AUTO_BASELINE_SAMPLES && + !crossed_buffer_index) { + // this is the auto-base lining during a move. It requires that + // a BaselineSensorRequest is sent prior to a move using the + // auto baseline. it works by taking the first + // AUTO_BASELINE_SAMPLES taken during the move (when index < 10 + // and we haven't crossed the circular buffer barrier yet) it + // then takes the average of those samples to create a new + // baseline factor + current_moving_pressure_baseline_pa = + std::accumulate( + sensor_buffer->begin(), + sensor_buffer->begin() + AUTO_BASELINE_SAMPLES, 0) / + AUTO_BASELINE_SAMPLES; + for (auto i = sensor_buffer_index - AUTO_BASELINE_SAMPLES; + i < sensor_buffer_index; 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 + // been applied sensor_buffer->at(sensor_buffer_index) = sensor_buffer->at(sensor_buffer_index) - - current_pressure_baseline_pa; + current_moving_pressure_baseline_pa; } } } @@ -561,6 +578,7 @@ class MMR920 { uint16_t total_baseline_reads = 1; float current_pressure_baseline_pa = 0; + float current_moving_pressure_baseline_pa = 0; float current_temperature_baseline = 0; size_t max_pressure_consecutive_readings = 0; From 0c0e09b340f0f3234aeabe508cba8fdecc25f6bc Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 25 Jun 2024 12:45:45 -0400 Subject: [PATCH 17/26] use the real world sensor speed --- include/sensors/core/tasks/pressure_driver.hpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index f81cee5a3..2cfcc1522 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -560,13 +560,8 @@ class MMR920 { * exceed the threshold for the entirety of this period. */ static constexpr uint16_t MAX_PRESSURE_TIME_MS = 200; -#ifdef USE_PRESSURE_MOVE - mmr920::MeasurementRate measurement_mode_rate = - mmr920::MeasurementRate::MEASURE_2; -#else mmr920::MeasurementRate measurement_mode_rate = mmr920::MeasurementRate::MEASURE_4; -#endif bool _initialized = false; bool echoing = false; From 5ba41c16a8d10e158eb9c55c238847294dc2ed79 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 25 Jun 2024 14:42:54 -0400 Subject: [PATCH 18/26] change auto baseline slightly to ignore the first N samples --- .../sensors/core/tasks/pressure_driver.hpp | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 2cfcc1522..090a73d59 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -31,7 +31,8 @@ using namespace can::ids; * @tparam CanClient */ -constexpr auto AUTO_BASELINE_SAMPLES = 10; +constexpr auto AUTO_BASELINE_START = 10; +constexpr auto AUTO_BASELINE_END = 20; template @@ -399,26 +400,26 @@ class MMR920 { sensor_buffer_log(response_pressure); - if (sensor_buffer_index == AUTO_BASELINE_SAMPLES && + if (sensor_buffer_index == AUTO_BASELINE_END && !crossed_buffer_index) { // this is the auto-base lining during a move. It requires that // a BaselineSensorRequest is sent prior to a move using the - // auto baseline. it works by taking the first - // AUTO_BASELINE_SAMPLES taken during the move (when index < 10 + // auto baseline. it works by taking several samples + // at the beginning of the move but after noise has stopped. // and we haven't crossed the circular buffer barrier yet) it // then takes the average of those samples to create a new // baseline factor current_moving_pressure_baseline_pa = std::accumulate( - sensor_buffer->begin(), - sensor_buffer->begin() + AUTO_BASELINE_SAMPLES, 0) / - AUTO_BASELINE_SAMPLES; - for (auto i = sensor_buffer_index - AUTO_BASELINE_SAMPLES; + sensor_buffer->begin() + AUTO_BASELINE_START, + sensor_buffer->begin() + AUTO_BASELINE_END, 0) / + (AUTO_BASELINE_END - AUTO_BASELINE_START); + for (auto i = sensor_buffer_index - AUTO_BASELINE_SAMPLES; i < sensor_buffer_index; 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 - // been applied + // 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 + // been applied sensor_buffer->at(sensor_buffer_index) = sensor_buffer->at(sensor_buffer_index) - current_moving_pressure_baseline_pa; From bbb5df4b1522ba5c667e3074561e3d7a6d73fa49 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 25 Jun 2024 14:43:23 -0400 Subject: [PATCH 19/26] add a new sensor sync value --- include/bootloader/core/ids.h | 1 + include/can/core/ids.hpp | 1 + 2 files changed, 2 insertions(+) diff --git a/include/bootloader/core/ids.h b/include/bootloader/core/ids.h index 92508d71f..78a9d69da 100644 --- a/include/bootloader/core/ids.h +++ b/include/bootloader/core/ids.h @@ -205,6 +205,7 @@ typedef enum { can_sensoroutputbinding_sync = 0x1, can_sensoroutputbinding_report = 0x2, can_sensoroutputbinding_max_threshold_sync = 0x4, + can_sensoroutputbinding_auto_baseline_report = 0x8, } CANSensorOutputBinding; /** How a sensor's threshold should be interpreted. */ diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp index 965edec03..e32dd038e 100644 --- a/include/can/core/ids.hpp +++ b/include/can/core/ids.hpp @@ -215,6 +215,7 @@ enum class SensorOutputBinding { sync = 0x1, report = 0x2, max_threshold_sync = 0x4, + auto_baseline_report = 0x8, }; /** How a sensor's threshold should be interpreted. */ From 31093175d9ec40b005219dc05540728a05b8e84b Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 25 Jun 2024 14:43:49 -0400 Subject: [PATCH 20/26] don't trigger before autobaseline is finished --- include/sensors/core/tasks/pressure_driver.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 090a73d59..4cabb5519 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -383,12 +383,12 @@ class MMR920 { hardware.reset_sync(); } } - if (bind_sync) { + if (bind_sync && + (sensor_buffer_index > AUTO_BASELINE_END || crossed_buffer_index)) { if (std::fabs(pressure - current_pressure_baseline_pa - current_moving_pressure_baseline_pa) > threshold_pascals) { hardware.set_sync(); - // force this to stay set long enough to debounce on other nodes } else { hardware.reset_sync(); } From 7a2610764389c92e166db47bc2bc58986b46c9e3 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 25 Jun 2024 14:49:26 -0400 Subject: [PATCH 21/26] add new binding type for sensors and use the auto-baselineing during sensor moves --- .../stepper_motor/motor_interrupt_handler.hpp | 5 +++-- .../sensors/core/tasks/pressure_driver.hpp | 22 +++++++++++++------ .../core/tasks/pressure_sensor_task.hpp | 4 ++++ 3 files changed, 22 insertions(+), 9 deletions(-) diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp index c8205a6bb..f79bd79c5 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -411,8 +411,9 @@ class MotorInterruptHandler { auto binding = static_cast(can::ids::SensorOutputBinding::sync) | static_cast( - can::ids::SensorOutputBinding::report); // sync and - // report + can::ids::SensorOutputBinding::report) | + static_cast( + can::ids::SensorOutputBinding::auto_baseline_report); if (buffered_move.sensor_id == can::ids::SensorId::BOTH) { send_bind_message(buffered_move.sensor_type, can::ids::SensorId::S0, binding); diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 4cabb5519..69690d91e 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -76,10 +76,16 @@ class MMR920 { sensor_buffer_index = 0; // reset buffer index crossed_buffer_index = false; sensor_buffer->fill(0.0); - current_moving_pressure_baseline_pa = 0.0; } } + void set_auto_baseline_report(bool should_auto) { + enable_auto_baseline = should_auto; + // Always set this to 0, we want to clear it if disabled and + // reset if if we haven't baselined yet + current_moving_pressure_baseline_pa = 0.0; + } + void set_bind_sync(bool should_bind) { bind_sync = should_bind; hardware.reset_sync(); @@ -400,7 +406,8 @@ class MMR920 { sensor_buffer_log(response_pressure); - if (sensor_buffer_index == AUTO_BASELINE_END && + if (enable_auto_baseline && + sensor_buffer_index == AUTO_BASELINE_END && !crossed_buffer_index) { // this is the auto-base lining during a move. It requires that // a BaselineSensorRequest is sent prior to a move using the @@ -414,12 +421,12 @@ class MMR920 { sensor_buffer->begin() + AUTO_BASELINE_START, sensor_buffer->begin() + AUTO_BASELINE_END, 0) / (AUTO_BASELINE_END - AUTO_BASELINE_START); - for (auto i = sensor_buffer_index - AUTO_BASELINE_SAMPLES; + for (auto i = sensor_buffer_index - AUTO_BASELINE_SAMPLES; i < sensor_buffer_index; 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 - // been applied + // 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 + // been applied sensor_buffer->at(sensor_buffer_index) = sensor_buffer->at(sensor_buffer_index) - current_moving_pressure_baseline_pa; @@ -566,6 +573,7 @@ class MMR920 { bool _initialized = false; bool echoing = false; + bool enable_auto_baseline = false; bool bind_sync = false; bool max_pressure_sync = false; diff --git a/include/sensors/core/tasks/pressure_sensor_task.hpp b/include/sensors/core/tasks/pressure_sensor_task.hpp index 60b0f45a2..83f4294a8 100644 --- a/include/sensors/core/tasks/pressure_sensor_task.hpp +++ b/include/sensors/core/tasks/pressure_sensor_task.hpp @@ -136,6 +136,10 @@ class PressureMessageHandler { driver.set_max_bind_sync( m.binding & static_cast( can::ids::SensorOutputBinding::max_threshold_sync)); + driver.set_auto_baseline_report( + m.binding & + static_cast( + can::ids::SensorOutputBinding::auto_baseline_report)); std::array tags{utils::ResponseTag::IS_PART_OF_POLL, utils::ResponseTag::POLL_IS_CONTINUOUS}; auto tags_as_int = utils::byte_from_tags(tags); From 787b983b1ba31b54ba386a47a7a1496e8c58f30b Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 25 Jun 2024 15:10:33 -0400 Subject: [PATCH 22/26] forgot to change an old reference --- include/sensors/core/tasks/pressure_driver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 69690d91e..c3c052d55 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -421,7 +421,7 @@ class MMR920 { sensor_buffer->begin() + AUTO_BASELINE_START, sensor_buffer->begin() + AUTO_BASELINE_END, 0) / (AUTO_BASELINE_END - AUTO_BASELINE_START); - for (auto i = sensor_buffer_index - AUTO_BASELINE_SAMPLES; + for (auto i = sensor_buffer_index - AUTO_BASELINE_END; i < sensor_buffer_index; i++) { // apply the moving baseline to older samples to so that // data is in the same format as later samples, don't apply From fdd6637dffede4fa2f6a33acf987197e0820476b Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 25 Jun 2024 16:13:11 -0400 Subject: [PATCH 23/26] format lint --- include/sensors/core/tasks/pressure_driver.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index c3c052d55..be5994c25 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -305,7 +305,7 @@ class MMR920 { void send_accumulated_sensor_data(uint32_t message_index) { auto start = 0; auto count = sensor_buffer_index; - if (crossed_buffer_index == true) { + if (crossed_buffer_index) { start = sensor_buffer_index; count = SENSOR_BUFFER_SIZE; } @@ -420,7 +420,7 @@ class MMR920 { std::accumulate( sensor_buffer->begin() + AUTO_BASELINE_START, sensor_buffer->begin() + AUTO_BASELINE_END, 0) / - (AUTO_BASELINE_END - AUTO_BASELINE_START); + float(AUTO_BASELINE_END - AUTO_BASELINE_START); for (auto i = sensor_buffer_index - AUTO_BASELINE_END; i < sensor_buffer_index; i++) { // apply the moving baseline to older samples to so that From 5a60de694ecf08d1e53a9f02ede619a11ca49245 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 25 Jun 2024 16:13:20 -0400 Subject: [PATCH 24/26] preserve old behavior --- .../sensors/core/tasks/pressure_driver.hpp | 43 +++++++++++++++---- 1 file changed, 34 insertions(+), 9 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index be5994c25..898eb4a8e 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -389,22 +389,47 @@ class MMR920 { hardware.reset_sync(); } } - if (bind_sync && - (sensor_buffer_index > AUTO_BASELINE_END || crossed_buffer_index)) { - if (std::fabs(pressure - current_pressure_baseline_pa - - current_moving_pressure_baseline_pa) > - threshold_pascals) { - hardware.set_sync(); + if (bind_sync) { + if (enable_auto_baseline) { + if ((sensor_buffer_index > AUTO_BASELINE_END || + crossed_buffer_index)) { + if (std::fabs(pressure - current_pressure_baseline_pa - + current_moving_pressure_baseline_pa) > + threshold_pascals) { + hardware.set_sync(); + } else { + hardware.reset_sync(); + } + } } else { - hardware.reset_sync(); + if (std::fabs(pressure - current_pressure_baseline_pa) > + threshold_pascals) { + hardware.set_sync(); + } else { + hardware.reset_sync(); + } } } if (echo_this_time) { - auto response_pressure = pressure - current_pressure_baseline_pa - - current_moving_pressure_baseline_pa; + auto response_pressure = pressure - current_pressure_baseline_pa; + if (enable_auto_baseline) { + // apply moving baseline if using + 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 (enable_auto_baseline && sensor_buffer_index == AUTO_BASELINE_END && From ecd943bf500b3cb7e2a3c4b6c8ad1093b44c2885 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 25 Jun 2024 16:45:33 -0400 Subject: [PATCH 25/26] reduce the complexity in the handle function to satisfy lint --- .../sensors/core/tasks/pressure_driver.hpp | 523 +++++++++--------- 1 file changed, 261 insertions(+), 262 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 898eb4a8e..2969b0d58 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -337,309 +337,308 @@ class MMR920 { can::messages::Acknowledgment{.message_index = message_index}); } - auto handle_ongoing_pressure_response(i2c::messages::TransactionResponse &m) - -> void { - if (!bind_sync && !echoing && !max_pressure_sync) { - auto reg_id = utils::reg_from_id(m.id.token); - stop_continuous_polling(m.id.token, static_cast(reg_id)); + auto compute_auto_baseline() -> void { + // this is the auto-base lining during a move. It requires that + // a BaselineSensorRequest is sent prior to a move using the + // auto baseline. it works by taking several samples + // at the beginning of the move but after noise has stopped. + // and we haven't crossed the circular buffer barrier yet) it + // then takes the average of those samples to create a new + // baseline factor + current_moving_pressure_baseline_pa = + 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++) { + // 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 + // been applied + sensor_buffer->at(sensor_buffer_index) = + sensor_buffer->at(sensor_buffer_index) - + current_moving_pressure_baseline_pa; } + } - bool echo_this_time = echoing; - - // Pressure is always a three-byte value - static_cast(bit_utils::bytes_to_int(m.read_buffer.cbegin(), - m.read_buffer.cend(), - temporary_data_store)); - - uint32_t shifted_data_store = temporary_data_store >> 8; - - save_pressure(shifted_data_store); - auto pressure = mmr920::PressureResult::to_pressure( - _registers.pressure_result.reading, sensor_version); - - if (max_pressure_sync) { - bool this_tick_over_threshold = - std::fabs(pressure - current_pressure_baseline_pa) >= - mmr920::get_max_pressure_reading(sensor_version); - bool over_threshold = false; - if (this_tick_over_threshold) { - max_pressure_consecutive_readings = - std::min(max_pressure_consecutive_readings + 1, - max_pressure_required_readings); - over_threshold = (max_pressure_consecutive_readings == - max_pressure_required_readings); - echo_this_time = true; - } else { - max_pressure_consecutive_readings = 0; - } - if (over_threshold) { + auto handle_sync_threshold(float pressure) -> void { + if (enable_auto_baseline) { + if ((sensor_buffer_index > AUTO_BASELINE_END || + crossed_buffer_index) && + (std::fabs(pressure - current_pressure_baseline_pa - + current_moving_pressure_baseline_pa) > + threshold_pascals)) { hardware.set_sync(); - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::ErrorMessage{ - .message_index = m.message_index, - .severity = can::ids::ErrorSeverity::unrecoverable, - .error_code = can::ids::ErrorCode::over_pressure}); - } else if (!bind_sync) { - // if we're not using bind sync turn off the sync line - // we don't do this during bind sync because if it's triggering - // the sync line on purpose this causes bouncing on the line - // that turns off the sync and then immediately turns it back on - // and this can cause disrupt the behavior + } else { hardware.reset_sync(); } } - if (bind_sync) { - if (enable_auto_baseline) { - if ((sensor_buffer_index > AUTO_BASELINE_END || - crossed_buffer_index)) { - if (std::fabs(pressure - current_pressure_baseline_pa - - current_moving_pressure_baseline_pa) > - threshold_pascals) { - hardware.set_sync(); - } else { - hardware.reset_sync(); - } - } + else { + if (std::fabs(pressure - current_pressure_baseline_pa) > + threshold_pascals) { + hardware.set_sync(); } else { - if (std::fabs(pressure - current_pressure_baseline_pa) > - threshold_pascals) { - hardware.set_sync(); - } else { - hardware.reset_sync(); - } + hardware.reset_sync(); } } + } - if (echo_this_time) { - auto response_pressure = pressure - current_pressure_baseline_pa; - if (enable_auto_baseline) { - // apply moving baseline if using - 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)}); - } + auto handle_ongoing_pressure_response(i2c::messages::TransactionResponse &m) + -> void { + if (!bind_sync && !echoing && !max_pressure_sync) { + auto reg_id = utils::reg_from_id(m.id.token); + stop_continuous_polling(m.id.token, static_cast(reg_id)); + } - if (enable_auto_baseline && - sensor_buffer_index == AUTO_BASELINE_END && - !crossed_buffer_index) { - // this is the auto-base lining during a move. It requires that - // a BaselineSensorRequest is sent prior to a move using the - // auto baseline. it works by taking several samples - // at the beginning of the move but after noise has stopped. - // and we haven't crossed the circular buffer barrier yet) it - // then takes the average of those samples to create a new - // baseline factor - current_moving_pressure_baseline_pa = - std::accumulate( - sensor_buffer->begin() + AUTO_BASELINE_START, - sensor_buffer->begin() + AUTO_BASELINE_END, 0) / - float(AUTO_BASELINE_END - AUTO_BASELINE_START); - for (auto i = sensor_buffer_index - AUTO_BASELINE_END; - i < sensor_buffer_index; 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 - // been applied - sensor_buffer->at(sensor_buffer_index) = - sensor_buffer->at(sensor_buffer_index) - - current_moving_pressure_baseline_pa; - } - } + bool echo_this_time = echoing; + + // Pressure is always a three-byte value + static_cast(bit_utils::bytes_to_int( + m.read_buffer.cbegin(), m.read_buffer.cend(), temporary_data_store)); + + uint32_t shifted_data_store = temporary_data_store >> 8; + + save_pressure(shifted_data_store); + auto pressure = mmr920::PressureResult::to_pressure( + _registers.pressure_result.reading, sensor_version); + + if (max_pressure_sync) { + bool this_tick_over_threshold = + std::fabs(pressure - current_pressure_baseline_pa) >= + mmr920::get_max_pressure_reading(sensor_version); + bool over_threshold = false; + if (this_tick_over_threshold) { + max_pressure_consecutive_readings = + std::min(max_pressure_consecutive_readings + 1, + max_pressure_required_readings); + over_threshold = (max_pressure_consecutive_readings == + max_pressure_required_readings); + echo_this_time = true; + } else { + max_pressure_consecutive_readings = 0; + } + if (over_threshold) { + hardware.set_sync(); + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = can::ids::ErrorCode::over_pressure}); + } else if (!bind_sync) { + // if we're not using bind sync turn off the sync line + // we don't do this during bind sync because if it's triggering + // the sync line on purpose this causes bouncing on the line + // that turns off the sync and then immediately turns it back on + // and this can cause disrupt the behavior + hardware.reset_sync(); } } + if (bind_sync) { + handle_sync_threshold(pressure); + } - auto handle_ongoing_temperature_response( - i2c::messages::TransactionResponse &m) -> void { - if (!bind_sync && !echoing) { - auto reg_id = utils::reg_from_id(m.id.token); - stop_continuous_polling(m.id.token, static_cast(reg_id)); + if (echo_this_time) { + auto response_pressure = pressure - current_pressure_baseline_pa; + if (enable_auto_baseline) { + // apply moving baseline if using + response_pressure -= current_moving_pressure_baseline_pa; } - - // Pressure is always a three-byte value - static_cast(bit_utils::bytes_to_int(m.read_buffer.cbegin(), - m.read_buffer.cend(), - temporary_data_store)); - - uint32_t shifted_data_store = temporary_data_store >> 8; - - save_temperature(shifted_data_store); - - if (echoing) { - auto temperature = mmr920::TemperatureResult::to_temperature( - _registers.temperature_result.reading); + 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 = m.message_index, - .sensor = can::ids::SensorType::pressure_temperature, + .message_index = 0, + .sensor = can::ids::SensorType::pressure, .sensor_id = sensor_id, .sensor_data = - mmr920::reading_to_fixed_point(temperature)}); + mmr920::reading_to_fixed_point(response_pressure)}); } - } - auto handle_baseline_pressure_response( - i2c::messages::TransactionResponse &m) -> void { - static_cast(bit_utils::bytes_to_int(m.read_buffer.cbegin(), - m.read_buffer.cend(), - temporary_data_store)); + if (enable_auto_baseline && sensor_buffer_index == AUTO_BASELINE_END && + !crossed_buffer_index) { + compute_auto_baseline(); + } + } +} - uint32_t shifted_data_store = temporary_data_store >> 8; +auto handle_ongoing_temperature_response(i2c::messages::TransactionResponse &m) + -> void { + if (!bind_sync && !echoing) { + auto reg_id = utils::reg_from_id(m.id.token); + stop_continuous_polling(m.id.token, static_cast(reg_id)); + } - auto pressure = mmr920::PressureResult::to_pressure(shifted_data_store, - sensor_version); - pressure_running_total += pressure; + // Pressure is always a three-byte value + static_cast(bit_utils::bytes_to_int( + m.read_buffer.cbegin(), m.read_buffer.cend(), temporary_data_store)); - if (!m.id.is_completed_poll) { - return; - } + uint32_t shifted_data_store = temporary_data_store >> 8; - auto current_pressure_baseline_pa = - pressure_running_total / total_baseline_reads; - auto pressure_fixed_point = - mmr920::reading_to_fixed_point(current_pressure_baseline_pa); + save_temperature(shifted_data_store); - // FIXME This should be tied to the set threshold - // command so we can completely remove the base line sensor - // request from all sensors! - if (utils::tag_in_token(m.id.token, - utils::ResponseTag::IS_THRESHOLD_SENSE)) { - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::BaselineSensorResponse{ - .message_index = m.message_index, - .sensor = can::ids::SensorType::pressure, - .offset_average = pressure_fixed_point}); - set_threshold(current_pressure_baseline_pa, - can::ids::SensorThresholdMode::auto_baseline, - m.message_index, false); - } else { - auto message = can::messages::ReadFromSensorResponse{ + if (echoing) { + auto temperature = mmr920::TemperatureResult::to_temperature( + _registers.temperature_result.reading); + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ReadFromSensorResponse{ .message_index = m.message_index, - .sensor = SensorType::pressure, + .sensor = can::ids::SensorType::pressure_temperature, .sensor_id = sensor_id, - .sensor_data = pressure_fixed_point}; - can_client.send_can_message(can::ids::NodeId::host, message); - } - pressure_running_total = 0x0; + .sensor_data = mmr920::reading_to_fixed_point(temperature)}); } +} - auto handle_baseline_temperature_response( - i2c::messages::TransactionResponse &m) -> void { - static_cast(bit_utils::bytes_to_int(m.read_buffer.cbegin(), - m.read_buffer.cend(), - temporary_data_store)); +auto handle_baseline_pressure_response(i2c::messages::TransactionResponse &m) + -> void { + static_cast(bit_utils::bytes_to_int( + m.read_buffer.cbegin(), m.read_buffer.cend(), temporary_data_store)); - uint32_t shifted_data_store = temporary_data_store >> 8; + uint32_t shifted_data_store = temporary_data_store >> 8; - auto temperature = - mmr920::TemperatureResult::to_temperature(shifted_data_store); - temperature_running_total += temperature; + auto pressure = + mmr920::PressureResult::to_pressure(shifted_data_store, sensor_version); + pressure_running_total += pressure; - if (!m.id.is_completed_poll) { - return; - } + if (!m.id.is_completed_poll) { + return; + } - auto current_temperature_baseline = - temperature_running_total / total_baseline_reads; - auto offset_fixed_point = - mmr920::reading_to_fixed_point(current_temperature_baseline); - if (echoing) { - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::BaselineSensorResponse{ - .message_index = m.message_index, - .sensor = can::ids::SensorType::pressure_temperature, - .offset_average = offset_fixed_point}); - } - temperature_running_total = 0x0; + auto current_pressure_baseline_pa = + pressure_running_total / total_baseline_reads; + auto pressure_fixed_point = + mmr920::reading_to_fixed_point(current_pressure_baseline_pa); + + // FIXME This should be tied to the set threshold + // command so we can completely remove the base line sensor + // request from all sensors! + if (utils::tag_in_token(m.id.token, + utils::ResponseTag::IS_THRESHOLD_SENSE)) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::BaselineSensorResponse{ + .message_index = m.message_index, + .sensor = can::ids::SensorType::pressure, + .offset_average = pressure_fixed_point}); + set_threshold(current_pressure_baseline_pa, + can::ids::SensorThresholdMode::auto_baseline, + m.message_index, false); + } else { + auto message = can::messages::ReadFromSensorResponse{ + .message_index = m.message_index, + .sensor = SensorType::pressure, + .sensor_id = sensor_id, + .sensor_data = pressure_fixed_point}; + can_client.send_can_message(can::ids::NodeId::host, message); } + pressure_running_total = 0x0; +} - auto get_can_client() -> CanClient & { return can_client; } +auto handle_baseline_temperature_response(i2c::messages::TransactionResponse &m) + -> void { + static_cast(bit_utils::bytes_to_int( + m.read_buffer.cbegin(), m.read_buffer.cend(), temporary_data_store)); - private: - I2CQueueWriter &writer; - I2CQueuePoller &poller; - CanClient &can_client; - OwnQueue &own_queue; - hardware::SensorHardwareBase &hardware; - const can::ids::SensorId &sensor_id; - const sensors::mmr920::SensorVersion sensor_version; + uint32_t shifted_data_store = temporary_data_store >> 8; - mmr920::MMR920RegisterMap _registers{}; - mmr920::FilterSetting filter_setting = - mmr920::FilterSetting::LOW_PASS_FILTER; + auto temperature = + mmr920::TemperatureResult::to_temperature(shifted_data_store); + temperature_running_total += temperature; - static constexpr std::array MeasurementTimings{0.405, 0.81, 1.62, - 3.24}; // in msec - static constexpr float DEFAULT_DELAY_BUFFER = - 1.0; // in msec (TODO might need to change to fit in uint16_t) - static constexpr uint16_t STOP_DELAY = 0; + if (!m.id.is_completed_poll) { + return; + } - /** - * Time required before raising a Max Pressure error. The pressure must - * exceed the threshold for the entirety of this period. - */ - static constexpr uint16_t MAX_PRESSURE_TIME_MS = 200; - mmr920::MeasurementRate measurement_mode_rate = - mmr920::MeasurementRate::MEASURE_4; - - bool _initialized = false; - bool echoing = false; - bool enable_auto_baseline = false; - bool bind_sync = false; - bool max_pressure_sync = false; - - float pressure_running_total = 0; - float temperature_running_total = 0; - uint16_t total_baseline_reads = 1; - - float current_pressure_baseline_pa = 0; - float current_moving_pressure_baseline_pa = 0; - float current_temperature_baseline = 0; - - size_t max_pressure_consecutive_readings = 0; - size_t max_pressure_required_readings = 0; - - // TODO(fs, 2022-11-11): Need to figure out a realistic threshold. Pretty - // sure this is an arbitrarily large number to enable continuous reads. - float threshold_pascals = 100.0F; - float offset_average = 0; - - uint32_t temporary_data_store = 0x0; - - template - requires registers::WritableRegister - auto build_register_command(Reg ®) -> uint8_t { - return static_cast(reg.address); - } - - template - requires registers::WritableRegister - auto set_register(Reg ®) -> bool { - auto value = - // Ignore the typical linter warning because we're only using - // this on __packed structures that mimic hardware registers - // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) - *reinterpret_cast(®); - value &= Reg::value_mask; - return write(Reg::address, value); - } - std::array *sensor_buffer; - uint16_t sensor_buffer_index = 0; - bool crossed_buffer_index = false; + auto current_temperature_baseline = + temperature_running_total / total_baseline_reads; + auto offset_fixed_point = + mmr920::reading_to_fixed_point(current_temperature_baseline); + if (echoing) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::BaselineSensorResponse{ + .message_index = m.message_index, + .sensor = can::ids::SensorType::pressure_temperature, + .offset_average = offset_fixed_point}); + } + temperature_running_total = 0x0; +} + +auto get_can_client() -> CanClient & { return can_client; } + +private: +I2CQueueWriter &writer; +I2CQueuePoller &poller; +CanClient &can_client; +OwnQueue &own_queue; +hardware::SensorHardwareBase &hardware; +const can::ids::SensorId &sensor_id; +const sensors::mmr920::SensorVersion sensor_version; + +mmr920::MMR920RegisterMap _registers{}; +mmr920::FilterSetting filter_setting = mmr920::FilterSetting::LOW_PASS_FILTER; + +static constexpr std::array MeasurementTimings{0.405, 0.81, 1.62, + 3.24}; // in msec +static constexpr float DEFAULT_DELAY_BUFFER = + 1.0; // in msec (TODO might need to change to fit in uint16_t) +static constexpr uint16_t STOP_DELAY = 0; + +/** + * Time required before raising a Max Pressure error. The pressure must + * exceed the threshold for the entirety of this period. + */ +static constexpr uint16_t MAX_PRESSURE_TIME_MS = 200; +mmr920::MeasurementRate measurement_mode_rate = + mmr920::MeasurementRate::MEASURE_4; + +bool _initialized = false; +bool echoing = false; +bool enable_auto_baseline = false; +bool bind_sync = false; +bool max_pressure_sync = false; + +float pressure_running_total = 0; +float temperature_running_total = 0; +uint16_t total_baseline_reads = 1; + +float current_pressure_baseline_pa = 0; +float current_moving_pressure_baseline_pa = 0; +float current_temperature_baseline = 0; + +size_t max_pressure_consecutive_readings = 0; +size_t max_pressure_required_readings = 0; + +// TODO(fs, 2022-11-11): Need to figure out a realistic threshold. Pretty +// sure this is an arbitrarily large number to enable continuous reads. +float threshold_pascals = 100.0F; +float offset_average = 0; + +uint32_t temporary_data_store = 0x0; + +template +requires registers::WritableRegister +auto build_register_command(Reg ®) -> uint8_t { + return static_cast(reg.address); +} + +template +requires registers::WritableRegister +auto set_register(Reg ®) -> bool { + auto value = + // Ignore the typical linter warning because we're only using + // this on __packed structures that mimic hardware registers + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) + *reinterpret_cast(®); + value &= Reg::value_mask; + return write(Reg::address, value); +} +std::array *sensor_buffer; +uint16_t sensor_buffer_index = 0; +bool crossed_buffer_index = false; }; } // namespace tasks From a9cce7b7966dd9bd5adb6d62ad82adef2bf21ed1 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Tue, 25 Jun 2024 17:09:19 -0400 Subject: [PATCH 26/26] format --- .../sensors/core/tasks/pressure_driver.hpp | 454 +++++++++--------- 1 file changed, 230 insertions(+), 224 deletions(-) diff --git a/include/sensors/core/tasks/pressure_driver.hpp b/include/sensors/core/tasks/pressure_driver.hpp index 2969b0d58..cde222888 100644 --- a/include/sensors/core/tasks/pressure_driver.hpp +++ b/include/sensors/core/tasks/pressure_driver.hpp @@ -372,8 +372,7 @@ class MMR920 { } else { hardware.reset_sync(); } - } - else { + } else { if (std::fabs(pressure - current_pressure_baseline_pa) > threshold_pascals) { hardware.set_sync(); @@ -385,260 +384,267 @@ class MMR920 { auto handle_ongoing_pressure_response(i2c::messages::TransactionResponse &m) -> void { - if (!bind_sync && !echoing && !max_pressure_sync) { - auto reg_id = utils::reg_from_id(m.id.token); - stop_continuous_polling(m.id.token, static_cast(reg_id)); - } - - bool echo_this_time = echoing; - - // Pressure is always a three-byte value - static_cast(bit_utils::bytes_to_int( - m.read_buffer.cbegin(), m.read_buffer.cend(), temporary_data_store)); - - uint32_t shifted_data_store = temporary_data_store >> 8; - - save_pressure(shifted_data_store); - auto pressure = mmr920::PressureResult::to_pressure( - _registers.pressure_result.reading, sensor_version); - - if (max_pressure_sync) { - bool this_tick_over_threshold = - std::fabs(pressure - current_pressure_baseline_pa) >= - mmr920::get_max_pressure_reading(sensor_version); - bool over_threshold = false; - if (this_tick_over_threshold) { - max_pressure_consecutive_readings = - std::min(max_pressure_consecutive_readings + 1, - max_pressure_required_readings); - over_threshold = (max_pressure_consecutive_readings == - max_pressure_required_readings); - echo_this_time = true; - } else { - max_pressure_consecutive_readings = 0; + if (!bind_sync && !echoing && !max_pressure_sync) { + auto reg_id = utils::reg_from_id(m.id.token); + stop_continuous_polling(m.id.token, static_cast(reg_id)); } - if (over_threshold) { - hardware.set_sync(); - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::ErrorMessage{ - .message_index = m.message_index, - .severity = can::ids::ErrorSeverity::unrecoverable, - .error_code = can::ids::ErrorCode::over_pressure}); - } else if (!bind_sync) { - // if we're not using bind sync turn off the sync line - // we don't do this during bind sync because if it's triggering - // the sync line on purpose this causes bouncing on the line - // that turns off the sync and then immediately turns it back on - // and this can cause disrupt the behavior - hardware.reset_sync(); - } - } - if (bind_sync) { - handle_sync_threshold(pressure); - } - if (echo_this_time) { - auto response_pressure = pressure - current_pressure_baseline_pa; - if (enable_auto_baseline) { - // apply moving baseline if using - response_pressure -= current_moving_pressure_baseline_pa; + bool echo_this_time = echoing; + + // Pressure is always a three-byte value + static_cast(bit_utils::bytes_to_int(m.read_buffer.cbegin(), + m.read_buffer.cend(), + temporary_data_store)); + + uint32_t shifted_data_store = temporary_data_store >> 8; + + save_pressure(shifted_data_store); + auto pressure = mmr920::PressureResult::to_pressure( + _registers.pressure_result.reading, sensor_version); + + if (max_pressure_sync) { + bool this_tick_over_threshold = + std::fabs(pressure - current_pressure_baseline_pa) >= + mmr920::get_max_pressure_reading(sensor_version); + bool over_threshold = false; + if (this_tick_over_threshold) { + max_pressure_consecutive_readings = + std::min(max_pressure_consecutive_readings + 1, + max_pressure_required_readings); + over_threshold = (max_pressure_consecutive_readings == + max_pressure_required_readings); + echo_this_time = true; + } else { + max_pressure_consecutive_readings = 0; + } + if (over_threshold) { + hardware.set_sync(); + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ErrorMessage{ + .message_index = m.message_index, + .severity = can::ids::ErrorSeverity::unrecoverable, + .error_code = can::ids::ErrorCode::over_pressure}); + } else if (!bind_sync) { + // if we're not using bind sync turn off the sync line + // we don't do this during bind sync because if it's triggering + // the sync line on purpose this causes bouncing on the line + // that turns off the sync and then immediately turns it back on + // and this can cause disrupt the behavior + hardware.reset_sync(); + } } - 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 (bind_sync) { + handle_sync_threshold(pressure); } - if (enable_auto_baseline && sensor_buffer_index == AUTO_BASELINE_END && - !crossed_buffer_index) { - compute_auto_baseline(); + if (echo_this_time) { + auto response_pressure = pressure - current_pressure_baseline_pa; + if (enable_auto_baseline) { + // apply moving baseline if using + 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 (enable_auto_baseline && + sensor_buffer_index == AUTO_BASELINE_END && + !crossed_buffer_index) { + compute_auto_baseline(); + } } } -} -auto handle_ongoing_temperature_response(i2c::messages::TransactionResponse &m) - -> void { - if (!bind_sync && !echoing) { - auto reg_id = utils::reg_from_id(m.id.token); - stop_continuous_polling(m.id.token, static_cast(reg_id)); - } + auto handle_ongoing_temperature_response( + i2c::messages::TransactionResponse &m) -> void { + if (!bind_sync && !echoing) { + auto reg_id = utils::reg_from_id(m.id.token); + stop_continuous_polling(m.id.token, static_cast(reg_id)); + } - // Pressure is always a three-byte value - static_cast(bit_utils::bytes_to_int( - m.read_buffer.cbegin(), m.read_buffer.cend(), temporary_data_store)); + // Pressure is always a three-byte value + static_cast(bit_utils::bytes_to_int(m.read_buffer.cbegin(), + m.read_buffer.cend(), + temporary_data_store)); - uint32_t shifted_data_store = temporary_data_store >> 8; + uint32_t shifted_data_store = temporary_data_store >> 8; - save_temperature(shifted_data_store); + save_temperature(shifted_data_store); - if (echoing) { - auto temperature = mmr920::TemperatureResult::to_temperature( - _registers.temperature_result.reading); - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::ReadFromSensorResponse{ - .message_index = m.message_index, - .sensor = can::ids::SensorType::pressure_temperature, - .sensor_id = sensor_id, - .sensor_data = mmr920::reading_to_fixed_point(temperature)}); + if (echoing) { + auto temperature = mmr920::TemperatureResult::to_temperature( + _registers.temperature_result.reading); + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::ReadFromSensorResponse{ + .message_index = m.message_index, + .sensor = can::ids::SensorType::pressure_temperature, + .sensor_id = sensor_id, + .sensor_data = + mmr920::reading_to_fixed_point(temperature)}); + } } -} -auto handle_baseline_pressure_response(i2c::messages::TransactionResponse &m) - -> void { - static_cast(bit_utils::bytes_to_int( - m.read_buffer.cbegin(), m.read_buffer.cend(), temporary_data_store)); + auto handle_baseline_pressure_response( + i2c::messages::TransactionResponse &m) -> void { + static_cast(bit_utils::bytes_to_int(m.read_buffer.cbegin(), + m.read_buffer.cend(), + temporary_data_store)); - uint32_t shifted_data_store = temporary_data_store >> 8; + uint32_t shifted_data_store = temporary_data_store >> 8; - auto pressure = - mmr920::PressureResult::to_pressure(shifted_data_store, sensor_version); - pressure_running_total += pressure; + auto pressure = mmr920::PressureResult::to_pressure(shifted_data_store, + sensor_version); + pressure_running_total += pressure; - if (!m.id.is_completed_poll) { - return; - } + if (!m.id.is_completed_poll) { + return; + } - auto current_pressure_baseline_pa = - pressure_running_total / total_baseline_reads; - auto pressure_fixed_point = - mmr920::reading_to_fixed_point(current_pressure_baseline_pa); + auto current_pressure_baseline_pa = + pressure_running_total / total_baseline_reads; + auto pressure_fixed_point = + mmr920::reading_to_fixed_point(current_pressure_baseline_pa); - // FIXME This should be tied to the set threshold - // command so we can completely remove the base line sensor - // request from all sensors! - if (utils::tag_in_token(m.id.token, - utils::ResponseTag::IS_THRESHOLD_SENSE)) { - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::BaselineSensorResponse{ + // FIXME This should be tied to the set threshold + // command so we can completely remove the base line sensor + // request from all sensors! + if (utils::tag_in_token(m.id.token, + utils::ResponseTag::IS_THRESHOLD_SENSE)) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::BaselineSensorResponse{ + .message_index = m.message_index, + .sensor = can::ids::SensorType::pressure, + .offset_average = pressure_fixed_point}); + set_threshold(current_pressure_baseline_pa, + can::ids::SensorThresholdMode::auto_baseline, + m.message_index, false); + } else { + auto message = can::messages::ReadFromSensorResponse{ .message_index = m.message_index, - .sensor = can::ids::SensorType::pressure, - .offset_average = pressure_fixed_point}); - set_threshold(current_pressure_baseline_pa, - can::ids::SensorThresholdMode::auto_baseline, - m.message_index, false); - } else { - auto message = can::messages::ReadFromSensorResponse{ - .message_index = m.message_index, - .sensor = SensorType::pressure, - .sensor_id = sensor_id, - .sensor_data = pressure_fixed_point}; - can_client.send_can_message(can::ids::NodeId::host, message); + .sensor = SensorType::pressure, + .sensor_id = sensor_id, + .sensor_data = pressure_fixed_point}; + can_client.send_can_message(can::ids::NodeId::host, message); + } + pressure_running_total = 0x0; } - pressure_running_total = 0x0; -} -auto handle_baseline_temperature_response(i2c::messages::TransactionResponse &m) - -> void { - static_cast(bit_utils::bytes_to_int( - m.read_buffer.cbegin(), m.read_buffer.cend(), temporary_data_store)); + auto handle_baseline_temperature_response( + i2c::messages::TransactionResponse &m) -> void { + static_cast(bit_utils::bytes_to_int(m.read_buffer.cbegin(), + m.read_buffer.cend(), + temporary_data_store)); - uint32_t shifted_data_store = temporary_data_store >> 8; + uint32_t shifted_data_store = temporary_data_store >> 8; - auto temperature = - mmr920::TemperatureResult::to_temperature(shifted_data_store); - temperature_running_total += temperature; + auto temperature = + mmr920::TemperatureResult::to_temperature(shifted_data_store); + temperature_running_total += temperature; - if (!m.id.is_completed_poll) { - return; - } + if (!m.id.is_completed_poll) { + return; + } - auto current_temperature_baseline = - temperature_running_total / total_baseline_reads; - auto offset_fixed_point = - mmr920::reading_to_fixed_point(current_temperature_baseline); - if (echoing) { - can_client.send_can_message( - can::ids::NodeId::host, - can::messages::BaselineSensorResponse{ - .message_index = m.message_index, - .sensor = can::ids::SensorType::pressure_temperature, - .offset_average = offset_fixed_point}); + auto current_temperature_baseline = + temperature_running_total / total_baseline_reads; + auto offset_fixed_point = + mmr920::reading_to_fixed_point(current_temperature_baseline); + if (echoing) { + can_client.send_can_message( + can::ids::NodeId::host, + can::messages::BaselineSensorResponse{ + .message_index = m.message_index, + .sensor = can::ids::SensorType::pressure_temperature, + .offset_average = offset_fixed_point}); + } + temperature_running_total = 0x0; } - temperature_running_total = 0x0; -} -auto get_can_client() -> CanClient & { return can_client; } + auto get_can_client() -> CanClient & { return can_client; } -private: -I2CQueueWriter &writer; -I2CQueuePoller &poller; -CanClient &can_client; -OwnQueue &own_queue; -hardware::SensorHardwareBase &hardware; -const can::ids::SensorId &sensor_id; -const sensors::mmr920::SensorVersion sensor_version; + private: + I2CQueueWriter &writer; + I2CQueuePoller &poller; + CanClient &can_client; + OwnQueue &own_queue; + hardware::SensorHardwareBase &hardware; + const can::ids::SensorId &sensor_id; + const sensors::mmr920::SensorVersion sensor_version; -mmr920::MMR920RegisterMap _registers{}; -mmr920::FilterSetting filter_setting = mmr920::FilterSetting::LOW_PASS_FILTER; + mmr920::MMR920RegisterMap _registers{}; + mmr920::FilterSetting filter_setting = + mmr920::FilterSetting::LOW_PASS_FILTER; -static constexpr std::array MeasurementTimings{0.405, 0.81, 1.62, - 3.24}; // in msec -static constexpr float DEFAULT_DELAY_BUFFER = - 1.0; // in msec (TODO might need to change to fit in uint16_t) -static constexpr uint16_t STOP_DELAY = 0; + static constexpr std::array MeasurementTimings{0.405, 0.81, 1.62, + 3.24}; // in msec + static constexpr float DEFAULT_DELAY_BUFFER = + 1.0; // in msec (TODO might need to change to fit in uint16_t) + static constexpr uint16_t STOP_DELAY = 0; -/** - * Time required before raising a Max Pressure error. The pressure must - * exceed the threshold for the entirety of this period. - */ -static constexpr uint16_t MAX_PRESSURE_TIME_MS = 200; -mmr920::MeasurementRate measurement_mode_rate = - mmr920::MeasurementRate::MEASURE_4; - -bool _initialized = false; -bool echoing = false; -bool enable_auto_baseline = false; -bool bind_sync = false; -bool max_pressure_sync = false; - -float pressure_running_total = 0; -float temperature_running_total = 0; -uint16_t total_baseline_reads = 1; - -float current_pressure_baseline_pa = 0; -float current_moving_pressure_baseline_pa = 0; -float current_temperature_baseline = 0; - -size_t max_pressure_consecutive_readings = 0; -size_t max_pressure_required_readings = 0; - -// TODO(fs, 2022-11-11): Need to figure out a realistic threshold. Pretty -// sure this is an arbitrarily large number to enable continuous reads. -float threshold_pascals = 100.0F; -float offset_average = 0; - -uint32_t temporary_data_store = 0x0; - -template -requires registers::WritableRegister -auto build_register_command(Reg ®) -> uint8_t { - return static_cast(reg.address); -} - -template -requires registers::WritableRegister -auto set_register(Reg ®) -> bool { - auto value = - // Ignore the typical linter warning because we're only using - // this on __packed structures that mimic hardware registers - // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) - *reinterpret_cast(®); - value &= Reg::value_mask; - return write(Reg::address, value); -} -std::array *sensor_buffer; -uint16_t sensor_buffer_index = 0; -bool crossed_buffer_index = false; + /** + * Time required before raising a Max Pressure error. The pressure must + * exceed the threshold for the entirety of this period. + */ + static constexpr uint16_t MAX_PRESSURE_TIME_MS = 200; + mmr920::MeasurementRate measurement_mode_rate = + mmr920::MeasurementRate::MEASURE_4; + + bool _initialized = false; + bool echoing = false; + bool enable_auto_baseline = false; + bool bind_sync = false; + bool max_pressure_sync = false; + + float pressure_running_total = 0; + float temperature_running_total = 0; + uint16_t total_baseline_reads = 1; + + float current_pressure_baseline_pa = 0; + float current_moving_pressure_baseline_pa = 0; + float current_temperature_baseline = 0; + + size_t max_pressure_consecutive_readings = 0; + size_t max_pressure_required_readings = 0; + + // TODO(fs, 2022-11-11): Need to figure out a realistic threshold. Pretty + // sure this is an arbitrarily large number to enable continuous reads. + float threshold_pascals = 100.0F; + float offset_average = 0; + + uint32_t temporary_data_store = 0x0; + + template + requires registers::WritableRegister + auto build_register_command(Reg ®) -> uint8_t { + return static_cast(reg.address); + } + + template + requires registers::WritableRegister + auto set_register(Reg ®) -> bool { + auto value = + // Ignore the typical linter warning because we're only using + // this on __packed structures that mimic hardware registers + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) + *reinterpret_cast(®); + value &= Reg::value_mask; + return write(Reg::address, value); + } + std::array *sensor_buffer; + uint16_t sensor_buffer_index = 0; + bool crossed_buffer_index = false; }; } // namespace tasks