From f26a7c4a45b1b58522ea97f9916827f8b294b81d Mon Sep 17 00:00:00 2001 From: Luca Conterio <luca.conterio@skywarder.eu> Date: Fri, 13 Mar 2020 17:40:49 +0100 Subject: [PATCH] [SM] Add new sm implementation --- sbs.conf | 32 ++++-- src/shared/sensors/SensorManager.cpp | 120 ++++++++++++++++++++ src/shared/sensors/SensorManager.h | 127 +++++++++++++++++++++ src/shared/sensors/SensorSampler.cpp | 92 ++++++++++++++++ src/shared/sensors/SensorSampler.h | 140 ++++++++++++++++++++++++ src/shared/sensors/SensorSampling.h | 99 ----------------- src/shared/utils/testutils/TestSensor.h | 62 +++++++++++ src/tests/catch/test-sensormanager.cpp | 60 ++++++++++ src/tests/drivers/calibrate-mpu9250.cpp | 11 +- src/tests/drivers/test-imu-adis.cpp | 20 ++-- src/tests/drivers/test-lsm.cpp | 11 +- src/tests/drivers/test-mpu9250.cpp | 11 +- src/tests/drivers/test-ms5803.cpp | 8 +- 13 files changed, 651 insertions(+), 142 deletions(-) create mode 100755 src/shared/sensors/SensorManager.cpp create mode 100755 src/shared/sensors/SensorManager.h create mode 100644 src/shared/sensors/SensorSampler.cpp create mode 100644 src/shared/sensors/SensorSampler.h delete mode 100644 src/shared/sensors/SensorSampling.h create mode 100755 src/shared/utils/testutils/TestSensor.h create mode 100755 src/tests/catch/test-sensormanager.cpp diff --git a/sbs.conf b/sbs.conf index 09a02da7f..03ad85533 100644 --- a/sbs.conf +++ b/sbs.conf @@ -142,6 +142,11 @@ Files: src/shared/logger/Logger.cpp libs/tscpp/buffer.cpp libs/tscpp/stream.cpp +[sensormanager] +Type: srcfiles +Files: src/shared/sensors/SensorManager.cpp + src/shared/sensors/SensorSampler.cpp + [gamma868] Type: srcfiles Files: src/shared/drivers/gamma868/Gamma868.cpp @@ -162,7 +167,7 @@ Files: src/shared/drivers/servo/servo.cpp Type: srcfiles Files: src/shared/utils/testutils/TestHelper.cpp -[tests] +[tests-boardcore] Type: srcfiles Files: src/tests/catch/test-aero.cpp src/tests/catch/test-buttonhandler.cpp @@ -173,7 +178,8 @@ Files: src/tests/catch/test-aero.cpp src/tests/catch/test-matrix.cpp src/tests/catch/test-packetqueue.cpp src/tests/catch/spidriver/test-spidriver - + src/tests/catch/test-sensormanager.cpp + #-------------------------------# # Entrypoints # @@ -218,7 +224,7 @@ Main: kernel-testsuite Type: test BoardId: stm32f429zi_stm32f4discovery BinName: tests-catch -Include: %tests %shared %test-utils %spi +Include: %tests-boardcore %shared %test-utils %spi %sensormanager Defines: Main: catch/catch-tests-entry @@ -278,6 +284,14 @@ Include: %shared Defines: Main: test-pinobserver +[test-sensormanager] +Type: test +BoardId: stm32f407vg_skyward_tortellino +BinName: test-sensormanager +Include: %shared %sensormanager +Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG +Main: catch/test-sensormanager + ## Drivers @@ -293,7 +307,7 @@ Main: drivers/test-dsgamma Type: test BoardId: stm32f429zi_skyward_death_stack BinName: test-imu-adis -Include: %shared +Include: %shared %sensormanager Defines: Main: drivers/test-imu-adis @@ -301,7 +315,7 @@ Main: drivers/test-imu-adis Type: test BoardId: stm32f429zi_skyward_homeone BinName: test-mpu9250 -Include: %shared +Include: %shared %sensormanager Defines: Main: drivers/test-mpu9250 @@ -309,7 +323,7 @@ Main: drivers/test-mpu9250 Type: test BoardId: stm32f429zi_skyward_death_stack BinName: test-lsm -Include: %shared +Include: %shared %sensormanager Defines: Main: drivers/test-lsm @@ -398,7 +412,7 @@ Main: drivers/test-ad7994 # Type: test # BoardId: stm32f429zi_stm32f4discovery # BinName: test-circularbuffer -# Include: %shared %test-utils +# Include: %shared %test-utils # Defines: -DSTANDALONE_CATCH1_TEST # Main: catch/test-circularbuffer @@ -455,7 +469,7 @@ Main: drivers/test-servo Type: test BoardId: stm32f429zi_skyward_death_stack BinName: calibrate-mpu9250 -Include: %shared +Include: %shared %sensormanager Defines: Main: drivers/calibrate-mpu9250 @@ -463,7 +477,7 @@ Main: drivers/calibrate-mpu9250 Type: test BoardId: stm32f429zi_skyward_death_stack BinName: test-ms5803 -Include: %shared %spi +Include: %shared %spi %sensormanager Defines: Main: drivers/test-ms5803 diff --git a/src/shared/sensors/SensorManager.cpp b/src/shared/sensors/SensorManager.cpp new file mode 100755 index 000000000..3bfcd024f --- /dev/null +++ b/src/shared/sensors/SensorManager.cpp @@ -0,0 +1,120 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Authors: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "SensorManager.h" + +SensorManager::SensorManager() { + +} + +SensorManager::~SensorManager() { + +} + +bool SensorManager::start() { + initScheduler(); + return scheduler.start(); +} + +void SensorManager::stop() { + scheduler.stop(); +} + +bool SensorManager::addSensor(Sensor* sensor, + uint32_t freq, + function_t sensor_callback, + SamplerType sampler_type) { + + // avoid adding sensors that fail to be initalized + if (!initSensor(sensor)) { + TRACE("[SM] Failed to initialize sensor \n"); + return false; + } + + TRACE("[SM] Adding sensor with frequency %u Hz \n", freq); + + // check if a sampler with the same frequency and the same type exists + for(auto s : samplers) { + if (freq == s.sampler->getFrequency() && + sampler_type == s.sampler->getType()) { + s.sampler->addSensor(sensor, sensor_callback); + return true; + } + } + + SensorSampler* new_sampler; + + if (sampler_type == SIMPLE_SAMPLER) // assign the correct type and frequency to the sampler + new_sampler = new SimpleSensorSampler(freq, current_id); + else + new_sampler = new DMASensorSampler(freq, current_id); + + current_id++; + + new_sampler->addSensor(sensor, sensor_callback); // add the sensor to the sampler + + /* + * std::bind syntax: + * std::bind(&MyClass::someFunction, &myclass_instance, [someFunction args]) + */ + // function to be periodically called by the scheduler (to sample the sensors) + function_t new_sampler_update_function = std::bind(&SensorSampler::sampleAndCallback, new_sampler); + + samplers.push_back({ + new_sampler, + new_sampler_update_function + }); + + return true; +} + +void SensorManager::addCallback(uint32_t freq, function_t callback, uint8_t id) { + scheduler.add( + callback, + 1000 / freq, + id, + miosix::getTick() + 10 + ); +} + +bool SensorManager::initSensor(Sensor* sensor) { + return sensor->init() && sensor->selfTest(); +} + +void SensorManager::initScheduler() { + uint64_t start_time = miosix::getTick() + 10; + uint32_t period = 0; + // add all the samplers to the scheduler + for(auto& s : samplers) { + period = 1000 / s.sampler->getFrequency(); // in milliseconds + // use the frequency as the ID of the task in the scheduler + scheduler.add(s.sampler_update_function, period, s.sampler->getId(), start_time); + } +} + +vector<SensorManager::Sampler_t>& SensorManager::getSamplers() { + return samplers; +} + +TaskScheduler& SensorManager::getScheduler() { + return scheduler; +} diff --git a/src/shared/sensors/SensorManager.h b/src/shared/sensors/SensorManager.h new file mode 100755 index 000000000..2467cc83f --- /dev/null +++ b/src/shared/sensors/SensorManager.h @@ -0,0 +1,127 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Authors: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <scheduler/TaskScheduler.h> +#include <sensors/SensorSampler.h> + +using std::vector; + +/** + * The SensorManager class manages all the sensors connected to the Board. + * + * Sensors are grouped by "type" (Simple or DMA) and "sample frequency" and + * grouped in various SensorSampler objects. These objects are then added to the + * scheduler that manages the timings for the sampling. + * After a SensorSampler has finished sampling its sensors, it will call a + * callback, where these samples can be processed and dispatched. + */ +class SensorManager +{ +public: + + typedef std::function<void()> function_t; + + /** + * @brief Structure used to keep track of already existing + * SimplerSensorSampler, their frequency and the function + * to be passed to the scheduler. + */ + struct Sampler_t { + SensorSampler* sampler; // the sensor sampler + function_t sampler_update_function; // function periodically called by the scheduler + }; + + SensorManager(); + + ~SensorManager(); + + /** + * @brief Start the sensor manager. + */ + bool start(); + + /** + * @brief Stop the sensor manager. + */ + void stop(); + + /** + * @brief Add a sensor to be sampled with a SensorSampler + * and the corresponding callback to be called at the given frequency. + * + * @param sensor the sensor to be added + * @param freq the frequency at which the sensor must be sampled + * @param callback the function to be called after the sensor has been sampled + * @param sampler_type the type of the sampler, SIMPLE_SAMPLER or DMA_SAMPLER + * @return boolen value indicating whether the operation complete successfully or not + */ + bool addSensor(Sensor* sensor, uint32_t freq, + function_t callback, + SamplerType sampler_type=SIMPLE_SAMPLER); + + /** + * @brief Add a callback to be called at the given frequency. + * + * @param freq the frequency at which the function must be called + * @param callback the function to be called periodically + * @param id the identifier for the task in the scheduler + */ + void addCallback(uint32_t freq, function_t callback, uint8_t id); + + /** + * @return refernece to the samplers vector + */ + vector<Sampler_t>& getSamplers(); + + /** + * @return reference to the sensors sampling task scheduler + */ + TaskScheduler& getScheduler(); + + //SensorManagerStatus getStatus() { return status; } + +private: + /** + * @brief Initialize a sensor and run the self-test. + * + * @param sensor the sensor to be initialized + */ + bool initSensor(Sensor* sensor); + + /** + * @brief Add all the SensorSamplers to the scheduler and begins sampling. + */ + void initScheduler(); + + + TaskScheduler scheduler; + + // vectors containing samplers + vector<Sampler_t> samplers; + + //SensorManagerStatus status; + //SensorStatus sensor_status; + + uint32_t current_id = 0; // incrementally assign IDs to scheduler tasks +}; diff --git a/src/shared/sensors/SensorSampler.cpp b/src/shared/sensors/SensorSampler.cpp new file mode 100644 index 000000000..1f8b853e8 --- /dev/null +++ b/src/shared/sensors/SensorSampler.cpp @@ -0,0 +1,92 @@ +/* Copyright (c) 2017-2020 Skyward Experimental Rocketry + * Author: Alain Carlucci, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "SensorSampler.h" + +using namespace std; + +SensorSampler::SensorSampler(SamplerType type, uint32_t freq, uint32_t id) : + type(type), freq(freq), id(id) {} + +void SensorSampler::sampleAndCallback() { + for (auto it = sensors_map.begin(); it != sensors_map.end(); it++) { + sampleSensor(it->first); + it->second(); // callback + } +} + +SamplerType SensorSampler::getType() { + return type; +} + +uint32_t SensorSampler::getId() { + return id; +} + +uint32_t SensorSampler::getFrequency() { + return freq; +} + +uint32_t SensorSampler::getNumSensors() { + return sensors_map.size(); +} + +// simple sampler + +SimpleSensorSampler::SimpleSensorSampler(uint32_t freq, uint32_t id) : + SensorSampler(SIMPLE_SAMPLER, freq, id) {} + +SimpleSensorSampler::~SimpleSensorSampler() {} + +void SimpleSensorSampler::addSensor(Sensor* sensor, function_t sensor_callback) { + sensors_map[sensor] = sensor_callback; +} + +void SimpleSensorSampler::sampleSensor(Sensor* s) { + s->onSimpleUpdate(); +} + +// DMA sampler +DMASensorSampler::DMASensorSampler(uint32_t freq, uint32_t id) : + SensorSampler(DMA_SAMPLER, freq, id) {} + + +DMASensorSampler::~DMASensorSampler() {} + +void DMASensorSampler::addSensor(Sensor* sensor, function_t sensor_callback) { + vector<SPIRequest> requests = sensor->buildDMARequest(); + + sensors_map[sensor] = sensor_callback; + requests_map.insert(pair<Sensor*, vector<SPIRequest>>(sensor, requests)); // can't use standard operator= +} + +void DMASensorSampler::sampleSensor(Sensor* s) { + auto& driver = SPIDriver::instance(); + + vector<SPIRequest> requests = requests_map[s]; + + if (driver.transaction(requests)) { + for (auto r : requests) { + s->onDMAUpdate(r); + } + } +} \ No newline at end of file diff --git a/src/shared/sensors/SensorSampler.h b/src/shared/sensors/SensorSampler.h new file mode 100644 index 000000000..1e43f276c --- /dev/null +++ b/src/shared/sensors/SensorSampler.h @@ -0,0 +1,140 @@ +/* Copyright (c) 2017-2020 Skyward Experimental Rocketry + * Author: Alain Carlucci, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#ifndef SENSOR_SAMPLING_H +#define SENSOR_SAMPLING_H + +#include <Common.h> +#include "Sensor.h" + +using namespace std; + +/** + * Enum to distinguish the sensor sampler type, + * if it a simple one or if it uses DMA. + */ +enum SamplerType { + SIMPLE_SAMPLER = 0, + DMA_SAMPLER = 1 +}; + + +/** + * Virtual sensor sampler class. + */ +class SensorSampler +{ + public: + + typedef function<void()> function_t; + + /** + * @brief Constructor. + * + * @param type sampler type, SIMPLER_SAMPLER or DMA_SAMPLER + * @param freq frequency at which the sampler performs sensors update + */ + SensorSampler(SamplerType type, uint32_t freq, uint32_t id); + + /** + * @brief Add a sensor to the sensors vector. + * + * @param sensor the sensor to be added + */ + virtual void addSensor(Sensor* sensor, function_t callback) = 0; + + /** + * @brief For each sensor, sample it and call the corresponding callback. + */ + void sampleAndCallback(); + + /** + * @return the sampler's type + */ + SamplerType getType(); + + /** + * @return the sampler's ID + */ + uint32_t getId(); + + /** + * @return the sampler's activation frequency + */ + uint32_t getFrequency(); + + /** + * @return the number of sensors assigned to this sampler + */ + uint32_t getNumSensors(); + + private: + /** + * @brief Perform the update of all the sensors in the sampler. + */ + virtual void sampleSensor(Sensor* s) = 0; + + SamplerType type; // the sampler's type + uint32_t freq; // frequency at which the sampler performs sensors update + uint32_t id; // id used for scheduler task + + protected: + map<Sensor*, function_t> sensors_map; // for each sensor a callback + +}; + + +/** + * @brief Sampler for simple sensors, those that are simply + * sampled by calling the onSimpleUpdate() method. + */ +class SimpleSensorSampler : public SensorSampler + { + public: + SimpleSensorSampler(uint32_t freq, uint32_t id); + + ~SimpleSensorSampler(); + + void addSensor(Sensor* sensor, function_t sensor_callback) override; + + void sampleSensor(Sensor* s) override; +}; + + +/** + * @brief Sampler for sensors that DMA for updates. + */ +class DMASensorSampler : public SensorSampler +{ + public: + DMASensorSampler(uint32_t freq, uint32_t id); + + ~DMASensorSampler(); + + void addSensor(Sensor* sensor, function_t sensor_callback) override; + + void sampleSensor(Sensor* s) override; + + private: + map<Sensor*, vector<SPIRequest>> requests_map; // SPI requests needed to perform DMA updates +}; + +#endif /* ifndef SENSOR_SAMPLING_H */ \ No newline at end of file diff --git a/src/shared/sensors/SensorSampling.h b/src/shared/sensors/SensorSampling.h deleted file mode 100644 index 06bb4b9ea..000000000 --- a/src/shared/sensors/SensorSampling.h +++ /dev/null @@ -1,99 +0,0 @@ -/* Copyright (c) 2017 Skyward Experimental Rocketry - * Author: Alain Carlucci - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ -#ifndef SENSOR_SAMPLING_H -#define SENSOR_SAMPLING_H - -#include <Common.h> -#include <diagnostic/Log.h> -#include <drivers/spi/SensorSpi.h> - -#include "Sensor.h" - -class DMASensorSampler -{ -public: - DMASensorSampler() {} - ~DMASensorSampler() {} - - void AddSensor(Sensor* sensor) - { - std::vector<SPIRequest> requests = sensor->buildDMARequest(); - for (size_t i = 0; i < requests.size(); i++) - { - mRequests.push_back(requests[i]); - mSensors.push_back(sensor); - } - } - - void Update() - { - TRACE("Update\n"); - auto& driver = SPIDriver::instance(); - bool ret = driver.transaction(mRequests); - - if (ret) - for (size_t i = 0; i < mSensors.size(); i++) - mSensors[i]->onDMAUpdate(mRequests[i]); - - TRACE("Update end\n"); - - } - - void UpdateAndCallback(std::function<void()> onSampleUpdateCallback) - { - Update(); - TRACE("Update callback\n"); - - onSampleUpdateCallback(); - } - -private: - std::vector<Sensor*> mSensors; - std::vector<SPIRequest> mRequests; -}; - -class SimpleSensorSampler -{ -public: - SimpleSensorSampler() {} - ~SimpleSensorSampler() {} - - void AddSensor(Sensor* sensor) { mSensors.push_back(sensor); } - - void Update() - { - for (Sensor* s : mSensors) - s->onSimpleUpdate(); - } - - void UpdateAndCallback(std::function<void()> onSampleUpdateCallback) - { - Update(); - - onSampleUpdateCallback(); - } - -private: - std::vector<Sensor*> mSensors; -}; - -#endif /* ifndef SENSOR_SAMPLING_H */ diff --git a/src/shared/utils/testutils/TestSensor.h b/src/shared/utils/testutils/TestSensor.h new file mode 100755 index 000000000..f424accf6 --- /dev/null +++ b/src/shared/utils/testutils/TestSensor.h @@ -0,0 +1,62 @@ +/* Copyright (c) 2015-2018 Skyward Experimental Rocketry + * Authors: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#ifndef SRC_SHARED_BOARDS_HOMEONE_SENSORMANAGER_TESTSENSOR_H +#define SRC_SHARED_BOARDS_HOMEONE_SENSORMANAGER_TESTSENSOR_H + +#include <cmath> +#include "Common.h" +#include "sensors/Sensor.h" + +using miosix::getTick; +using miosix::TICK_FREQ; + +class TestSensor : public virtual Sensor +{ +public: + TestSensor() : mLastSample(0) {} + ~TestSensor() {} + + bool init() override + { + return true; + } + + bool onSimpleUpdate() override + { + // printf("onSimpleUpdate\n"); + mLastSample = 10 * sin(PI * static_cast<float>(getTick()) / + static_cast<float>(TICK_FREQ)); + return true; + } + + bool selfTest() override + { + return true; + } + + float* testDataPtr() { return &mLastSample; } + +private: + float mLastSample; +}; + +#endif /* SRC_SHARED_BOARDS_HOMEONE_SENSORMANAGER_TESTSENSOR_H */ diff --git a/src/tests/catch/test-sensormanager.cpp b/src/tests/catch/test-sensormanager.cpp new file mode 100755 index 000000000..fb4466f90 --- /dev/null +++ b/src/tests/catch/test-sensormanager.cpp @@ -0,0 +1,60 @@ +/* Copyright (c) 2018-2020 Skyward Experimental Rocketry + * Authors: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifdef STANDALONE_CATCH1_TEST +#include "catch-tests-entry.cpp" +#endif + +#include "utils/testutils/catch.hpp" + +#include "utils/testutils/TestSensor.h" +#include "sensors/SensorManager.h" + +TestSensor* sensor1 = new TestSensor(); +TestSensor* sensor2 = new TestSensor(); +TestSensor* sensor3 = new TestSensor(); + + +TEST_CASE("check that sensors are correctly added to the samplers") { + + SensorManager sensor_manager; + + sensor_manager.addSensor(sensor1, 1, std::bind([&]() {})); // 1 Hz + sensor_manager.addSensor(sensor2, 1, std::bind([&]() {})); // 1 Hz + sensor_manager.addSensor(sensor3, 2, std::bind([&]() {})); // 2 Hz + + // check that 2 samplers exist (1 hz and 2 hz) + REQUIRE(sensor_manager.getSamplers().size() == 2); + + // check that sampler at 1 Hz has 2 sensors, sampler at 2 Hz has 1 sensor + for(auto s : sensor_manager.getSamplers()) { + if (s.sampler->getFrequency() == 1) { + REQUIRE(s.sampler->getNumSensors() == 2); + } + else if (s.sampler->getFrequency() == 2) { + REQUIRE(s.sampler->getNumSensors() == 1); + } + else { + FAIL("Can't exist a sampler with frequency different from 1 or 2 Hz"); // no sampler with a different frequency exist + } + } +} \ No newline at end of file diff --git a/src/tests/drivers/calibrate-mpu9250.cpp b/src/tests/drivers/calibrate-mpu9250.cpp index 54467bace..69076a5c9 100644 --- a/src/tests/drivers/calibrate-mpu9250.cpp +++ b/src/tests/drivers/calibrate-mpu9250.cpp @@ -26,29 +26,28 @@ #include <sensors/MPU9250/MPU9250.h> #include <drivers/spi/SensorSpi.h> -#include <sensors/SensorSampling.h> +#include <sensors/SensorSampler.h> using namespace miosix; using namespace miosix::interfaces; typedef BusSPI<1,spi1::mosi,spi1::miso,spi1::sck> busSPI1; typedef ProtocolSPI<busSPI1,sensors::mpu9250::cs> spiMPU9250_a; -typedef MPU9250<spiMPU9250_a> mpu_t; int main() { float bias[3]; - SimpleSensorSampler sampler; + SimpleSensorSampler sampler(250, 1); spiMPU9250_a::init(); - mpu_t* mpu = new mpu_t(1, 1); + MPU9250<spiMPU9250_a>* mpu = new MPU9250<spiMPU9250_a>(1, 1); Thread::sleep(100); if(mpu->init()){ printf("MPU9250 Init succeeded\n" ); - sampler.AddSensor(mpu); + sampler.addSensor(mpu, std::bind([&]() {})); } else { printf("MPU9250 Init failed\n"); @@ -81,7 +80,7 @@ int main() while(true) { - sampler.Update(); + sampler.sampleAndCallback(); const Vec3* last_data = mpu->accelDataPtr(); printf("%f %f %f\n", last_data->getX(), last_data->getY(), diff --git a/src/tests/drivers/test-imu-adis.cpp b/src/tests/drivers/test-imu-adis.cpp index 52889cacb..8d570150e 100644 --- a/src/tests/drivers/test-imu-adis.cpp +++ b/src/tests/drivers/test-imu-adis.cpp @@ -24,7 +24,7 @@ #include <drivers/BusTemplate.h> #include <interfaces-impl/hwmapping.h> #include <sensors/ADIS16405/ADIS16405.h> -#include <sensors/SensorSampling.h> +#include <sensors/SensorSampler.h> #include <math/Stats.h> #include <diagnostic/CpuMeter.h> #include <drivers/spi/SensorSpi.h> @@ -38,31 +38,27 @@ typedef Gpio<GPIOD_BASE, 5> rstPin; // PD5 for the HomeoneBoard // SPI1 binding to the sensor typedef BusSPI<1,spi1::mosi,spi1::miso,spi1::sck> busSPI1; //Create SPI1 typedef ProtocolSPI<busSPI1,miosix::sensors::adis16405::cs> spiADIS16405; //La lego al Chip Select 1 per la IMU 1 -typedef ADIS16405<spiADIS16405,rstPin> adis_t; //Passo il bus creato al sensore int main() { spiADIS16405::init(); Thread::sleep(1000); - adis_t adis(adis_t::GYRO_FS_300); + ADIS16405<spiADIS16405,rstPin>* adis = new ADIS16405<spiADIS16405,rstPin>(adis->GYRO_FS_300); - if(adis.init()) + if(adis->init()) printf("[ADIS16405] Init succeeded\n" ); else printf("[ADIS16405] Init failed\n"); - if(adis.selfTest()) + if(adis->selfTest()) printf("[ADIS16405] Self test succeeded\n" ); else printf("[ADIS16405] Self test failed\n"); - SimpleSensorSampler sampler; - sampler.AddSensor(&adis); - - // DMASensorSampler sampler; - // sampler.AddSensor(&adis); + SimpleSensorSampler sampler(250, 1); + sampler.addSensor(adis, std::bind([&]() {})); StatsResult statResult; Stats stats; @@ -70,7 +66,7 @@ int main() int counter = 0; while(true) { - sampler.Update(); + sampler.sampleAndCallback(); stats.add(averageCpuUtilization()); @@ -79,7 +75,7 @@ int main() printf("CPU usage: %f\n", statResult.mean); counter = 0; - const Vec3* last_data = adis.accelDataPtr(); + const Vec3* last_data = adis->accelDataPtr(); printf("%f %f %f\n", last_data->getX(), last_data->getY(), last_data->getZ()); } diff --git a/src/tests/drivers/test-lsm.cpp b/src/tests/drivers/test-lsm.cpp index 641d490a1..83c3d8e66 100644 --- a/src/tests/drivers/test-lsm.cpp +++ b/src/tests/drivers/test-lsm.cpp @@ -26,7 +26,7 @@ #include <sensors/LSM6DS3H/LSM6DS3H.h> #include <drivers/spi/SensorSpi.h> -#include <sensors/SensorSampling.h> +#include <sensors/SensorSampler.h> #include <math/Stats.h> #include <diagnostic/CpuMeter.h> @@ -36,19 +36,18 @@ using namespace miosix::interfaces; /* SPI1 binding to the sensor */ typedef BusSPI<1,spi1::mosi,spi1::miso,spi1::sck> busSPI1; typedef ProtocolSPI<busSPI1,sensors::lsm6ds3h::cs> spiLSM6DS3H0_a; -typedef LSM6DS3H<spiLSM6DS3H0_a> lsm6ds3h_t; int main() { - SimpleSensorSampler sampler; + SimpleSensorSampler sampler(250, 1); spiLSM6DS3H0_a::init(); - lsm6ds3h_t* lsm6ds3h = new lsm6ds3h_t(3,3); + LSM6DS3H<spiLSM6DS3H0_a>* lsm6ds3h = new LSM6DS3H<spiLSM6DS3H0_a>(3,3); if(lsm6ds3h->init()) { printf("[LSM6DS3H] Init succeeded\n" ); - sampler.AddSensor(lsm6ds3h); + sampler.addSensor(lsm6ds3h, std::bind([&]() {})); } else { @@ -64,7 +63,7 @@ int main() while(true) { - sampler.Update(); + sampler.sampleAndCallback(); // const Vec3* last_data = lsm6ds3h->gyroDataPtr(); // printf("%f %f %f\n", last_data->getX(), last_data->getY(), diff --git a/src/tests/drivers/test-mpu9250.cpp b/src/tests/drivers/test-mpu9250.cpp index f53889a6d..268b59522 100644 --- a/src/tests/drivers/test-mpu9250.cpp +++ b/src/tests/drivers/test-mpu9250.cpp @@ -26,27 +26,26 @@ #include <sensors/MPU9250/MPU9250.h> #include <drivers/spi/SensorSpi.h> -#include <sensors/SensorSampling.h> +#include <sensors/SensorSampler.h> using namespace miosix; using namespace miosix::interfaces; typedef BusSPI<1,spi1::mosi,spi1::miso,spi1::sck> busSPI1; typedef ProtocolSPI<busSPI1,sensors::mpu9250::cs> spiMPU9250_a; -typedef MPU9250<spiMPU9250_a> mpu_t; int main() { - SimpleSensorSampler sampler; + SimpleSensorSampler sampler(250, 1); spiMPU9250_a::init(); - mpu_t* mpu = new mpu_t(1, 1); + MPU9250<spiMPU9250_a>* mpu = new MPU9250<spiMPU9250_a>(1, 1); Thread::sleep(100); if(mpu->init()){ printf("MPU9250 Init succeeded\n" ); - sampler.AddSensor(mpu); + sampler.addSensor(mpu, std::bind([&]() {})); } else { printf("MPU9250 Init failed\n"); @@ -61,7 +60,7 @@ int main() while(true) { - sampler.Update(); + sampler.sampleAndCallback(); mpu->updateMagneto(); const Vec3* last_data = mpu->compassDataPtr(); diff --git a/src/tests/drivers/test-ms5803.cpp b/src/tests/drivers/test-ms5803.cpp index 1a339433d..bb25d974f 100644 --- a/src/tests/drivers/test-ms5803.cpp +++ b/src/tests/drivers/test-ms5803.cpp @@ -24,7 +24,7 @@ #include <drivers/spi/SPIDriver.h> #include <drivers/spi/SensorSpi.h> #include <interfaces-impl/hwmapping.h> -#include <sensors/SensorSampling.h> +#include <sensors/SensorSampler.h> #include "sensors/MS580301BA07/MS580301BA07.h" @@ -44,7 +44,7 @@ int main() // SCK, MISO, MOSI already initialized in the bsp } - SimpleSensorSampler sampler; + SimpleSensorSampler sampler(50, 1); MS580301BA07* ms58 = new MS580301BA07(bus, chip_select); @@ -53,7 +53,7 @@ int main() if (ms58->init()) { printf("MS58 Init succeeded\n"); - sampler.AddSensor(ms58); + sampler.addSensor(ms58, std::bind([&]() {})); } else { @@ -70,7 +70,7 @@ int main() printf("raw_p,p,raw_t,t\n"); while (true) { - sampler.Update(); + sampler.sampleAndCallback(); const float* last_pressure = ms58->pressureDataPtr(); const float* last_temp = ms58->tempDataPtr(); -- GitLab