diff --git a/include/bootloader/core/ids.h b/include/bootloader/core/ids.h index 10e6078b9..445b1488a 100644 --- a/include/bootloader/core/ids.h +++ b/include/bootloader/core/ids.h @@ -204,6 +204,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 8bcee3c42..0c76831da 100644 --- a/include/can/core/ids.hpp +++ b/include/can/core/ids.hpp @@ -214,6 +214,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);