From f0a0fb89b902656abc585632e16a140265dbfee1 Mon Sep 17 00:00:00 2001 From: Ryan Howard Date: Wed, 26 Jun 2024 14:30:01 -0400 Subject: [PATCH] feat(sensors): Self baselining during sensor moves (#786) * Use logic or instead of hardcoded enums * when filling the bufffer auto baseline after the first 10 * 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 * few tweaks to the way this works * only compute the first 10 elements to self-baselien * don't immediatly turn of the recording when move completes * save response pressure * fix the pressure leveling * rebase fixups * fix the hardware delay hack and get rid of another instance of it * send ack after sending buffer * fixes to the circular buffer * don't need this and can cause the process to choke * send the can messages faster * format * make the moving baseline log clearer * use the real world sensor speed * change auto baseline slightly to ignore the first N samples * add a new sensor sync value * don't trigger before autobaseline is finished * add new binding type for sensors and use the auto-baselineing during sensor moves * forgot to change an old reference * format lint * preserve old behavior * reduce the complexity in the handle function to satisfy lint * format --- include/bootloader/core/ids.h | 1 + include/can/core/ids.hpp | 1 + include/common/core/hardware_delay.hpp | 11 +- .../stepper_motor/motor_interrupt_handler.hpp | 22 +-- .../sensors/core/tasks/capacitive_driver.hpp | 23 +-- .../sensors/core/tasks/pressure_driver.hpp | 140 ++++++++++++++---- .../core/tasks/pressure_sensor_task.hpp | 5 +- 7 files changed, 136 insertions(+), 67 deletions(-) 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. */ 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/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp index 577d1cb41..f79bd79c5 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,12 @@ 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) | + 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); @@ -502,21 +507,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 diff --git a/include/sensors/core/tasks/capacitive_driver.hpp b/include/sensors/core/tasks/capacitive_driver.hpp index 21efa5164..3b5c4c7b8 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" @@ -64,11 +50,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; } @@ -232,6 +218,9 @@ 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 a57867b49..cde222888 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" @@ -30,6 +31,9 @@ using namespace can::ids; * @tparam CanClient */ +constexpr auto AUTO_BASELINE_START = 10; +constexpr auto AUTO_BASELINE_END = 20; + template class MMR920 { @@ -70,9 +74,18 @@ class MMR920 { echoing = should_echo; if (should_echo) { sensor_buffer_index = 0; // reset buffer index + crossed_buffer_index = false; + sensor_buffer->fill(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(); @@ -230,12 +243,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 { @@ -290,11 +303,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) { + 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) % - static_cast(SENSOR_BUFFER_SIZE); + int current_index = + (i + start) % static_cast(SENSOR_BUFFER_SIZE); can_client.send_can_message( can::ids::NodeId::host, @@ -306,9 +329,56 @@ 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( + can::ids::NodeId::host, + can::messages::Acknowledgment{.message_index = message_index}); + } + + 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; + } + } + + 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(); + } else { + hardware.reset_sync(); + } + } else { + if (std::fabs(pressure - current_pressure_baseline_pa) > + threshold_pascals) { + hardware.set_sync(); + } else { + hardware.reset_sync(); } - (*sensor_buffer).at(current_index) = 0; } } @@ -355,31 +425,43 @@ 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(); } } if (bind_sync) { - if (std::fabs(pressure - current_pressure_baseline_pa) > - threshold_pascals) { - hardware.set_sync(); - } else { - hardware.reset_sync(); - } + handle_sync_threshold(pressure); } if (echo_this_time) { auto response_pressure = pressure - current_pressure_baseline_pa; - // do we want pressure or response pressure - sensor_buffer_log(pressure); - 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)}); + 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(); + } } } @@ -516,16 +598,12 @@ 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_1; -#else mmr920::MeasurementRate measurement_mode_rate = mmr920::MeasurementRate::MEASURE_4; -#endif bool _initialized = false; bool echoing = false; + bool enable_auto_baseline = false; bool bind_sync = false; bool max_pressure_sync = false; @@ -534,6 +612,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; @@ -565,6 +644,7 @@ class MMR920 { } std::array *sensor_buffer; uint16_t sensor_buffer_index = 0; + bool crossed_buffer_index = false; }; } // namespace tasks diff --git a/include/sensors/core/tasks/pressure_sensor_task.hpp b/include/sensors/core/tasks/pressure_sensor_task.hpp index 39b158c9d..83f4294a8 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); } @@ -137,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);