diff --git a/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc b/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc index 8a8c6ea04..69bfeed93 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc +++ b/src/algorithms/libs/sensor_data/sensor_data_aggregator.cc @@ -110,6 +110,35 @@ SensorDataSample SensorDataAggregator::get_last_f32(SensorIdentifier::val return samples.back(); } +SensorDataSample SensorDataAggregator::get_average_f32(SensorIdentifier::value_type sensor_id) const +{ + // The map is populated on construction with empty vectors for each provided sensor. + // If a required sensor is not provided, the error is handled on construction. + const std::vector> samples = f32_data_.at(sensor_id); + if (samples.empty()) + { + return {0, 0}; + } + float acc = 0.0; + float count = 0; + bool first = true; + for (const auto& sample : samples) + { + if (first) + { + first = false; + continue; + } + acc += sample.value; + ++count; + } + if (count == 0) + { + return {samples.back().timestamp, 0}; + } + return {samples.back().timestamp, acc / count}; +} + void SensorDataAggregator::append_data(const pmt::pmt_t& data_dict) { diff --git a/src/algorithms/libs/sensor_data/sensor_data_aggregator.h b/src/algorithms/libs/sensor_data/sensor_data_aggregator.h index b2df3aa74..3f1516c69 100644 --- a/src/algorithms/libs/sensor_data/sensor_data_aggregator.h +++ b/src/algorithms/libs/sensor_data/sensor_data_aggregator.h @@ -53,6 +53,7 @@ public: const std::vector>& get_f32(SensorIdentifier::value_type sensor_id) const; SensorDataSample get_last_f32(SensorIdentifier::value_type sensor_id) const; + SensorDataSample get_average_f32(SensorIdentifier::value_type sensor_id) const; // More getters to be added in the future for different types // For now, all supported sensors are represented as f32