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