From 5f1c2a6a2ec5930b3b9656657ba568cde403b5c0 Mon Sep 17 00:00:00 2001 From: Emilio Corigliano <emilio.corigliano@skywarder.eu> Date: Mon, 19 Aug 2024 22:45:18 +0000 Subject: [PATCH] [HIL] Moved HIL framework from OBSW to boardcore, enhanced with HILSensor and hillificator, turned all sensor drivers HIL compliant - HIL is now the single point of contact of the whole framework (HILTransceiver and HILPhasesManager); Its run method implements the periodic set of the actuators status to be sent back to the simulator. - HILPhasesManager is in charge of changing HIL flags, registering callbacks for specific flags, registering outcomes for important flight phases. - HILTransceiver is in charge of handling the HIL communication through the serial port - Implemented a move constructor for Sensor; - To Mock a sensor and use in the HIL framework it's not needed a specific HILSensors for each data type but it is mocked by means of the move constructor thanks to the hillificator helper function. An HILSensor behaves just like the real sensor (real sensor sampling can be enabled to use also the driver's code) but exposing the samples coming from the simulator. - A test has been implemented to give a usage example of the framework; - All sensors have been checked so that they aren't final and they have sampleImpl protected; --- CMakeLists.txt | 6 + src/shared/hil/HIL.h | 154 ++++ src/shared/hil/HILPhasesManager.h | 196 +++++ src/shared/hil/HILTransceiver.h | 214 ++++++ src/shared/logger/LogTypes.h | 2 + src/shared/sensors/ADS1118/ADS1118.h | 3 +- src/shared/sensors/ADS131M04/ADS131M04.h | 3 +- src/shared/sensors/ADS131M08/ADS131M08.h | 3 +- src/shared/sensors/BME280/BME280.h | 3 +- src/shared/sensors/BME280/BME280I2C.h | 3 +- src/shared/sensors/BMP280/BMP280.h | 3 +- src/shared/sensors/BMP280/BMP280I2C.h | 3 +- .../sensors/BMX160/BMX160WithCorrection.h | 3 +- src/shared/sensors/HILSensor.h | 144 ++++ src/shared/sensors/HILSimulatorData.h | 84 ++ src/shared/sensors/HX711/HX711.h | 3 +- src/shared/sensors/LIS2MDL/LIS2MDL.h | 3 +- src/shared/sensors/LIS331HH/LIS331HH.h | 3 +- src/shared/sensors/LIS3MDL/LIS3MDL.h | 3 +- src/shared/sensors/LPS22DF/LPS22DF.h | 3 +- src/shared/sensors/LPS28DFW/LPS28DFW.h | 3 +- src/shared/sensors/LPS331AP/LPS331AP.h | 3 +- src/shared/sensors/MAX31855/MAX31855.h | 3 +- src/shared/sensors/MAX31856/MAX31856.h | 3 +- src/shared/sensors/MAX6675/MAX6675.h | 3 +- src/shared/sensors/MPU9250/MPU9250.h | 3 +- src/shared/sensors/MS5803/MS5803.h | 3 +- src/shared/sensors/MS5803/MS5803I2C.h | 3 +- src/shared/sensors/Sensor.h | 16 +- src/shared/sensors/UBXGPS/UBXGPSSerial.h | 3 +- src/shared/sensors/UBXGPS/UBXGPSSpi.h | 3 +- src/shared/sensors/VN100/VN100Serial.h | 3 +- .../analog/pressure/honeywell/HSCMAND015PA.h | 2 +- .../analog/pressure/honeywell/HSCMRNN015PA.h | 2 +- .../analog/pressure/honeywell/HSCMRNN030PA.h | 2 +- .../analog/pressure/honeywell/HSCMRNN160KA.h | 2 +- .../analog/pressure/honeywell/SSCDANN030PAA.h | 2 +- .../analog/pressure/honeywell/SSCDRRN015PDA.h | 2 +- .../analog/pressure/honeywell/SSCMRNN030PA.h | 2 +- .../sensors/analog/pressure/nxp/MPXH6115A.h | 2 +- .../sensors/analog/pressure/nxp/MPXH6400A.h | 2 +- .../sensors/analog/pressure/nxp/MPXHZ6130A.h | 2 +- src/tests/hil/BoardScheduler.cpp | 65 ++ src/tests/hil/BoardScheduler.h | 62 ++ src/tests/hil/Buses.h | 56 ++ src/tests/hil/HILSimulationConfig.h | 530 +++++++++++++ src/tests/hil/Sensors/HILSensors.h | 360 +++++++++ src/tests/hil/Sensors/Sensors.cpp | 725 ++++++++++++++++++ src/tests/hil/Sensors/Sensors.h | 232 ++++++ src/tests/hil/Sensors/SensorsConfig.h | 121 +++ src/tests/hil/Sensors/SensorsData.h | 140 ++++ src/tests/hil/common/Events.h | 295 +++++++ src/tests/hil/common/ReferenceConfig.h | 79 ++ src/tests/hil/common/Topics.h | 55 ++ src/tests/hil/test-hil.cpp | 124 +++ 55 files changed, 3714 insertions(+), 38 deletions(-) create mode 100644 src/shared/hil/HIL.h create mode 100644 src/shared/hil/HILPhasesManager.h create mode 100644 src/shared/hil/HILTransceiver.h create mode 100644 src/shared/sensors/HILSensor.h create mode 100644 src/shared/sensors/HILSimulatorData.h create mode 100644 src/tests/hil/BoardScheduler.cpp create mode 100644 src/tests/hil/BoardScheduler.h create mode 100644 src/tests/hil/Buses.h create mode 100644 src/tests/hil/HILSimulationConfig.h create mode 100644 src/tests/hil/Sensors/HILSensors.h create mode 100644 src/tests/hil/Sensors/Sensors.cpp create mode 100644 src/tests/hil/Sensors/Sensors.h create mode 100644 src/tests/hil/Sensors/SensorsConfig.h create mode 100644 src/tests/hil/Sensors/SensorsData.h create mode 100644 src/tests/hil/common/Events.h create mode 100644 src/tests/hil/common/ReferenceConfig.h create mode 100644 src/tests/hil/common/Topics.h create mode 100644 src/tests/hil/test-hil.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 72dfec3d7..83d05228c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -117,6 +117,12 @@ sbs_target(test-taskscheduler stm32f407vg_stm32f4discovery) add_executable(test-trace-logger src/tests/test-trace-logger.cpp) sbs_target(test-trace-logger stm32f429zi_stm32f4discovery) +add_executable(test-hil + src/tests/hil/test-hil.cpp + src/tests/hil/Sensors/Sensors.cpp +) +sbs_target(test-hil stm32f767zi_death_stack_v4) + #-----------------------------------------------------------------------------# # Tests - Catch # #-----------------------------------------------------------------------------# diff --git a/src/shared/hil/HIL.h b/src/shared/hil/HIL.h new file mode 100644 index 000000000..bb11d0174 --- /dev/null +++ b/src/shared/hil/HIL.h @@ -0,0 +1,154 @@ +/* Copyright (c) 2021-2024 Skyward Experimental Rocketry + * Authors: Luca Conterio, Emilio Corigliano + * + * 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 <Singleton.h> +#include <diagnostic/SkywardStack.h> +#include <utils/KernelTime.h> +#include <utils/TimeUtils.h> + +#include "HILPhasesManager.h" +#include "HILTransceiver.h" + +namespace Boardcore +{ + +/** + * @brief Single interface to the hardware-in-the-loop framework. + */ +template <class FlightPhases, class SimulatorData, class ActuatorData> +class HIL : public Boardcore::ActiveObject +{ + using PhasesCallback = + typename HILPhasesManager<FlightPhases, SimulatorData, + ActuatorData>::PhasesCallback; + +public: + /** + * @brief Constructor of the HIL framework. + * @param hilTransceiver The pointer to the already built HILTransceiver. + * @param hilPhasesManager The pointer to the already built + * HILPhasesManager. + * @param updateActuatorData Function which returns the current ActuatorData + * situation. + * @param simulationPeriod Period of the simulation [ms]. + */ + HIL(HILTransceiver<FlightPhases, SimulatorData, ActuatorData> + *hilTransceiver, + HILPhasesManager<FlightPhases, SimulatorData, ActuatorData> + *hilPhasesManager, + std::function<ActuatorData()> updateActuatorData, int simulationPeriod) + : Boardcore::ActiveObject(Boardcore::STACK_MIN_FOR_SKYWARD, + miosix::PRIORITY_MAX - 1), + hilTransceiver(hilTransceiver), hilPhasesManager(hilPhasesManager), + updateActuatorData(updateActuatorData), + simulationPeriod(simulationPeriod) + { + } + + /** + * @brief Start the needed hardware-in-the-loop components. + */ + [[nodiscard]] bool start() override + { + bool initOk = true; + + if (!hilTransceiver->start()) + { + LOG_ERR(logger, "hilTransceiver started with errors"); + initOk = false; + } + + if (!hilPhasesManager->start()) + { + LOG_ERR(logger, "hilPhasesManager started with errors"); + initOk = false; + } + + if (!Boardcore::ActiveObject::start()) + { + LOG_ERR(logger, "hil started with errors"); + initOk = false; + } + + return initOk; + } + + void stop() + { + hilTransceiver->stop(); + hilPhasesManager->stop(); + Boardcore::ActiveObject::stop(); + LOG_INFO(logger, "HIL framework stopped"); + } + + void waitStartSimulation() + { + LOG_INFO(logger, "Waiting for simulation to start..."); + while (!hilPhasesManager->isSimulationRunning()) + { + miosix::Thread::sleep(1); + } + } + + void registerToFlightPhase(const FlightPhases &flag, + const PhasesCallback &func) + { + hilPhasesManager->registerToFlightPhase(flag, func); + } + + int getSimulationPeriod() const { return simulationPeriod; } + + int64_t getTimestampSimulatorData() const + { + return hilTransceiver->getTimestampSimulatorData(); + } + + const SimulatorData *getSensorData() const + { + return hilTransceiver->getSensorData(); + } + +protected: + HILTransceiver<FlightPhases, SimulatorData, ActuatorData> *hilTransceiver; + HILPhasesManager<FlightPhases, SimulatorData, ActuatorData> + *hilPhasesManager; + +private: + void run() override + { + uint64_t ts = miosix::getTime(); + while (!shouldStop()) + { + ts += msToNs(simulationPeriod); + hilTransceiver->setActuatorData(updateActuatorData()); + miosix::Thread::nanoSleepUntil(ts); + } + } + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("HIL"); + + std::function<ActuatorData()> updateActuatorData; + int simulationPeriod; // Simulation period in milliseconds +}; +} // namespace Boardcore \ No newline at end of file diff --git a/src/shared/hil/HILPhasesManager.h b/src/shared/hil/HILPhasesManager.h new file mode 100644 index 000000000..db00cb7d3 --- /dev/null +++ b/src/shared/hil/HILPhasesManager.h @@ -0,0 +1,196 @@ +/* Copyright (c) 2021-2024 Skyward Experimental Rocketry + * Authors: Luca Conterio, Emilio Corigliano + * + * 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 <algorithms/AirBrakes/TrajectoryPoint.h> +#include <diagnostic/PrintLogger.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/Event.h> +#include <events/EventHandler.h> +#include <miosix.h> +#include <utils/Debug.h> + +#include <iostream> +#include <map> + +namespace Boardcore +{ + +struct Outcomes +{ + uint64_t t = 0; + float z = 0; + float vz = 0; + + Outcomes() : t(0), z(0), vz(0) {} + Outcomes(float z, float vz) + : t(TimestampTimer::getTimestamp()), z(z), vz(vz) + { + } + + void print(uint64_t t_start) const + { + std::cout << "@time : " << (double)(t - t_start) / 1000000 + << " [sec]\n"; + std::cout << "@z : " << z << " [m]\n"; + std::cout << "@vz : " << vz << " [m/s]\n\n"; + } +}; + +class HILPhasesManagerBase : public EventHandler +{ +public: + explicit HILPhasesManagerBase( + std::function<TimedTrajectoryPoint()> getCurrentPosition) + : EventHandler(), getCurrentPosition(getCurrentPosition) + { + } + + bool isSimulationRunning() const { return (t_start != 0) && (t_stop == 0); } + + virtual void simulationStarted() + { + t_start = TimestampTimer::getTimestamp(); + LOG_INFO(logger, "SIMULATION STARTED!"); + } + + virtual void liftoff() + { + t_liftoff = TimestampTimer::getTimestamp(); + LOG_INFO(logger, "LIFTOFF!"); + } + + virtual void simulationStopped() + { + t_stop = TimestampTimer::getTimestamp(); + LOG_INFO(logger, "SIMULATION STOPPED!"); + } + +protected: + uint64_t t_start = 0; + uint64_t t_liftoff = 0; + uint64_t t_stop = 0; + std::function<TimedTrajectoryPoint()> getCurrentPosition; + PrintLogger logger = Logging::getLogger("HILPhasesManager"); +}; + +/** + * @brief Singleton object that manages all the phases of the simulation. + * After his instantiation we need to set the source of the current position in + * order to be able to save the outcomes for each event. + */ +template <class FlightPhases, class SimulatorData, class ActuatorData> +class HILPhasesManager : public HILPhasesManagerBase +{ +public: + using PhasesCallback = std::function<void()>; + + explicit HILPhasesManager( + std::function<TimedTrajectoryPoint()> getCurrentPosition) + : HILPhasesManagerBase(getCurrentPosition) + { + } + + bool isFlagActive(const FlightPhases& flag) const + { + return flagsFlightPhases.at(flag); + } + + void registerToFlightPhase(const FlightPhases& flag, + const PhasesCallback& func) + { + callbacks[flag].push_back(func); + } + + void setFlagFlightPhase(const FlightPhases& flag, bool isEnable) + { + flagsFlightPhases[flag] = isEnable; + } + + void processFlags(const SimulatorData& hil_flags) + { + std::vector<FlightPhases> changed_flags; + + processFlagsImpl(hil_flags, changed_flags); + + /* calling the callbacks subscribed to the changed flags */ + for (unsigned int i = 0; i < changed_flags.size(); i++) + { + for (const auto& callback : callbacks[changed_flags[i]]) + { + callback(); + } + } + + prev_flagsFlightPhases = flagsFlightPhases; + } + + virtual void printOutcomes() = 0; + +protected: + virtual void processFlagsImpl(const SimulatorData& hil_flags, + std::vector<FlightPhases>& changed_flags) = 0; + + virtual void handleEventImpl(const Event& e, + std::vector<FlightPhases>& changed_flags) = 0; + + void handleEvent(const Boardcore::Event& e) override + { + std::vector<FlightPhases> changed_flags; + + handleEventImpl(e, changed_flags); + + /* calling the callbacks subscribed to the changed flags */ + for (unsigned int i = 0; i < changed_flags.size(); i++) + { + for (const auto& callback : callbacks[changed_flags[i]]) + { + callback(); + } + } + + prev_flagsFlightPhases = flagsFlightPhases; + } + + void registerOutcomes(const FlightPhases& phase) + { + TimedTrajectoryPoint temp = getCurrentPosition(); + outcomes[phase] = Outcomes(temp.z, temp.vz); + } + + bool isSetTrue(const FlightPhases& phase) const + { + return flagsFlightPhases.at(phase) && !prev_flagsFlightPhases.at(phase); + } + + bool isSetFalse(const FlightPhases& phase) const + { + return !flagsFlightPhases.at(phase) && prev_flagsFlightPhases.at(phase); + } + + std::map<FlightPhases, bool> flagsFlightPhases; + std::map<FlightPhases, bool> prev_flagsFlightPhases; + std::map<FlightPhases, std::vector<PhasesCallback>> callbacks; + std::map<FlightPhases, Outcomes> outcomes; +}; +} // namespace Boardcore \ No newline at end of file diff --git a/src/shared/hil/HILTransceiver.h b/src/shared/hil/HILTransceiver.h new file mode 100644 index 000000000..dbd402ef9 --- /dev/null +++ b/src/shared/hil/HILTransceiver.h @@ -0,0 +1,214 @@ +/* Copyright (c) 2020-2024 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * 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 <ActiveObject.h> +#include <drivers/timer/TimestampTimer.h> +#include <drivers/usart/USART.h> +#include <utils/Debug.h> + +#include "HIL.h" +#include "drivers/usart/USART.h" + +namespace Boardcore +{ + +template <class FlightPhases, class SimulatorData, class ActuatorData> +class HIL; + +class HILTransceiverBase : public ActiveObject +{ +public: + /** + * @brief Construct a serial connection attached to a control algorithm + */ + explicit HILTransceiverBase(USART &hilSerial) : hilSerial(hilSerial) {} + + /** + * @brief Returns the number of lost updates. + * @return the number of updates lost due to communication with the + * simulator. + */ + int getLostUpdates() const { return nLostUpdates; } + + /** + * @brief Returns the value in ns of the timestamp of the last received + * simulatorData. + */ + int64_t getTimestampSimulatorData() const { return timestampSimulatorData; } + +protected: + /** + * @brief Waits for the control algorithm(s) to update actuatorData. + */ + void waitActuatorData() + { + miosix::Lock<miosix::FastMutex> l(mutex); + while (!updated) + { + condVar.wait(l); + } + updated = false; + } + + USART &hilSerial; + bool receivedFirstPacket = false; + bool updated = false; + int nLostUpdates = 0; + int64_t timestampSimulatorData = 0; // timestamp of the last received + // simulatorData [ns] + miosix::FastMutex mutex; + miosix::ConditionVariable condVar; + PrintLogger logger = Logging::getLogger("HILTransceiver"); +}; + +/** + * @brief HILTransceiver is a Singleton and provides an easy interface for + * the control algorithms to send and receive data during a simulation + */ +template <class FlightPhases, class SimulatorData, class ActuatorData> +class HILTransceiver : public HILTransceiverBase +{ +public: + /** + * @brief Construct a serial connection attached to a control algorithm. + * + * @param hilSerial Serial port for the HIL communication. + */ + explicit HILTransceiver(USART &hilSerial, + HILPhasesManager<FlightPhases, SimulatorData, + ActuatorData> *hilPhasesManager) + : HILTransceiverBase(hilSerial), actuatorData(), + hilPhasesManager(hilPhasesManager) + { + } + + /** + * @brief sets the actuator data and then wakes up the + * MatlabTransceiver thread in order to send the data back to the + * simulator (called by the control algorithm) + * @param actuatorData sets the data that will be sent to the + * simulator + */ + void setActuatorData(ActuatorData actuatorData) + { + miosix::Lock<miosix::FastMutex> l(mutex); + + // If already updated increment lost updates + if (updated) + { + nLostUpdates++; + } + + this->actuatorData = actuatorData; + updated = true; + condVar.signal(); + } + + /** + * @brief returns the reference of the SimulatorData + * + * @return reference to the data simulated by matlab + */ + const SimulatorData *getSensorData() const { return &simulatorData; } + +private: + void run() override; + + SimulatorData simulatorData; + ActuatorData actuatorData; + HILPhasesManager<FlightPhases, SimulatorData, ActuatorData> + *hilPhasesManager; +}; + +/** + * @brief The thread deals with the communication between the simulator and the + * board. + * + * After the first time the data is received, the loop of this thread: + * - Reads the simulated data and copies them in the SensorData structure; + * - Notifies every sensor that new data arrived; + * - Waits for the control algorithms to update the actuator data; + * - Sends back the value to the simulator. + */ +template <class FlightPhases, class SimulatorData, class ActuatorData> +void HILTransceiver<FlightPhases, SimulatorData, ActuatorData>::run() +{ + LOG_INFO(logger, "HIL Transceiver started"); + hilSerial.clearQueue(); + + miosix::led2On(); + hilSerial.write(&actuatorData, sizeof(ActuatorData)); + miosix::led2Off(); + + while (!shouldStop()) + { + // Pausing the kernel in order to copy the data in the shared structure + { + SimulatorData tempData; + nLostUpdates = 0; + miosix::led3On(); + size_t nRead = 0; + if (!hilSerial.readBlocking(&tempData, sizeof(SimulatorData), + nRead)) + { + LOG_ERR(logger, "Failed serial read"); + } + + assert(nRead == sizeof(SimulatorData) && + "Read less then SimulatorData bytes"); + + hilSerial.clearQueue(); + miosix::led3Off(); + + miosix::PauseKernelLock kLock; + simulatorData = tempData; + timestampSimulatorData = miosix::getTime(); + } + + // If this is the first packet to be received, then update the flight + // phase manager + if (!receivedFirstPacket) + { + receivedFirstPacket = true; + hilPhasesManager->simulationStarted(); + } + + // Trigger events relative to the flight phases + hilPhasesManager->processFlags(simulatorData); + + if (nLostUpdates > 0) + { + // This means also that the number of samples used for the mean sent + // to the HIL simulator is made up of more than the number of + // samples we though + LOG_WARN(logger, "%d Lost updates", nLostUpdates); + } + + waitActuatorData(); + miosix::led2On(); + hilSerial.write(&actuatorData, sizeof(ActuatorData)); + miosix::led2Off(); + } +} +} // namespace Boardcore \ No newline at end of file diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h index 70e6426fb..678b4d59c 100644 --- a/src/shared/logger/LogTypes.h +++ b/src/shared/logger/LogTypes.h @@ -53,6 +53,7 @@ #include <sensors/LPS22DF/LPS22DFData.h> #include <sensors/LPS28DFW/LPS28DFWData.h> #include <sensors/LSM6DSRX/LSM6DSRXData.h> +#include <sensors/MAX31856/MAX31856Data.h> #include <sensors/MBLoadCell/MBLoadCellData.h> #include <sensors/MPU9250/MPU9250Data.h> #include <sensors/MS5803/MS5803Data.h> @@ -144,6 +145,7 @@ void registerTypes(Deserializer& ds) ds.registerType<LSM6DSRXData>(); ds.registerType<MagnetometerData>(); ds.registerType<MavlinkStatus>(); + ds.registerType<MAX31856Data>(); ds.registerType<MBLoadCellData>(); ds.registerType<MPU9250Data>(); ds.registerType<MPXH6115AData>(); diff --git a/src/shared/sensors/ADS1118/ADS1118.h b/src/shared/sensors/ADS1118/ADS1118.h index dfdb2bab8..828cdc9bb 100644 --- a/src/shared/sensors/ADS1118/ADS1118.h +++ b/src/shared/sensors/ADS1118/ADS1118.h @@ -281,7 +281,7 @@ public: */ bool selfTest() override; -private: +protected: /** * @brief Reads the previously configured channel while writing the next * enabled configuration. @@ -290,6 +290,7 @@ private: */ ADS1118Data sampleImpl() override; +private: /** * @brief Writes the configuration specified, reads the previous written * configuration's value and stores it. If enabled checks also that the diff --git a/src/shared/sensors/ADS131M04/ADS131M04.h b/src/shared/sensors/ADS131M04/ADS131M04.h index d8142f18c..2d4e3adb5 100644 --- a/src/shared/sensors/ADS131M04/ADS131M04.h +++ b/src/shared/sensors/ADS131M04/ADS131M04.h @@ -124,9 +124,10 @@ public: */ bool selfTest() override; -private: +protected: ADS131M04Data sampleImpl() override; +private: /** * @brief Overwrites the channel settings. * diff --git a/src/shared/sensors/ADS131M08/ADS131M08.h b/src/shared/sensors/ADS131M08/ADS131M08.h index a83fdaf09..41f049a58 100644 --- a/src/shared/sensors/ADS131M08/ADS131M08.h +++ b/src/shared/sensors/ADS131M08/ADS131M08.h @@ -124,9 +124,10 @@ public: */ bool selfTest() override; -private: +protected: ADS131M08Data sampleImpl() override; +private: /** * @brief Overwrites the channel settings. * diff --git a/src/shared/sensors/BME280/BME280.h b/src/shared/sensors/BME280/BME280.h index 1e7e9f1b7..99cdb4017 100644 --- a/src/shared/sensors/BME280/BME280.h +++ b/src/shared/sensors/BME280/BME280.h @@ -238,9 +238,10 @@ public: */ bool selfTest() override; -private: +protected: BME280Data sampleImpl() override; +private: void reset(); void setConfiguration(); diff --git a/src/shared/sensors/BME280/BME280I2C.h b/src/shared/sensors/BME280/BME280I2C.h index fcc512bdb..3c6f76cad 100644 --- a/src/shared/sensors/BME280/BME280I2C.h +++ b/src/shared/sensors/BME280/BME280I2C.h @@ -238,9 +238,10 @@ public: */ bool selfTest() override; -private: +protected: BME280Data sampleImpl() override; +private: bool reset(); void setConfiguration(); diff --git a/src/shared/sensors/BMP280/BMP280.h b/src/shared/sensors/BMP280/BMP280.h index 8ba79d6f4..3b7abf775 100644 --- a/src/shared/sensors/BMP280/BMP280.h +++ b/src/shared/sensors/BMP280/BMP280.h @@ -216,9 +216,10 @@ public: */ bool selfTest() override; -private: +protected: BMP280Data sampleImpl() override; +private: void reset(); void setConfiguration(); diff --git a/src/shared/sensors/BMP280/BMP280I2C.h b/src/shared/sensors/BMP280/BMP280I2C.h index 1c682f6a2..bf1f5e0e1 100644 --- a/src/shared/sensors/BMP280/BMP280I2C.h +++ b/src/shared/sensors/BMP280/BMP280I2C.h @@ -216,9 +216,10 @@ public: */ bool selfTest() override; -private: +protected: BMP280Data sampleImpl() override; +private: bool reset(); void setConfiguration(); diff --git a/src/shared/sensors/BMX160/BMX160WithCorrection.h b/src/shared/sensors/BMX160/BMX160WithCorrection.h index e805592cd..1be3ae193 100644 --- a/src/shared/sensors/BMX160/BMX160WithCorrection.h +++ b/src/shared/sensors/BMX160/BMX160WithCorrection.h @@ -60,9 +60,10 @@ public: void stopCalibration(); -private: +protected: BMX160WithCorrectionData sampleImpl() override; +private: /** * @brief Rotates data axes as specified. */ diff --git a/src/shared/sensors/HILSensor.h b/src/shared/sensors/HILSensor.h new file mode 100644 index 000000000..d69bf9149 --- /dev/null +++ b/src/shared/sensors/HILSensor.h @@ -0,0 +1,144 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Davide Mor, Emilio Corigliano + * + * 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 <miosix.h> + +#include <type_traits> + +#include "HILSimulatorData.h" +#include "Sensor.h" + +namespace Boardcore +{ + +/** + * @brief Class that wraps a real sensor to perform HIL simulations. + * + * This class extends a real sensor class T. For the real sensor construction is + * used the move constructor of an already created T sensor. The sampleImpl + * method is overridden in order to expose the sample returned by the updateData + * function passed in the constructor. + * In this way, the corresponding HILSensor behaves exactly like a sensor T, but + * returns the data given by the simulator. + * + * The real hardware sensor can be used by creating an instance of this class + * with the `enableHw` constructor parameter set to `true`. In that case, + * when the `sample` method is called, the real sensor is sampled and then + * the data from the simulator is mapped into the SensorData struct. + */ + +template <class T> +class HILSensor : public T +{ +public: + // A requirement is that the class T must be move constructible + static_assert(std::is_move_constructible<T>::value, + "T must be move constructible!"); + // A requirement is that the class T must be a Sensor + static_assert(std::is_base_of<Sensor<typename T::DataType>, T>::value, + "T must inherit from Sensor<T::DataType>"); + + using UpdateFn = std::function<typename T::DataType(void)>; + + /** + * @brief Constructor of the HILSensor which decorates the passed sensor + * with the sampling of the simulator data. + * + * @param sensor The sensor to be wrapped. + * @param enableHw Flag to enable the sampling of the real hardware. + * @param updateData Function that maps the data from the simulator data to + * the sensor current sample. + */ + HILSensor(T&& sensor, bool enableHw, UpdateFn updateData) + : T{std::move(sensor)}, enableHw{enableHw}, updateData{ + std::move(updateData)} + { + } + + bool init() override { return (enableHw ? T::init() : true); } + + bool selfTest() override { return (enableHw ? T::selfTest() : true); } + + /** + * @brief Overridden sampleImpl method so that the data could be actually + * taken from the simulator. + * + * If the hardware is enabled, it also samples the real sensor. + * + * @return The data struct with the current simulator sample. + */ + typename T::DataType sampleImpl() override + { + if (enableHw) + { + auto realSample = T::sampleImpl(); + { + miosix::Lock<miosix::FastMutex> l(mutexRealData); + lastRealSample = realSample; + } + } + + return updateData(); + } + + /** + * @brief Method analogous to getLastSample but returns the real sample + * taken from the hardware sensor. + * + * @return The data struct with the current sensor sample. + */ + typename T::DataType getRealLastSample() + { + miosix::Lock<miosix::FastMutex> l(mutexRealData); + return lastRealSample; + } + +protected: + typename T::DataType lastRealSample; // Last sample of the real sensor + bool enableHw; // Flag to enable hardware sampling + UpdateFn updateData; // Function to get the lastSample from simulatorData + + // Thread safe mutex to synchronize writes and reads on lastRealSample + miosix::FastMutex mutexRealData; +}; + +/** + * @brief Static method that hillificates sensors, turning a real sensor in + * a HILSensor. + * + * Creates a HILSensor of the sensor type T. The passed sensor must be an + * already instantiated sensor. + */ +template <typename T> +static void hillificator(std::unique_ptr<T>& sensor, bool enableHw, + typename HILSensor<T>::UpdateFn updateData) +{ + if (sensor) + { + sensor = std::make_unique<Boardcore::HILSensor<T>>( + std::move(*sensor), enableHw, std::move(updateData)); + } +} + +} // namespace Boardcore \ No newline at end of file diff --git a/src/shared/sensors/HILSimulatorData.h b/src/shared/sensors/HILSimulatorData.h new file mode 100644 index 000000000..dacd6898f --- /dev/null +++ b/src/shared/sensors/HILSimulatorData.h @@ -0,0 +1,84 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * 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 <Eigen/Core> +#include <ostream> + +namespace Boardcore +{ + +template <int N_DATA> +struct AccelerometerSimulatorData +{ + static constexpr int NDATA = N_DATA; + float measures[N_DATA][3]; +}; + +template <int N_DATA> +struct GyroscopeSimulatorData +{ + static constexpr int NDATA = N_DATA; + float measures[N_DATA][3]; +}; + +template <int N_DATA> +struct MagnetometerSimulatorData +{ + static constexpr int NDATA = N_DATA; + float measures[N_DATA][3]; +}; + +template <int N_DATA> +struct BarometerSimulatorData +{ + static constexpr int NDATA = N_DATA; + float measures[N_DATA]; +}; + +template <int N_DATA> +struct GPSSimulatorData +{ + static constexpr int NDATA = N_DATA; + float positionMeasures[N_DATA][3]; + float velocityMeasures[N_DATA][3]; + float fix; + float num_satellites; +}; + +template <int N_DATA> +struct PitotSimulatorData +{ + static constexpr int NDATA = N_DATA; + float deltaP[N_DATA]; + float staticPressure[N_DATA]; +}; + +template <int N_DATA> +struct TemperatureSimulatorData +{ + static constexpr int NDATA = N_DATA; + float measures[N_DATA]; +}; + +} // namespace Boardcore diff --git a/src/shared/sensors/HX711/HX711.h b/src/shared/sensors/HX711/HX711.h index 4674ea16e..460b8ccac 100644 --- a/src/shared/sensors/HX711/HX711.h +++ b/src/shared/sensors/HX711/HX711.h @@ -112,9 +112,10 @@ public: */ float getOffset(); -private: +protected: HX711Data sampleImpl() override; +private: SPIBusInterface& bus; miosix::GpioPin sckPin; const SPIBusConfig config; diff --git a/src/shared/sensors/LIS2MDL/LIS2MDL.h b/src/shared/sensors/LIS2MDL/LIS2MDL.h index d7a93eb2b..7e781f955 100644 --- a/src/shared/sensors/LIS2MDL/LIS2MDL.h +++ b/src/shared/sensors/LIS2MDL/LIS2MDL.h @@ -99,9 +99,10 @@ public: */ bool applyConfig(Config config); -private: +protected: LIS2MDLData sampleImpl() override; +private: SPISlave slave; Config configuration; diff --git a/src/shared/sensors/LIS331HH/LIS331HH.h b/src/shared/sensors/LIS331HH/LIS331HH.h index 252e7bf37..8df1374b5 100644 --- a/src/shared/sensors/LIS331HH/LIS331HH.h +++ b/src/shared/sensors/LIS331HH/LIS331HH.h @@ -58,9 +58,10 @@ public: void setFullScaleRange(FullScaleRange fs); -private: +protected: LIS331HHData sampleImpl() override; +private: SPISlave slave; float sensitivity = 6.0 / 32767.0; diff --git a/src/shared/sensors/LIS3MDL/LIS3MDL.h b/src/shared/sensors/LIS3MDL/LIS3MDL.h index ed98e2768..2e0a5d905 100644 --- a/src/shared/sensors/LIS3MDL/LIS3MDL.h +++ b/src/shared/sensors/LIS3MDL/LIS3MDL.h @@ -145,9 +145,10 @@ public: */ bool applyConfig(Config config); -private: +protected: LIS3MDLData sampleImpl() override; +private: void updateUnit(FullScale fs); SPISlave slave; diff --git a/src/shared/sensors/LPS22DF/LPS22DF.h b/src/shared/sensors/LPS22DF/LPS22DF.h index 876c5fc49..63a761dd6 100644 --- a/src/shared/sensors/LPS22DF/LPS22DF.h +++ b/src/shared/sensors/LPS22DF/LPS22DF.h @@ -138,9 +138,10 @@ public: */ void setOutputDataRate(ODR odr); -private: +protected: LPS22DFData sampleImpl() override; +private: SPISlave slave; Config config; diff --git a/src/shared/sensors/LPS28DFW/LPS28DFW.h b/src/shared/sensors/LPS28DFW/LPS28DFW.h index 33012760c..2f2cf5010 100644 --- a/src/shared/sensors/LPS28DFW/LPS28DFW.h +++ b/src/shared/sensors/LPS28DFW/LPS28DFW.h @@ -154,9 +154,10 @@ public: */ bool setDRDYInterrupt(bool drdy); -private: +protected: LPS28DFWData sampleImpl() override; +private: /** * @brief Converting the bytes read from the sensor to the pressure in Pa. */ diff --git a/src/shared/sensors/LPS331AP/LPS331AP.h b/src/shared/sensors/LPS331AP/LPS331AP.h index 2af56dcee..3a0bc3c1c 100644 --- a/src/shared/sensors/LPS331AP/LPS331AP.h +++ b/src/shared/sensors/LPS331AP/LPS331AP.h @@ -48,9 +48,10 @@ public: bool selfTest() override; -private: +protected: LPS331APData sampleImpl() override; +private: bool checkWhoAmI(); static constexpr uint8_t WHO_AM_I_VAL = 0x58; ///< Who am I value diff --git a/src/shared/sensors/MAX31855/MAX31855.h b/src/shared/sensors/MAX31855/MAX31855.h index fcfcd412d..9391e6099 100644 --- a/src/shared/sensors/MAX31855/MAX31855.h +++ b/src/shared/sensors/MAX31855/MAX31855.h @@ -64,9 +64,10 @@ public: */ TemperatureData readInternalTemperature(); -private: +protected: TemperatureData sampleImpl() override; +private: SPISlave slave; PrintLogger logger = Logging::getLogger("max31855"); diff --git a/src/shared/sensors/MAX31856/MAX31856.h b/src/shared/sensors/MAX31856/MAX31856.h index 7e329e282..1b4a408ba 100644 --- a/src/shared/sensors/MAX31856/MAX31856.h +++ b/src/shared/sensors/MAX31856/MAX31856.h @@ -82,9 +82,10 @@ public: void setColdJunctionOffset(float offset); -private: +protected: MAX31856Data sampleImpl() override; +private: SPISlave slave; ThermocoupleType type; diff --git a/src/shared/sensors/MAX6675/MAX6675.h b/src/shared/sensors/MAX6675/MAX6675.h index 0ee3e01bd..743a46fc8 100644 --- a/src/shared/sensors/MAX6675/MAX6675.h +++ b/src/shared/sensors/MAX6675/MAX6675.h @@ -55,9 +55,10 @@ public: */ bool checkConnection(); -private: +protected: TemperatureData sampleImpl() override; +private: const SPISlave slave; PrintLogger logger = Logging::getLogger("max6675"); diff --git a/src/shared/sensors/MPU9250/MPU9250.h b/src/shared/sensors/MPU9250/MPU9250.h index 5d262adc7..c0caf5f8d 100644 --- a/src/shared/sensors/MPU9250/MPU9250.h +++ b/src/shared/sensors/MPU9250/MPU9250.h @@ -220,9 +220,10 @@ public: */ bool selfTest() override { return true; }; -private: +protected: MPU9250Data sampleImpl() override; +private: void resetDevice(); void selectAutoClock(); diff --git a/src/shared/sensors/MS5803/MS5803.h b/src/shared/sensors/MS5803/MS5803.h index 7147d7ffe..5fa1466bc 100644 --- a/src/shared/sensors/MS5803/MS5803.h +++ b/src/shared/sensors/MS5803/MS5803.h @@ -79,7 +79,7 @@ public: bool selfTest() override; -private: +protected: /** * Implements a state machines composed of 3 states: * 1. Command pressure sample @@ -94,6 +94,7 @@ private: */ MS5803Data sampleImpl() override; +private: MS5803Data updateData(); SPISlave spiSlave; diff --git a/src/shared/sensors/MS5803/MS5803I2C.h b/src/shared/sensors/MS5803/MS5803I2C.h index 96db302d6..3c983fbf1 100644 --- a/src/shared/sensors/MS5803/MS5803I2C.h +++ b/src/shared/sensors/MS5803/MS5803I2C.h @@ -78,7 +78,7 @@ public: bool selfTest() override; -private: +protected: /** * Implements a state machines composed of 3 states: * 1. Command pressure sample @@ -93,6 +93,7 @@ private: */ MS5803Data sampleImpl() override; +private: MS5803Data updateData(); uint16_t readReg(uint8_t reg); diff --git a/src/shared/sensors/Sensor.h b/src/shared/sensors/Sensor.h index f26781e0a..bfbae2740 100644 --- a/src/shared/sensors/Sensor.h +++ b/src/shared/sensors/Sensor.h @@ -1,5 +1,5 @@ /* Copyright (c) 2016-2020 Skyward Experimental Rocketry - * Authors: Alain Carlucci, Luca Conterio + * Authors: Alain Carlucci, Luca Conterio, Davide Mor, Emilio Corigliano * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -25,6 +25,7 @@ #include <miosix.h> #include <array> +#include <memory> #include <type_traits> #include "SensorData.h" @@ -88,8 +89,11 @@ public: template <typename Data> class Sensor : public virtual AbstractSensor { +public: + using DataType = Data; + protected: - Data lastSample; + DataType lastSample; /** * @brief Read a data sample from the sensor. @@ -97,19 +101,23 @@ protected: * available correct sample. * @return sensor data sample */ - virtual Data sampleImpl() = 0; + virtual DataType sampleImpl() = 0; // Thread safe mutex to synchronize writes and reads miosix::FastMutex mutex; public: + Sensor() {} + + Sensor(Sensor&& other) : lastSample{std::move(other.lastSample)}, mutex{} {} + virtual ~Sensor() {} void sample() override { // Sampling outside of the protected zone ensures that the sampling // action cannot cause locks or delays - Data d = sampleImpl(); + DataType d = sampleImpl(); { miosix::Lock<miosix::FastMutex> l(mutex); diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.h b/src/shared/sensors/UBXGPS/UBXGPSSerial.h index 733d2ed6c..9187466a6 100644 --- a/src/shared/sensors/UBXGPS/UBXGPSSerial.h +++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.h @@ -81,9 +81,10 @@ public: */ bool selfTest() override; -private: +protected: UBXGPSData sampleImpl() override; +private: /** * @brief Resets the device to its default configuration. * diff --git a/src/shared/sensors/UBXGPS/UBXGPSSpi.h b/src/shared/sensors/UBXGPS/UBXGPSSpi.h index e36b6d5b4..eb14fb4e2 100644 --- a/src/shared/sensors/UBXGPS/UBXGPSSpi.h +++ b/src/shared/sensors/UBXGPS/UBXGPSSpi.h @@ -74,9 +74,10 @@ public: bool selfTest() override; -private: +protected: UBXGPSData sampleImpl() override; +private: /** * @brief Resets the device to its default configuration. * diff --git a/src/shared/sensors/VN100/VN100Serial.h b/src/shared/sensors/VN100/VN100Serial.h index cd0959f74..36ce97ac4 100644 --- a/src/shared/sensors/VN100/VN100Serial.h +++ b/src/shared/sensors/VN100/VN100Serial.h @@ -116,12 +116,13 @@ public: bool selfTest() override; -private: +protected: /** * @brief Sample action implementation. */ VN100SerialData sampleImpl() override; +private: /** * @brief Active object method, about the thread execution */ diff --git a/src/shared/sensors/analog/pressure/honeywell/HSCMAND015PA.h b/src/shared/sensors/analog/pressure/honeywell/HSCMAND015PA.h index e7baf675b..1e82396f0 100644 --- a/src/shared/sensors/analog/pressure/honeywell/HSCMAND015PA.h +++ b/src/shared/sensors/analog/pressure/honeywell/HSCMAND015PA.h @@ -31,7 +31,7 @@ namespace Boardcore /** * @brief Absolute pressure sensor with a 0-103kPa range (0-15psi) */ -class HSCMAND015PA final : public HoneywellPressureSensor<HSCMAND015PAData> +class HSCMAND015PA : public HoneywellPressureSensor<HSCMAND015PAData> { public: HSCMAND015PA(std::function<ADCData()> getSensorVoltage, diff --git a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN015PA.h b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN015PA.h index 7669f3f4f..1f207637d 100644 --- a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN015PA.h +++ b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN015PA.h @@ -31,7 +31,7 @@ namespace Boardcore /** * @brief Absolute pressure sensor with a 0-103kPa range (0-15psi) */ -class HSCMRNN015PA final : public HoneywellPressureSensor<HSCMRNN015PAData> +class HSCMRNN015PA : public HoneywellPressureSensor<HSCMRNN015PAData> { public: HSCMRNN015PA(std::function<ADCData()> getSensorVoltage, diff --git a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN030PA.h b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN030PA.h index e35a3f86e..2b1f3a25a 100644 --- a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN030PA.h +++ b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN030PA.h @@ -31,7 +31,7 @@ namespace Boardcore /** * @brief Absolute pressure sensor with a 0-206kPa range (0-30psi) */ -class HSCMRNN030PA final : public HoneywellPressureSensor<HSCMRNN030PAData> +class HSCMRNN030PA : public HoneywellPressureSensor<HSCMRNN030PAData> { public: HSCMRNN030PA(std::function<ADCData()> getSensorVoltage, diff --git a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN160KA.h b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN160KA.h index f2c17aa74..7b3428685 100644 --- a/src/shared/sensors/analog/pressure/honeywell/HSCMRNN160KA.h +++ b/src/shared/sensors/analog/pressure/honeywell/HSCMRNN160KA.h @@ -31,7 +31,7 @@ namespace Boardcore /** * @brief Absolute pressure sensor with a 0-160kPa range */ -class HSCMRNN160KA final : public HoneywellPressureSensor<HSCMRNN160KAData> +class HSCMRNN160KA : public HoneywellPressureSensor<HSCMRNN160KAData> { public: HSCMRNN160KA(std::function<ADCData()> getSensorVoltage, diff --git a/src/shared/sensors/analog/pressure/honeywell/SSCDANN030PAA.h b/src/shared/sensors/analog/pressure/honeywell/SSCDANN030PAA.h index d18cb5fa0..7cbbe1601 100644 --- a/src/shared/sensors/analog/pressure/honeywell/SSCDANN030PAA.h +++ b/src/shared/sensors/analog/pressure/honeywell/SSCDANN030PAA.h @@ -31,7 +31,7 @@ namespace Boardcore /** * @brief Absolute pressure sensor with a 0-206kPa range (0-30psi) */ -class SSCDANN030PAA final : public HoneywellPressureSensor<SSCDANN030PAAData> +class SSCDANN030PAA : public HoneywellPressureSensor<SSCDANN030PAAData> { public: SSCDANN030PAA(std::function<ADCData()> getSensorVoltage, diff --git a/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h b/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h index b144f9334..4659631f0 100644 --- a/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h +++ b/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h @@ -33,7 +33,7 @@ namespace Boardcore /** * @brief Differential pressure sensor with a ±103kPa range (±15psi) */ -class SSCDRRN015PDA final : public HoneywellPressureSensor<SSCDRRN015PDAData> +class SSCDRRN015PDA : public HoneywellPressureSensor<SSCDRRN015PDAData> { public: SSCDRRN015PDA(std::function<ADCData()> getSensorVoltage, diff --git a/src/shared/sensors/analog/pressure/honeywell/SSCMRNN030PA.h b/src/shared/sensors/analog/pressure/honeywell/SSCMRNN030PA.h index beaac6843..3cd4ab058 100644 --- a/src/shared/sensors/analog/pressure/honeywell/SSCMRNN030PA.h +++ b/src/shared/sensors/analog/pressure/honeywell/SSCMRNN030PA.h @@ -31,7 +31,7 @@ namespace Boardcore /** * @brief Absolute pressure sensor with a 0-206kPa range (0-30psi) */ -class SSCMRNN030PA final : public HoneywellPressureSensor<SSCMRNN030PAData> +class SSCMRNN030PA : public HoneywellPressureSensor<SSCMRNN030PAData> { public: SSCMRNN030PA(std::function<ADCData()> getSensorVoltage, diff --git a/src/shared/sensors/analog/pressure/nxp/MPXH6115A.h b/src/shared/sensors/analog/pressure/nxp/MPXH6115A.h index d72485d90..e55cd7c81 100644 --- a/src/shared/sensors/analog/pressure/nxp/MPXH6115A.h +++ b/src/shared/sensors/analog/pressure/nxp/MPXH6115A.h @@ -33,7 +33,7 @@ namespace Boardcore /** * @brief Driver for NXP's MPXH6115A pressure sensor */ -class MPXH6115A final : public AnalogPressureSensor<MPXH6115AData> +class MPXH6115A : public AnalogPressureSensor<MPXH6115AData> { public: MPXH6115A(std::function<ADCData()> getVoltage, diff --git a/src/shared/sensors/analog/pressure/nxp/MPXH6400A.h b/src/shared/sensors/analog/pressure/nxp/MPXH6400A.h index ab136e27e..fdb1d02fb 100644 --- a/src/shared/sensors/analog/pressure/nxp/MPXH6400A.h +++ b/src/shared/sensors/analog/pressure/nxp/MPXH6400A.h @@ -33,7 +33,7 @@ namespace Boardcore /** * @brief Driver for NXP's MPXHZ6130A pressure sensor */ -class MPXH6400A final : public AnalogPressureSensor<MPXH6400AData> +class MPXH6400A : public AnalogPressureSensor<MPXH6400AData> { public: MPXH6400A(std::function<ADCData()> getVoltage, diff --git a/src/shared/sensors/analog/pressure/nxp/MPXHZ6130A.h b/src/shared/sensors/analog/pressure/nxp/MPXHZ6130A.h index f2beffc5d..26e0ab5b1 100644 --- a/src/shared/sensors/analog/pressure/nxp/MPXHZ6130A.h +++ b/src/shared/sensors/analog/pressure/nxp/MPXHZ6130A.h @@ -33,7 +33,7 @@ namespace Boardcore /** * @brief Driver for NXP's MPXHZ6130A pressure sensor */ -class MPXHZ6130A final : public AnalogPressureSensor<MPXHZ6130AData> +class MPXHZ6130A : public AnalogPressureSensor<MPXHZ6130AData> { public: MPXHZ6130A(std::function<ADCData()> getVoltage, diff --git a/src/tests/hil/BoardScheduler.cpp b/src/tests/hil/BoardScheduler.cpp new file mode 100644 index 000000000..b8acb5a4f --- /dev/null +++ b/src/tests/hil/BoardScheduler.cpp @@ -0,0 +1,65 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 "BoardScheduler.h" + +using namespace Boardcore; + +namespace HILTest +{ +BoardScheduler::BoardScheduler() +{ + scheduler1 = new TaskScheduler(miosix::PRIORITY_MAX - 4); + scheduler2 = new TaskScheduler(miosix::PRIORITY_MAX - 3); + scheduler3 = new TaskScheduler(miosix::PRIORITY_MAX - 2); + scheduler4 = new TaskScheduler(miosix::PRIORITY_MAX - 1); +} + +TaskScheduler* BoardScheduler::getScheduler(miosix::Priority priority) +{ + switch (priority.get()) + { + case miosix::PRIORITY_MAX: + return scheduler4; + case miosix::PRIORITY_MAX - 1: + return scheduler3; + case miosix::PRIORITY_MAX - 2: + return scheduler2; + case miosix::MAIN_PRIORITY: + return scheduler1; + default: + return scheduler1; + } +} + +bool BoardScheduler::start() +{ + return scheduler1->start() && scheduler2->start() && scheduler3->start() && + scheduler4->start(); +} + +bool BoardScheduler::isStarted() +{ + return scheduler1->isRunning() && scheduler2->isRunning() && + scheduler3->isRunning() && scheduler4->isRunning(); +} +} // namespace HILTest \ No newline at end of file diff --git a/src/tests/hil/BoardScheduler.h b/src/tests/hil/BoardScheduler.h new file mode 100644 index 000000000..5664a8095 --- /dev/null +++ b/src/tests/hil/BoardScheduler.h @@ -0,0 +1,62 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <utils/ModuleManager/ModuleManager.hpp> + +namespace HILTest +{ +/** + * @brief Class that wraps the 4 main task schedulers of the entire OBSW. + * There is a task scheduler for every miosix priority + */ +class BoardScheduler : public Boardcore::Module +{ +public: + BoardScheduler(); + + /** + * @brief Get the Scheduler object relative to the requested priority + * + * @param priority The task scheduler priority + * @return Boardcore::TaskScheduler& Reference to the requested task + * scheduler. + * @note Min priority scheduler is returned in case of non valid priority. + */ + Boardcore::TaskScheduler* getScheduler(miosix::Priority priority); + + [[nodiscard]] bool start(); + + /** + * @brief Returns if all the schedulers are up and running + */ + bool isStarted(); + +private: + Boardcore::TaskScheduler* scheduler1; + Boardcore::TaskScheduler* scheduler2; + Boardcore::TaskScheduler* scheduler3; + Boardcore::TaskScheduler* scheduler4; +}; +} // namespace HILTest \ No newline at end of file diff --git a/src/tests/hil/Buses.h b/src/tests/hil/Buses.h new file mode 100644 index 000000000..33dffab57 --- /dev/null +++ b/src/tests/hil/Buses.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <drivers/i2c/I2C.h> +#include <drivers/spi/SPIBus.h> +#include <drivers/usart/USART.h> +#include <interfaces-impl/hwmapping.h> + +#include <utils/ModuleManager/ModuleManager.hpp> +namespace HILTest +{ +class Buses : public Boardcore::Module +{ +public: + Boardcore::SPIBus spi1; + Boardcore::SPIBus spi3; + Boardcore::SPIBus spi4; + Boardcore::SPIBus spi6; + + Boardcore::I2C i2c1; + + Boardcore::USART usart1; + Boardcore::USART usart2; + Boardcore::USART uart4; + + Buses() + : spi1(SPI1), spi3(SPI3), spi4(SPI4), spi6(SPI6), + i2c1(I2C1, miosix::interfaces::i2c1::scl::getPin(), + miosix::interfaces::i2c1::sda::getPin()), + usart1(USART1, 115200), usart2(USART2, 256000, 1024), + uart4(UART4, 115200) + { + } +}; +} // namespace HILTest \ No newline at end of file diff --git a/src/tests/hil/HILSimulationConfig.h b/src/tests/hil/HILSimulationConfig.h new file mode 100644 index 000000000..2e9e218db --- /dev/null +++ b/src/tests/hil/HILSimulationConfig.h @@ -0,0 +1,530 @@ +/* Copyright (c) 2023-2024 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * 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 <drivers/timer/TimestampTimer.h> +#include <drivers/usart/USART.h> +#include <events/EventBroker.h> +#include <hil/HIL.h> +#include <math.h> +#include <sensors/HILSimulatorData.h> +#include <sensors/SensorInfo.h> +#include <utils/Debug.h> +#include <utils/Stats/Stats.h> + +#include <list> +#include <utils/ModuleManager/ModuleManager.hpp> + +#include "Buses.h" +#include "Sensors/SensorsConfig.h" +#include "common/Events.h" + +namespace HILConfig +{ + +constexpr bool IS_FULL_HIL = true; + +/** Period of simulation [ms] */ +constexpr int SIMULATION_PERIOD = 100; + +/** sampling periods of sensors [ms] */ +constexpr int ACCEL_PERIOD = HILTest::SensorsConfig::LSM6DSRX_PERIOD; +constexpr int GYRO_PERIOD = HILTest::SensorsConfig::LSM6DSRX_PERIOD; +constexpr int MAGN_PERIOD = HILTest::SensorsConfig::LIS2MDL_PERIOD; +constexpr int IMU_PERIOD = HILTest::SensorsConfig::IMU_PERIOD; +constexpr int ANALOG_BARO_PERIOD = HILTest::SensorsConfig::ADS131M08_PERIOD; +constexpr int DIGITAL_BARO_PERIOD = HILTest::SensorsConfig::LPS22DF_PERIOD; +constexpr int TEMP_PERIOD = SIMULATION_PERIOD; // just one sample +constexpr int GPS_PERIOD = HILTest::SensorsConfig::UBXGPS_PERIOD; +constexpr int BARO_CHAMBER_PERIOD = 20; +constexpr int PITOT_PERIOD = 50; + +static_assert((SIMULATION_PERIOD % ACCEL_PERIOD) == 0, + "N_DATA_ACCEL not an integer"); +static_assert((SIMULATION_PERIOD % GYRO_PERIOD) == 0, + "N_DATA_GYRO not an integer"); +static_assert((SIMULATION_PERIOD % MAGN_PERIOD) == 0, + "N_DATA_MAGN not an integer"); +static_assert((SIMULATION_PERIOD % IMU_PERIOD) == 0, + "N_DATA_IMU not an integer"); +static_assert((SIMULATION_PERIOD % ANALOG_BARO_PERIOD) == 0, + "N_DATA_ANALOG_BARO not an integer"); +static_assert((SIMULATION_PERIOD % DIGITAL_BARO_PERIOD) == 0, + "N_DATA_DIGITAL_BARO not an integer"); +static_assert((SIMULATION_PERIOD % BARO_CHAMBER_PERIOD) == 0, + "N_DATA_BARO_CHAMBER not an integer"); +static_assert((SIMULATION_PERIOD % PITOT_PERIOD) == 0, + "N_DATA_PITOT not an integer"); +static_assert((SIMULATION_PERIOD % TEMP_PERIOD) == 0, + "N_DATA_TEMP not an integer"); +static_assert((SIMULATION_PERIOD % GPS_PERIOD) == 0, + "N_DATA_GPS not an integer"); + +/** Number of samples per sensor at each simulator iteration */ +constexpr int N_DATA_ACCEL = SIMULATION_PERIOD / ACCEL_PERIOD; +constexpr int N_DATA_GYRO = SIMULATION_PERIOD / GYRO_PERIOD; +constexpr int N_DATA_MAGNETO = SIMULATION_PERIOD / MAGN_PERIOD; +constexpr int N_DATA_IMU = SIMULATION_PERIOD / IMU_PERIOD; +constexpr int N_DATA_ANALOG_BARO = SIMULATION_PERIOD / ANALOG_BARO_PERIOD; +constexpr int N_DATA_DIGITAL_BARO = SIMULATION_PERIOD / DIGITAL_BARO_PERIOD; +constexpr int N_DATA_BARO_CHAMBER = SIMULATION_PERIOD / BARO_CHAMBER_PERIOD; +constexpr int N_DATA_PITOT = SIMULATION_PERIOD / PITOT_PERIOD; +constexpr int N_DATA_GPS = SIMULATION_PERIOD / GPS_PERIOD; +constexpr int N_DATA_TEMP = SIMULATION_PERIOD / TEMP_PERIOD; + +// Sensors Data +using MainAccelerometerSimulatorData = + Boardcore::AccelerometerSimulatorData<N_DATA_ACCEL>; +using MainGyroscopeSimulatorData = + Boardcore::GyroscopeSimulatorData<N_DATA_GYRO>; +using MainMagnetometerSimulatorData = + Boardcore::MagnetometerSimulatorData<N_DATA_MAGNETO>; +using MainGPSSimulatorData = Boardcore::GPSSimulatorData<N_DATA_GPS>; +using MainBarometerSimulatorData = + Boardcore::BarometerSimulatorData<N_DATA_DIGITAL_BARO>; +using MainChamberPressureSimulatorData = + Boardcore::BarometerSimulatorData<N_DATA_BARO_CHAMBER>; +using MainPitotSimulatorData = Boardcore::PitotSimulatorData<N_DATA_PITOT>; +using MainTemperatureSimulatorData = + Boardcore::TemperatureSimulatorData<N_DATA_TEMP>; + +/** + * @brief ADA data sent to the simulator + */ +struct ADAStateHIL +{ + float mslAltitude = 0; // Altitude at mean sea level [m]. + float aglAltitude = 0; // Altitude above ground level [m]. + float verticalSpeed = 0; // Vertical speed [m/s]. + float apogeeDetected = 0; // Flag if apogee is detected [bool] + float updating = 0; // + + ADAStateHIL() + : mslAltitude(0), aglAltitude(0), verticalSpeed(0), apogeeDetected(0), + updating(0) + { + } + + void print() + { + printf( + "mslAltitude: %+.3f\n" + "aglAltitude: %+.3f\n" + "verticalSpeed: %+.3f\n" + "apogeeDetected: %+.3f\n" + "updating: %+.3f\n", + mslAltitude, aglAltitude, verticalSpeed, apogeeDetected, updating); + } +}; + +/** + * @brief NAS data sent to the simulator + */ +struct NASStateHIL +{ + float n = 0; + float e = 0; + float d = 0; + + // Velocity [m/s] + float vn = 0; + float ve = 0; + float vd = 0; + + // Attitude as quaternion + float qx = 0; + float qy = 0; + float qz = 0; + float qw = 1; + + float updating = 0; // Flag if apogee is detected [bool] + + NASStateHIL() + : n(0), e(0), d(0), vn(0), ve(0), vd(0), qx(0), qy(0), qz(0), qw(1), + updating(0) + { + } + + void print() + { + printf( + "n: %+.3f\n" + "e: %+.3f\n" + "d: %+.3f\n" + "vn: %+.3f\n" + "ve: %+.3f\n" + "vd: %+.3f\n" + "qx: %+.3f\n" + "qy: %+.3f\n" + "qz: %+.3f\n" + "qw: %+.3f\n" + "updating: %+.3f\n", + n, e, d, vn, ve, vd, qx, qy, qz, qw, updating); + } +}; + +/** + * @brief ABK data sent to the simulator + */ +struct AirBrakesStateHIL +{ + float updating = 0; // Flag if apogee is detected [bool] + + AirBrakesStateHIL() : updating(0) {} + + void print() { printf("updating: %+.3f\n", updating); } +}; + +/** + * @brief MEA data sent to the simulator + */ +struct MEAStateHIL +{ + float correctedPressure = 0; + + float estimatedMass = 0; + float estimatedApogee = 0; + + float updating = 0; // Flag if apogee is detected [bool] + + MEAStateHIL() + : correctedPressure(0), estimatedMass(0), estimatedApogee(0), + updating(0) + { + } + + void print() + { + printf( + "correctedPressure: %+.3f\n" + "estimatedMass: %+.3f\n" + "estimatedApogee: %+.3f\n" + "updating: %+.3f\n", + correctedPressure, estimatedMass, estimatedApogee, updating); + } +}; + +struct ActuatorsStateHIL +{ + float airbrakesPercentage = 0; + float expulsionPercentage = 0; + float mainValvePercentage = 0; + float ventingValvePercentage = 0; + float cutterState = 0; + + ActuatorsStateHIL() + : airbrakesPercentage(0.0f), expulsionPercentage(0.0f), + mainValvePercentage(0.0f), ventingValvePercentage(0.0f) + { + } + + void print() + { + printf( + "airbrakes: %f perc\n" + "expulsion: %f perc\n" + "mainValve: %f perc\n" + "venting: %f perc\n" + "cutter: %f\n", + airbrakesPercentage * 100, expulsionPercentage * 100, + mainValvePercentage * 100, ventingValvePercentage * 100, + cutterState); + } +}; + +/** + * @brief Data structure used by the simulator in order to directly deserialize + * the data received + * + * This structure then is accessed by sensors and other components in order to + * get the data they need + */ +struct SimulatorData +{ + MainAccelerometerSimulatorData accelerometer; + MainGyroscopeSimulatorData gyro; + MainMagnetometerSimulatorData magnetometer; + MainGPSSimulatorData gps; + MainBarometerSimulatorData barometer1, barometer2, barometer3; + MainChamberPressureSimulatorData pressureChamber; + MainPitotSimulatorData pitot; + MainTemperatureSimulatorData temperature; +}; + +/** + * @brief Data structure expected by the simulator + */ +struct ActuatorData +{ + ADAStateHIL adaState; + NASStateHIL nasState; + AirBrakesStateHIL airBrakesState; + MEAStateHIL meaState; + ActuatorsStateHIL actuatorsState; + + ActuatorData() + : adaState(), nasState(), airBrakesState(), meaState(), actuatorsState() + { + } + + void print() + { + adaState.print(); + nasState.print(); + airBrakesState.print(); + meaState.print(); + actuatorsState.print(); + } +}; + +enum MainFlightPhases +{ + SIM_FLYING, + SIM_ASCENT, + SIM_BURNING, + SIM_AEROBRAKES, + SIM_PARA1, + SIM_PARA2, + SIMULATION_STARTED, + CALIBRATION, + CALIBRATION_OK, + ARMED, + LIFTOFF_PIN_DETACHED, + LIFTOFF, + AEROBRAKES, + APOGEE, + PARA1, + PARA2, + SIMULATION_STOPPED +}; + +class MainHILTransceiver + : public Boardcore::HILTransceiver<MainFlightPhases, SimulatorData, + ActuatorData>, + public Boardcore::Module +{ + using Boardcore::HILTransceiver<MainFlightPhases, SimulatorData, + ActuatorData>::HILTransceiver; +}; + +class MainHIL + : public Boardcore::HIL<MainFlightPhases, SimulatorData, ActuatorData>, + public Boardcore::Module +{ + using Boardcore::HIL<MainFlightPhases, SimulatorData, ActuatorData>::HIL; +}; + +class MainHILPhasesManager + : public Boardcore::HILPhasesManager<MainFlightPhases, SimulatorData, + ActuatorData>, + public Boardcore::Module +{ +public: + explicit MainHILPhasesManager( + std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition) + : Boardcore::HILPhasesManager<MainFlightPhases, SimulatorData, + ActuatorData>(getCurrentPosition) + { + flagsFlightPhases = {{MainFlightPhases::SIM_FLYING, false}, + {MainFlightPhases::SIM_ASCENT, false}, + {MainFlightPhases::SIM_BURNING, false}, + {MainFlightPhases::SIM_AEROBRAKES, false}, + {MainFlightPhases::SIM_PARA1, false}, + {MainFlightPhases::SIM_PARA2, false}, + {MainFlightPhases::SIMULATION_STARTED, false}, + {MainFlightPhases::CALIBRATION, false}, + {MainFlightPhases::CALIBRATION_OK, false}, + {MainFlightPhases::ARMED, false}, + {MainFlightPhases::LIFTOFF_PIN_DETACHED, false}, + {MainFlightPhases::LIFTOFF, false}, + {MainFlightPhases::AEROBRAKES, false}, + {MainFlightPhases::APOGEE, false}, + {MainFlightPhases::PARA1, false}, + {MainFlightPhases::PARA2, false}, + {MainFlightPhases::SIMULATION_STOPPED, false}}; + + prev_flagsFlightPhases = flagsFlightPhases; + + auto& eventBroker = Boardcore::EventBroker::getInstance(); + eventBroker.subscribe(this, Common::TOPIC_ABK); + eventBroker.subscribe(this, Common::TOPIC_ADA); + eventBroker.subscribe(this, Common::TOPIC_MEA); + eventBroker.subscribe(this, Common::TOPIC_DPL); + eventBroker.subscribe(this, Common::TOPIC_CAN); + eventBroker.subscribe(this, Common::TOPIC_FLIGHT); + eventBroker.subscribe(this, Common::TOPIC_FMM); + eventBroker.subscribe(this, Common::TOPIC_FSR); + eventBroker.subscribe(this, Common::TOPIC_NAS); + eventBroker.subscribe(this, Common::TOPIC_TMTC); + eventBroker.subscribe(this, Common::TOPIC_MOTOR); + eventBroker.subscribe(this, Common::TOPIC_TARS); + eventBroker.subscribe(this, Common::TOPIC_ALT); + } + + void processFlagsImpl(const SimulatorData& simulatorData, + std::vector<MainFlightPhases>& changed_flags) override + { + // set true when the first packet from the simulator arrives + if (isSetTrue(MainFlightPhases::SIMULATION_STARTED)) + { + t_start = Boardcore::TimestampTimer::getTimestamp(); + + TRACE("[HIL] ------- SIMULATION STARTED ! ------- \n"); + changed_flags.push_back(MainFlightPhases::SIMULATION_STARTED); + } + + if (flagsFlightPhases[MainFlightPhases::SIM_FLYING]) + { + if (isSetTrue(MainFlightPhases::SIM_FLYING)) + { + registerOutcomes(MainFlightPhases::SIM_FLYING); + TRACE("[HIL] ------- SIMULATOR LIFTOFF ! ------- \n"); + changed_flags.push_back(MainFlightPhases::SIM_FLYING); + } + } + } + + void printOutcomes() + { + printf("OUTCOMES: (times dt from liftoff)\n\n"); + printf("Simulation time: %.3f [sec]\n\n", + (double)(t_stop - t_start) / 1000000.0f); + + printf("Motor stopped burning (simulation flag): \n"); + outcomes[MainFlightPhases::SIM_BURNING].print(t_liftoff); + + printf("Airbrakes exit shadowmode: \n"); + outcomes[MainFlightPhases::AEROBRAKES].print(t_liftoff); + + printf("Apogee: \n"); + outcomes[MainFlightPhases::APOGEE].print(t_liftoff); + + printf("Parachute 1: \n"); + outcomes[MainFlightPhases::PARA1].print(t_liftoff); + + printf("Parachute 2: \n"); + outcomes[MainFlightPhases::PARA2].print(t_liftoff); + + printf("Simulation Stopped: \n"); + outcomes[MainFlightPhases::SIMULATION_STOPPED].print(t_liftoff); + + // auto cpuMeter = Boardcore::CpuMeter::getCpuStats(); + // printf("max cpu usage: %+.3f\n", cpuMeter.maxValue); + // printf("avg cpu usage: %+.3f\n", cpuMeter.mean); + // printf("min free heap: %+.3lu\n", cpuMeter.minFreeHeap); + // printf("max free stack:%+.3lu\n", cpuMeter.minFreeStack); + } + +private: + void handleEventImpl(const Boardcore::Event& e, + std::vector<MainFlightPhases>& changed_flags) override + { + switch (e) + { + case Common::Events::FMM_INIT_ERROR: + printf("[HIL] ------- INIT FAILED ! ------- \n"); + case Common::Events::FMM_INIT_OK: + setFlagFlightPhase(MainFlightPhases::CALIBRATION, true); + TRACE("[HIL] ------- CALIBRATION ! ------- \n"); + changed_flags.push_back(MainFlightPhases::CALIBRATION); + break; + case Common::Events::FLIGHT_DISARMED: + setFlagFlightPhase(MainFlightPhases::CALIBRATION_OK, true); + TRACE("[HIL] CALIBRATION OK!\n"); + changed_flags.push_back(MainFlightPhases::CALIBRATION_OK); + break; + case Common::Events::FLIGHT_ARMED: + setFlagFlightPhase(MainFlightPhases::ARMED, true); + printf("[HIL] ------- READY TO LAUNCH ! ------- \n"); + changed_flags.push_back(MainFlightPhases::ARMED); + break; + case Common::Events::FLIGHT_LAUNCH_PIN_DETACHED: + setFlagFlightPhase(MainFlightPhases::LIFTOFF_PIN_DETACHED, + true); + TRACE("[HIL] ------- LIFTOFF PIN DETACHED ! ------- \n"); + changed_flags.push_back(MainFlightPhases::LIFTOFF_PIN_DETACHED); + break; + case Common::Events::FLIGHT_LIFTOFF: + case Common::Events::TMTC_FORCE_LAUNCH: + t_liftoff = Boardcore::TimestampTimer::getTimestamp(); + printf("[HIL] ------- LIFTOFF -------: %f, %f \n", + getCurrentPosition().z, getCurrentPosition().vz); + changed_flags.push_back(MainFlightPhases::LIFTOFF); + break; + case Common::Events::ABK_SHADOW_MODE_TIMEOUT: + setFlagFlightPhase(MainFlightPhases::AEROBRAKES, true); + registerOutcomes(MainFlightPhases::AEROBRAKES); + TRACE("[HIL] ABK shadow mode timeout\n"); + changed_flags.push_back(MainFlightPhases::AEROBRAKES); + break; + case Common::Events::ADA_SHADOW_MODE_TIMEOUT: + TRACE("[HIL] ADA shadow mode timeout\n"); + break; + case Common::Events::ABK_DISABLE: + setFlagFlightPhase(MainFlightPhases::AEROBRAKES, false); + TRACE("[HIL] ABK disabled\n"); + break; + case Common::Events::FLIGHT_APOGEE_DETECTED: + case Common::Events::CAN_APOGEE_DETECTED: + setFlagFlightPhase(MainFlightPhases::AEROBRAKES, false); + registerOutcomes(MainFlightPhases::APOGEE); + printf("[HIL] ------- APOGEE DETECTED ! ------- %f, %f \n", + getCurrentPosition().z, getCurrentPosition().vz); + changed_flags.push_back(MainFlightPhases::APOGEE); + break; + case Common::Events::FLIGHT_DROGUE_DESCENT: + case Common::Events::TMTC_FORCE_EXPULSION: + setFlagFlightPhase(MainFlightPhases::PARA1, true); + registerOutcomes(MainFlightPhases::PARA1); + printf("[HIL] ------- PARA1 ! -------%f, %f \n", + getCurrentPosition().z, getCurrentPosition().vz); + changed_flags.push_back(MainFlightPhases::PARA1); + break; + case Common::Events::FLIGHT_WING_DESCENT: + case Common::Events::FLIGHT_DPL_ALT_DETECTED: + case Common::Events::TMTC_FORCE_DEPLOYMENT: + setFlagFlightPhase(MainFlightPhases::PARA1, false); + setFlagFlightPhase(MainFlightPhases::PARA2, true); + registerOutcomes(MainFlightPhases::PARA2); + printf("[HIL] ------- PARA2 ! ------- %f, %f \n", + getCurrentPosition().z, getCurrentPosition().vz); + changed_flags.push_back(MainFlightPhases::PARA2); + break; + case Common::Events::FLIGHT_LANDING_DETECTED: + case Common::Events::TMTC_FORCE_LANDING: + t_stop = Boardcore::TimestampTimer::getTimestamp(); + setFlagFlightPhase(MainFlightPhases::PARA2, false); + setFlagFlightPhase(MainFlightPhases::SIMULATION_STOPPED, true); + changed_flags.push_back(MainFlightPhases::SIMULATION_STOPPED); + registerOutcomes(MainFlightPhases::SIMULATION_STOPPED); + TRACE("[HIL] ------- SIMULATION STOPPED ! -------: %f \n\n\n", + (double)t_stop / 1000000.0f); + printOutcomes(); + break; + default: + TRACE("%s event\n", Common::getEventString(e).c_str()); + } + } +}; + +} // namespace HILConfig \ No newline at end of file diff --git a/src/tests/hil/Sensors/HILSensors.h b/src/tests/hil/Sensors/HILSensors.h new file mode 100644 index 000000000..7d642072a --- /dev/null +++ b/src/tests/hil/Sensors/HILSensors.h @@ -0,0 +1,360 @@ +/* Copyright (c) 2023-2024 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * 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 <hil/HIL.h> +#include <sensors/HILSensor.h> +#include <sensors/analog/Pitot/Pitot.h> + +#include "../HILSimulationConfig.h" +#include "Sensors.h" + +namespace HILTest +{ + +/** + * @brief Mock sensor which is used as fake sensor for the combustion chamber + * pressure coming from the canbus. + */ +class MockChamberSensor : public Boardcore::Sensor<Boardcore::PressureData> +{ +public: + explicit MockChamberSensor() {} + + Boardcore::PressureData sampleImpl() override + { + return Boardcore::PressureData{0, 0}; + } + +protected: + bool init() override { return true; } + + bool selfTest() override { return true; } +}; + +class HILSensors : public Sensors +{ +public: + explicit HILSensors(Boardcore::TaskScheduler* sched, Buses* buses, + HILConfig::MainHIL* hil, bool enableHw) + : Sensors{sched, buses}, enableHw{enableHw} + { + using namespace HILConfig; + using namespace Boardcore; + + // Creating the fake sensors for the can transmitted samples + chamberPressureCreation(); + pitotCreation(hil); + + hillificator<>(lps28dfw_1, enableHw, + [hil]() { return updateLPS28DFWData(hil); }); + hillificator<>(lps28dfw_2, enableHw, + [hil]() { return updateLPS28DFWData(hil); }); + hillificator<>(lps22df, enableHw, + [hil]() { return updateLPS22DFData(hil); }); + hillificator<>(h3lis331dl, enableHw, + [hil]() { return updateH3LIS331DLData(hil); }); + hillificator<>(lis2mdl, enableHw, + [hil]() { return updateLIS2MDLData(hil); }); + hillificator<>(ubxgps, enableHw, + [hil]() { return updateUBXGPSData(hil); }); + hillificator<>(lsm6dsrx, enableHw, + [hil]() { return updateLSM6DSRXData(hil); }); + hillificator<>(hscmrnn015pa_1, enableHw, + [hil]() { return updateStaticPressureData(hil); }); + hillificator<>(hscmrnn015pa_2, enableHw, + [hil]() { return updateStaticPressureData(hil); }); + hillificator<>(imu, enableHw, + [this, hil]() { return updateIMUData(hil, *this); }); + hillificator<>(chamber, enableHw, + [hil]() { return updateCCData(hil); }); + }; + + bool start() override + { + // Registering the fake can sensors + if (chamber) + { + registerSensor(chamber.get(), "chamber", + HILConfig::BARO_CHAMBER_PERIOD, + [this]() { this->chamberPressureCallback(); }); + } + + if (pitot) + { + registerSensor(pitot.get(), "Pitot", HILConfig::PITOT_PERIOD, + [this]() { this->pitotCallback(); }); + } + + return Sensors::start(); + } + +private: + void chamberPressureCreation() + { + chamber = std::make_unique<MockChamberSensor>(); + } + + void chamberPressureCallback() + { + // Warning! Here we must use sensor->getLastSample() instead of + // getSensorLastSample() since we have to update the "can received + // value" + auto lastSample = chamber->getLastSample(); + Boardcore::Logger::getInstance().log(lastSample); + } + + void pitotCreation(HILConfig::MainHIL* hil) + { + pitot = std::make_unique<Boardcore::Pitot>( + [hil]() { return getTotalPressurePitot(hil); }, + [hil]() { return getStaticPressurePitot(hil); }); + pitot->setReferenceValues(Boardcore::ReferenceValues()); + } + + void pitotCallback() + { + // Warning! Here we must use sensor->getLastSample() instead of + // getSensorLastSample() since we have to update the "can received + // value" + auto lastSample = pitot->getLastSample(); + Boardcore::Logger::getInstance().log(lastSample); + setPitot(lastSample); + } + + static int getSampleCounter(int nData) + { + auto ts = miosix::getTime(); + auto tsSensorData = Boardcore::ModuleManager::getInstance() + .get<HILConfig::MainHIL>() + ->getTimestampSimulatorData(); + auto simulationPeriod = Boardcore::ModuleManager::getInstance() + .get<HILConfig::MainHIL>() + ->getSimulationPeriod(); + + assert(ts >= tsSensorData && + "Actual timestamp is lesser then the packet timestamp"); + + if (ts >= tsSensorData + simulationPeriod) + { + // TODO: Register this as an error + return nData - 1; // Return the last valid index + } + + // Getting the index floored + int sampleCounter = (ts - tsSensorData) * nData / simulationPeriod; + + if (sampleCounter < 0) + { + printf("sampleCounter: %d\n", sampleCounter); + assert(sampleCounter < 0 && "Calculated a negative index"); + return 0; + } + + return sampleCounter; + } + + static Boardcore::LPS28DFWData updateLPS28DFWData(HILConfig::MainHIL* hil) + { + Boardcore::LPS28DFWData data; + + auto* sensorData = hil->getSensorData(); + + int iBaro = getSampleCounter(sensorData->barometer1.NDATA); + int iTemp = getSampleCounter(sensorData->temperature.NDATA); + + data.pressureTimestamp = data.temperatureTimestamp = miosix::getTime(); + data.pressure = sensorData->barometer1.measures[iBaro]; + data.temperature = sensorData->temperature.measures[iTemp]; + + return data; + }; + + static Boardcore::LPS22DFData updateLPS22DFData(HILConfig::MainHIL* hil) + { + Boardcore::LPS22DFData data; + + auto* sensorData = hil->getSensorData(); + + int iBaro = getSampleCounter(sensorData->barometer1.NDATA); + int iTemp = getSampleCounter(sensorData->temperature.NDATA); + + data.pressureTimestamp = data.temperatureTimestamp = miosix::getTime(); + data.pressure = sensorData->barometer1.measures[iBaro]; + data.temperature = sensorData->temperature.measures[iTemp]; + + return data; + }; + + static Boardcore::H3LIS331DLData updateH3LIS331DLData( + HILConfig::MainHIL* hil) + { + Boardcore::H3LIS331DLData data; + + auto* sensorData = hil->getSensorData(); + + int iAcc = getSampleCounter(sensorData->accelerometer.NDATA); + + data.accelerationTimestamp = miosix::getTime(); + data.accelerationX = sensorData->accelerometer.measures[iAcc][0]; + data.accelerationY = sensorData->accelerometer.measures[iAcc][1]; + data.accelerationZ = sensorData->accelerometer.measures[iAcc][2]; + + return data; + }; + + static Boardcore::LIS2MDLData updateLIS2MDLData(HILConfig::MainHIL* hil) + { + Boardcore::LIS2MDLData data; + + auto* sensorData = hil->getSensorData(); + + int iMag = getSampleCounter(sensorData->magnetometer.NDATA); + + data.magneticFieldTimestamp = miosix::getTime(); + data.magneticFieldX = sensorData->magnetometer.measures[iMag][0]; + data.magneticFieldY = sensorData->magnetometer.measures[iMag][1]; + data.magneticFieldZ = sensorData->magnetometer.measures[iMag][2]; + + return data; + }; + + static Boardcore::UBXGPSData updateUBXGPSData(HILConfig::MainHIL* hil) + { + Boardcore::UBXGPSData data; + + auto* sensorData = hil->getSensorData(); + + int iGps = getSampleCounter(sensorData->gps.NDATA); + + data.gpsTimestamp = miosix::getTime(); + + data.latitude = sensorData->gps.positionMeasures[iGps][0]; + data.longitude = sensorData->gps.positionMeasures[iGps][1]; + data.height = sensorData->gps.positionMeasures[iGps][2]; + + data.velocityNorth = sensorData->gps.velocityMeasures[iGps][0]; + data.velocityEast = sensorData->gps.velocityMeasures[iGps][1]; + data.velocityDown = sensorData->gps.velocityMeasures[iGps][2]; + data.speed = sqrtf(data.velocityNorth * data.velocityNorth + + data.velocityEast * data.velocityEast + + data.velocityDown * data.velocityDown); + data.positionDOP = 0; + + data.fix = static_cast<uint8_t>(sensorData->gps.fix); + data.satellites = static_cast<uint8_t>(sensorData->gps.num_satellites); + + return data; + }; + + static Boardcore::LSM6DSRXData updateLSM6DSRXData(HILConfig::MainHIL* hil) + { + Boardcore::LSM6DSRXData data; + + auto* sensorData = hil->getSensorData(); + + int iAcc = getSampleCounter(sensorData->accelerometer.NDATA); + int iGyro = getSampleCounter(sensorData->gyro.NDATA); + + data.accelerationTimestamp = data.angularSpeedTimestamp = + miosix::getTime(); + + data.accelerationX = sensorData->accelerometer.measures[iAcc][0]; + data.accelerationY = sensorData->accelerometer.measures[iAcc][1]; + data.accelerationZ = sensorData->accelerometer.measures[iAcc][2]; + + data.angularSpeedX = sensorData->gyro.measures[iGyro][0]; + data.angularSpeedY = sensorData->gyro.measures[iGyro][1]; + data.angularSpeedZ = sensorData->gyro.measures[iGyro][2]; + + return data; + }; + + static Boardcore::HSCMRNN015PAData updateStaticPressureData( + HILConfig::MainHIL* hil) + { + Boardcore::HSCMRNN015PAData data; + + auto* sensorData = hil->getSensorData(); + + int iBaro = getSampleCounter(sensorData->barometer1.NDATA); + + data.pressureTimestamp = miosix::getTime(); + data.pressure = sensorData->barometer1.measures[iBaro]; + + return data; + }; + + static Boardcore::IMUData updateIMUData(HILConfig::MainHIL* hil, + Sensors& sensors) + { + return Boardcore::IMUData{ + sensors.getLSM6DSRXLastSample(), sensors.getLSM6DSRXLastSample(), + sensors.getCalibratedMagnetometerLastSample()}; + }; + + static Boardcore::PressureData updateCCData(HILConfig::MainHIL* hil) + { + Boardcore::PressureData data; + + auto* sensorData = hil->getSensorData(); + + int iCC = getSampleCounter(sensorData->pressureChamber.NDATA); + + data.pressureTimestamp = miosix::getTime(); + data.pressure = sensorData->pressureChamber.measures[iCC]; + + return data; + }; + + static float getTotalPressurePitot(HILConfig::MainHIL* hil) + { + float totalPressure; + + auto* sensorData = hil->getSensorData(); + + int iPitot = getSampleCounter(sensorData->pitot.NDATA); + + totalPressure = sensorData->pitot.staticPressure[iPitot] + + sensorData->pitot.deltaP[iPitot]; + + return totalPressure; + }; + + static float getStaticPressurePitot(HILConfig::MainHIL* hil) + { + float staticPressure; + + auto* sensorData = hil->getSensorData(); + + int iPitot = getSampleCounter(sensorData->pitot.NDATA); + + staticPressure = sensorData->pitot.staticPressure[iPitot]; + + return staticPressure; + }; + + std::unique_ptr<MockChamberSensor> chamber; + std::unique_ptr<Boardcore::Pitot> pitot; + bool enableHw; +}; +} // namespace HILTest \ No newline at end of file diff --git a/src/tests/hil/Sensors/Sensors.cpp b/src/tests/hil/Sensors/Sensors.cpp new file mode 100644 index 000000000..1877bd5c7 --- /dev/null +++ b/src/tests/hil/Sensors/Sensors.cpp @@ -0,0 +1,725 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 "Sensors.h" + +#include <interfaces-impl/hwmapping.h> + +#include "../Buses.h" +#include "SensorsConfig.h" + +namespace HILTest +{ +using namespace Boardcore; +using namespace std; +using namespace SensorsConfig; + +LPS22DFData Sensors::getLPS22DFLastSample() +{ + miosix::PauseKernelLock lock; + return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{}; +} +LPS28DFWData Sensors::getLPS28DFW_1LastSample() +{ + miosix::PauseKernelLock lock; + return lps28dfw_1 != nullptr ? lps28dfw_1->getLastSample() : LPS28DFWData{}; +} +LPS28DFWData Sensors::getLPS28DFW_2LastSample() +{ + miosix::PauseKernelLock lock; + return lps28dfw_2 != nullptr ? lps28dfw_2->getLastSample() : LPS28DFWData{}; +} +H3LIS331DLData Sensors::getH3LIS331DLLastSample() +{ + miosix::PauseKernelLock lock; + return h3lis331dl != nullptr ? h3lis331dl->getLastSample() + : H3LIS331DLData{}; +} +LIS2MDLData Sensors::getLIS2MDLLastSample() +{ + miosix::PauseKernelLock lock; + return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{}; +} +UBXGPSData Sensors::getGPSLastSample() +{ + miosix::PauseKernelLock lock; + return ubxgps != nullptr ? ubxgps->getLastSample() : UBXGPSData{}; +} +LSM6DSRXData Sensors::getLSM6DSRXLastSample() +{ + miosix::PauseKernelLock lock; + return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{}; +} +ADS131M08Data Sensors::getADS131M08LastSample() +{ + miosix::PauseKernelLock lock; + return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{}; +} + +PitotData Sensors::getPitotLastSample() +{ + miosix::PauseKernelLock lock; + return canPitot; +} +PressureData Sensors::getCCPressureLastSample() +{ + miosix::PauseKernelLock lock; + return canCCPressure; +} +PressureData Sensors::getBottomTankPressureLastSample() +{ + miosix::PauseKernelLock lock; + return canBottomTankPressure; +} +PressureData Sensors::getTopTankPressureLastSample() +{ + miosix::PauseKernelLock lock; + return canTopTankPressure; +} +TemperatureData Sensors::getTankTemperatureLastSample() +{ + miosix::PauseKernelLock lock; + return canTankTemperature; +} +BatteryVoltageSensorData Sensors::getMotorBatteryVoltage() +{ + miosix::PauseKernelLock lock; + return canMotorBatteryVoltage; +} +CurrentData Sensors::getMotorCurrent() +{ + miosix::PauseKernelLock lock; + return canMotorCurrent; +} + +// Processed Getters +BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + ADS131M08Data sample = getADS131M08LastSample(); + BatteryVoltageSensorData data; + + // Populate the data + data.voltageTimestamp = sample.timestamp; + data.channelId = 1; + data.voltage = sample.voltage[1]; + data.batVoltage = sample.voltage[1] * BATTERY_VOLTAGE_CONVERSION_FACTOR; + return data; +} + +BatteryVoltageSensorData Sensors::getCamBatteryVoltageLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + ADS131M08Data sample = getADS131M08LastSample(); + BatteryVoltageSensorData data; + + // Populate the data + data.voltageTimestamp = sample.timestamp; + data.channelId = 7; + data.voltage = sample.voltage[7]; + data.batVoltage = sample.voltage[7] * BATTERY_VOLTAGE_CONVERSION_FACTOR; + return data; +} + +CurrentData Sensors::getCurrentLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + ADS131M08Data sample = getADS131M08LastSample(); + CurrentData data; + + // Populate the data + data.currentTimestamp = sample.timestamp; + data.current = + (sample.voltage[5] - CURRENT_OFFSET) * CURRENT_CONVERSION_FACTOR; + return data; +} + +MPXH6400AData Sensors::getDeploymentPressureLastSample() +{ + miosix::PauseKernelLock lock; + return mpxh6400a != nullptr ? mpxh6400a->getLastSample() : MPXH6400AData{}; +} + +HSCMRNN015PAData Sensors::getStaticPressure1LastSample() +{ + miosix::PauseKernelLock lock; + return hscmrnn015pa_1 != nullptr ? hscmrnn015pa_1->getLastSample() + : HSCMRNN015PAData{}; +} + +HSCMRNN015PAData Sensors::getStaticPressure2LastSample() +{ + miosix::PauseKernelLock lock; + return hscmrnn015pa_2 != nullptr ? hscmrnn015pa_2->getLastSample() + : HSCMRNN015PAData{}; +} + +IMUData Sensors::getIMULastSample() +{ + miosix::PauseKernelLock lock; + return imu != nullptr ? imu->getLastSample() : IMUData{}; +} + +MagnetometerData Sensors::getCalibratedMagnetometerLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + MagnetometerData lastSample = getLIS2MDLLastSample(); + MagnetometerData result; + + // Correct the result and copy the timestamp + { + miosix::Lock<FastMutex> l(calibrationMutex); + result = + static_cast<MagnetometerData>(magCalibration.correct(lastSample)); + } + + result.magneticFieldTimestamp = lastSample.magneticFieldTimestamp; + return result; +} + +void Sensors::setPitot(PitotData data) +{ + miosix::PauseKernelLock lock; + canPitot = data; + canPitot.timestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setCCPressure(PressureData data) +{ + miosix::PauseKernelLock lock; + canCCPressure = data; + canCCPressure.pressureTimestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setBottomTankPressure(PressureData data) +{ + miosix::PauseKernelLock lock; + canBottomTankPressure = data; + canBottomTankPressure.pressureTimestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setTopTankPressure(PressureData data) +{ + miosix::PauseKernelLock lock; + canTopTankPressure = data; + canTopTankPressure.pressureTimestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setTankTemperature(TemperatureData data) +{ + miosix::PauseKernelLock lock; + canTankTemperature = data; + canTankTemperature.temperatureTimestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setMotorBatteryVoltage(BatteryVoltageSensorData data) +{ + miosix::PauseKernelLock lock; + canMotorBatteryVoltage.batVoltage = data.batVoltage; + canMotorBatteryVoltage.voltageTimestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setMotorCurrent(CurrentData data) +{ + miosix::PauseKernelLock lock; + canMotorCurrent.current = data.current; + canMotorCurrent.currentTimestamp = TimestampTimer::getTimestamp(); +} + +Sensors::Sensors(TaskScheduler* sched, Buses* buses) + : scheduler(sched), buses(buses) +{ + // Init all the sensors + lps22dfCreation(); + lps28dfw_1Creation(); + // lps28dfw_2Creation(); + h3lis331dlCreation(); + lis2mdlCreation(); + ubxgpsCreation(); + lsm6dsrxCreation(); + ads131m08Creation(); + deploymentPressureCreation(); + staticPressure1Creation(); + staticPressure2Creation(); + imuCreation(); +} + +bool Sensors::start() +{ + // Read the magnetometer calibration from predefined file + magCalibration.fromFile("/sd/magCalibration.csv"); + + if (lps22df) + { + registerSensor(lps22df.get(), "LPS22DF", LPS22DF_PERIOD, + [this]() { this->lps22dfCallback(); }); + addInfoGetter(lps22df.get()); + } + + if (lps28dfw_1) + { + registerSensor(lps28dfw_1.get(), "LPS28DFW_1", LPS28DFW_PERIOD, + [this]() { this->lps28dfw_1Callback(); }); + addInfoGetter(lps28dfw_1.get()); + } + + if (lps28dfw_2) + { + registerSensor(lps28dfw_2.get(), "LPS28DFW_2", LPS28DFW_PERIOD, + [this]() { this->lps28dfw_2Callback(); }); + addInfoGetter(lps28dfw_2.get()); + } + + if (h3lis331dl) + { + registerSensor(h3lis331dl.get(), "h3lis331dl", H3LIS331DL_PERIOD, + [this]() { this->h3lis331dlCallback(); }); + addInfoGetter(h3lis331dl.get()); + } + + if (lis2mdl) + { + registerSensor(lis2mdl.get(), "lis2mdl", LIS2MDL_PERIOD, + [this]() { this->lis2mdlCallback(); }); + addInfoGetter(lis2mdl.get()); + } + + if (ubxgps) + { + registerSensor(ubxgps.get(), "ubxgps", UBXGPS_PERIOD, + [this]() { this->ubxgpsCallback(); }); + addInfoGetter(ubxgps.get()); + } + + if (lsm6dsrx) + { + registerSensor(lsm6dsrx.get(), "lsm6dsrx", LSM6DSRX_PERIOD, + [this]() { this->lsm6dsrxCallback(); }); + addInfoGetter(lsm6dsrx.get()); + } + + if (ads131m08) + { + registerSensor(ads131m08.get(), "ads131m08", ADS131M08_PERIOD, + [this]() { this->ads131m08Callback(); }); + addInfoGetter(ads131m08.get()); + } + + if (mpxh6400a) + { + registerSensor(mpxh6400a.get(), "mpxh6400a", ADS131M08_PERIOD, + [this]() { this->deploymentPressureCallback(); }); + } + + if (hscmrnn015pa_1) + { + registerSensor(hscmrnn015pa_1.get(), "hscmrnn015pa_1", ADS131M08_PERIOD, + [this]() { this->staticPressure1Callback(); }); + } + + if (hscmrnn015pa_2) + { + registerSensor(hscmrnn015pa_2.get(), "hscmrnn015pa_2", ADS131M08_PERIOD, + [this]() { this->staticPressure2Callback(); }); + } + + if (imu) + { + registerSensor(imu.get(), "RotatedIMU", IMU_PERIOD, + [this]() { this->imuCallback(); }); + } + + // Add the magnetometer calibration to the scheduler + size_t result = scheduler->addTask( + [&]() + { + // Gather the last sample data + MagnetometerData lastSample = getLIS2MDLLastSample(); + + // Feed the data to the calibrator inside a protected area. + // Contention is not high and the use of a mutex is suitable to + // avoid pausing the kernel for this calibration operation + { + miosix::Lock<FastMutex> l(calibrationMutex); + magCalibrator.feed(lastSample); + } + }, + MAG_CALIBRATION_PERIOD); + + // Create sensor manager with populated map and configured scheduler + manager = std::make_unique<SensorManager>(sensorMap, scheduler); + return manager->start() && result != 0; +} + +void Sensors::stop() { manager->stop(); } + +bool Sensors::isStarted() +{ + return manager->areAllSensorsInitialized() && scheduler->isRunning(); +} + +void Sensors::calibrate() +{ + // Create the stats to calibrate the barometers + Stats lps28dfw1Stats; + // Stats lps28dfw2Stats; + Stats staticPressure1Stats; + Stats staticPressure2Stats; + Stats deploymentPressureStats; + + // Add N samples to the stats + for (unsigned int i = 0; i < SensorsConfig::CALIBRATION_SAMPLES; i++) + { + lps28dfw1Stats.add(getLPS28DFW_1LastSample().pressure); + // lps28dfw2Stats.add(getLPS28DFW_2LastSample().pressure); + staticPressure1Stats.add(getStaticPressure1LastSample().pressure); + staticPressure2Stats.add(getStaticPressure2LastSample().pressure); + deploymentPressureStats.add(getDeploymentPressureLastSample().pressure); + + // Delay for the expected period + miosix::Thread::sleep(SensorsConfig::CALIBRATION_PERIOD); + } + + // Compute the difference between the mean value from LPS28DFW + float reference = lps28dfw1Stats.getStats().mean; + + hscmrnn015pa_1->updateOffset(staticPressure1Stats.getStats().mean - + reference); + hscmrnn015pa_2->updateOffset(staticPressure2Stats.getStats().mean - + reference); + mpxh6400a->updateOffset(deploymentPressureStats.getStats().mean - + reference); + + // Log the offsets + SensorsCalibrationParameter cal{}; + cal.timestamp = TimestampTimer::getTimestamp(); + cal.offsetStatic1 = staticPressure1Stats.getStats().mean - reference; + cal.offsetStatic2 = staticPressure2Stats.getStats().mean - reference; + cal.offsetDeployment = deploymentPressureStats.getStats().mean - reference; + cal.referencePressure = reference; + + Logger::getInstance().log(cal); +} + +bool Sensors::writeMagCalibration() +{ + // Compute the calibration result in protected area + { + miosix::Lock<FastMutex> l(calibrationMutex); + SixParametersCorrector cal = magCalibrator.computeResult(); + + // Check result validity + if (!isnan(cal.getb()[0]) && !isnan(cal.getb()[1]) && + !isnan(cal.getb()[2]) && !isnan(cal.getA()[0]) && + !isnan(cal.getA()[1]) && !isnan(cal.getA()[2])) + { + magCalibration = cal; + + // Save the calibration to the calibration file + return magCalibration.toFile("/sd/magCalibration.csv"); + } + return false; + } +} + +std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> +Sensors::getSensorInfo() +{ + std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> sensorState; + for (size_t i = 0; i < sensorsInit.size(); i++) + { + // Check wether the lambda is existent + if (sensorsInit[i]) + { + sensorState[i] = sensorsInit[i](); + } + } + return sensorState; +} + +void Sensors::lps22dfCreation() +{ + + // Get the correct SPI configuration + SPIBusConfig config = LPS22DF::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_16; + + // Configure the device + LPS22DF::Config sensorConfig; + sensorConfig.avg = LPS22DF_AVG; + sensorConfig.odr = LPS22DF_ODR; + + // Create sensor instance with configured parameters + lps22df = std::make_unique<LPS22DF>(buses->spi3, + miosix::sensors::LPS22DF::cs::getPin(), + config, sensorConfig); +} +void Sensors::lps28dfw_1Creation() +{ + + // Configure the sensor + LPS28DFW::SensorConfig config{false, LPS28DFW_FSR, LPS28DFW_AVG, + LPS28DFW_ODR, false}; + + // Create sensor instance with configured parameters + lps28dfw_1 = std::make_unique<LPS28DFW>(buses->i2c1, config); +} +void Sensors::lps28dfw_2Creation() +{ + + // Configure the sensor + LPS28DFW::SensorConfig config{true, LPS28DFW_FSR, LPS28DFW_AVG, + LPS28DFW_ODR, false}; + + // Create sensor instance with configured parameters + lps28dfw_2 = std::make_unique<LPS28DFW>(buses->i2c1, config); +} +void Sensors::h3lis331dlCreation() +{ + + // Get the correct SPI configuration + SPIBusConfig config = H3LIS331DL::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_16; + + // Create sensor instance with configured parameters + h3lis331dl = std::make_unique<H3LIS331DL>( + buses->spi3, miosix::sensors::H3LIS331DL::cs::getPin(), config, + H3LIS331DL_ODR, H3LIS331DL_BDU, H3LIS331DL_FSR); +} +void Sensors::lis2mdlCreation() +{ + + // Get the correct SPI configuration + SPIBusConfig config = LIS2MDL::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_16; + + // Configure the sensor + LIS2MDL::Config sensorConfig; + sensorConfig.deviceMode = LIS2MDL_OPERATIVE_MODE; + sensorConfig.odr = LIS2MDL_ODR; + sensorConfig.temperatureDivider = LIS2MDL_TEMPERATURE_DIVIDER; + + // Create sensor instance with configured parameters + lis2mdl = std::make_unique<LIS2MDL>(buses->spi3, + miosix::sensors::LIS2MDL::cs::getPin(), + config, sensorConfig); +} + +void Sensors::ubxgpsCreation() +{ + + // Get the correct SPI configuration + SPIBusConfig config = UBXGPSSpi::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_64; + + // Create sensor instance with configured parameters + ubxgps = std::make_unique<UBXGPSSpi>( + buses->spi4, miosix::sensors::GPS::cs::getPin(), config, 5); +} + +void Sensors::lsm6dsrxCreation() +{ + + // Configure the SPI + SPIBusConfig config; + config.clockDivider = SPI::ClockDivider::DIV_32; + config.mode = SPI::Mode::MODE_0; + + // Configure the sensor + LSM6DSRXConfig sensorConfig; + sensorConfig.bdu = LSM6DSRX_BDU; + + // Accelerometer + sensorConfig.fsAcc = LSM6DSRX_ACC_FS; + sensorConfig.odrAcc = LSM6DSRX_ACC_ODR; + sensorConfig.opModeAcc = LSM6DSRX_OPERATING_MODE; + + // Gyroscope + sensorConfig.fsGyr = LSM6DSRX_GYR_FS; + sensorConfig.odrGyr = LSM6DSRX_GYR_ODR; + sensorConfig.opModeGyr = LSM6DSRX_OPERATING_MODE; + + // Fifo + sensorConfig.fifoMode = LSM6DSRX_FIFO_MODE; + sensorConfig.fifoTimestampDecimation = LSM6DSRX_FIFO_TIMESTAMP_DECIMATION; + sensorConfig.fifoTemperatureBdr = LSM6DSRX_FIFO_TEMPERATURE_BDR; + + // Create sensor instance with configured parameters + lsm6dsrx = std::make_unique<LSM6DSRX>( + buses->spi1, miosix::sensors::LSM6DSRX::cs::getPin(), config, + sensorConfig); +} + +void Sensors::ads131m08Creation() +{ + + // Configure the SPI + SPIBusConfig config; + config.clockDivider = SPI::ClockDivider::DIV_32; + + // Configure the device + ADS131M08::Config sensorConfig; + sensorConfig.oversamplingRatio = ADS131M08_OVERSAMPLING_RATIO; + sensorConfig.globalChopModeEnabled = ADS131M08_GLOBAL_CHOP_MODE; + + // Create the sensor instance with configured parameters + ads131m08 = std::make_unique<ADS131M08>( + buses->spi4, miosix::sensors::ADS131::cs::getPin(), config, + sensorConfig); +} + +void Sensors::deploymentPressureCreation() +{ + // Create the lambda function to get the voltage + function<ADCData()> getVoltage = [&]() + { + // No need to synchronize, the sampling thread is the same + ADS131M08Data sample = ads131m08->getLastSample(); + return sample.getVoltage(ADC_CH_DPL); + }; + + // Create the sensor instance with created function + mpxh6400a = std::make_unique<MPXH6400A>(getVoltage, ADC_VOLTAGE_RANGE); +} + +void Sensors::staticPressure1Creation() +{ + // Create the lambda function to get the voltage + function<ADCData()> getVoltage = [&]() + { + // No need to synchronize, the sampling thread is the same + ADS131M08Data sample = ads131m08->getLastSample(); + return sample.getVoltage(ADC_CH_STATIC_1); + }; + + // Create the sensor instance with created function + hscmrnn015pa_1 = + std::make_unique<HSCMRNN015PA>(getVoltage, ADC_VOLTAGE_RANGE); +} + +void Sensors::staticPressure2Creation() +{ + // Create the lambda function to get the voltage + function<ADCData()> getVoltage = [&]() + { + // No need to synchronize, the sampling thread is the same + ADS131M08Data sample = ads131m08->getLastSample(); + return sample.getVoltage(ADC_CH_STATIC_2); + }; + + // Create the sensor instance with created function + hscmrnn015pa_2 = + std::make_unique<HSCMRNN015PA>(getVoltage, ADC_VOLTAGE_RANGE); +} + +void Sensors::imuCreation() +{ + // Register the IMU as the fake sensor, passing as parameters the methods to + // retrieve real data. The sensor is not synchronized, but the sampling + // thread is always the same. + imu = std::make_unique<RotatedIMU>( + [this]() + { + auto imu6 = getLSM6DSRXLastSample(); + auto mag = getLIS2MDLLastSample(); + + return IMUData{imu6, imu6, mag}; + }); + + // Invert the Y axis on the magnetometer + Eigen::Matrix3f m{{1, 0, 0}, {0, -1, 0}, {0, 0, 1}}; + imu->addMagTransformation(m); + imu->addAccTransformation(imu->rotateAroundX(45)); + imu->addGyroTransformation(imu->rotateAroundX(45)); + imu->addMagTransformation(imu->rotateAroundX(45)); +} + +void Sensors::lps22dfCallback() +{ + LPS22DFData lastSample = lps22df->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::lps28dfw_1Callback() +{ + LPS28DFW_1Data lastSample = + static_cast<LPS28DFW_1Data>(lps28dfw_1->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::lps28dfw_2Callback() +{ + LPS28DFW_2Data lastSample = + static_cast<LPS28DFW_2Data>(lps28dfw_2->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::h3lis331dlCallback() +{ + H3LIS331DLData lastSample = h3lis331dl->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::lis2mdlCallback() +{ + LIS2MDLData lastSample = lis2mdl->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::ubxgpsCallback() +{ + UBXGPSData lastSample = ubxgps->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::lsm6dsrxCallback() +{ + auto& fifo = lsm6dsrx->getLastFifo(); + uint16_t fifoSize = lsm6dsrx->getLastFifoSize(); + + // For every instance inside the fifo log the sample + for (uint16_t i = 0; i < fifoSize; i++) + { + Logger::getInstance().log(fifo.at(i)); + } +} +void Sensors::ads131m08Callback() +{ + ADS131M08Data lastSample = ads131m08->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::deploymentPressureCallback() +{ + MPXH6400AData lastSample = mpxh6400a->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::staticPressure1Callback() +{ + HSCMRNN015PA_1Data lastSample = + static_cast<HSCMRNN015PA_1Data>(hscmrnn015pa_1->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::staticPressure2Callback() +{ + HSCMRNN015PA_2Data lastSample = + static_cast<HSCMRNN015PA_2Data>(hscmrnn015pa_2->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::imuCallback() +{ + IMUData lastSample = imu->getLastSample(); + Logger::getInstance().log(lastSample); +} + +} // namespace HILTest \ No newline at end of file diff --git a/src/tests/hil/Sensors/Sensors.h b/src/tests/hil/Sensors/Sensors.h new file mode 100644 index 000000000..b487b7cd6 --- /dev/null +++ b/src/tests/hil/Sensors/Sensors.h @@ -0,0 +1,232 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <sensors/ADS131M08/ADS131M08.h> +#include <sensors/H3LIS331DL/H3LIS331DL.h> +#include <sensors/LIS2MDL/LIS2MDL.h> +#include <sensors/LPS22DF/LPS22DF.h> +#include <sensors/LPS28DFW/LPS28DFW.h> +#include <sensors/LSM6DSRX/LSM6DSRX.h> +#include <sensors/RotatedIMU/RotatedIMU.h> +#include <sensors/SensorData.h> +#include <sensors/SensorManager.h> +#include <sensors/UBXGPS/UBXGPSSpi.h> +#include <sensors/analog/BatteryVoltageSensorData.h> +#include <sensors/analog/Pitot/PitotData.h> +#include <sensors/analog/pressure/honeywell/HSCMRNN015PA.h> +#include <sensors/analog/pressure/nxp/MPXH6400A.h> +#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> +#include <stdint.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +#include "../Buses.h" +#include "SensorsConfig.h" +#include "SensorsData.h" + +namespace HILTest +{ +class Sensors : public Boardcore::Module +{ +public: + explicit Sensors(Boardcore::TaskScheduler* sched, Buses* buses); + + [[nodiscard]] virtual bool start(); + + /** + * @brief Stops the sensor manager + * @warning Stops the passed scheduler + */ + virtual void stop(); + + /** + * @brief Returns if all the sensors are started successfully + */ + virtual bool isStarted(); + + /** + * @brief Calibrates the sensors with an offset + */ + virtual void calibrate(); + + /** + * @brief Takes the result of the live magnetometer calibration and applies + * it to the current calibration + writes it in the csv file + */ + virtual bool writeMagCalibration(); + + // Sensor getters + virtual Boardcore::LPS22DFData getLPS22DFLastSample(); + virtual Boardcore::LPS28DFWData getLPS28DFW_1LastSample(); + virtual Boardcore::LPS28DFWData getLPS28DFW_2LastSample(); + virtual Boardcore::H3LIS331DLData getH3LIS331DLLastSample(); + virtual Boardcore::LIS2MDLData getLIS2MDLLastSample(); + virtual Boardcore::UBXGPSData getGPSLastSample(); + virtual Boardcore::LSM6DSRXData getLSM6DSRXLastSample(); + virtual Boardcore::ADS131M08Data getADS131M08LastSample(); + + // Processed getters + virtual Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample(); + virtual Boardcore::BatteryVoltageSensorData + getCamBatteryVoltageLastSample(); + virtual Boardcore::CurrentData getCurrentLastSample(); + virtual Boardcore::MPXH6400AData getDeploymentPressureLastSample(); + virtual Boardcore::HSCMRNN015PAData getStaticPressure1LastSample(); + virtual Boardcore::HSCMRNN015PAData getStaticPressure2LastSample(); + virtual Boardcore::IMUData getIMULastSample(); + virtual Boardcore::MagnetometerData getCalibratedMagnetometerLastSample(); + + // CAN fake sensors setters + virtual void setPitot(Boardcore::PitotData data); + virtual void setCCPressure(Boardcore::PressureData data); + virtual void setBottomTankPressure(Boardcore::PressureData data); + virtual void setTopTankPressure(Boardcore::PressureData data); + virtual void setTankTemperature(Boardcore::TemperatureData data); + virtual void setMotorBatteryVoltage( + Boardcore::BatteryVoltageSensorData data); + virtual void setMotorCurrent(Boardcore::CurrentData data); + + // CAN fake sensors getters + virtual Boardcore::PitotData getPitotLastSample(); + virtual Boardcore::PressureData getCCPressureLastSample(); + virtual Boardcore::PressureData getBottomTankPressureLastSample(); + virtual Boardcore::PressureData getTopTankPressureLastSample(); + virtual Boardcore::TemperatureData getTankTemperatureLastSample(); + virtual Boardcore::BatteryVoltageSensorData getMotorBatteryVoltage(); + virtual Boardcore::CurrentData getMotorCurrent(); + + // Returns the sensors statuses + std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> + getSensorInfo(); + +protected: + /** + * @brief Method to put a sensor in the sensorMap with the relative infos + */ + template <typename T> + void registerSensor(Boardcore::Sensor<T>* sensor, const std::string& name, + uint32_t period, std::function<void(void)> callback) + { + // Emplace the sensor inside the map + Boardcore::SensorInfo info(name, period, callback); + sensorMap.emplace(std::make_pair(sensor, info)); + } + + /** + * @brief Insert a sensor in the infoGetter. + */ + template <typename T> + void addInfoGetter(Boardcore::Sensor<T>* sensor) + { + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> Boardcore::SensorInfo + { return manager->getSensorInfo(sensor); }; + } + + // Creation and callbacks methods + void lps22dfCreation(); + virtual void lps22dfCallback(); + + void lps28dfw_1Creation(); + virtual void lps28dfw_1Callback(); + + void lps28dfw_2Creation(); + virtual void lps28dfw_2Callback(); + + void h3lis331dlCreation(); + virtual void h3lis331dlCallback(); + + void lis2mdlCreation(); + virtual void lis2mdlCallback(); + + void ubxgpsCreation(); + virtual void ubxgpsCallback(); + + void lsm6dsrxCreation(); + virtual void lsm6dsrxCallback(); + + void ads131m08Creation(); + virtual void ads131m08Callback(); + + void deploymentPressureCreation(); + virtual void deploymentPressureCallback(); + + void staticPressure1Creation(); + virtual void staticPressure1Callback(); + + void staticPressure2Creation(); + virtual void staticPressure2Callback(); + + void imuCreation(); + virtual void imuCallback(); + + // Can sensors + Boardcore::PitotData canPitot{0, 0, 0}; + Boardcore::PressureData canCCPressure{0, 0}; + Boardcore::PressureData canBottomTankPressure{0, 0}; + Boardcore::PressureData canTopTankPressure{0, 0}; + Boardcore::TemperatureData canTankTemperature{0, 0}; + Boardcore::BatteryVoltageSensorData canMotorBatteryVoltage{}; + Boardcore::CurrentData canMotorCurrent{}; + + // Magnetometer live calibration + Boardcore::SoftAndHardIronCalibration magCalibrator; + Boardcore::SixParametersCorrector magCalibration; + miosix::FastMutex calibrationMutex; + + // Fake processed sensors + std::unique_ptr<Boardcore::RotatedIMU> imu; + + // Sensor manager + std::unique_ptr<Boardcore::SensorManager> manager; + Boardcore::SensorManager::SensorMap_t sensorMap; + Boardcore::TaskScheduler* scheduler = nullptr; + Buses* buses = nullptr; + + // Collection of lambdas to get the sensor init statuses + std::array<std::function<Boardcore::SensorInfo()>, + SensorsConfig::NUMBER_OF_SENSORS> + sensorsInit; + uint8_t sensorsId = 0; + + // SD logger + Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance(); + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors"); + + // Sensors instances + std::unique_ptr<Boardcore::LPS22DF> lps22df; + std::unique_ptr<Boardcore::LPS28DFW> lps28dfw_1; + std::unique_ptr<Boardcore::LPS28DFW> lps28dfw_2; + std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl; + std::unique_ptr<Boardcore::LIS2MDL> lis2mdl; + std::unique_ptr<Boardcore::UBXGPSSpi> ubxgps; + std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx; + std::unique_ptr<Boardcore::ADS131M08> ads131m08; + + // Fake processed sensors + std::unique_ptr<Boardcore::MPXH6400A> mpxh6400a; + std::unique_ptr<Boardcore::HSCMRNN015PA> hscmrnn015pa_1; + std::unique_ptr<Boardcore::HSCMRNN015PA> hscmrnn015pa_2; +}; +} // namespace HILTest \ No newline at end of file diff --git a/src/tests/hil/Sensors/SensorsConfig.h b/src/tests/hil/Sensors/SensorsConfig.h new file mode 100644 index 000000000..09c6cb3e2 --- /dev/null +++ b/src/tests/hil/Sensors/SensorsConfig.h @@ -0,0 +1,121 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <sensors/ADS131M08/ADS131M08.h> +#include <sensors/H3LIS331DL/H3LIS331DL.h> +#include <sensors/LIS2MDL/LIS2MDL.h> +#include <sensors/LPS22DF/LPS22DF.h> +#include <sensors/LPS28DFW/LPS28DFW.h> +#include <sensors/LSM6DSRX/LSM6DSRX.h> +#include <sensors/UBXGPS/UBXGPSSpi.h> + +namespace HILTest +{ +namespace SensorsConfig +{ +constexpr Boardcore::LPS22DF::AVG LPS22DF_AVG = Boardcore::LPS22DF::AVG_4; +constexpr Boardcore::LPS22DF::ODR LPS22DF_ODR = Boardcore::LPS22DF::ODR_100; + +constexpr Boardcore::LPS28DFW::AVG LPS28DFW_AVG = Boardcore::LPS28DFW::AVG_4; +constexpr Boardcore::LPS28DFW::ODR LPS28DFW_ODR = Boardcore::LPS28DFW::ODR_100; +constexpr Boardcore::LPS28DFW::FullScaleRange LPS28DFW_FSR = + Boardcore::LPS28DFW::FS_1260; + +constexpr Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS331DL_ODR = + Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400; +constexpr Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS331DL_BDU = + Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE; +constexpr Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS331DL_FSR = + Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100; + +constexpr Boardcore::LIS2MDL::OperativeMode LIS2MDL_OPERATIVE_MODE = + Boardcore::LIS2MDL::MD_CONTINUOUS; +constexpr Boardcore::LIS2MDL::ODR LIS2MDL_ODR = Boardcore::LIS2MDL::ODR_100_HZ; +constexpr unsigned int LIS2MDL_TEMPERATURE_DIVIDER = 5; + +constexpr uint8_t UBXGPS_SAMPLE_RATE = 10; + +constexpr Boardcore::LSM6DSRXConfig::BDU LSM6DSRX_BDU = + Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE; + +constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE LSM6DSRX_ACC_FS = + Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16; +constexpr Boardcore::LSM6DSRXConfig::ACC_ODR LSM6DSRX_ACC_ODR = + Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_416; +constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE LSM6DSRX_OPERATING_MODE = + Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL; + +constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE LSM6DSRX_GYR_FS = + Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000; +constexpr Boardcore::LSM6DSRXConfig::GYR_ODR LSM6DSRX_GYR_ODR = + Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_416; + +constexpr Boardcore::LSM6DSRXConfig::FIFO_MODE LSM6DSRX_FIFO_MODE = + Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS; +constexpr Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION + LSM6DSRX_FIFO_TIMESTAMP_DECIMATION = + Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1; +constexpr Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR + LSM6DSRX_FIFO_TEMPERATURE_BDR = + Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED; + +constexpr Boardcore::ADS131M08Defs::OversamplingRatio + ADS131M08_OVERSAMPLING_RATIO = + Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192; +constexpr bool ADS131M08_GLOBAL_CHOP_MODE = true; + +constexpr uint32_t LPS22DF_PERIOD = 20; // [ms] 50Hz +constexpr uint32_t LPS28DFW_PERIOD = 20; // [ms] 50Hz +constexpr uint32_t H3LIS331DL_PERIOD = 10; // [ms] 100Hz +constexpr uint32_t LIS2MDL_PERIOD = 10; // [ms] 100Hz +constexpr uint32_t UBXGPS_PERIOD = 1000 / UBXGPS_SAMPLE_RATE; // [ms] +constexpr uint32_t LSM6DSRX_PERIOD = 10; // [ms] 100Hz +constexpr uint32_t ADS131M08_PERIOD = 20; // [ms] 50Hz +constexpr uint32_t IMU_PERIOD = 20; // [ms] Fake processed IMU 50Hz +constexpr uint32_t MAG_CALIBRATION_PERIOD = 20; // [ms] 50Hz + +// ADC sensors configs +constexpr float ADC_VOLTAGE_RANGE = 1.2f; // [V] Voltage reading range +constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_DPL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_3; +constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_1 = + Boardcore::ADS131M08Defs::Channel::CHANNEL_4; +constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_2 = + Boardcore::ADS131M08Defs::Channel::CHANNEL_2; + +// ADC Voltage divider +constexpr float BATTERY_VOLTAGE_CONVERSION_FACTOR = + (20.f / 2.4f) + + 1; // 20 kOhm resistor and 2.4 kOhm resistor for voltage divider +constexpr float CURRENT_CONVERSION_FACTOR = + (20.f / 4.f) / (12.f / (12.f + 33.f)); +constexpr float CURRENT_OFFSET = 0.133333333f; // V in ADC + +// Calibration samples +constexpr unsigned int CALIBRATION_SAMPLES = 20; +constexpr unsigned int CALIBRATION_PERIOD = 100; + +constexpr unsigned int NUMBER_OF_SENSORS = 8; + +} // namespace SensorsConfig +} // namespace HILTest \ No newline at end of file diff --git a/src/tests/hil/Sensors/SensorsData.h b/src/tests/hil/Sensors/SensorsData.h new file mode 100644 index 000000000..8bc5ca1ef --- /dev/null +++ b/src/tests/hil/Sensors/SensorsData.h @@ -0,0 +1,140 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <sensors/LPS28DFW/LPS28DFWData.h> +#include <sensors/analog/pressure/honeywell/HSCMRNN015PAData.h> + +namespace HILTest +{ +struct SensorsCalibrationParameter +{ + uint64_t timestamp; + float referencePressure; + float offsetStatic1; + float offsetStatic2; + float offsetDeployment; + + SensorsCalibrationParameter(uint64_t timestamp, float referencePressure, + float offsetStatic1, float offsetStatic2, + float offsetDeployment) + : timestamp(timestamp), referencePressure(referencePressure), + offsetStatic1(offsetStatic1), offsetStatic2(offsetStatic2), + offsetDeployment(offsetDeployment) + { + } + + SensorsCalibrationParameter() : SensorsCalibrationParameter(0, 0, 0, 0, 0) + { + } + + static std::string header() + { + return "timestamp,referencePressure,offsetStatic1,offsetStatic2," + "offsetDeployment\n"; + } + + void print(std::ostream& os) const + { + os << timestamp << "," << referencePressure << "," << offsetStatic1 + << "," << offsetStatic2 << "," << offsetDeployment << "\n"; + } +}; +struct LPS28DFW_1Data : Boardcore::LPS28DFWData +{ + explicit LPS28DFW_1Data(const Boardcore::LPS28DFWData& data) + : Boardcore::LPS28DFWData(data) + { + } + + LPS28DFW_1Data() {} + + static std::string header() + { + return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n "; + } + + void print(std::ostream& os) const + { + os << pressureTimestamp << "," << pressure << "," + << temperatureTimestamp << "," << temperature << "\n"; + } +}; + +struct LPS28DFW_2Data : Boardcore::LPS28DFWData +{ + explicit LPS28DFW_2Data(const Boardcore::LPS28DFWData& data) + : Boardcore::LPS28DFWData(data) + { + } + + LPS28DFW_2Data() {} + + static std::string header() + { + return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n "; + } + + void print(std::ostream& os) const + { + os << pressureTimestamp << "," << pressure << "," + << temperatureTimestamp << "," << temperature << "\n"; + } +}; + +struct HSCMRNN015PA_1Data : Boardcore::HSCMRNN015PAData +{ + explicit HSCMRNN015PA_1Data(const Boardcore::HSCMRNN015PAData& data) + : Boardcore::HSCMRNN015PAData(data) + { + } + + HSCMRNN015PA_1Data() {} + + static std::string header() { return "pressureTimestamp,pressure\n "; } + + void print(std::ostream& os) const + { + os << pressureTimestamp << "," << pressure << "," + << "\n"; + } +}; + +struct HSCMRNN015PA_2Data : Boardcore::HSCMRNN015PAData +{ + explicit HSCMRNN015PA_2Data(const Boardcore::HSCMRNN015PAData& data) + : Boardcore::HSCMRNN015PAData(data) + { + } + + HSCMRNN015PA_2Data() {} + + static std::string header() { return "pressureTimestamp,pressure\n "; } + + void print(std::ostream& os) const + { + os << pressureTimestamp << "," << pressure << "," + << "\n"; + } +}; + +} // namespace HILTest \ No newline at end of file diff --git a/src/tests/hil/common/Events.h b/src/tests/hil/common/Events.h new file mode 100644 index 000000000..69ef7d659 --- /dev/null +++ b/src/tests/hil/common/Events.h @@ -0,0 +1,295 @@ +/* Copyright (c) 2018-2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * 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 <events/Event.h> + +#include <iostream> +#include <map> +#include <string> +#include <vector> + +#include "Topics.h" + +namespace Common +{ + +enum Events : uint8_t +{ + ABK_DISABLE = Boardcore::EV_FIRST_CUSTOM, + ABK_OPEN, + ABK_RESET, + ABK_SHADOW_MODE_TIMEOUT, + ABK_WIGGLE, + ADA_CALIBRATE, + ADA_PRESS_STAB_TIMEOUT, + ADA_READY, + ADA_FORCE_START, + ADA_FORCE_STOP, + ADA_SHADOW_MODE_TIMEOUT, + ADA_APOGEE_DETECTED, + MEA_SHUTDOWN_DETECTED, + DPL_CUT_DROGUE, + DPL_CUT_TIMEOUT, + DPL_NC_OPEN, + DPL_NC_RESET, + DPL_SERVO_ACTUATION_DETECTED, + DPL_WIGGLE, + DPL_WES_CAL_DONE, + CAN_FORCE_INIT, + CAN_ARM, + CAN_ENTER_TEST_MODE, + CAN_EXIT_TEST_MODE, + CAN_CALIBRATE, + CAN_DISARM, + CAN_LIFTOFF, + CAN_APOGEE_DETECTED, + CAN_IGNITION, + FLIGHT_APOGEE_DETECTED, + FLIGHT_ARMED, + FLIGHT_DROGUE_DESCENT, + FLIGHT_DISARMED, + FLIGHT_DPL_ALT_DETECTED, + FLIGHT_ERROR_DETECTED, + FLIGHT_LANDING_DETECTED, + FLIGHT_LANDING_TIMEOUT, + FLIGHT_LAUNCH_PIN_DETACHED, + FLIGHT_LIFTOFF, + FLIGHT_MOTOR_SHUTDOWN, + FLIGHT_MISSION_TIMEOUT, + FLIGHT_NC_DETACHED, + FLIGHT_WING_DESCENT, + FMM_ASCENDING, + FMM_ALGOS_CAL_DONE, + FMM_STOP_LOGGING, + FMM_INIT_OK, + FMM_INIT_ERROR, + FMM_CALIBRATE, + FMM_ALGOS_CALIBRATE, + FMM_SENSORS_CAL_DONE, + FMM_READY, + FSR_STATS_TIMEOUT, + NAS_CALIBRATE, + NAS_READY, + NAS_FORCE_START, + NAS_FORCE_STOP, + TMTC_ARM, + TMTC_DISARM, + TMTC_CALIBRATE, + TMTC_FORCE_INIT, + TMTC_FORCE_LAUNCH, + TMTC_FORCE_LANDING, + TMTC_FORCE_APOGEE, + TMTC_FORCE_EXPULSION, + TMTC_FORCE_DEPLOYMENT, + TMTC_START_LOGGING, + TMTC_STOP_LOGGING, + TMTC_RESET_BOARD, + TMTC_ENTER_TEST_MODE, + TMTC_EXIT_TEST_MODE, + TMTC_START_RECORDING, + TMTC_STOP_RECORDING, + MOTOR_START_TARS, + MOTOR_STOP_TARS, + MOTOR_OPEN_VENTING_VALVE, + MOTOR_CLOSE_VENTING_VALVE, + MOTOR_OPEN_FILLING_VALVE, + MOTOR_CLOSE_FILLING_VALVE, + MOTOR_OPEN_RELEASE_VALVE, + MOTOR_CLOSE_RELEASE_VALVE, + MOTOR_DISCONNECT, + MOTOR_IGNITION, + MOTOR_OPEN_FEED_VALVE, + MOTOR_CLOSE_FEED_VALVE, + MOTOR_MANUAL_ACTION, + MOTOR_OPEN_OXIDANT, + MOTOR_SHADOW_MODE_TIMEOUT, + TARS_WASHING_DONE, + TARS_CHECK_PRESSURE_STABILIZE, + TARS_PRESSURE_STABILIZED, + TARS_FILLING_DONE, + TARS_REFINING_DONE, + ALTITUDE_TRIGGER_ALTITUDE_REACHED, + LAST_EVENT +}; + +inline std::string getEventString(uint8_t event) +{ + static const std::map<uint8_t, std::string> event_string_map{ + {ABK_DISABLE, "ABK_DISABLE"}, + {ABK_OPEN, "ABK_OPEN"}, + {ABK_RESET, "ABK_RESET"}, + {ABK_SHADOW_MODE_TIMEOUT, "ABK_SHADOW_MODE_TIMEOUT"}, + {ABK_WIGGLE, "ABK_WIGGLE"}, + {ADA_CALIBRATE, "ADA_CALIBRATE"}, + {ADA_PRESS_STAB_TIMEOUT, "ADA_PRESS_STAB_TIMEOUT"}, + {ADA_READY, "ADA_READY"}, + {ADA_FORCE_START, "ADA_FORCE_START"}, + {ADA_FORCE_STOP, "ADA_FORCE_STOP"}, + {ADA_SHADOW_MODE_TIMEOUT, "ADA_SHADOW_MODE_TIMEOUT"}, + {ADA_APOGEE_DETECTED, "ADA_APOGEE_DETECTED"}, + {MEA_SHUTDOWN_DETECTED, "MEA_SHUTDOWN_DETECTED"}, + {DPL_CUT_DROGUE, "DPL_CUT_DROGUE"}, + {DPL_CUT_TIMEOUT, "DPL_CUT_TIMEOUT"}, + {DPL_NC_OPEN, "DPL_NC_OPEN"}, + {DPL_NC_RESET, "DPL_NC_RESET"}, + {DPL_SERVO_ACTUATION_DETECTED, "DPL_SERVO_ACTUATION_DETECTED"}, + {DPL_WIGGLE, "DPL_WIGGLE"}, + {DPL_WES_CAL_DONE, "DPL_WES_CAL_DONE"}, + {CAN_FORCE_INIT, "CAN_FORCE_INIT"}, + {CAN_ARM, "CAN_ARM"}, + {CAN_ENTER_TEST_MODE, "CAN_ENTER_TEST_MODE"}, + {CAN_EXIT_TEST_MODE, "CAN_EXIT_TEST_MODE"}, + {CAN_CALIBRATE, "CAN_CALIBRATE"}, + {CAN_DISARM, "CAN_DISARM"}, + {CAN_LIFTOFF, "CAN_LIFTOFF"}, + {CAN_APOGEE_DETECTED, "CAN_APOGEE_DETECTED"}, + {CAN_IGNITION, "CAN_IGNITION"}, + {FLIGHT_APOGEE_DETECTED, "FLIGHT_APOGEE_DETECTED"}, + {FLIGHT_ARMED, "FLIGHT_ARMED"}, + {FLIGHT_DROGUE_DESCENT, "FLIGHT_DROGUE_DESCENT"}, + {FLIGHT_DISARMED, "FLIGHT_DISARMED"}, + {FLIGHT_DPL_ALT_DETECTED, "FLIGHT_DPL_ALT_DETECTED"}, + {FLIGHT_ERROR_DETECTED, "FLIGHT_ERROR_DETECTED"}, + {FLIGHT_LANDING_DETECTED, "FLIGHT_LANDING_DETECTED"}, + {FLIGHT_LAUNCH_PIN_DETACHED, "FLIGHT_LAUNCH_PIN_DETACHED"}, + {FLIGHT_LIFTOFF, "FLIGHT_LIFTOFF"}, + {FLIGHT_MOTOR_SHUTDOWN, "FLIGHT_MOTOR_SHUTDOWN"}, + {FLIGHT_LANDING_TIMEOUT, "FLIGHT_LANDING_TIMEOUT"}, + {FLIGHT_NC_DETACHED, "FLIGHT_NC_DETACHED"}, + {FLIGHT_MISSION_TIMEOUT, "FLIGHT_MISSION_TIMEOUT"}, + {FLIGHT_WING_DESCENT, "FLIGHT_WING_DESCENT"}, + {FMM_ASCENDING, "FMM_ASCENDING"}, + {FMM_ALGOS_CAL_DONE, "FMM_ALGOS_CAL_DONE"}, + {FMM_STOP_LOGGING, "FMM_STOP_LOGGING"}, + {FMM_INIT_OK, "FMM_INIT_OK"}, + {FMM_INIT_ERROR, "FMM_INIT_ERROR"}, + {FMM_CALIBRATE, "FMM_CALIBRATE"}, + {FMM_ALGOS_CALIBRATE, "FMM_ALGOS_CALIBRATE"}, + {FMM_SENSORS_CAL_DONE, "FMM_SENSORS_CAL_DONE"}, + {FMM_READY, "FMM_READY"}, + {FSR_STATS_TIMEOUT, "FSR_STATS_TIMEOUT"}, + {NAS_CALIBRATE, "NAS_CALIBRATE"}, + {NAS_FORCE_START, "NAS_FORCE_START"}, + {NAS_FORCE_STOP, "NAS_FORCE_STOP"}, + {NAS_READY, "NAS_READY"}, + {TMTC_ARM, "TMTC_ARM"}, + {TMTC_DISARM, "TMTC_DISARM"}, + {TMTC_CALIBRATE, "TMTC_CALIBRATE"}, + {TMTC_FORCE_INIT, "TMTC_FORCE_INIT"}, + {TMTC_FORCE_LAUNCH, "TMTC_FORCE_LAUNCH"}, + {TMTC_FORCE_LANDING, "TMTC_FORCE_LANDING"}, + {TMTC_FORCE_APOGEE, "TMTC_FORCE_APOGEE"}, + {TMTC_FORCE_EXPULSION, "TMTC_FORCE_EXPULSION"}, + {TMTC_FORCE_DEPLOYMENT, "TMTC_FORCE_DEPLOYMENT"}, + {TMTC_START_LOGGING, "TMTC_START_LOGGING"}, + {TMTC_STOP_LOGGING, "TMTC_STOP_LOGGING"}, + {TMTC_RESET_BOARD, "TMTC_RESET_BOARD"}, + {TMTC_ENTER_TEST_MODE, "TMTC_ENTER_TEST_MODE"}, + {TMTC_EXIT_TEST_MODE, "TMTC_EXIT_TEST_MODE"}, + {TMTC_START_RECORDING, "TMTC_START_RECORDING"}, + {TMTC_STOP_RECORDING, "TMTC_STOP_RECORDING"}, + {MOTOR_START_TARS, "MOTOR_START_TARS"}, + {MOTOR_STOP_TARS, "MOTOR_STOP_TARS"}, + {MOTOR_OPEN_VENTING_VALVE, "MOTOR_OPEN_VENTING_VALVE"}, + {MOTOR_CLOSE_VENTING_VALVE, "MOTOR_CLOSE_VENTING_VALVE"}, + {MOTOR_OPEN_FILLING_VALVE, "MOTOR_OPEN_FILLING_VALVE"}, + {MOTOR_CLOSE_FILLING_VALVE, "MOTOR_CLOSE_FILLING_VALVE"}, + {MOTOR_OPEN_RELEASE_VALVE, "MOTOR_OPEN_RELEASE_VALVE"}, + {MOTOR_CLOSE_RELEASE_VALVE, "MOTOR_CLOSE_RELEASE_VALVE"}, + {MOTOR_DISCONNECT, "MOTOR_DISCONNECT"}, + {MOTOR_IGNITION, "MOTOR_IGNITION"}, + {MOTOR_OPEN_FEED_VALVE, "MOTOR_OPEN_FEED_VALVE"}, + {MOTOR_CLOSE_FEED_VALVE, "MOTOR_CLOSE_FEED_VALVE"}, + {MOTOR_MANUAL_ACTION, "MOTOR_MANUAL_ACTION"}, + {MOTOR_OPEN_OXIDANT, "MOTOR_OPEN_OXIDANT"}, + {MOTOR_SHADOW_MODE_TIMEOUT, "MOTOR_SHADOW_MODE_TIMEOUT"}, + {TARS_WASHING_DONE, "TARS_WASHING_DONE"}, + {TARS_CHECK_PRESSURE_STABILIZE, "TARS_CHECK_PRESSURE_STABILIZE"}, + {TARS_PRESSURE_STABILIZED, "TARS_PRESSURE_STABILIZED"}, + {TARS_FILLING_DONE, "TARS_FILLING_DONE"}, + {TARS_REFINING_DONE, "TARS_REFINING_DONE"}, + {ALTITUDE_TRIGGER_ALTITUDE_REACHED, + "ALTITUDE_TRIGGER_ALTITUDE_REACHED"}, + {LAST_EVENT, "LAST_EVENT"}, + }; + + auto it = event_string_map.find(event); + return it == event_string_map.end() ? "EV_UNKNOWN" : it->second; +} + +struct LiftoffEvent +{ + uint64_t timestamp; + + static std::string header() { return "timestamp\n"; } + + void print(std::ostream& os) const { os << timestamp << "\n"; } +}; + +struct ApogeeEvent +{ + uint64_t timestamp; + + static std::string header() { return "timestamp\n"; } + + void print(std::ostream& os) const { os << timestamp << "\n"; } +}; + +struct NoseconeEvent +{ + uint64_t timestamp; + + static std::string header() { return "timestamp\n"; } + + void print(std::ostream& os) const { os << timestamp << "\n"; } +}; + +struct ExpulsionEvent +{ + uint64_t timestamp; + + static std::string header() { return "timestamp\n"; } + + void print(std::ostream& os) const { os << timestamp << "\n"; } +}; + +struct MainEvent +{ + uint64_t timestamp; + + static std::string header() { return "timestamp\n"; } + + void print(std::ostream& os) const { os << timestamp << "\n"; } +}; + +struct LandingEvent +{ + uint64_t timestamp; + + static std::string header() { return "timestamp\n"; } + + void print(std::ostream& os) const { os << timestamp << "\n"; } +}; + +} // namespace Common diff --git a/src/tests/hil/common/ReferenceConfig.h b/src/tests/hil/common/ReferenceConfig.h new file mode 100644 index 000000000..2118c7709 --- /dev/null +++ b/src/tests/hil/common/ReferenceConfig.h @@ -0,0 +1,79 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * 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 <algorithms/ReferenceValues.h> + +namespace Common +{ + +namespace ReferenceConfig +{ + +#if defined(EUROC) + +static const Boardcore::ReferenceValues defaultReferenceValues = { + 160.0, // [m] Altitude + 99418.0, // [Pa] Pressure + 288.15, // [K] Temperature + 39.389733, // [deg] Start latitude + -8.288992, // [deg] Start longitude + Boardcore::Constants::MSL_PRESSURE, + Boardcore::Constants::MSL_TEMPERATURE, +}; + +const Eigen::Vector3f nedMag(0.5939, -0.0126, 0.8044); + +#elif defined(ROCCARASO) + +static const Boardcore::ReferenceValues defaultReferenceValues = { + 1414.0, // [m] Altitude + 85452.0, // [Pa] Pressure + 278.95, // [K] Temperature + 41.8089005, // [deg] Start latitude + 14.0546716, // [deg] Start longitude + Boardcore::Constants::MSL_PRESSURE, + Boardcore::Constants::MSL_TEMPERATURE, +}; + +const Eigen::Vector3f nedMag(0.5244, 0.0368, 0.8507); + +#else // Milan + +static const Boardcore::ReferenceValues defaultReferenceValues = { + 135.0, // [m] Altitude + 99714.0, // [Pa] Pressure + 278.27, // [K] Temperature + 45.50106793771145, // [deg] Start latitude + 9.156376900740167, // [deg] Start longitude + Boardcore::Constants::MSL_PRESSURE, + Boardcore::Constants::MSL_TEMPERATURE, +}; + +const Eigen::Vector3f nedMag(0.4732, 0.0272, 0.8805); + +#endif + +} // namespace ReferenceConfig + +} // namespace Common diff --git a/src/tests/hil/common/Topics.h b/src/tests/hil/common/Topics.h new file mode 100644 index 000000000..eaa0429e8 --- /dev/null +++ b/src/tests/hil/common/Topics.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2018-2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * 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 <cstdint> +#include <string> +#include <vector> + +namespace Common +{ + +enum Topics : uint8_t +{ + TOPIC_ABK, + TOPIC_ADA, + TOPIC_MEA, + TOPIC_DPL, + TOPIC_CAN, + TOPIC_FLIGHT, + TOPIC_FMM, + TOPIC_FSR, + TOPIC_NAS, + TOPIC_TMTC, + TOPIC_MOTOR, + TOPIC_TARS, + TOPIC_ALT, +}; + +const std::vector<uint8_t> TOPICS_LIST{ + TOPIC_ABK, TOPIC_ADA, TOPIC_MEA, TOPIC_DPL, TOPIC_CAN, + TOPIC_FLIGHT, TOPIC_FMM, TOPIC_FSR, TOPIC_NAS, TOPIC_TMTC, + TOPIC_MOTOR, TOPIC_TARS, TOPIC_ALT, +}; + +} // namespace Common diff --git a/src/tests/hil/test-hil.cpp b/src/tests/hil/test-hil.cpp new file mode 100644 index 000000000..2b5790f6c --- /dev/null +++ b/src/tests/hil/test-hil.cpp @@ -0,0 +1,124 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * 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 <algorithms/AirBrakes/TrajectoryPoint.h> +#include <diagnostic/CpuMeter/CpuMeter.h> +#include <diagnostic/PrintLogger.h> +#include <diagnostic/StackLogger.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> +#include <events/EventData.h> +#include <events/utils/EventSniffer.h> +#include <hil/HIL.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +#include "Buses.h" +#include "HILSimulationConfig.h" +#include "Sensors/HILSensors.h" +#include "Sensors/Sensors.h" +#include "Sensors/SensorsConfig.h" + +using namespace Boardcore; +using namespace HILConfig; +using namespace HILTest; + +static const bool HIL_TEST = true; + +int main() +{ + // Overall status, if at some point it becomes false, there is a problem + // somewhere + bool initResult = true; + PrintLogger logger = Logging::getLogger("main"); + Boardcore::TaskScheduler scheduler; + + // Create modules + Buses* buses = new Buses(); + Sensors* sensors; + + if (HIL_TEST) + { + // Create hil modules + auto* hilPhasesManager = new MainHILPhasesManager( + []() { return Boardcore::TimedTrajectoryPoint(); }); + auto* hilTransceiver = + new MainHILTransceiver(buses->usart2, hilPhasesManager); + + // Create HIL class where we specify how to use previous modules to + // assemble ActuatorData + MainHIL* hil = new MainHIL( + hilTransceiver, hilPhasesManager, + [&]() + { + auto actuatorData = ActuatorData(); + return actuatorData; + }, + SIMULATION_PERIOD); + + if (HIL_TEST) + { + initResult &= ModuleManager::getInstance().insert<MainHIL>(hil); + } + sensors = new HILSensors(&scheduler, buses, hil, false); + } + else + { + sensors = new Sensors(&scheduler, buses); + } + + // Insert modules + initResult &= ModuleManager::getInstance().insert<Buses>(buses); + initResult &= ModuleManager::getInstance().insert<Sensors>(sensors); + + // Start modules + initResult &= EventBroker::getInstance().start(); + initResult &= scheduler.start(); + + if (HIL_TEST) + { + initResult &= ModuleManager::getInstance().get<MainHIL>()->start(); + + ModuleManager::getInstance().get<MainHIL>()->waitStartSimulation(); + } + + initResult &= ModuleManager::getInstance().get<Sensors>()->start(); + + // Check the init result and launch an event + printf(initResult ? "Init OK\n" : "Init ERROR\n"); + + // Periodic statistics + while (true) + { + Thread::sleep(1000); + printf("max: %.3f, min: %.3f, free stack: %ld, free heap: %ld\n", + CpuMeter::getCpuStats().maxValue, + CpuMeter::getCpuStats().minValue, + CpuMeter::getCpuStats().freeStack, + CpuMeter::getCpuStats().freeHeap); + CpuMeter::resetCpuStats(); + StackLogger::getInstance().log(); + } + + return 0; +} \ No newline at end of file -- GitLab