diff --git a/src/shared/sensors/SensorInfo.h b/src/shared/sensors/SensorInfo.h index 804c0b0a863adfd8b97003e5563613e5d3716b6c..1a08751ac55530067071086caf6d172a42673377 100644 --- a/src/shared/sensors/SensorInfo.h +++ b/src/shared/sensors/SensorInfo.h @@ -22,7 +22,10 @@ #pragma once -#include <atomic> +#include <units/Frequency.h> +#include <utils/KernelTime.h> + +#include <chrono> #include <functional> namespace Boardcore @@ -38,14 +41,39 @@ namespace Boardcore struct SensorInfo { std::string id; - uint32_t period; ///< Period in ms + std::chrono::nanoseconds period; std::function<void()> callback; bool isEnabled; bool isInitialized; SensorInfo( + // cppcheck-suppress passedByValue const std::string id = "", uint32_t period = 0, - std::function<void()> callback = []() {}, bool isEnabled = true); + std::function<void()> callback = []() {}, bool isEnabled = true) + : SensorInfo(id, std::chrono::milliseconds{period}, std::move(callback), + isEnabled) + { + } + + SensorInfo( + // cppcheck-suppress passedByValue + const std::string id, Units::Frequency::Hertz frequency, + std::function<void()> callback = []() {}, bool isEnabled = true) + : SensorInfo(id, + std::chrono::nanoseconds{ + static_cast<int64_t>(sToNs(1) / frequency.value())}, + std::move(callback), isEnabled) + { + } + + SensorInfo( + // cppcheck-suppress passedByValue + const std::string id, std::chrono::nanoseconds period, + std::function<void()> callback = []() {}, bool isEnabled = true) + : id(id), period(period), callback(std::move(callback)), + isEnabled(isEnabled), isInitialized(false) + { + } SensorInfo& operator=(const SensorInfo& info) { @@ -66,27 +94,6 @@ struct SensorInfo callback.target_type() == info.callback.target_type() && callback.target<void()>() == info.callback.target<void()>(); }; - -private: - SensorInfo(const std::string id, uint32_t period, - std::function<void()> callback, bool isEnabled, - bool isInitialized); }; -// cppcheck-suppress passedByValue -inline SensorInfo::SensorInfo(const std::string id, uint32_t period, - std::function<void()> callback, bool isEnabled) - : SensorInfo(id, period, callback, isEnabled, false) -{ -} - -// cppcheck-suppress passedByValue -inline SensorInfo::SensorInfo(const std::string id, uint32_t period, - std::function<void()> callback, bool isEnabled, - bool isInitialized) - : id(id), period(period), callback(callback), isEnabled(isEnabled), - isInitialized(isInitialized) -{ -} - -} // namespace Boardcore +} // namespace Boardcore \ No newline at end of file diff --git a/src/shared/sensors/SensorManager.cpp b/src/shared/sensors/SensorManager.cpp index ee35f9ab6fe83ac7cc3345b8a64848b059ae1b08..d7f74fc4cf90f9380abf2eb26081e2939ccbe50e 100644 --- a/src/shared/sensors/SensorManager.cpp +++ b/src/shared/sensors/SensorManager.cpp @@ -139,9 +139,9 @@ bool SensorManager::init(const SensorMap_t& sensorsMap) LOG_ERR( logger, - "Failed to initialize sensor {} -> Error: {} (period: {} ms)", + "Failed to initialize sensor {} -> Error: {} (period: {} ns)", sensorInfo.id.c_str(), sensor->getLastError(), - sensorInfo.period); + sensorInfo.period.count()); } else { @@ -150,8 +150,8 @@ bool SensorManager::init(const SensorMap_t& sensorsMap) // Add sensor even if not initialized correctly, its isInitialized info // field will be false - LOG_DEBUG(logger, "Adding {} -> period: {} ms, enabled = {}", - sensorInfo.id.c_str(), sensorInfo.period, + LOG_DEBUG(logger, "Adding {} -> period: {} ns, enabled = {}", + sensorInfo.id.c_str(), sensorInfo.period.count(), sensorInfo.isEnabled); // Check if a sampler with the same sampling period exists @@ -201,7 +201,7 @@ void SensorManager::initScheduler() // Sort the vector to have lower period samplers (higher frequency) inserted // before higher period ones into the TaskScheduler std::stable_sort(samplers.begin(), samplers.end(), - SensorSampler::comparareByPeriod); + SensorSampler::compareByPeriod); // Add all the samplers to the scheduler for (auto& sampler : samplers) @@ -229,10 +229,11 @@ uint8_t SensorManager::getFirstTaskID() return max->id + 1; } -SensorSampler* SensorManager::createSampler(uint8_t id, uint32_t period) +SensorSampler* SensorManager::createSampler(uint8_t id, + std::chrono::nanoseconds period) { - LOG_DEBUG(logger, "Creating Sampler {} with sampling period {} ms", id, - period); + LOG_DEBUG(logger, "Creating Sampler {} with sampling period {} ns", id, + period.count()); return new SimpleSensorSampler(id, period); } diff --git a/src/shared/sensors/SensorManager.h b/src/shared/sensors/SensorManager.h index 890116fb12216a6115b5e000a8d40efdbe9ca48f..105099b299050c978192f0b3ee8da62fea1717dd 100644 --- a/src/shared/sensors/SensorManager.h +++ b/src/shared/sensors/SensorManager.h @@ -145,7 +145,7 @@ private: * * @return Pointer to the newly created sampler. */ - SensorSampler* createSampler(uint8_t id, uint32_t period); + SensorSampler* createSampler(uint8_t id, std::chrono::nanoseconds period); const uint8_t MAX_TASK_ID = 255; ///< Max id for tasks in the scheduler. diff --git a/src/shared/sensors/SensorSampler.cpp b/src/shared/sensors/SensorSampler.cpp index 035d546f10917f2be86fee278ba5170f1af8c05d..30183fd2a45a785526303734f2e5731a5bd45c33 100644 --- a/src/shared/sensors/SensorSampler.cpp +++ b/src/shared/sensors/SensorSampler.cpp @@ -28,14 +28,14 @@ using namespace std; namespace Boardcore { -SensorSampler::SensorSampler(uint8_t id, uint32_t period) +SensorSampler::SensorSampler(uint8_t id, std::chrono::nanoseconds period) : id(id), period(period) { } SensorSampler::~SensorSampler() { sensors.clear(); } -bool SensorSampler::comparareByPeriod(SensorSampler* left, SensorSampler* right) +bool SensorSampler::compareByPeriod(SensorSampler* left, SensorSampler* right) { return left->getSamplingPeriod() < right->getSamplingPeriod(); } @@ -84,7 +84,7 @@ void SensorSampler::sampleAndCallback() uint8_t SensorSampler::getID() { return id; } -uint32_t SensorSampler::getSamplingPeriod() { return period; } +std::chrono::nanoseconds SensorSampler::getSamplingPeriod() { return period; } unsigned int SensorSampler::getNumSensors() { return sensors.size(); } @@ -104,7 +104,8 @@ const SensorInfo SensorSampler::getSensorInfo(AbstractSensor* sensor) return SensorInfo{}; } -SimpleSensorSampler::SimpleSensorSampler(uint8_t id, uint32_t period) +SimpleSensorSampler::SimpleSensorSampler(uint8_t id, + std::chrono::nanoseconds period) : SensorSampler(id, period) { } diff --git a/src/shared/sensors/SensorSampler.h b/src/shared/sensors/SensorSampler.h index b2908d793a149b82bdbfe91860856397ac753b30..1e6f3c4f0c25398e15e3e32002f1ef305bdea5e6 100644 --- a/src/shared/sensors/SensorSampler.h +++ b/src/shared/sensors/SensorSampler.h @@ -24,6 +24,8 @@ #include <diagnostic/PrintLogger.h> +#include <chrono> + #include "Sensor.h" #include "SensorInfo.h" @@ -44,7 +46,7 @@ public: * @param id Sampler identifier. * @param period Period at which the sampler performs sensors update. */ - SensorSampler(uint8_t id, uint32_t period); + SensorSampler(uint8_t id, std::chrono::nanoseconds period); virtual ~SensorSampler(); @@ -54,7 +56,7 @@ public: sensors.size() == sampler.sensors.size(); } - static bool comparareByPeriod(SensorSampler* left, SensorSampler* right); + static bool compareByPeriod(SensorSampler* left, SensorSampler* right); /** * @brief Add a sensor to the sensors map. @@ -89,7 +91,7 @@ public: uint8_t getID(); - uint32_t getSamplingPeriod(); + std::chrono::nanoseconds getSamplingPeriod(); unsigned int getNumSensors(); @@ -101,8 +103,8 @@ private: */ virtual void sampleSensor(AbstractSensor* s) = 0; - uint8_t id; ///< Sampler id used in the task scheduler. - uint32_t period; ///< Sampler update/activation period. + uint8_t id; ///< Sampler id used in the task scheduler. + std::chrono::nanoseconds period; ///< Sampler update/activation period. protected: std::vector<std::pair<AbstractSensor*, SensorInfo>> sensors; @@ -117,7 +119,7 @@ protected: class SimpleSensorSampler : public virtual SensorSampler { public: - SimpleSensorSampler(uint8_t id, uint32_t period); + SimpleSensorSampler(uint8_t id, std::chrono::nanoseconds period); ~SimpleSensorSampler(); diff --git a/src/tests/catch/test-sensormanager-catch.cpp b/src/tests/catch/test-sensormanager-catch.cpp index 0b30271e9620ec2a4f5da764d5a9e86d5b0c5a15..01ca815a316e1194964eeb4b8b1d4b98069028f8 100644 --- a/src/tests/catch/test-sensormanager-catch.cpp +++ b/src/tests/catch/test-sensormanager-catch.cpp @@ -36,6 +36,8 @@ #include <sensors/SensorManager.h> using namespace Boardcore; +using namespace Boardcore::Units::Frequency; +using namespace std::chrono_literals; static const size_t FIRST_TASK_ID = 1; // used to test IDs assignment to tasks @@ -103,7 +105,7 @@ private: TestSensor s2; SensorInfo s2_info{ /*ID=*/"s2", - /*Period=*/1000, + /*Period=*/1000ms, /*Callback=*/[]() { std::cout << "Callback 2!" << std::endl; }, /*Enabled=*/false}; @@ -117,7 +119,7 @@ private: TestSensor s4; SensorInfo s4_info{ /*ID=*/"s4", - /*Period=*/1000, + /*Period=*/1_hz, /*Callback=*/[]() { std::cout << "Callback 4!" << std::endl; }, /*Enabled=*/true}; @@ -215,15 +217,15 @@ TEST_CASE_METHOD(SensorManagerFixture, // sampler at 2000 ms (2 Hz) has 1 sensor for (auto s : sensorManager->samplers) { - if (s->getSamplingPeriod() == 1000) + if (s->getSamplingPeriod() == std::chrono::milliseconds{1000}) { REQUIRE(s->getNumSensors() == 3); } - else if (s->getSamplingPeriod() == 500) + else if (s->getSamplingPeriod() == std::chrono::milliseconds{500}) { REQUIRE(s->getNumSensors() == 1); } - else if (s->getSamplingPeriod() == 2000) + else if (s->getSamplingPeriod() == std::chrono::milliseconds{2000}) { REQUIRE(s->getNumSensors() == 1); }