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