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