From b82e9c20f3ced8e71573d46bb2339fea27d26ff7 Mon Sep 17 00:00:00 2001 From: Giulia Facchi <giulia.facchi@skywarder.eu> Date: Sun, 17 Dec 2023 21:51:15 +0100 Subject: [PATCH] [Payload][HIL] Modified entrypoint for hil --- src/entrypoints/Payload/payload-entry.cpp | 119 ++++++++++------------ 1 file changed, 51 insertions(+), 68 deletions(-) diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp index 7247b9862..2a15d9a07 100644 --- a/src/entrypoints/Payload/payload-entry.cpp +++ b/src/entrypoints/Payload/payload-entry.cpp @@ -258,6 +258,57 @@ int main() LOG_ERR(logger, "Error inserting the FlightStatsRecorder module"); } +#ifdef HILPayload + if (!modules.get<HIL>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error inserting the HIL module"); + } + else + { + LOG_INFO(logger, "Started the HIL module"); + } + + hil->flightPhasesManager->setCurrentPositionSource( + [&]() { return Boardcore::TimedTrajectoryPoint(nas->getNasState()); }); + + hil->flightPhasesManager->registerToFlightPhase( + FlightPhases::ARMED, + [&]() + { + EventBroker::getInstance().post(Events::TMTC_FORCE_LAUNCH, + Topics::TOPIC_TMTC); + miosix::ledOn(); + }); + + hil->flightPhasesManager->registerToFlightPhase( + FlightPhases::SIMULATION_STARTED, + [&]() + { + EventBroker::getInstance().post(Events::TMTC_CALIBRATE, + Topics::TOPIC_TMTC); + + hil->flightPhasesManager->registerToFlightPhase( + FlightPhases::CALIBRATION_OK, + [&]() + { + TRACE("ARM COMMAND SENT\n"); + EventBroker::getInstance().post(Events::TMTC_ARM, + Topics::TOPIC_TMTC); + }); + }); + + bool simulation_started = false; + + hil->flightPhasesManager->registerToFlightPhase( + FlightPhases::SIMULATION_STARTED, [&]() { simulation_started = true; }); + + while (!simulation_started) + { + Thread::sleep(HILConfig::SIMULATION_PERIOD); + } +#endif + // Start modules if (!Logger::getInstance().start()) { @@ -349,61 +400,6 @@ int main() LOG_ERR(logger, "Error starting the Board Scheduler module"); } -#ifdef HILPayload - if (!modules.get<HIL>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error inserting the HIL module"); - } - else - { - LOG_INFO(logger, "Started the HIL module"); - } - - hil->flightPhasesManager->setCurrentPositionSource( - [&]() { return Boardcore::TimedTrajectoryPoint(nas->getNasState()); }); - - hil->flightPhasesManager->registerToFlightPhase( - FlightPhases::SIMULATION_STARTED, - [&]() - { - Thread::sleep(2000); - EventBroker::getInstance().post(Events::TMTC_CALIBRATE, - Topics::TOPIC_TMTC); - Thread::sleep(4000); - EventBroker::getInstance().post(Events::TMTC_ARM, - Topics::TOPIC_TMTC); - printf("ARM COMMAND SENT\n"); - }); - - hil->flightPhasesManager->registerToFlightPhase( - FlightPhases::SIM_FLYING, - [&]() - { - EventBroker::getInstance().post(Events::TMTC_FORCE_LAUNCH, - Topics::TOPIC_FLIGHT); - - hil->flightPhasesManager->registerToFlightPhase( - FlightPhases::SIM_FLYING, - [&]() - { - EventBroker::getInstance().post( - Events::FLIGHT_MISSION_TIMEOUT, Topics::TOPIC_FLIGHT); - }); - - miosix::userLed4::high(); - }); - - hil->flightPhasesManager->registerToFlightPhase( - FlightPhases::SIM_PARA1, - [&]() - { - // FLIGHT_NC_DETACHED - EventBroker::getInstance().post(Events::CAN_APOGEE_DETECTED, - Topics::TOPIC_CAN); - }); -#endif - // Log all the events EventSniffer sniffer( EventBroker::getInstance(), TOPICS_LIST, @@ -429,19 +425,6 @@ int main() } #ifdef HILPayload - // bool simulation_started = false; - - // hil->flightPhasesManager->registerToFlightPhase( - // FlightPhases::SIMULATION_STARTED, [&]() { simulation_started = true; - // }); - - // while (!simulation_started) - // { - // HILConfig::ActuatorData actuatorData; - // buses->usart2.write(&actuatorData, sizeof(HILConfig::ActuatorData)); - // Thread::sleep(HILConfig::SIMULATION_PERIOD); - // } - modules.get<BoardScheduler>() ->getScheduler(miosix::PRIORITY_MAX - 1) ->addTask( -- GitLab