mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-09-06 12:58:00 +00:00
Code Issues Releases Wiki Activity

feat(sensor_data): added support for double precision (f64) sensor measurements

Browse Source
This commit is contained in:
Victor Castillo
2025-08-22 14:57:18 +02:00
committed by Carles Fernandez
parent 01f3b79025
commit a847240e59
3 changed files with 79 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
src/algorithms/libs/sensor_data/sensor_data_aggregator.cc
View File

@@ -37,6 +37,9 @@ SensorDataAggregator::SensorDataAggregator(const SensorDataSourceConfiguration&
case SensorDataType::F32:
f32_data_[required_sensor] = {};
break;
case SensorDataType::F64:
f64_data_[required_sensor] = {};
break;
// More maps to be populated in the future for different types
// For now, all supported sensors are represented as f32
@@ -76,6 +79,16 @@ void SensorDataAggregator::update(const std::vector<gr::tag_t>& tags)
sensor_samples.emplace_back(last_sample);
}
}
for (auto& sensor_data : f64_data_)
{
std::vector<SensorDataSample<double>>& sensor_samples = sensor_data.second;
if (not sensor_samples.empty())
{
SensorDataSample<double> last_sample = sensor_samples.back();
sensor_samples.clear();
sensor_samples.emplace_back(last_sample);
}
}
// More maps to be cleared in the future for different types
// For now, all supported sensors are represented as f32
@@ -140,6 +153,56 @@ SensorDataSample<float> SensorDataAggregator::get_average_f32(SensorIdentifier::
}
const std::vector<SensorDataSample<double>>& SensorDataAggregator::get_f64(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.
return f64_data_.at(sensor_id);
}
SensorDataSample<double> SensorDataAggregator::get_last_f64(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<SensorDataSample<double>> samples = f64_data_.at(sensor_id);
if (samples.empty())
{
return {0, 0};
}
return samples.back();
}
SensorDataSample<double> SensorDataAggregator::get_average_f64(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<SensorDataSample<double>> samples = f64_data_.at(sensor_id);
if (samples.empty())
{
return {0, 0};
}
double acc = 0.0;
double 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)
{
static pmt::pmt_t SAMPLE_STAMP_KEY = pmt::mp(SensorIdentifier::to_string(SensorIdentifier::SAMPLE_STAMP));
@@ -165,6 +228,12 @@ void SensorDataAggregator::append_data(const pmt::pmt_t& data_dict)
f32_data_.at(sensor_id).emplace_back(sample_stamp, pmt::to_float(std::move(val)));
}
break;
case SensorDataType::F64:
if (f64_data_.find(sensor_id) != f64_data_.end())
{
f64_data_.at(sensor_id).emplace_back(sample_stamp, pmt::to_double(std::move(val)));
}
break;
// More types to be handled in the future for different types
// For now, all supported sensors are represented as f32

5
src/algorithms/libs/sensor_data/sensor_data_aggregator.h
View File

@@ -55,12 +55,17 @@ public:
SensorDataSample<float> get_last_f32(SensorIdentifier::value_type sensor_id) const;
SensorDataSample<float> get_average_f32(SensorIdentifier::value_type sensor_id) const;
const std::vector<SensorDataSample<double>>& get_f64(SensorIdentifier::value_type sensor_id) const;
SensorDataSample<double> get_last_f64(SensorIdentifier::value_type sensor_id) const;
SensorDataSample<double> get_average_f64(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
private:
void append_data(const pmt::pmt_t& data_dict);
std::unordered_map<SensorIdentifier::value_type, std::vector<SensorDataSample<float>>> f32_data_{};
std::unordered_map<SensorIdentifier::value_type, std::vector<SensorDataSample<double>>> f64_data_{};
// More maps to be added in the future for different types
// For now, all supported sensors are represented as f32
};

6
src/algorithms/libs/sensor_data/sensor_identifier.cc
View File

@@ -33,6 +33,10 @@ static const ConversionEntry conversion_table[] = {
{SensorDataType::F64, SensorDataType::F32, [](const pmt::pmt_t& val) { return pmt::from_float(pmt::to_double(val)); }},
{SensorDataType::I32, SensorDataType::F32, [](const pmt::pmt_t& val) { return pmt::from_float(pmt::to_long(val)); }},
{SensorDataType::I64, SensorDataType::F32, [](const pmt::pmt_t& val) { return pmt::from_float(pmt::to_long(val)); }},
{SensorDataType::F32, SensorDataType::F64, [](const pmt::pmt_t& val) { return pmt::from_double(pmt::to_float(val)); }},
{SensorDataType::F64, SensorDataType::F64, [](const pmt::pmt_t& val) { return val; }},
{SensorDataType::I32, SensorDataType::F64, [](const pmt::pmt_t& val) { return pmt::from_double(pmt::to_long(val)); }},
{SensorDataType::I64, SensorDataType::F64, [](const pmt::pmt_t& val) { return pmt::from_double(pmt::to_long(val)); }},
};
@@ -174,7 +178,7 @@ SensorDataType::value_type SensorIdentifier::get_internal_type(value_type sensor
case IMU_ANG_ACC_X:
case IMU_ANG_ACC_Y:
case IMU_ANG_ACC_Z:
return SensorDataType::F32;
return SensorDataType::F64;
default:
return SensorDataType::F32;
}