diff --git a/CMakeLists.txt b/CMakeLists.txt
index 53311e9d981bc77ffbd23d942d41ec22f727eca0..e8d9af4bd870c8f526e8d14396fb65c98c6b655f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -118,7 +118,6 @@ 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)
-target_compile_definitions(test-hil PRIVATE HILTest)
sbs_target(test-hil stm32f767zi_compute_unit)
#-----------------------------------------------------------------------------#
diff --git a/src/shared/hil/Events.h b/src/shared/hil/Events.h
deleted file mode 100644
index 9571ef54aeb0ff4317bef283baf6d403d53bf1ef..0000000000000000000000000000000000000000
--- a/src/shared/hil/Events.h
+++ /dev/null
@@ -1,293 +0,0 @@
-/* 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>
-
-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/shared/hil/HIL.h b/src/shared/hil/HIL.h
index d5e74a07ace7425a7e54aaa72a8609c07fe9651d..e9ba8b49bcbb7199e649afcf82e8cc2dc9f40b8a 100644
--- a/src/shared/hil/HIL.h
+++ b/src/shared/hil/HIL.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2021-2023 Skyward Experimental Rocketry
+/* 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
@@ -23,46 +23,106 @@
#pragma once
#include <Singleton.h>
+#include <diagnostic/SkywardStack.h>
#include <utils/ModuleManager/ModuleManager.hpp>
-#include "HILConfig.h"
-#include "HILFlightPhasesManager.h"
+#include "HILPhasesManager.h"
#include "HILTransceiver.h"
/**
* @brief Single interface to the hardware-in-the-loop framework.
*/
-class HIL : public Boardcore::Module
+template <class FlightPhases, class SimulatorData, class ActuatorData>
+class HIL : public Boardcore::Module, public Boardcore::ActiveObject
{
public:
- HIL(Boardcore::USART &hilSerial,
- HILFlightPhasesManager *flightPhasesManager)
- : flightPhasesManager(flightPhasesManager)
+ HIL(HILTransceiver<FlightPhases, SimulatorData, ActuatorData>
+ *hilTransceiver,
+ HILPhasesManager<FlightPhases, SimulatorData, ActuatorData>
+ *hilPhasesManager,
+ std::function<ActuatorData()> updateActuatorData, int updatePeriod)
+ : Boardcore::ActiveObject(Boardcore::STACK_MIN_FOR_SKYWARD,
+ miosix::PRIORITY_MAX - 1),
+ hilTransceiver(hilTransceiver), hilPhasesManager(hilPhasesManager),
+ updateActuatorData(updateActuatorData), updatePeriod(updatePeriod)
{
- simulator = new HILTransceiver(hilSerial);
- }
+ if (!Boardcore::ModuleManager::getInstance().insert<HILTransceiverBase>(
+ hilTransceiver))
+ {
+ LOG_ERR(logger, "HILTransceiver not inserted correctly");
+ }
- HILTransceiver *simulator;
- HILFlightPhasesManager *flightPhasesManager;
+ if (!Boardcore::ModuleManager::getInstance()
+ .insert<HILPhasesManagerBase>(hilPhasesManager))
+ {
+ LOG_ERR(logger, "HILPhasesManager not inserted correctly");
+ }
+ }
/**
* @brief Start the needed hardware-in-the-loop components.
*/
- [[nodiscard]] bool start()
+ [[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()
{
- return simulator->start() && flightPhasesManager->start();
+ hilTransceiver->stop();
+ hilPhasesManager->stop();
+ Boardcore::ActiveObject::stop();
+ LOG_INFO(logger, "HIL framework stopped");
}
- void stop() { simulator->stop(); }
+ void waitStartSimulation()
+ {
+ LOG_INFO(logger, "Waiting for simulation to start...");
+ while (!hilPhasesManager->isSimulationRunning())
+ {
+ Thread::sleep(updatePeriod);
+ }
+ }
- void send(HILConfig::ActuatorData actuatorData)
+ HILTransceiver<FlightPhases, SimulatorData, ActuatorData> *hilTransceiver;
+ HILPhasesManager<FlightPhases, SimulatorData, ActuatorData>
+ *hilPhasesManager;
+
+private:
+ void run() override
{
- simulator->setActuatorData(actuatorData);
+ while (!shouldStop())
+ {
+ if (hilPhasesManager->isSimulationRunning())
+ {
+ hilTransceiver->setActuatorData(updateActuatorData());
+ }
+ Thread::sleep(updatePeriod);
+ }
}
- /**
- * @brief Returns if all the schedulers are up and running
- */
- bool isStarted();
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("HIL");
+ std::function<ActuatorData()> updateActuatorData;
+ int updatePeriod;
};
diff --git a/src/shared/hil/HILConfig.h b/src/shared/hil/HILConfig.h
deleted file mode 100644
index bf731068f577dd8a9a6a6e64b7999ac1af54aa64..0000000000000000000000000000000000000000
--- a/src/shared/hil/HILConfig.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/* Copyright (c) 2020-2023 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
-
-/**
- * @brief Configuration file that includes only the right structures described
- * in the config file of the test.
- *
- * Usage:
- * #elif <Flag>
- * #include "<test-directory>/HILSimulationConfig.h"
- *
- * REMEMBER:
- * when defining the entry in "CMakeLists" you should add
- * target_compile_definitions(<test-directory> PRIVATE <Flag>)
- *
- * WARNING:
- * You should always CLEAN your board before flashing a new entrypoint. Some
- * flags could still be in memory
- */
-
-/* Hardware in the loop entrypoint */
-#if defined(HILTest)
-#include "../tests/hil/HILSimulationConfig.h"
-/*
-#elif defined(HIL_<tuoFlag>)
-#include "<test-directory>/HILSimulationConfig.h"
-*/
-#else
-#error You have add the flag of your configuration file for the HIL testing!
-#endif
diff --git a/src/shared/hil/HILFlightPhasesManager.cpp b/src/shared/hil/HILFlightPhasesManager.cpp
deleted file mode 100644
index fa833297ca2fb916512142d35f53ab303cbfd936..0000000000000000000000000000000000000000
--- a/src/shared/hil/HILFlightPhasesManager.cpp
+++ /dev/null
@@ -1,296 +0,0 @@
-/* Copyright (c) 2021-2023 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.
- */
-
-#include "HILFlightPhasesManager.h"
-
-#include <events/Event.h>
-
-#include "events/EventBroker.h"
-
-using namespace Common;
-using namespace HILConfig;
-
-HILFlightPhasesManager::HILFlightPhasesManager()
- : Boardcore::EventHandler(),
- flagsFlightPhases({{FlightPhases::SIM_FLYING, false},
- {FlightPhases::SIM_ASCENT, false},
- {FlightPhases::SIM_BURNING, false},
- {FlightPhases::SIM_AEROBRAKES, false},
- {FlightPhases::SIM_PARA1, false},
- {FlightPhases::SIM_PARA2, false},
- {FlightPhases::SIMULATION_STARTED, false},
- {FlightPhases::CALIBRATION, false},
- {FlightPhases::CALIBRATION_OK, false},
- {FlightPhases::ARMED, false},
- {FlightPhases::LIFTOFF_PIN_DETACHED, false},
- {FlightPhases::LIFTOFF, false},
- {FlightPhases::AEROBRAKES, false},
- {FlightPhases::APOGEE, false},
- {FlightPhases::PARA1, false},
- {FlightPhases::PARA2, false},
- {FlightPhases::SIMULATION_STOPPED, false}})
-{
- prev_flagsFlightPhases = flagsFlightPhases;
-
- auto& eventBroker = Boardcore::EventBroker::getInstance();
- eventBroker.subscribe(this, TOPIC_ABK);
- eventBroker.subscribe(this, TOPIC_ADA);
- eventBroker.subscribe(this, TOPIC_MEA);
- eventBroker.subscribe(this, TOPIC_DPL);
- eventBroker.subscribe(this, TOPIC_CAN);
- eventBroker.subscribe(this, TOPIC_FLIGHT);
- eventBroker.subscribe(this, TOPIC_FMM);
- eventBroker.subscribe(this, TOPIC_FSR);
- eventBroker.subscribe(this, TOPIC_NAS);
- eventBroker.subscribe(this, TOPIC_TMTC);
- eventBroker.subscribe(this, TOPIC_MOTOR);
- eventBroker.subscribe(this, TOPIC_TARS);
- eventBroker.subscribe(this, TOPIC_ALT);
-}
-
-void HILFlightPhasesManager::setCurrentPositionSource(
- std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition)
-{
- this->getCurrentPosition = getCurrentPosition;
-}
-
-bool HILFlightPhasesManager::isFlagActive(FlightPhases flag)
-{
- return flagsFlightPhases[flag];
-}
-
-void HILFlightPhasesManager::registerToFlightPhase(FlightPhases flag,
- TCallback func)
-{
- callbacks[flag].push_back(func);
-}
-
-void HILFlightPhasesManager::setFlagFlightPhase(FlightPhases flag,
- bool isEnable)
-{
- flagsFlightPhases[flag] = isEnable;
-}
-
-void HILFlightPhasesManager::processFlags(FlightPhasesFlags hil_flags)
-{
- updateSimulatorFlags(hil_flags);
-
- std::vector<FlightPhases> changed_flags;
-
- // set true when the first packet from the simulator arrives
- if (isSetTrue(FlightPhases::SIMULATION_STARTED))
- {
- t_start = Boardcore::TimestampTimer::getTimestamp();
-
- TRACE("[HIL] ------- SIMULATION STARTED ! ------- \n");
- changed_flags.push_back(FlightPhases::SIMULATION_STARTED);
- }
-
- if (flagsFlightPhases[FlightPhases::SIM_FLYING])
- {
- if (isSetTrue(FlightPhases::SIM_FLYING))
- {
- registerOutcomes(FlightPhases::SIM_FLYING);
- TRACE("[HIL] ------- SIMULATOR LIFTOFF ! ------- \n");
- changed_flags.push_back(FlightPhases::SIM_FLYING);
- }
- }
-
- /* calling the callbacks subscribed to the changed flags */
- for (unsigned int i = 0; i < changed_flags.size(); i++)
- {
- std::vector<TCallback> callbacksToCall = callbacks[changed_flags[i]];
- for (unsigned int j = 0; j < callbacksToCall.size(); j++)
- {
- callbacksToCall[j]();
- }
- }
-
- prev_flagsFlightPhases = flagsFlightPhases;
-}
-
-void HILFlightPhasesManager::registerOutcomes(FlightPhases phase)
-{
- Boardcore::TimedTrajectoryPoint temp = getCurrentPosition();
- outcomes[phase] = Outcomes(temp.z, temp.vz);
-}
-
-void HILFlightPhasesManager::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[FlightPhases::SIM_BURNING].print(t_liftoff);
-
- printf("Airbrakes exit shadowmode: \n");
- outcomes[FlightPhases::AEROBRAKES].print(t_liftoff);
-
- printf("Apogee: \n");
- outcomes[FlightPhases::APOGEE].print(t_liftoff);
-
- printf("Parachute 1: \n");
- outcomes[FlightPhases::PARA1].print(t_liftoff);
-
- printf("Parachute 2: \n");
- outcomes[FlightPhases::PARA2].print(t_liftoff);
-
- printf("Simulation Stopped: \n");
- outcomes[FlightPhases::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);
-}
-
-/**
- * @brief Updates the flags of the object with the flags sent from matlab
- * and checks for the apogee
- */
-void HILFlightPhasesManager::updateSimulatorFlags(FlightPhasesFlags hil_flags)
-{
- flagsFlightPhases[FlightPhases::SIM_ASCENT] = hil_flags.flag_ascent;
- flagsFlightPhases[FlightPhases::SIM_FLYING] = hil_flags.flag_flight;
- flagsFlightPhases[FlightPhases::SIM_BURNING] = hil_flags.flag_burning;
- flagsFlightPhases[FlightPhases::SIM_AEROBRAKES] = hil_flags.flag_airbrakes;
- flagsFlightPhases[FlightPhases::SIM_PARA1] = hil_flags.flag_para1;
- flagsFlightPhases[FlightPhases::SIM_PARA2] = hil_flags.flag_para2;
-
- flagsFlightPhases[FlightPhases::SIMULATION_STOPPED] =
- isSetFalse(FlightPhases::SIM_FLYING) ||
- prev_flagsFlightPhases[FlightPhases::SIMULATION_STOPPED];
-}
-
-void HILFlightPhasesManager::handleEvent(const Boardcore::Event& e)
-{
- std::vector<FlightPhases> changed_flags;
-
- switch (e)
- {
- case FMM_INIT_ERROR:
- printf("[HIL] ------- INIT FAILED ! ------- \n");
- case FMM_INIT_OK:
- setFlagFlightPhase(FlightPhases::CALIBRATION, true);
- TRACE("[HIL] ------- CALIBRATION ! ------- \n");
- changed_flags.push_back(FlightPhases::CALIBRATION);
- break;
- case FLIGHT_DISARMED:
- setFlagFlightPhase(FlightPhases::CALIBRATION_OK, true);
- TRACE("[HIL] CALIBRATION OK!\n");
- changed_flags.push_back(FlightPhases::CALIBRATION_OK);
- break;
- case FLIGHT_ARMED:
- setFlagFlightPhase(FlightPhases::ARMED, true);
- printf("[HIL] ------- READY TO LAUNCH ! ------- \n");
- changed_flags.push_back(FlightPhases::ARMED);
- break;
- case FLIGHT_LAUNCH_PIN_DETACHED:
- setFlagFlightPhase(FlightPhases::LIFTOFF_PIN_DETACHED, true);
- TRACE("[HIL] ------- LIFTOFF PIN DETACHED ! ------- \n");
- changed_flags.push_back(FlightPhases::LIFTOFF_PIN_DETACHED);
- break;
- case FLIGHT_LIFTOFF:
- case TMTC_FORCE_LAUNCH:
- t_liftoff = Boardcore::TimestampTimer::getTimestamp();
- printf("[HIL] ------- LIFTOFF -------: %f, %f \n",
- getCurrentPosition().z, getCurrentPosition().vz);
- changed_flags.push_back(FlightPhases::LIFTOFF);
- break;
- case ABK_SHADOW_MODE_TIMEOUT:
- setFlagFlightPhase(FlightPhases::AEROBRAKES, true);
- registerOutcomes(FlightPhases::AEROBRAKES);
- TRACE("[HIL] ABK shadow mode timeout\n");
- changed_flags.push_back(FlightPhases::AEROBRAKES);
- break;
- case ADA_SHADOW_MODE_TIMEOUT:
- TRACE("[HIL] ADA shadow mode timeout\n");
- break;
- case ABK_DISABLE:
- setFlagFlightPhase(FlightPhases::AEROBRAKES, false);
- TRACE("[HIL] ABK disabled\n");
- break;
- case FLIGHT_APOGEE_DETECTED:
- case CAN_APOGEE_DETECTED:
- setFlagFlightPhase(FlightPhases::AEROBRAKES, false);
- registerOutcomes(FlightPhases::APOGEE);
- printf("[HIL] ------- APOGEE DETECTED ! ------- %f, %f \n",
- getCurrentPosition().z, getCurrentPosition().vz);
- changed_flags.push_back(FlightPhases::APOGEE);
- break;
- case FLIGHT_DROGUE_DESCENT:
- case TMTC_FORCE_EXPULSION:
- setFlagFlightPhase(FlightPhases::PARA1, true);
- registerOutcomes(FlightPhases::PARA1);
- printf("[HIL] ------- PARA1 ! -------%f, %f \n",
- getCurrentPosition().z, getCurrentPosition().vz);
- changed_flags.push_back(FlightPhases::PARA1);
- break;
- case FLIGHT_WING_DESCENT:
- case FLIGHT_DPL_ALT_DETECTED:
- case TMTC_FORCE_DEPLOYMENT:
- setFlagFlightPhase(FlightPhases::PARA1, false);
- setFlagFlightPhase(FlightPhases::PARA2, true);
- registerOutcomes(FlightPhases::PARA2);
- printf("[HIL] ------- PARA2 ! ------- %f, %f \n",
- getCurrentPosition().z, getCurrentPosition().vz);
- changed_flags.push_back(FlightPhases::PARA2);
- break;
- case FLIGHT_LANDING_DETECTED:
- case TMTC_FORCE_LANDING:
- t_stop = Boardcore::TimestampTimer::getTimestamp();
- setFlagFlightPhase(FlightPhases::PARA2, false);
- setFlagFlightPhase(FlightPhases::SIMULATION_STOPPED, true);
- changed_flags.push_back(FlightPhases::SIMULATION_STOPPED);
- registerOutcomes(FlightPhases::SIMULATION_STOPPED);
- TRACE("[HIL] ------- SIMULATION STOPPED ! -------: %f \n\n\n",
- (double)t_stop / 1000000.0f);
- printOutcomes();
- break;
- default:
- TRACE("%s invalid event\n", getEventString(e).c_str());
- }
-
- /* calling the callbacks subscribed to the changed flags */
- for (unsigned int i = 0; i < changed_flags.size(); i++)
- {
- std::vector<TCallback> callbacksToCall = callbacks[changed_flags[i]];
- for (unsigned int j = 0; j < callbacksToCall.size(); j++)
- {
- callbacksToCall[j]();
- }
- }
-
- prev_flagsFlightPhases = flagsFlightPhases;
-}
-
-bool HILFlightPhasesManager::isSetTrue(FlightPhases phase)
-{
- return flagsFlightPhases[phase] && !prev_flagsFlightPhases[phase];
-}
-
-bool HILFlightPhasesManager::isSetFalse(FlightPhases phase)
-{
- return !flagsFlightPhases[phase] && prev_flagsFlightPhases[phase];
-}
diff --git a/src/shared/hil/HILFlightPhasesManager.h b/src/shared/hil/HILPhasesManager.h
similarity index 53%
rename from src/shared/hil/HILFlightPhasesManager.h
rename to src/shared/hil/HILPhasesManager.h
index 68390d9839b1cd4d06f77cebb3f50136d590fb2d..0131f20d8fa9e5fecfae99de44f15948629d83d3 100644
--- a/src/shared/hil/HILFlightPhasesManager.h
+++ b/src/shared/hil/HILPhasesManager.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2021-2023 Skyward Experimental Rocketry
+/* 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
@@ -26,40 +26,13 @@
#include <drivers/timer/TimestampTimer.h>
#include <events/Event.h>
#include <events/EventHandler.h>
-#include <hil/HILTransceiver.h>
#include <miosix.h>
#include <iostream>
#include <map>
-
-#include "Events.h"
+#include <utils/ModuleManager/ModuleManager.hpp>
typedef std::function<void()> TCallback;
-class HILTransceiver;
-
-enum class FlightPhases
-{
- // simulator flags
- SIM_FLYING,
- SIM_ASCENT,
- SIM_BURNING,
- SIM_AEROBRAKES,
- SIM_PARA1,
- SIM_PARA2,
-
- // flight flags
- SIMULATION_STARTED,
- CALIBRATION,
- CALIBRATION_OK,
- ARMED,
- LIFTOFF_PIN_DETACHED,
- LIFTOFF,
- AEROBRAKES,
- APOGEE,
- PARA1,
- PARA2,
- SIMULATION_STOPPED
-};
struct Outcomes
{
@@ -81,53 +54,103 @@ struct Outcomes
}
};
+class HILPhasesManagerBase : public Boardcore::EventHandler,
+ public Boardcore::Module
+{
+public:
+ explicit HILPhasesManagerBase(
+ std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition)
+ : Boardcore::EventHandler(), getCurrentPosition(getCurrentPosition)
+ {
+ }
+
+ bool isSimulationRunning() { return (t_start != 0) && (t_stop == 0); }
+
+ virtual void simulationStarted()
+ {
+ t_start = Boardcore::TimestampTimer::getTimestamp();
+ LOG_INFO(logger, "SIMULATION STARTED!");
+ }
+
+ virtual void liftoff()
+ {
+ t_liftoff = Boardcore::TimestampTimer::getTimestamp();
+ LOG_INFO(logger, "LIFTOFF!");
+ }
+
+ virtual void simulationStopped()
+ {
+ t_stop = Boardcore::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<Boardcore::TimedTrajectoryPoint()> getCurrentPosition;
+ Boardcore::PrintLogger logger =
+ Boardcore::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.
*/
-class HILFlightPhasesManager : public Boardcore::EventHandler,
- public Boardcore::Module
+template <class FlightPhases, class SimulatorData, class ActuatorData>
+class HILPhasesManager : public HILPhasesManagerBase
{
- using FlightPhasesFlags = HILConfig::SimulatorData::Flags;
-
public:
- HILFlightPhasesManager();
+ explicit HILPhasesManager(
+ std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition)
+ : HILPhasesManagerBase(getCurrentPosition)
+ {
+ }
- void setCurrentPositionSource(
- std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition);
+ bool isFlagActive(FlightPhases flag) { return flagsFlightPhases[flag]; }
- bool isFlagActive(FlightPhases flag);
+ void registerToFlightPhase(FlightPhases flag, TCallback func)
+ {
+ callbacks[flag].push_back(func);
+ }
- void registerToFlightPhase(FlightPhases flag, TCallback func);
+ void setFlagFlightPhase(FlightPhases flag, bool isEnable)
+ {
+ flagsFlightPhases[flag] = isEnable;
+ }
- void setFlagFlightPhase(FlightPhases flag, bool isEnable);
+ virtual void processFlags(const SimulatorData& hil_flags) = 0;
- virtual void processFlags(FlightPhasesFlags hil_flags);
+ virtual void printOutcomes() = 0;
protected:
- virtual void handleEvent(const Boardcore::Event& e) override;
-
- virtual void registerOutcomes(FlightPhases phase);
-
- virtual void printOutcomes();
+ virtual void handleEvent(const Boardcore::Event& e) = 0;
/**
- * @brief Updates the flags of the object with the flags sent from matlab
- * and checks for the apogee
+ * @brief Updates the flags of the object with the flags sent from
+ * matlab and checks for the apogee
*/
- virtual void updateSimulatorFlags(FlightPhasesFlags hil_flags);
+ virtual void updateSimulatorFlags(const SimulatorData& hil_flags) = 0;
- bool isSetTrue(FlightPhases phase);
+ void registerOutcomes(FlightPhases phase)
+ {
+ Boardcore::TimedTrajectoryPoint temp = getCurrentPosition();
+ outcomes[phase] = Outcomes(temp.z, temp.vz);
+ }
- bool isSetFalse(FlightPhases phase);
+ bool isSetTrue(FlightPhases phase)
+ {
+ return flagsFlightPhases[phase] && !prev_flagsFlightPhases[phase];
+ }
+
+ bool isSetFalse(FlightPhases phase)
+ {
+ return !flagsFlightPhases[phase] && prev_flagsFlightPhases[phase];
+ }
- uint64_t t_start = 0;
- uint64_t t_liftoff = 0;
- uint64_t t_stop = 0;
std::map<FlightPhases, bool> flagsFlightPhases;
std::map<FlightPhases, bool> prev_flagsFlightPhases;
std::map<FlightPhases, vector<TCallback>> callbacks;
std::map<FlightPhases, Outcomes> outcomes;
- std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition;
};
diff --git a/src/shared/hil/HILTransceiver.cpp b/src/shared/hil/HILTransceiver.cpp
deleted file mode 100644
index 8eb5cc26dc636c7848d4c052e726935942cb2c07..0000000000000000000000000000000000000000
--- a/src/shared/hil/HILTransceiver.cpp
+++ /dev/null
@@ -1,157 +0,0 @@
-/* Copyright (c) 2020-2023 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 "HIL.h"
-#include "drivers/usart/USART.h"
-
-using namespace HILConfig;
-
-/**
- * @brief Construct a serial connection attached to a control algorithm
- */
-HILTransceiver::HILTransceiver(Boardcore::USART &hilSerial)
- : hilSerial(hilSerial), actuatorData{}
-{
-}
-
-/**
- * @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 HILTransceiver::setActuatorData(ActuatorData actuatorData)
-{
- this->actuatorData = actuatorData;
- updated = true;
- condVar.signal();
-}
-
-/**
- * @brief returns the reference of the SimulatorData
- *
- * @return reference to the data simulated by matlab
- */
-SimulatorData *HILTransceiver::getSensorData() { return &sensorData; }
-
-/**
- * @brief adds to the resetSampleCounter list an object that has to be
- * notified when a new packet of data is arrived from the simulator
- *
- * @param t SimTimestampManagement object
- */
-void HILTransceiver::addResetSampleCounter(HILTimestampManagement *t)
-{
- sensorsTimestamp.push_back(t);
-}
-
-/**
- * @brief The thread deals with the communication between the simulator and the
- * board.
- *
- * TODO: Check:
- * The first read is done in the init() function
- *
- * 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.
- */
-void HILTransceiver::run()
-{
- TRACE("[HILT] Transceiver started\n");
- bool lostUpdate = false;
- hilSerial.clearQueue();
-
- while (true)
- {
- // Pausing the kernel in order to copy the data in the shared structure
- {
- SimulatorData tempData;
- miosix::led3On();
- if (!hilSerial.readBlocking(&tempData, sizeof(SimulatorData)))
- {
- TRACE("Failed Serial read\n");
- }
- hilSerial.clearQueue();
- miosix::led3Off();
-
- miosix::PauseKernelLock kLock;
- sensorData = tempData;
-
- if (updated)
- {
- lostUpdate = true;
- updated = false; // We want the last computation
- }
- }
-
- // If this is the first packet to be received, then update the flight
- // phase manager
- if (!receivedFirstPacket)
- {
- receivedFirstPacket = true;
- Boardcore::ModuleManager::getInstance()
- .get<HIL>()
- ->flightPhasesManager->setFlagFlightPhase(
- FlightPhases::SIMULATION_STARTED, true);
- }
-
- // Notify all sensors that a new set of data is arrived
- //[REVIEW] Could be moved in HILFlightPhasesManager
- for (auto st : sensorsTimestamp)
- st->resetSampleCounter();
-
- // Trigger events relative to the flight phases
- Boardcore::ModuleManager::getInstance()
- .get<HIL>()
- ->flightPhasesManager->processFlags(sensorData.flags);
-
- if (lostUpdate)
- {
- // 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
- TRACE("[HILT] lost updates!\n");
- lostUpdate = false;
- }
-
- waitActuatorData();
- miosix::led2On();
- hilSerial.write(&actuatorData, sizeof(ActuatorData));
- miosix::led2Off();
- }
-}
-
-/**
- * @brief Waits for the control algorithms to update actuatorData.
- */
-void HILTransceiver::waitActuatorData()
-{
- miosix::Lock<miosix::FastMutex> l(mutex);
- while (!updated)
- {
- condVar.wait(l);
- }
- updated = false;
-}
diff --git a/src/shared/hil/HILTransceiver.h b/src/shared/hil/HILTransceiver.h
index 4306f9c8ff91043300f74955027621a672d6e81e..9e15e45a960229fff1079937b0eb6ce739a6908d 100644
--- a/src/shared/hil/HILTransceiver.h
+++ b/src/shared/hil/HILTransceiver.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2020-2023 Skyward Experimental Rocketry
+/* Copyright (c) 2020-2024 Skyward Experimental Rocketry
* Author: Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -25,36 +25,26 @@
#include <ActiveObject.h>
#include <drivers/timer/TimestampTimer.h>
#include <drivers/usart/USART.h>
-#include <hil/HILConfig.h>
#include <sensors/HILSensors/HILTimestampManagement.h>
#include <utils/Debug.h>
-/**
- * @brief HILTransceiver is a Singleton and provides an easy interface for
- * the control algorithms to send and receive data during a simulation
- */
-class HILTransceiver : public Boardcore::ActiveObject
+#include "HIL.h"
+#include "drivers/usart/USART.h"
+
+template <class FlightPhases, class SimulatorData, class ActuatorData>
+class HIL;
+
+class HILTransceiverBase : public Boardcore::ActiveObject,
+ public Boardcore::Module
{
public:
/**
* @brief Construct a serial connection attached to a control algorithm
*/
- explicit HILTransceiver(Boardcore::USART &hilSerial);
-
- /**
- * @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(HILConfig::ActuatorData actuatorData);
-
- /**
- * @brief returns the reference of the SimulatorData
- *
- * @return reference to the data simulated by matlab
- */
- HILConfig::SimulatorData *getSensorData();
+ explicit HILTransceiverBase(Boardcore::USART &hilSerial)
+ : hilSerial(hilSerial)
+ {
+ }
/**
* @brief adds to the resetSampleCounter list an object that has to be
@@ -62,22 +52,166 @@ public:
*
* @param t SimTimestampManagement object
*/
- void addResetSampleCounter(HILTimestampManagement *t);
+ void addResetSampleCounter(HILTimestampManagement *t)
+ {
+ sensorsTimestamp.push_back(t);
+ }
-private:
/**
- * @brief Waits for the control algorithm(s) to update actuatorData.
+ * @brief Returns the number of lost updates.
+ * @return the number of updates lost due to communication with the
+ * simulator.
*/
- void waitActuatorData();
+ int getLostUpdates() { return nLostUpdates; }
- void run() override;
+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;
+ }
Boardcore::USART &hilSerial;
bool receivedFirstPacket = false;
bool updated = false;
- HILConfig::SimulatorData sensorData;
- HILConfig::ActuatorData actuatorData;
+ int nLostUpdates = 0;
std::vector<HILTimestampManagement *> sensorsTimestamp;
miosix::FastMutex mutex;
miosix::ConditionVariable condVar;
+ Boardcore::PrintLogger logger =
+ Boardcore::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
+ */
+ explicit HILTransceiver(Boardcore::USART &hilSerial)
+ : HILTransceiverBase(hilSerial), actuatorData()
+ {
+ }
+
+ /**
+ * @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() { return &simulatorData; }
+
+private:
+ void run() override;
+
+ SimulatorData simulatorData;
+ ActuatorData actuatorData;
};
+
+/**
+ * @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");
+ auto *hilPhasesManager =
+ Boardcore::ModuleManager::getInstance()
+ .get<HIL<FlightPhases, SimulatorData, ActuatorData>>()
+ ->hilPhasesManager;
+ bool lostUpdate = false;
+ hilSerial.clearQueue();
+
+ while (!shouldStop())
+ {
+ // Pausing the kernel in order to copy the data in the shared structure
+ {
+ SimulatorData tempData;
+ miosix::led3On();
+ if (!hilSerial.readBlocking(&tempData, sizeof(SimulatorData)))
+ {
+ LOG_ERR(logger, "Failed serial read");
+ }
+ hilSerial.clearQueue();
+ miosix::led3Off();
+
+ miosix::PauseKernelLock kLock;
+ simulatorData = tempData;
+
+ if (updated)
+ {
+ lostUpdate = true;
+ updated = false; // We want the last computation
+ }
+ }
+
+ // If this is the first packet to be received, then update the flight
+ // phase manager
+ if (!receivedFirstPacket)
+ {
+ receivedFirstPacket = true;
+ hilPhasesManager->simulationStarted();
+ }
+
+ // Notify all sensors that a new set of data is arrived
+ for (auto st : sensorsTimestamp)
+ st->resetSampleCounter();
+
+ // Trigger events relative to the flight phases
+ hilPhasesManager->processFlags(simulatorData);
+
+ if (lostUpdate)
+ {
+ // 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, "Lost updates");
+ lostUpdate = false;
+ }
+
+ waitActuatorData();
+ miosix::led2On();
+ hilSerial.write(&actuatorData, sizeof(ActuatorData));
+ miosix::led2Off();
+ }
+}
diff --git a/src/shared/sensors/HILSensors/HILAccelerometer.h b/src/shared/sensors/HILSensors/HILAccelerometer.h
index 8542349657720ffe72084f10d6840d34ea49de67..604843ff04db93c2f10aec9d29c241055c730536 100644
--- a/src/shared/sensors/HILSensors/HILAccelerometer.h
+++ b/src/shared/sensors/HILSensors/HILAccelerometer.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2020-2023 Skyward Experimental Rocketry
+/* Copyright (c) 2020-2024 Skyward Experimental Rocketry
* Author: Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -26,6 +26,12 @@
#include "HILSensor.h"
+template <int N_DATA>
+struct AccelerometerSimulatorData
+{
+ float measures[N_DATA][3];
+};
+
/**
* @brief fake accelerometer sensor used for the simulation.
*
@@ -33,11 +39,19 @@
* OBSW during the flight, using fake sensors classes instead of the real
* ones, taking their data from the data received from a simulator.
*/
-class HILAccelerometer : public HILSensor<HILAccelerometerData>
+template <int N_DATA>
+class HILAccelerometer
+ : public HILSensor<HILAccelerometerData, AccelerometerSimulatorData<N_DATA>,
+ N_DATA>
{
+ using Base = HILSensor<HILAccelerometerData,
+ AccelerometerSimulatorData<N_DATA>, N_DATA>;
+
public:
- HILAccelerometer(int n_data_sensor, void *sensorData)
- : HILSensor(n_data_sensor, sensorData)
+ explicit HILAccelerometer(
+ const AccelerometerSimulatorData<N_DATA> *sensorData)
+ : HILSensor<HILAccelerometerData, AccelerometerSimulatorData<N_DATA>,
+ N_DATA>(sensorData)
{
}
@@ -45,17 +59,16 @@ protected:
HILAccelerometerData updateData() override
{
HILAccelerometerData tempData;
-
- miosix::PauseKernelLock pkLock;
- HILConfig::SimulatorData::Accelerometer *accelerometer =
- reinterpret_cast<HILConfig::SimulatorData::Accelerometer *>(
- sensorData);
-
- tempData.accelerationX = accelerometer->measures[sampleCounter][0];
- tempData.accelerationY = accelerometer->measures[sampleCounter][1];
- tempData.accelerationZ = accelerometer->measures[sampleCounter][2];
- tempData.accelerationTimestamp = updateTimestamp();
-
+ {
+ miosix::PauseKernelLock pkLock;
+ tempData.accelerationX =
+ Base::sensorData->measures[Base::sampleCounter][0];
+ tempData.accelerationY =
+ Base::sensorData->measures[Base::sampleCounter][1];
+ tempData.accelerationZ =
+ Base::sensorData->measures[Base::sampleCounter][2];
+ tempData.accelerationTimestamp = Base::updateTimestamp();
+ }
Boardcore::Logger::getInstance().log(tempData);
return tempData;
diff --git a/src/shared/sensors/HILSensors/HILBarometer.h b/src/shared/sensors/HILSensors/HILBarometer.h
index 9e283f925ee7d637f2265f1a027b48c008a4aa01..d146c7ac2f1d37d17ba64aecc02b1c8ac74360af 100644
--- a/src/shared/sensors/HILSensors/HILBarometer.h
+++ b/src/shared/sensors/HILSensors/HILBarometer.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2020-2023 Skyward Experimental Rocketry
+/* Copyright (c) 2020-2024 Skyward Experimental Rocketry
* Author: Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -26,6 +26,12 @@
#include "HILSensor.h"
+template <int N_DATA>
+struct BarometerSimulatorData
+{
+ float measures[N_DATA];
+};
+
/**
* @brief fake barometer sensor used for the simulation.
*
@@ -33,11 +39,17 @@
* OBSW during the flight, using fake sensors classes instead of the real
* ones, taking their data from the data received from a simulator.
*/
-class HILBarometer : public HILSensor<HILBarometerData>
+template <int N_DATA>
+class HILBarometer
+ : public HILSensor<HILBarometerData, BarometerSimulatorData<N_DATA>, N_DATA>
{
+ using Base =
+ HILSensor<HILBarometerData, BarometerSimulatorData<N_DATA>, N_DATA>;
+
public:
- HILBarometer(int n_data_sensor, void *sensorData)
- : HILSensor(n_data_sensor, sensorData)
+ explicit HILBarometer(const BarometerSimulatorData<N_DATA> *sensorData)
+ : HILSensor<HILBarometerData, BarometerSimulatorData<N_DATA>, N_DATA>(
+ sensorData)
{
}
@@ -45,14 +57,11 @@ protected:
HILBarometerData updateData() override
{
HILBarometerData tempData;
-
- miosix::PauseKernelLock pkLock;
- HILConfig::SimulatorData::Barometer *barometer =
- reinterpret_cast<HILConfig::SimulatorData::Barometer *>(sensorData);
-
- tempData.pressure = barometer->measures[sampleCounter];
- tempData.pressureTimestamp = updateTimestamp();
-
+ {
+ miosix::PauseKernelLock pkLock;
+ tempData.pressure = Base::sensorData->measures[Base::sampleCounter];
+ tempData.pressureTimestamp = Base::updateTimestamp();
+ }
Boardcore::Logger::getInstance().log(tempData);
return tempData;
diff --git a/src/shared/sensors/HILSensors/HILGps.h b/src/shared/sensors/HILSensors/HILGps.h
index aecd70ba9a2bc3aaf3aa0eecaab6b252c76ebe6a..296d5b670fe81749453704f039a6df13e2e93f60 100644
--- a/src/shared/sensors/HILSensors/HILGps.h
+++ b/src/shared/sensors/HILSensors/HILGps.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2020-2023 Skyward Experimental Rocketry
+/* Copyright (c) 2020-2024 Skyward Experimental Rocketry
* Author: Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -28,6 +28,15 @@
#include "HILSensor.h"
+template <int N_DATA>
+struct GPSSimulatorData
+{
+ float positionMeasures[N_DATA][3];
+ float velocityMeasures[N_DATA][3];
+ float fix;
+ float num_satellites;
+};
+
/**
* @brief fake gps sensor used for the HILulation.
*
@@ -35,11 +44,14 @@
* OBSW during the flight, using fake sensors classes instead of the real
* ones, taking their data from the data received from a HILulator.
*/
-class HILGps : public HILSensor<HILGpsData>
+template <int N_DATA>
+class HILGps : public HILSensor<HILGpsData, GPSSimulatorData<N_DATA>, N_DATA>
{
+ using Base = HILSensor<HILGpsData, GPSSimulatorData<N_DATA>, N_DATA>;
+
public:
- HILGps(int n_data_sensor, void *sensorData)
- : HILSensor(n_data_sensor, sensorData)
+ explicit HILGps(const GPSSimulatorData<N_DATA> *sensorData)
+ : HILSensor<HILGpsData, GPSSimulatorData<N_DATA>, N_DATA>(sensorData)
{
}
@@ -47,28 +59,34 @@ protected:
HILGpsData updateData() override
{
HILGpsData tempData;
+ {
+ miosix::PauseKernelLock pkLock;
- miosix::PauseKernelLock pkLock;
- HILConfig::SimulatorData::Gps *gps =
- reinterpret_cast<HILConfig::SimulatorData::Gps *>(sensorData);
-
- tempData.latitude =
- gps->positionMeasures[sampleCounter][0]; // divide by earth radius
- tempData.longitude = gps->positionMeasures[sampleCounter][1];
- tempData.height = gps->positionMeasures[sampleCounter][2];
-
- tempData.velocityNorth = gps->velocityMeasures[sampleCounter][0];
- tempData.velocityEast = gps->velocityMeasures[sampleCounter][1];
- tempData.velocityDown = gps->velocityMeasures[sampleCounter][2];
- tempData.speed = sqrtf(tempData.velocityNorth * tempData.velocityNorth +
- tempData.velocityDown * tempData.velocityDown);
- tempData.positionDOP = 0;
+ tempData.latitude =
+ Base::sensorData->positionMeasures[Base::sampleCounter][0];
+ tempData.longitude =
+ Base::sensorData->positionMeasures[Base::sampleCounter][1];
+ tempData.height =
+ Base::sensorData->positionMeasures[Base::sampleCounter][2];
- tempData.fix = static_cast<uint8_t>(gps->fix);
- tempData.satellites = static_cast<uint8_t>(gps->num_satellites);
+ tempData.velocityNorth =
+ Base::sensorData->velocityMeasures[Base::sampleCounter][0];
+ tempData.velocityEast =
+ Base::sensorData->velocityMeasures[Base::sampleCounter][1];
+ tempData.velocityDown =
+ Base::sensorData->velocityMeasures[Base::sampleCounter][2];
+ tempData.speed =
+ sqrtf(tempData.velocityNorth * tempData.velocityNorth +
+ tempData.velocityEast * tempData.velocityEast +
+ tempData.velocityDown * tempData.velocityDown);
+ tempData.positionDOP = 0;
- tempData.gpsTimestamp = updateTimestamp();
+ tempData.fix = static_cast<uint8_t>(Base::sensorData->fix);
+ tempData.satellites =
+ static_cast<uint8_t>(Base::sensorData->num_satellites);
+ tempData.gpsTimestamp = Base::updateTimestamp();
+ }
Boardcore::Logger::getInstance().log(tempData);
return tempData;
diff --git a/src/shared/sensors/HILSensors/HILGyroscope.h b/src/shared/sensors/HILSensors/HILGyroscope.h
index 78b10e9075410005e084a0dcba45f1ac064b41d4..f76ed2c745a77825fcad3c765b90804d738d795e 100644
--- a/src/shared/sensors/HILSensors/HILGyroscope.h
+++ b/src/shared/sensors/HILSensors/HILGyroscope.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2020-2023 Skyward Experimental Rocketry
+/* Copyright (c) 2020-2024 Skyward Experimental Rocketry
* Author: Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -26,6 +26,12 @@
#include "HILSensor.h"
+template <int N_DATA>
+struct GyroscopeSimulatorData
+{
+ float measures[N_DATA][3];
+};
+
/**
* @brief fake gyroscope sensor used for the simulation.
*
@@ -33,11 +39,17 @@
* OBSW during the flight, using fake sensors classes instead of the real
* ones, taking their data from the data received from a simulator.
*/
-class HILGyroscope : public HILSensor<HILGyroscopeData>
+template <int N_DATA>
+class HILGyroscope
+ : public HILSensor<HILGyroscopeData, GyroscopeSimulatorData<N_DATA>, N_DATA>
{
+ using Base =
+ HILSensor<HILGyroscopeData, GyroscopeSimulatorData<N_DATA>, N_DATA>;
+
public:
- HILGyroscope(int n_data_sensor, void *sensorData)
- : HILSensor(n_data_sensor, sensorData)
+ explicit HILGyroscope(const GyroscopeSimulatorData<N_DATA> *sensorData)
+ : HILSensor<HILGyroscopeData, GyroscopeSimulatorData<N_DATA>, N_DATA>(
+ sensorData)
{
}
@@ -45,16 +57,16 @@ protected:
HILGyroscopeData updateData() override
{
HILGyroscopeData tempData;
-
- miosix::PauseKernelLock pkLock;
- HILConfig::SimulatorData::Gyro *gyroscope =
- reinterpret_cast<HILConfig::SimulatorData::Gyro *>(sensorData);
-
- tempData.angularSpeedX = gyroscope->measures[sampleCounter][0];
- tempData.angularSpeedY = gyroscope->measures[sampleCounter][1];
- tempData.angularSpeedZ = gyroscope->measures[sampleCounter][2];
- tempData.angularSpeedTimestamp = updateTimestamp();
-
+ {
+ miosix::PauseKernelLock pkLock;
+ tempData.angularSpeedX =
+ Base::sensorData->measures[Base::sampleCounter][0];
+ tempData.angularSpeedY =
+ Base::sensorData->measures[Base::sampleCounter][1];
+ tempData.angularSpeedZ =
+ Base::sensorData->measures[Base::sampleCounter][2];
+ tempData.angularSpeedTimestamp = Base::updateTimestamp();
+ }
Boardcore::Logger::getInstance().log(tempData);
return tempData;
diff --git a/src/shared/sensors/HILSensors/HILMagnetometer.h b/src/shared/sensors/HILSensors/HILMagnetometer.h
index 8c79ce53a3f6306202346bb2a7e20474ca65d7aa..12ded6df8258ef90e5dae0eece559c749410773b 100644
--- a/src/shared/sensors/HILSensors/HILMagnetometer.h
+++ b/src/shared/sensors/HILSensors/HILMagnetometer.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2020-2023 Skyward Experimental Rocketry
+/* Copyright (c) 2020-2024 Skyward Experimental Rocketry
* Author: Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -26,6 +26,12 @@
#include "HILSensor.h"
+template <int N_DATA>
+struct MagnetometerSimulatorData
+{
+ float measures[N_DATA][3];
+};
+
/**
* @brief fake magnetometer sensor used for the simulation.
*
@@ -33,11 +39,19 @@
* OBSW during the flight, using fake sensors classes instead of the real
* ones, taking their data from the data received from a simulator.
*/
-class HILMagnetometer : public HILSensor<HILMagnetometerData>
+template <int N_DATA>
+class HILMagnetometer
+ : public HILSensor<HILMagnetometerData, MagnetometerSimulatorData<N_DATA>,
+ N_DATA>
{
+ using Base = HILSensor<HILMagnetometerData,
+ MagnetometerSimulatorData<N_DATA>, N_DATA>;
+
public:
- HILMagnetometer(int n_data_sensor, void *sensorData)
- : HILSensor(n_data_sensor, sensorData)
+ explicit HILMagnetometer(
+ const MagnetometerSimulatorData<N_DATA> *sensorData)
+ : HILSensor<HILMagnetometerData, MagnetometerSimulatorData<N_DATA>,
+ N_DATA>(sensorData)
{
}
@@ -45,17 +59,16 @@ protected:
HILMagnetometerData updateData() override
{
HILMagnetometerData tempData;
-
- miosix::PauseKernelLock pkLock;
- HILConfig::SimulatorData::Magnetometer *magnetometer =
- reinterpret_cast<HILConfig::SimulatorData::Magnetometer *>(
- sensorData);
-
- tempData.magneticFieldX = magnetometer->measures[sampleCounter][0];
- tempData.magneticFieldY = magnetometer->measures[sampleCounter][1];
- tempData.magneticFieldZ = magnetometer->measures[sampleCounter][2];
- tempData.magneticFieldTimestamp = updateTimestamp();
-
+ {
+ miosix::PauseKernelLock pkLock;
+ tempData.magneticFieldX =
+ Base::sensorData->measures[Base::sampleCounter][0];
+ tempData.magneticFieldY =
+ Base::sensorData->measures[Base::sampleCounter][1];
+ tempData.magneticFieldZ =
+ Base::sensorData->measures[Base::sampleCounter][2];
+ tempData.magneticFieldTimestamp = Base::updateTimestamp();
+ }
Boardcore::Logger::getInstance().log(tempData);
return tempData;
diff --git a/src/shared/sensors/HILSensors/HILPitot.h b/src/shared/sensors/HILSensors/HILPitot.h
index 13c0b654c27061690442845b404412a555c04e88..a402927d1fc1398fd920a7ba66132f3a646467e9 100644
--- a/src/shared/sensors/HILSensors/HILPitot.h
+++ b/src/shared/sensors/HILSensors/HILPitot.h
@@ -28,6 +28,13 @@
#include "HILSensor.h"
+template <int N_DATA>
+struct PitotSimulatorData
+{
+ float deltaP[N_DATA];
+ float airspeed[N_DATA];
+};
+
/**
* @brief fake pitot (differential pressure) sensor used for the simulation.
*
@@ -35,27 +42,29 @@
* OBSW during the flight, using fake sensors classes instead of the real
* ones, taking their data from the data received from a simulator.
*/
-class HILPitot : public HILSensor<HILPitotData>
+template <int N_DATA>
+class HILPitot
+ : public HILSensor<HILPitotData, PitotSimulatorData<N_DATA>, N_DATA>
{
+ using Base = HILSensor<HILPitotData, PitotSimulatorData<N_DATA>, N_DATA>;
+
public:
- HILPitot(int n_data_sensor, void *sensorData)
- : HILSensor(n_data_sensor, sensorData)
+ explicit HILPitot(const PitotSimulatorData<N_DATA> *sensorData)
+ : HILSensor<HILPitotData, PitotSimulatorData<N_DATA>, N_DATA>(
+ sensorData)
{
}
protected:
HILPitotData updateData() override
{
- miosix::PauseKernelLock pkLock;
-
- auto *pitotData =
- reinterpret_cast<HILConfig::SimulatorData::Pitot *>(sensorData);
-
HILPitotData tempData;
- tempData.deltaP = pitotData->deltaP[sampleCounter];
- tempData.airspeed = pitotData->airspeed[sampleCounter];
- tempData.timestamp = updateTimestamp();
-
+ {
+ miosix::PauseKernelLock pkLock;
+ tempData.deltaP = Base::sensorData->deltaP[Base::sampleCounter];
+ tempData.airspeed = Base::sensorData->airspeed[Base::sampleCounter];
+ tempData.timestamp = Base::updateTimestamp();
+ }
Boardcore::Logger::getInstance().log(tempData);
return tempData;
diff --git a/src/shared/sensors/HILSensors/HILSensor.h b/src/shared/sensors/HILSensors/HILSensor.h
index d9f185784afc5b56aa88a88f3e3754281d020d72..ce2bfc264b2ed650fbb271584b3538bf9a362d8e 100644
--- a/src/shared/sensors/HILSensors/HILSensor.h
+++ b/src/shared/sensors/HILSensors/HILSensor.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2020-2023 Skyward Experimental Rocketry
+/* Copyright (c) 2020-2024 Skyward Experimental Rocketry
* Author: Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -24,11 +24,10 @@
#include <typeinfo>
-#include "HIL.h"
-#include "HILConfig.h"
-#include "HILSensorsData.h"
#include "HILTimestampManagement.h"
#include "drivers/timer/TimestampTimer.h"
+#include "hil/HIL.h"
+#include "sensors/HILSensors/HILSensorsData.h"
#include "sensors/Sensor.h"
#include "sensors/SensorData.h"
@@ -40,7 +39,7 @@
* OBSW during the flight, using fake sensors classes instead of the real
* ones, taking their data from the data received from a simulator.
*/
-template <typename HILSensorData>
+template <typename HILSensorData, typename SimulatorSensorData, int N_DATA>
class HILSensor : public virtual HILTimestampManagement,
public virtual Boardcore::Sensor<HILSensorData>
{
@@ -50,18 +49,17 @@ public:
*
* @param matlab reference of the MatlabTransceiver object that deals with
* the simulator
- * @param n_data_sensor number of samples in every period of simulation
+ * @param addResetSampleCounter should be something like:
+ * Boardcore::ModuleManager::getInstance().get<HIL>()->simulator->addResetSampleCounter()
*/
- HILSensor(int n_data_sensor, void *sensorData)
+ explicit HILSensor(const SimulatorSensorData *sensorData)
+ : sensorData(sensorData)
{
- this->sensorData = sensorData;
- this->n_data_sensor = n_data_sensor;
-
- /* Registers the sensor on the MatlabTransceiver to be notified when a
+ /* Registers the sensor on the HILTransceiver to be notified when a
* new packet of simulated data arrives */
Boardcore::ModuleManager::getInstance()
- .get<HIL>()
- ->simulator->addResetSampleCounter(this);
+ .get<HILTransceiverBase>()
+ ->addResetSampleCounter(this);
}
/**
@@ -85,7 +83,7 @@ public:
if (initialized)
{
this->lastError = Boardcore::SensorErrors::ALREADY_INIT;
- TRACE("ALREADY INITIALIZED!");
+ LOG_WARN(logger, "%s already initialized", typeid(this).name());
}
else
{
@@ -110,11 +108,10 @@ protected:
if (initialized)
{
/* updates the last_sensor only if there is still data to be read */
- if (sampleCounter >= n_data_sensor)
+ if (sampleCounter >= N_DATA)
{
this->lastError = Boardcore::SensorErrors::NO_NEW_DATA;
- /*TRACE("[%s] NO NEW DATA! Simulation error\n",
- typeid(this).name());*/
+ LOG_WARN(logger, "%s: No new data", typeid(this).name());
}
else if (this->lastError != Boardcore::SensorErrors::NO_NEW_DATA)
{
@@ -124,9 +121,7 @@ protected:
else
{
this->lastError = Boardcore::SensorErrors::NOT_INIT;
- TRACE(
- "[HILSensor] sampleImpl() : not initialized, unable to "
- "sample data \n");
+ LOG_WARN(logger, "%s is not initialized", typeid(this).name());
}
return this->lastSample;
@@ -155,6 +150,7 @@ protected:
bool initialized = false;
int sampleCounter = 0; /**< counter of the next sample to take */
- int n_data_sensor; /**< number of samples in every period */
- void *sensorData; /**< reference to the Buffer structure */
+ const SimulatorSensorData
+ *sensorData; /**< reference to the Buffer structure */
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("HILSensor");
};
diff --git a/src/shared/sensors/HILSensors/HILTemperature.h b/src/shared/sensors/HILSensors/HILTemperature.h
index 7653828560d646c5271333cf276aa1e9f463b6f1..f6d8420be7dfbeb6e6e6f967992557efc9274821 100644
--- a/src/shared/sensors/HILSensors/HILTemperature.h
+++ b/src/shared/sensors/HILSensors/HILTemperature.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2020-2023 Skyward Experimental Rocketry
+/* Copyright (c) 2020-2024 Skyward Experimental Rocketry
* Author: Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -26,6 +26,12 @@
#include "HILSensor.h"
+template <int N_DATA>
+struct TemperatureSimulatorData
+{
+ float measures[N_DATA];
+};
+
/**
* @brief fake barometer sensor used for the simulation.
*
@@ -33,11 +39,17 @@
* OBSW during the flight, using fake sensors classes instead of the real
* ones, taking their data from the data received from a simulator.
*/
-class HILTemperature : public HILSensor<HILTempData>
+template <int N_DATA>
+class HILTemperature
+ : public HILSensor<HILTempData, TemperatureSimulatorData<N_DATA>, N_DATA>
{
+ using Base =
+ HILSensor<HILTempData, TemperatureSimulatorData<N_DATA>, N_DATA>;
+
public:
- HILTemperature(int n_data_sensor, void *sensorData)
- : HILSensor(n_data_sensor, sensorData)
+ explicit HILTemperature(const TemperatureSimulatorData<N_DATA> *sensorData)
+ : HILSensor<HILTempData, TemperatureSimulatorData<N_DATA>, N_DATA>(
+ sensorData)
{
}
@@ -45,13 +57,12 @@ protected:
HILTempData updateData() override
{
HILTempData tempData;
-
- tempData.temperature =
- reinterpret_cast<HILConfig::SimulatorData::Temperature *>(
- sensorData)
- ->measure;
- tempData.temperatureTimestamp = updateTimestamp();
-
+ {
+ miosix::PauseKernelLock pkLock;
+ tempData.temperature =
+ Base::sensorData->measures[Base::sampleCounter];
+ tempData.temperatureTimestamp = Base::updateTimestamp();
+ }
Boardcore::Logger::getInstance().log(tempData);
return tempData;
diff --git a/src/tests/hil/HILSimulationConfig.h b/src/tests/hil/HILSimulationConfig.h
index 55dee4d62d51490969bd73800131627405fc72f0..3c71184a316f5626dee79906bf40407c6bf680e3 100644
--- a/src/tests/hil/HILSimulationConfig.h
+++ b/src/tests/hil/HILSimulationConfig.h
@@ -22,8 +22,15 @@
#pragma once
+#include <algorithms/AirBrakes/TrajectoryPoint.h>
#include <drivers/timer/TimestampTimer.h>
#include <drivers/usart/USART.h>
+#include <events/Event.h>
+#include <events/EventHandler.h>
+#include <hil/HILPhasesManager.h>
+#include <hil/HILTransceiver.h>
+#include <math.h>
+#include <sensors/HILSensors/IncludeHILSensors.h>
#include <utils/Debug.h>
#include <utils/Stats/Stats.h>
@@ -33,6 +40,7 @@
#include "algorithms/ADA/ADAData.h"
#include "algorithms/NAS/NAS.h"
#include "algorithms/NAS/NASState.h"
+#include "events/EventBroker.h"
#include "sensors/SensorInfo.h"
namespace HILConfig
@@ -50,24 +58,18 @@ struct SensorConfig : public Boardcore::SensorInfo
const int SIM_BAUDRATE = 115200;
/** Period of simulation in milliseconds */
-const int SIMULATION_PERIOD = 100;
+constexpr int SIMULATION_PERIOD = 100;
/** sample frequency of sensor (samples/second) */
-const int ACCEL_FREQ = 100;
-const int GYRO_FREQ = 100;
-const int MAGN_FREQ = 100;
-const int IMU_FREQ = 100;
-const int BARO_FREQ = 20;
-const int PITOT_FREQ = 20;
-const int TEMP_FREQ = 10;
-const int GPS_FREQ = 10;
-
-// /** update frequency of airbrakes control algorithm */
-// const int CONTROL_FREQ = 10;
-
-// /** min and max values in radiants of the actuator */
-// const float MinAlphaDegree = 0.0;
-// const float MaxAlphaDegree = 0.84;
+constexpr int ACCEL_FREQ = 100;
+constexpr int GYRO_FREQ = 100;
+constexpr int MAGN_FREQ = 100;
+constexpr int IMU_FREQ = 100;
+constexpr int BARO_FREQ = 50;
+constexpr int BARO_CHAMBER_FREQ = 50;
+constexpr int PITOT_FREQ = 20;
+constexpr int TEMP_FREQ = 10;
+constexpr int GPS_FREQ = 10;
/** sensors configuration */
const SensorConfig accelConfig("accel", ACCEL_FREQ);
@@ -80,248 +82,439 @@ const SensorConfig gpsConfig("gps", GPS_FREQ);
const SensorConfig tempConfig("temp", TEMP_FREQ);
/** Number of samples per sensor at each simulator iteration */
-const int N_DATA_ACCEL = (ACCEL_FREQ * SIMULATION_PERIOD) / 1000; // 10
-const int N_DATA_GYRO = (GYRO_FREQ * SIMULATION_PERIOD) / 1000; // 10
-const int N_DATA_MAGN = (MAGN_FREQ * SIMULATION_PERIOD) / 1000; // 10
-const int N_DATA_IMU = (IMU_FREQ * SIMULATION_PERIOD) / 1000; // 10
-const int N_DATA_BARO = (BARO_FREQ * SIMULATION_PERIOD) / 1000; // 2
-const int N_DATA_PITOT = (PITOT_FREQ * SIMULATION_PERIOD) / 1000; // 2
-const int N_DATA_GPS = (GPS_FREQ * SIMULATION_PERIOD) / 1000; // 1
-const int N_DATA_TEMP = (TEMP_FREQ * SIMULATION_PERIOD) / 1000; // 1
-
-/**
- * @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
+constexpr int N_DATA_ACCEL =
+ static_cast<int>((ACCEL_FREQ * SIMULATION_PERIOD) / 1000.0);
+constexpr int N_DATA_GYRO =
+ static_cast<int>((GYRO_FREQ * SIMULATION_PERIOD) / 1000.0);
+constexpr int N_DATA_MAGNETO =
+ static_cast<int>((MAGN_FREQ * SIMULATION_PERIOD) / 1000.0);
+constexpr int N_DATA_IMU =
+ static_cast<int>((IMU_FREQ * SIMULATION_PERIOD) / 1000.0);
+constexpr int N_DATA_BARO =
+ static_cast<int>((BARO_FREQ * SIMULATION_PERIOD) / 1000.0);
+constexpr int N_DATA_BARO_CHAMBER =
+ static_cast<int>((BARO_CHAMBER_FREQ * SIMULATION_PERIOD) / 1000.0);
+constexpr int N_DATA_PITOT =
+ static_cast<int>((PITOT_FREQ * SIMULATION_PERIOD) / 1000.0);
+constexpr int N_DATA_GPS =
+ static_cast<int>((GPS_FREQ * SIMULATION_PERIOD) / 1000.0);
+constexpr int N_DATA_TEMP =
+ static_cast<int>((TEMP_FREQ * SIMULATION_PERIOD) / 1000.0);
+
+struct FlagsHIL
{
- struct Accelerometer
- {
- float measures[N_DATA_ACCEL][3];
- } accelerometer;
-
- struct Gyro
+ float flag_flight;
+ float flag_ascent;
+ float flag_burning;
+ float flag_airbrakes;
+ float flag_para1;
+ float flag_para2;
+
+ FlagsHIL(float flag_flight, float flag_ascent, float flag_burning,
+ float flag_airbrakes, float flag_para1, float flag_para2)
+ : flag_flight(flag_flight), flag_ascent(flag_ascent),
+ flag_burning(flag_burning), flag_airbrakes(flag_airbrakes),
+ flag_para1(flag_para1), flag_para2(flag_para2)
{
- float measures[N_DATA_GYRO][3];
- } gyro;
-
- struct Magnetometer
- {
- float measures[N_DATA_MAGN][3];
- } magnetometer;
-
- struct Gps
- {
- float positionMeasures[N_DATA_GPS][3];
- float velocityMeasures[N_DATA_GPS][3];
- float fix;
- float num_satellites;
- } gps;
-
- struct Barometer
- {
- float measures[N_DATA_BARO];
- } barometer1, barometer2, barometer3;
-
- struct Pitot
- {
- float deltaP[N_DATA_PITOT];
- float staticPressure[N_DATA_PITOT];
- } pitot;
-
- struct Temperature
- {
- float measure;
- } temperature;
+ }
- struct Flags
+ FlagsHIL()
+ : flag_flight(0.0f), flag_ascent(0.0f), flag_burning(0.0f),
+ flag_airbrakes(0.0f), flag_para1(0.0f), flag_para2(0.0f)
{
- float flag_flight;
- float flag_ascent;
- float flag_burning;
- float flag_airbrakes;
- float flag_para1;
- float flag_para2;
- } flags;
-
- void printAccelerometer()
- {
- TRACE("accel\n");
- for (int i = 0; i < N_DATA_ACCEL; i++)
- TRACE("%+.3f\t%+.3f\t%+.3f\n", accelerometer.measures[i][0],
- accelerometer.measures[i][1], accelerometer.measures[i][2]);
}
- void printGyro()
+ void print()
{
- TRACE("gyro\n");
- for (int i = 0; i < N_DATA_GYRO; i++)
- TRACE("%+.3f\t%+.3f\t%+.3f\n", gyro.measures[i][0],
- gyro.measures[i][1], gyro.measures[i][2]);
+ printf(
+ "flag_flight: %f\n"
+ "flag_ascent: %f\n"
+ "flag_burning: %f\n"
+ "flag_airbrakes: %f\n"
+ "flag_para1: %f\n"
+ "flag_para2: %f\n",
+ flag_flight, flag_ascent, flag_burning, flag_airbrakes, flag_para1,
+ flag_para2);
}
+};
- void printMagnetometer()
+/**
+ * @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)
{
- TRACE("magneto\n");
- for (int i = 0; i < N_DATA_MAGN; i++)
- TRACE("%+.3f\t%+.3f\t%+.3f\n", magnetometer.measures[i][0],
- magnetometer.measures[i][1], magnetometer.measures[i][2]);
}
- void printGPS()
+ void print()
{
- TRACE("gps\n");
- TRACE("pos\n");
- for (int i = 0; i < N_DATA_GPS; i++)
- TRACE("%+.3f\t%+.3f\t%+.3f\n", gps.positionMeasures[i][0],
- gps.positionMeasures[i][1], gps.positionMeasures[i][2]);
-
- TRACE("vel\n");
- for (int i = 0; i < N_DATA_GPS; i++)
- TRACE("%+.3f\t%+.3f\t%+.3f\n", gps.velocityMeasures[i][0],
- gps.velocityMeasures[i][1], gps.velocityMeasures[i][2]);
- TRACE("fix:%+.3f\tnsat:%+.3f\n", gps.fix, gps.num_satellites);
+ printf(
+ "mslAltitude: %+.3f\n"
+ "aglAltitude: %+.3f\n"
+ "verticalSpeed: %+.3f\n"
+ "apogeeDetected: %+.3f\n"
+ "updating: %+.3f\n",
+ mslAltitude, aglAltitude, verticalSpeed, apogeeDetected, updating);
}
+};
- void printBarometer1()
+/**
+ * @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)
{
- TRACE("press1\n");
- for (int i = 0; i < N_DATA_BARO; i++)
- TRACE("%+.3f\n", barometer1.measures[i]);
}
- void printBarometer2()
+ void print()
{
- TRACE("press2\n");
- for (int i = 0; i < N_DATA_BARO; i++)
- TRACE("%+.3f\n", barometer2.measures[i]);
+ 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); }
+};
- void printBarometer3()
+/**
+ * @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)
{
- TRACE("press3\n");
- for (int i = 0; i < N_DATA_BARO; i++)
- TRACE("%+.3f\n", barometer3.measures[i]);
}
- void printPitot()
+ void print()
{
- TRACE("pitot\n");
- for (int i = 0; i < N_DATA_PITOT; i++)
- TRACE("%+.3f, \n", pitot.staticPressure[i], pitot.deltaP[i]);
+ printf(
+ "correctedPressure: %+.3f\n"
+ "estimatedMass: %+.3f\n"
+ "estimatedApogee: %+.3f\n"
+ "updating: %+.3f\n",
+ correctedPressure, estimatedMass, estimatedApogee, updating);
}
+};
- void printTemperature()
+struct ActuatorsStateHIL
+{
+ float airbrakesPercentage = 0;
+ float expulsionPercentage = 0;
+ float mainValvePercentage = 0;
+ float ventingValvePercentage = 0;
+
+ ActuatorsStateHIL()
+ : airbrakesPercentage(0.0f), expulsionPercentage(0.0f),
+ mainValvePercentage(0.0f), ventingValvePercentage(0.0f)
{
- TRACE("temp\n");
- for (int i = 0; i < N_DATA_TEMP; i++)
- TRACE("%+.3f\n", temperature.measure);
}
- void printFlags()
+ ActuatorsStateHIL(float airbrakesPercentage, float expulsionPercentage,
+ float mainValvePercentage, float ventingValvePercentage)
+ : airbrakesPercentage(airbrakesPercentage),
+ expulsionPercentage(expulsionPercentage),
+ mainValvePercentage(mainValvePercentage),
+ ventingValvePercentage(ventingValvePercentage)
{
- TRACE("flags\n");
- TRACE(
- "flight:\t%+.3f\n"
- "ascent:\t%+.3f\n"
- "burning:\t%+.3f\n"
- "airbrakes:\t%+.3f\n"
- "para1:\t%+.3f\n"
- "para2:\t%+.3f\n",
- flags.flag_flight, flags.flag_ascent, flags.flag_burning,
- flags.flag_airbrakes, flags.flag_para1, flags.flag_para2);
}
void print()
{
- printAccelerometer();
- printGyro();
- printMagnetometer();
- printGPS();
- printBarometer1();
- printBarometer2();
- printBarometer3();
- printPitot();
- printTemperature();
- printFlags();
+ printf(
+ "airbrakes: %f perc\n"
+ "expulsion: %f perc\n"
+ "mainValve: %f perc\n"
+ "venting: %f perc\n",
+ airbrakesPercentage * 100, expulsionPercentage * 100,
+ mainValvePercentage * 100, ventingValvePercentage * 100);
}
};
/**
- * @brief ADA data sent to the simulator
+ * @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 ADAdataHIL
+struct SimulatorData
{
- uint64_t ada_timestamp;
- float aglAltitude; // Altitude at mean sea level [m].
- float verticalSpeed; // Vertical speed [m/s].
+ struct AccelerometerSimulatorData<N_DATA_ACCEL> accelerometer;
+ struct GyroscopeSimulatorData<N_DATA_GYRO> gyro;
+ struct MagnetometerSimulatorData<N_DATA_MAGNETO> magnetometer;
+ struct GPSSimulatorData<N_DATA_GPS> gps;
+ struct BarometerSimulatorData<N_DATA_BARO> barometer1, barometer2,
+ barometer3;
+ struct BarometerSimulatorData<N_DATA_BARO_CHAMBER> pressureChamber;
+ struct PitotSimulatorData<N_DATA_PITOT> pitot;
+ struct TemperatureSimulatorData<N_DATA_TEMP> temperature;
+ struct FlagsHIL flags;
+};
- ADAdataHIL& operator+=(const ADAdataHIL& x)
+/**
+ * @brief Data structure expected by the simulator
+ */
+struct ActuatorData
+{
+ ADAStateHIL adaState;
+ NASStateHIL nasState;
+ AirBrakesStateHIL airBrakesState;
+ MEAStateHIL meaState;
+ ActuatorsStateHIL actuatorsState;
+ FlagsHIL flags;
+
+ ActuatorData()
+ : adaState(), nasState(), airBrakesState(), meaState(),
+ actuatorsState(), flags()
{
- this->ada_timestamp += x.ada_timestamp;
- this->aglAltitude += x.aglAltitude;
- this->verticalSpeed += x.verticalSpeed;
+ }
- return *this; // return the result by reference
+ void print()
+ {
+ adaState.print();
+ nasState.print();
+ airBrakesState.print();
+ meaState.print();
+ actuatorsState.print();
+ flags.print();
}
+};
- ADAdataHIL operator/(int x)
+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
+};
+
+using MainHILAccelerometer = HILAccelerometer<N_DATA_ACCEL>;
+using MainHILGyroscope = HILGyroscope<N_DATA_GYRO>;
+using MainHILMagnetometer = HILMagnetometer<N_DATA_MAGNETO>;
+using MainHILGps = HILGps<N_DATA_GPS>;
+using MainHILBarometer = HILBarometer<N_DATA_BARO>;
+using MainHILBarometer = HILBarometer<N_DATA_BARO_CHAMBER>;
+using MainHILPitot = HILPitot<N_DATA_PITOT>;
+using MainHILTemperature = HILTemperature<N_DATA_TEMP>;
+
+using MainHILTransceiver =
+ HILTransceiver<MainFlightPhases, SimulatorData, ActuatorData>;
+using MainHIL = HIL<MainFlightPhases, SimulatorData, ActuatorData>;
+
+class MainHILPhasesManager
+ : public HILPhasesManager<MainFlightPhases, SimulatorData, ActuatorData>
+{
+public:
+ MainHILPhasesManager(
+ std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition)
+ : HILPhasesManager<MainFlightPhases, SimulatorData, ActuatorData>(
+ getCurrentPosition)
{
- return ADAdataHIL{this->ada_timestamp / x, this->aglAltitude / x,
- this->verticalSpeed / x};
+ 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;
}
- ADAdataHIL operator*(int x)
+ void processFlags(const SimulatorData& simulatorData) override
{
- return ADAdataHIL{this->ada_timestamp * x, this->aglAltitude * x,
- this->verticalSpeed * x};
+ updateSimulatorFlags(simulatorData);
+
+ std::vector<MainFlightPhases> changed_flags;
+
+ // 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);
+ }
+ }
+
+ /* calling the callbacks subscribed to the changed flags */
+ for (unsigned int i = 0; i < changed_flags.size(); i++)
+ {
+ std::vector<TCallback> callbacksToCall =
+ callbacks[changed_flags[i]];
+ for (unsigned int j = 0; j < callbacksToCall.size(); j++)
+ {
+ callbacksToCall[j]();
+ }
+ }
+
+ prev_flagsFlightPhases = flagsFlightPhases;
}
- static std::string header()
+ void printOutcomes()
{
- return "timestamp,aglAltitude,verticalSpeed\n";
+ 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);
}
- void print(std::ostream& os) const
+private:
+ void handleEvent(const Boardcore::Event& e) override
{
- os << ada_timestamp << "," << aglAltitude << "," << verticalSpeed
- << "\n";
+ std::vector<MainFlightPhases> changed_flags;
+
+ TRACE("%d invalid event\n", e);
+
+ /* calling the callbacks subscribed to the changed flags */
+ for (unsigned int i = 0; i < changed_flags.size(); i++)
+ {
+ std::vector<TCallback> callbacksToCall =
+ callbacks[changed_flags[i]];
+ for (unsigned int j = 0; j < callbacksToCall.size(); j++)
+ {
+ callbacksToCall[j]();
+ }
+ }
+
+ prev_flagsFlightPhases = flagsFlightPhases;
}
-};
-/**
- * @brief Data structure expected by the simulator
- */
-struct ActuatorData
-{
- Boardcore::NASState nasState; ///< NAS
- ADAdataHIL adaState; ///< ADA
- float airbrakes_opening; ///< Airbrakes opening (percentage)
- float estimated_mass; ///< Estimated mass of the rocket
- float liftoff; ///< Flag for liftoff
- float burning_shutdown; ///< Flag for engine shutdown
-
- void print() const
+ /**
+ * @brief Updates the flags of the object with the flags sent from matlab
+ * and checks for the apogee
+ */
+ void updateSimulatorFlags(const SimulatorData& simulatorData)
{
- TRACE(
- "size:%u, %u, %u\n"
- "abk:%f\n"
- "tsnas:%f\n"
- "ned:%f,%f,%f\n"
- "vned:%f,%f,%f\n"
- "q:%f,%f,%f,%f\n"
- "bias:%f,%f,%f\n"
- "tsada:%f\n"
- "ada:%f,%f\n\n",
- sizeof(airbrakes_opening), sizeof(Boardcore::NASState),
- sizeof(ADAdataHIL), airbrakes_opening, nasState.timestamp,
- nasState.n, nasState.e, nasState.d, nasState.vn, nasState.ve,
- nasState.vd, nasState.qx, nasState.qy, nasState.qz, nasState.qw,
- nasState.bx, nasState.by, nasState.bz, adaState.ada_timestamp,
- adaState.aglAltitude, adaState.verticalSpeed);
+ flagsFlightPhases[MainFlightPhases::SIM_ASCENT] =
+ simulatorData.flags.flag_ascent;
+ flagsFlightPhases[MainFlightPhases::SIM_FLYING] =
+ simulatorData.flags.flag_flight;
+ flagsFlightPhases[MainFlightPhases::SIM_BURNING] =
+ simulatorData.flags.flag_burning;
+ flagsFlightPhases[MainFlightPhases::SIM_AEROBRAKES] =
+ simulatorData.flags.flag_airbrakes;
+ flagsFlightPhases[MainFlightPhases::SIM_PARA1] =
+ simulatorData.flags.flag_para1;
+ flagsFlightPhases[MainFlightPhases::SIM_PARA2] =
+ simulatorData.flags.flag_para2;
+
+ flagsFlightPhases[MainFlightPhases::SIMULATION_STOPPED] =
+ isSetFalse(MainFlightPhases::SIM_FLYING) ||
+ prev_flagsFlightPhases[MainFlightPhases::SIMULATION_STOPPED];
}
};
diff --git a/src/tests/hil/test-hil.cpp b/src/tests/hil/test-hil.cpp
index bd712dfdcccd0d67e7285391ec78cba300a86d45..e17c266ce7c4ac46d88770d961b6f72af8572dd1 100644
--- a/src/tests/hil/test-hil.cpp
+++ b/src/tests/hil/test-hil.cpp
@@ -1,4 +1,4 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
+/* Copyright (c) 2024 Skyward Experimental Rocketry
* Author: Emilio Corigliano
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -20,6 +20,7 @@
* THE SOFTWARE.
*/
+#include <algorithms/AirBrakes/TrajectoryPoint.h>
#include <diagnostic/CpuMeter/CpuMeter.h>
#include <diagnostic/PrintLogger.h>
#include <diagnostic/StackLogger.h>
@@ -27,10 +28,7 @@
#include <events/EventBroker.h>
#include <events/EventData.h>
#include <events/utils/EventSniffer.h>
-#include <hil/Events.h>
#include <hil/HIL.h>
-#include <hil/HILConfig.h>
-#include <hil/HILFlightPhasesManager.h>
#include <scheduler/TaskScheduler.h>
#include <utils/ModuleManager/ModuleManager.hpp>
@@ -38,23 +36,62 @@
#include "HILSimulationConfig.h"
using namespace Boardcore;
-using namespace Common;
using namespace HILConfig;
+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
- USART usart2(USART2, 115200);
- HIL* hil = new HIL(usart2, new HILFlightPhasesManager());
- Boardcore::TaskScheduler scheduler;
+ USART usart2(USART2, SIM_BAUDRATE);
+
+ // Create hil modules
+ auto* hilTransceiver = new MainHILTransceiver(usart2);
+ auto* hilPhasesManager = new MainHILPhasesManager(
+ []() { return Boardcore::TimedTrajectoryPoint(); });
+
+ auto* sensorData = hilTransceiver->getSensorData();
+
+ auto* accelerometer = new MainHILAccelerometer(&sensorData->accelerometer);
+ auto* gyroscope = new MainHILGyroscope(&sensorData->gyro);
+ auto* magnetometer = new MainHILMagnetometer(&sensorData->magnetometer);
+ auto* gps = new MainHILGps(&sensorData->gps);
+ auto* barometer1 = new MainHILBarometer(&sensorData->barometer1);
+ auto* barometer2 = new MainHILBarometer(&sensorData->barometer2);
+ auto* barometer3 = new MainHILBarometer(&sensorData->barometer3);
+ auto* baroChamber = new MainHILBarometer(&sensorData->pressureChamber);
+ auto* pitot = new MainHILPitot(&sensorData->pitot);
+ auto* temperature = new MainHILTemperature(&sensorData->temperature);
+
+ // Create HIL class where we specify how to use previous modules to assemble
+ // ActuatorData
+ MainHIL* hil = new MainHIL(
+ hilTransceiver, hilPhasesManager,
+ [&]()
+ {
+ auto actuatorData = ActuatorData();
+
+ // ada
+ // const auto gpsSample = gps->getLastSample();
+ // actuatorData.adaState.aglAltitude = gpsSample.height;
+ // actuatorData.adaState.mslAltitude = gpsSample.height - 160;
+ // actuatorData.adaState.verticalSpeed = -gpsSample.velocityDown;
+ // actuatorData.adaState.apogeeDetected = static_cast<float>(false);
+ // actuatorData.adaState.updating = static_cast<float>(true);
+
+ actuatorData.flags = sensorData->flags;
+ return actuatorData;
+ },
+ SIMULATION_PERIOD);
// Insert modules
- if (!ModuleManager::getInstance().insert<HIL>(hil))
+ if (!ModuleManager::getInstance().insert<MainHIL>(hil))
{
initResult = false;
LOG_ERR(logger, "Error inserting the HIL module");
@@ -73,40 +110,16 @@ int main()
LOG_ERR(logger, "Error starting the board scheduler module");
}
- if (!ModuleManager::getInstance().get<HIL>()->start())
+ if (HIL_TEST)
{
- initResult = false;
- LOG_ERR(logger, "Error starting the HIL module");
- }
-
- scheduler.addTask(
- [&]()
+ if (!ModuleManager::getInstance().get<MainHIL>()->start())
{
- HILConfig::SimulatorData* sensorData =
- ModuleManager::getInstance()
- .get<HIL>()
- ->simulator->getSensorData();
- HILConfig::ADAdataHIL adaDataHil{
- Boardcore::TimestampTimer::getTimestamp(),
- sensorData->gps.positionMeasures[0][2],
- sensorData->gps.velocityMeasures[0][2]};
- HILConfig::ActuatorData actuatorData{
- NASState{},
- adaDataHil,
- 0,
- 30,
- sensorData->flags.flag_ascent,
- sensorData->flags.flag_burning};
-
- // Sending to the simulator
- ModuleManager::getInstance().get<HIL>()->send(actuatorData);
- },
- HILConfig::SIMULATION_PERIOD);
+ initResult = false;
+ LOG_ERR(logger, "Error starting the HIL module");
+ }
- // scheduler.addTask(
- // [&]() {
- // ModuleManager::getInstance().get<HIL>()->simulator->getSensorData()->print();
- // }, 1000);
+ ModuleManager::getInstance().get<MainHIL>()->waitStartSimulation();
+ }
// Check the init result and launch an event
if (initResult)