diff --git a/CMakeLists.txt b/CMakeLists.txt index 72dfec3d779256845c691e855da0396714f59ce2..83d05228c5f8d06d1276fc95447bd2514fd84375 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 0000000000000000000000000000000000000000..bb11d0174836f3be5d461392075e64df25297d4a --- /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 0000000000000000000000000000000000000000..db00cb7d30936431f21a1f09e848579bc785e65f --- /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 0000000000000000000000000000000000000000..dbd402ef945822c7be6aa5e4879ea6bab5453c6e --- /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 70e6426fbbdcab9915279a870c77cce2cf9296b5..678b4d59cb368d34a2fa8db25e6c128f0a7be112 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 dfdb2bab87f1431518c45a121c4540fde5aac831..828cdc9bb08024c3a933e0714ac6f2e505afa24c 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 d8142f18c33e9e28e3321f82f4158fcf02985bc4..2d4e3adb51ec28977db7804501cf7a20f3b044e5 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 a83fdaf0978013227e8990c83a7540ce7268cf9d..41f049a580a53456639bfaf45d609a6bd3e70534 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 1e7e9f1b764dee8da541f2e0294d4ee626eeb551..99cdb4017719ec3396a10de5c35addc3c01218c2 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 fcc512bdb16ae7210753fdb2f454273f09f4df6f..3c6f76cad66f430a1f49573b4eb241d286c0c881 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 8ba79d6f49f2ba7c2faac1fd91d95ea7dbf3d415..3b7abf775d1d0cd9aa930891d61729cf15f8f2cd 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 1c682f6a290f7fb821ab965a36ed36724db77c9f..bf1f5e0e1f905ae2e3bdb633968092f23c01c7cf 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 e805592cd35b300b4866c562c462093a13a7a374..1be3ae19390c8b7cf8e87bc4dba0126dba0389ce 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 0000000000000000000000000000000000000000..d69bf914910e8ad9fac266b0c8408d20e2c98483 --- /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 0000000000000000000000000000000000000000..dacd6898f5102b06d2b5bb7f470901733a31fcf5 --- /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 4674ea16e84c255470bb80ec54aebb997d624665..460b8ccacea99247940b44b75a4dd487f4b3da68 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 d7a93eb2bfc9dc4a56cdc69cebb1b5779058b179..7e781f9557c2cea4ad0d446bf33d32ea50e7c8b1 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 252e7bf375a40563f9cab47181c704e296ddf821..8df1374b54cfa1e3c4ec91ed950e9dde26f0b5aa 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 ed98e2768a85097cdde7ea0e13b74fbc83d56289..2e0a5d90514b563930a3ef55532ceb86111fbf5a 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 876c5fc499d2ea303754df8e0f4abe28a7e4fc6b..63a761dd67494cb64acfc27a97d9ed0359043e06 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 33012760c5b6eeac46f921c3bee81ab5af94ccd0..2f2cf5010433758e198c986f9ba50e549cd1dec0 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 2af56dceecb6a3dfe6bc98ea0e4b706b1c836a15..3a0bc3c1c4b7bb4c60e9a827f95f03f264d887a8 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 fcfcd412d6234e325489f2f61fbfab244ce01dec..9391e609912fb712c7c850bd0304c43a559986a3 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 7e329e28222d1b0c46a049bba3580f2604bb4843..1b4a408ba10c2c50861bbdea8e630c7770b88036 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 0ee3e01bdf07bf807ee76a6287c80d7cb6d53733..743a46fc8b33a53ca760cb10641462d9eef6e887 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 5d262adc7f8fe83879d552b6db57af055b93a9d3..c0caf5f8d5ad3c1e13f1f0f0f0b558a4fd06415b 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 7147d7ffeeeedb61484c1ea8e9feb133256da5c1..5fa1466bc11a0672484f98d22eeebd80a7de1a14 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 96db302d636c1e11143b1faf5e6758df4e23b087..3c983fbf1653e4b9f521d732f515f562013745d1 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 f26781e0ab931c62bd2926f5535296c0d8f65c98..bfbae274092d983a504ddc459b9f49b8a78fc5c9 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 733d2ed6ca2893abf4eb22e9ec2db96ed75885f5..9187466a692e354a3454855c7d8f43073835a845 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 e36b6d5b431ab603acae63f597e5e46a95c7ac6f..eb14fb4e274abb9b6f3a729395407204d029f887 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 cd0959f743c0efef266f30d92165c7de5fc2786f..36ce97ac473533c8a33801546b5f40d841cd36b1 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 e7baf675b592dcdae54029c084f2bd7c1a8536d0..1e82396f0933adf973f100a4f6fb577f6b164b92 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 7669f3f4f686ea4e024533f6d90d35384945f20a..1f207637d243f1b9016f0df94be200f6be0542ef 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 e35a3f86eb841652bd726958358b1f7f28fbbcf3..2b1f3a25a241d27273547e5dde4c7e5ac6ed234c 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 f2c17aa74bfc4fdd11519a322f7fec56494a6524..7b34286855ae7902eb18c8e0bb024d5d6f04e506 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 d18cb5fa08952a8b1999428fde5eb4404b7c066e..7cbbe1601aec2166037fa824f0039eae54728420 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 b144f93344813b9a9a60f18b6d7cdd2fefc30ba1..4659631f014e5f0fdc3c2af4b02009892c0c0227 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 beaac6843465e6112b8f9348d11b1dde73636659..3cd4ab0580676a54c9f0cc70efdd7ce64a21fe0d 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 d72485d906ec4fbbd8d6d9a882b0ff5355b53e57..e55cd7c81fe1b684ea2ae9b20e4f57e033f3bc88 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 ab136e27eddc8b19a92af746ec273d8340711aec..fdb1d02fb7b938f8ee3ab2e68c1a21c93a52d0b0 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 f2beffc5d4466bfed53d9ec5f2a719ac325a6947..26e0ab5b1efc9bcb8fec9e411f8a5c8af3445767 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 0000000000000000000000000000000000000000..b8acb5a4f1e0fdb01bf46e1552cdbe0b7489306f --- /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 0000000000000000000000000000000000000000..5664a8095336cdcfdcf03e83d0f9fde7e4b16838 --- /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 0000000000000000000000000000000000000000..33dffab577e4cee189b06da57ffb5b7abed0c50c --- /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 0000000000000000000000000000000000000000..2e9e218dbec5e785be9e95303c63f5bdd46f5e5f --- /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 0000000000000000000000000000000000000000..7d642072a419e0428dd72c861ca87a3e461437e1 --- /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 0000000000000000000000000000000000000000..1877bd5c7a98925a66592f57bc34d9005beeffaa --- /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 0000000000000000000000000000000000000000..b487b7cd6ec3b49b648fc7fad2b2500fbbf53181 --- /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 0000000000000000000000000000000000000000..09c6cb3e2d007b7be48759c50f42b2ba87ebe7dd --- /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 0000000000000000000000000000000000000000..8bc5ca1efeebc1f1ae014c502f953a3db6370c50 --- /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 0000000000000000000000000000000000000000..69ef7d659a5120cc5e60ae6da39dbb833e43bcb4 --- /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 0000000000000000000000000000000000000000..2118c7709b7598d531a5499f1b68813ccaf14920 --- /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 0000000000000000000000000000000000000000..eaa0429e89f472c4efe71081ac28124aea898faf --- /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 0000000000000000000000000000000000000000..2b5790f6ce38e6835fdafe7b3fb983ec354e8396 --- /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