From 3d46d7131edeba9c6d679c01e2d314c51bc303bb Mon Sep 17 00:00:00 2001 From: Emilio Corigliano <emilio.corigliano@skywarder.eu> Date: Sat, 24 Aug 2024 18:08:28 +0200 Subject: [PATCH] [Payload][HIL] Initial implementation of HIL simulation support - Introduced support for the HIL framework in the Payload board - Wait for simulation start before starting sensors - Introduce a persistent flag to reboot into HIL mode - Initialize Mavlink over serial only if not in HIL mode --- cmake/dependencies.cmake | 2 + src/boards/Payload/CanHandler/CanHandler.cpp | 2 +- .../Payload/Configs/HILSimulationConfig.h | 79 +++++ src/boards/Payload/HIL/HIL.cpp | 287 ++++++++++++++++++ src/boards/Payload/HIL/HIL.h | 85 ++++++ src/boards/Payload/HIL/HILData.h | 259 ++++++++++++++++ .../Payload/PersistentVars/PersistentVars.cpp | 48 +++ .../Payload/PersistentVars/PersistentVars.h | 37 +++ src/boards/Payload/Radio/MessageHandler.cpp | 2 +- src/boards/Payload/Radio/Radio.cpp | 6 +- src/boards/Payload/Radio/Radio.h | 1 + src/boards/Payload/Sensors/HILSensors.h | 253 +++++++++++++++ .../FlightModeManager/FlightModeManager.cpp | 44 +++ .../FlightModeManager/FlightModeManager.h | 1 + src/entrypoints/Payload/payload-entry.cpp | 31 +- 15 files changed, 1130 insertions(+), 7 deletions(-) create mode 100644 src/boards/Payload/Configs/HILSimulationConfig.h create mode 100644 src/boards/Payload/HIL/HIL.cpp create mode 100644 src/boards/Payload/HIL/HIL.h create mode 100644 src/boards/Payload/HIL/HILData.h create mode 100644 src/boards/Payload/PersistentVars/PersistentVars.cpp create mode 100644 src/boards/Payload/PersistentVars/PersistentVars.h create mode 100644 src/boards/Payload/Sensors/HILSensors.h diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 240c2798b..cf03a37d9 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -82,7 +82,9 @@ set(PAYLOAD_COMPUTER src/boards/Payload/Actuators/Actuators.cpp src/boards/Payload/CanHandler/CanHandler.cpp src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp + src/boards/Payload/HIL/HIL.cpp src/boards/Payload/Sensors/Sensors.cpp + src/boards/Payload/PersistentVars/PersistentVars.cpp src/boards/Payload/PinHandler/PinHandler.cpp src/boards/Payload/Radio/Radio.cpp src/boards/Payload/Radio/MessageHandler.cpp diff --git a/src/boards/Payload/CanHandler/CanHandler.cpp b/src/boards/Payload/CanHandler/CanHandler.cpp index 549e05d6e..14eb457bd 100644 --- a/src/boards/Payload/CanHandler/CanHandler.cpp +++ b/src/boards/Payload/CanHandler/CanHandler.cpp @@ -115,7 +115,7 @@ bool CanHandler::start() .logNumber = static_cast<int16_t>(logStats.logNumber), .state = static_cast<uint8_t>(state), .armed = state == FlightModeManagerState::ARMED, - .hil = false, // TODO: hil + .hil = PersistentVars::getHilMode(), .logGood = logStats.lastWriteError == 0, }; diff --git a/src/boards/Payload/Configs/HILSimulationConfig.h b/src/boards/Payload/Configs/HILSimulationConfig.h new file mode 100644 index 000000000..18ba5b5fa --- /dev/null +++ b/src/boards/Payload/Configs/HILSimulationConfig.h @@ -0,0 +1,79 @@ +/* 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 <units/Frequency.h> +#include <units/Units.h> + +#include <chrono> + +#include "SensorsConfig.h" + +namespace Payload +{ +namespace Config +{ +namespace HIL +{ + +/* linter off */ using namespace Boardcore::Units::Frequency; + +constexpr bool ENABLE_HW = false; + +// Period of simulation [ms] +constexpr auto SIMULATION_RATE = 10_hz; + +// Number of samples per sensor at each simulator iteration +constexpr int N_DATA_ACCEL = 10; // #samples +constexpr int N_DATA_GYRO = 10; // #samples +constexpr int N_DATA_MAGNETO = 10; // #samples +constexpr int N_DATA_GPS = 1; // #samples +constexpr int N_DATA_BARO_STATIC = 10; // #samples +constexpr int N_DATA_BARO_PITOT = 10; // #samples +constexpr int N_DATA_TEMP = 1; // #samples + +// clang-format off +// Checking if the data coming from simulator is enough +static_assert(N_DATA_ACCEL * SIMULATION_RATE >= Sensors::LSM6DSRX::SAMPLING_RATE, + "N_DATA_ACCEL not enough"); +static_assert(N_DATA_GYRO * SIMULATION_RATE >= Sensors::LSM6DSRX::SAMPLING_RATE, + "N_DATA_GYRO not enough"); +static_assert(N_DATA_MAGNETO * SIMULATION_RATE >= Sensors::LIS2MDL::SAMPLING_RATE, + "N_DATA_MAGNETO not enough"); +static_assert(N_DATA_GPS * SIMULATION_RATE >= Sensors::UBXGPS::SAMPLING_RATE, + "N_DATA_GPS not enough"); +static_assert(N_DATA_BARO_STATIC * SIMULATION_RATE >= Sensors::ADS131M08::SAMPLING_RATE, + "N_DATA_BARO_STATIC not enough"); +static_assert(N_DATA_BARO_STATIC * SIMULATION_RATE >= Sensors::LPS22DF::SAMPLING_RATE, + "N_DATA_BARO_STATIC not enough"); +static_assert(N_DATA_BARO_STATIC * SIMULATION_RATE >= Sensors::LPS28DFW::SAMPLING_RATE, + "N_DATA_BARO_STATIC not enough"); +static_assert(N_DATA_BARO_PITOT * SIMULATION_RATE >= Sensors::StaticPressure::SAMPLING_RATE, + "N_DATA_BARO_PITOT not enough"); +static_assert(N_DATA_BARO_PITOT * SIMULATION_RATE >= Sensors::DynamicPressure::SAMPLING_RATE, + "N_DATA_BARO_PITOT not enough"); +// clang-format on + +} // namespace HIL +} // namespace Config +} // namespace Payload diff --git a/src/boards/Payload/HIL/HIL.cpp b/src/boards/Payload/HIL/HIL.cpp new file mode 100644 index 000000000..560faeb89 --- /dev/null +++ b/src/boards/Payload/HIL/HIL.cpp @@ -0,0 +1,287 @@ +/* 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 "HIL.h" + +#include <Payload/Actuators/Actuators.h> +#include <Payload/Buses.h> +#include <Payload/Configs/HILSimulationConfig.h> +#include <Payload/Configs/SensorsConfig.h> +#include <Payload/HIL/HILData.h> +#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Payload/StateMachines/NASController/NASController.h> +#include <Payload/StateMachines/WingController/WingController.h> +#include <Payload/WindEstimationScheme/WindEstimation.h> +#include <common/Events.h> +#include <events/EventBroker.h> +#include <hil/HIL.h> +#include <interfaces-impl/hwmapping.h> +#include <utils/DependencyManager/DependencyManager.h> + +#include "HILData.h" + +namespace Payload +{ + +PayloadHILPhasesManager::PayloadHILPhasesManager( + std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition) + : HILPhasesManager<PayloadFlightPhases, SimulatorData, ActuatorData>( + getCurrentPosition) +{ + flagsFlightPhases = {{PayloadFlightPhases::SIMULATION_STARTED, false}, + {PayloadFlightPhases::INITIALIZED, false}, + {PayloadFlightPhases::CALIBRATION, false}, + {PayloadFlightPhases::CALIBRATION_OK, false}, + {PayloadFlightPhases::ARMED, false}, + {PayloadFlightPhases::LIFTOFF_PIN_DETACHED, false}, + {PayloadFlightPhases::LIFTOFF, false}, + {PayloadFlightPhases::MOTOR_SHUTDOWN, false}, + {PayloadFlightPhases::AEROBRAKES, false}, + {PayloadFlightPhases::APOGEE, false}, + {PayloadFlightPhases::PARA1, false}, + {PayloadFlightPhases::PARA2, false}, + {PayloadFlightPhases::SIMULATION_STOPPED, false}}; + + prev_flagsFlightPhases = flagsFlightPhases; + + // Subscribe to all the topics + auto& eventBroker = Boardcore::EventBroker::getInstance(); + eventBroker.subscribe(this, Common::TOPIC_ABK); + eventBroker.subscribe(this, Common::TOPIC_ADA); + 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 PayloadHILPhasesManager::processFlagsImpl( + const SimulatorData& simulatorData, + std::vector<PayloadFlightPhases>& changed_flags) +{ + if (simulatorData.signal == + static_cast<float>(HILSignal::SIMULATION_STARTED)) + { + miosix::reboot(); + } + + if (simulatorData.signal == + static_cast<float>(HILSignal::SIMULATION_STOPPED)) + { + Boardcore::EventBroker::getInstance().post(Common::TMTC_FORCE_LANDING, + Common::TOPIC_TMTC); + } + + if (simulatorData.signal == + static_cast<float>(HILSignal::SIMULATION_FORCE_LAUNCH)) + { + Boardcore::EventBroker::getInstance().post(Common::TMTC_ARM, + Common::TOPIC_TMTC); + Thread::sleep(250); + Boardcore::EventBroker::getInstance().post(Common::TMTC_FORCE_LAUNCH, + Common::TOPIC_TMTC); + } + + // set true when the first packet from the simulator arrives + if (isSetTrue(PayloadFlightPhases::SIMULATION_STARTED)) + { + t_start = Boardcore::TimestampTimer::getTimestamp(); + + printf("[HIL] ------- SIMULATION STARTED ! ------- \n"); + changed_flags.push_back(PayloadFlightPhases::SIMULATION_STARTED); + } +} + +void PayloadHILPhasesManager::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 shutdown: \n"); + outcomes[PayloadFlightPhases::MOTOR_SHUTDOWN].print(t_liftoff); + + printf("Apogee: \n"); + outcomes[PayloadFlightPhases::APOGEE].print(t_liftoff); + + printf("Parachute 1: \n"); + outcomes[PayloadFlightPhases::PARA1].print(t_liftoff); + + printf("Parachute 2: \n"); + outcomes[PayloadFlightPhases::PARA2].print(t_liftoff); + + printf("Simulation Stopped: \n"); + outcomes[PayloadFlightPhases::SIMULATION_STOPPED].print(t_liftoff); +} + +void PayloadHILPhasesManager::handleEventImpl( + const Boardcore::Event& e, std::vector<PayloadFlightPhases>& changed_flags) +{ + switch (e) + { + case Common::Events::FMM_INIT_ERROR: + printf("[HIL] ------- INIT FAILED ! ------- \n"); + + case Common::Events::FMM_INIT_OK: + setFlagFlightPhase(PayloadFlightPhases::INITIALIZED, true); + TRACE("[HIL] ------- INITIALIZED ! ------- \n"); + changed_flags.push_back(PayloadFlightPhases::INITIALIZED); + break; + case Common::Events::FMM_CALIBRATE: + case Common::Events::CAN_CALIBRATE: + case Common::Events::TMTC_CALIBRATE: + setFlagFlightPhase(PayloadFlightPhases::CALIBRATION, true); + TRACE("[HIL] ------- CALIBRATION ! ------- \n"); + changed_flags.push_back(PayloadFlightPhases::CALIBRATION); + break; + case Common::Events::FLIGHT_DISARMED: + setFlagFlightPhase(PayloadFlightPhases::CALIBRATION_OK, true); + TRACE("[HIL] ------- CALIBRATION OK ! ------- \n"); + changed_flags.push_back(PayloadFlightPhases::CALIBRATION_OK); + break; + case Common::Events::FLIGHT_ARMED: + setFlagFlightPhase(PayloadFlightPhases::ARMED, true); + printf("[HIL] ------- READY TO LAUNCH ! ------- \n"); + changed_flags.push_back(PayloadFlightPhases::ARMED); + break; + case Common::Events::FLIGHT_LAUNCH_PIN_DETACHED: + setFlagFlightPhase(PayloadFlightPhases::LIFTOFF_PIN_DETACHED, true); + TRACE("[HIL] ------- LIFTOFF PIN DETACHED ! ------- \n"); + changed_flags.push_back(PayloadFlightPhases::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(PayloadFlightPhases::LIFTOFF); + break; + case Common::Events::FLIGHT_MOTOR_SHUTDOWN: + setFlagFlightPhase(PayloadFlightPhases::MOTOR_SHUTDOWN, true); + registerOutcomes(PayloadFlightPhases::MOTOR_SHUTDOWN); + printf("[HIL] ------- MOTOR SHUTDOWN ! ------- %f, %f \n", + getCurrentPosition().z, getCurrentPosition().vz); + changed_flags.push_back(PayloadFlightPhases::MOTOR_SHUTDOWN); + break; + case Common::Events::FLIGHT_APOGEE_DETECTED: + case Common::Events::CAN_APOGEE_DETECTED: + case Common::Events::TMTC_FORCE_APOGEE: + setFlagFlightPhase(PayloadFlightPhases::AEROBRAKES, false); + registerOutcomes(PayloadFlightPhases::APOGEE); + printf("[HIL] ------- APOGEE DETECTED ! ------- %f, %f \n", + getCurrentPosition().z, getCurrentPosition().vz); + changed_flags.push_back(PayloadFlightPhases::APOGEE); + break; + case Common::Events::FLIGHT_DROGUE_DESCENT: + case Common::Events::TMTC_FORCE_EXPULSION: + setFlagFlightPhase(PayloadFlightPhases::PARA1, true); + registerOutcomes(PayloadFlightPhases::PARA1); + printf("[HIL] ------- PARA1 ! -------%f, %f \n", + getCurrentPosition().z, getCurrentPosition().vz); + changed_flags.push_back(PayloadFlightPhases::PARA1); + break; + case Common::Events::FLIGHT_WING_DESCENT: + case Common::Events::FLIGHT_DPL_ALT_DETECTED: + case Common::Events::TMTC_FORCE_DEPLOYMENT: + setFlagFlightPhase(PayloadFlightPhases::PARA1, false); + setFlagFlightPhase(PayloadFlightPhases::PARA2, true); + registerOutcomes(PayloadFlightPhases::PARA2); + printf("[HIL] ------- PARA2 ! ------- %f, %f \n", + getCurrentPosition().z, getCurrentPosition().vz); + changed_flags.push_back(PayloadFlightPhases::PARA2); + break; + case Common::Events::FLIGHT_LANDING_DETECTED: + case Common::Events::TMTC_FORCE_LANDING: + t_stop = Boardcore::TimestampTimer::getTimestamp(); + setFlagFlightPhase(PayloadFlightPhases::PARA2, false); + setFlagFlightPhase(PayloadFlightPhases::SIMULATION_STOPPED, true); + changed_flags.push_back(PayloadFlightPhases::SIMULATION_STOPPED); + registerOutcomes(PayloadFlightPhases::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()); + } +} + +PayloadHIL::PayloadHIL() + : Boardcore::HIL<PayloadFlightPhases, SimulatorData, ActuatorData>( + nullptr, nullptr, [this]() { return updateActuatorData(); }, + 1000 / Config::HIL::SIMULATION_RATE.value()) +{ +} + +bool PayloadHIL::start() +{ + auto* nas = getModule<Payload::NASController>(); + auto& hilUsart = getModule<Payload::Buses>()->HILUart(); + + hilPhasesManager = new PayloadHILPhasesManager( + [nas]() + { return Boardcore::TimedTrajectoryPoint(nas->getNasState()); }); + + hilTransceiver = new PayloadHILTransceiver(hilUsart, hilPhasesManager); + + return Boardcore::HIL<PayloadFlightPhases, SimulatorData, + ActuatorData>::start(); +} + +ActuatorData PayloadHIL::updateActuatorData() +{ + auto nas = getModule<Payload::NASController>(); + auto wes = getModule<Payload::WindEstimation>(); + auto fmm = getModule<Payload::FlightModeManager>(); + auto wing = getModule<Payload::WingController>(); + auto actuators = getModule<Payload::Actuators>(); + + NASStateHIL nasStateHIL(nas->getNasState()); + + ActuatorsStateHIL actuatorsStateHIL( + actuators->getServoPosition(ServosList::PARAFOIL_LEFT_SERVO), + actuators->getServoPosition(ServosList::PARAFOIL_RIGHT_SERVO), + static_cast<float>(miosix::gpios::mainDeploy::value())); + + WESDataHIL wesDataHIL(wes->getWindEstimationScheme()); + + auto deltaA = actuators->getServoPosition(ServosList::PARAFOIL_LEFT_SERVO) - + actuators->getServoPosition(ServosList::PARAFOIL_RIGHT_SERVO); + + Eigen::Vector2f heading; + + auto psiRef = wing->calculateTargetAngle( + {nasStateHIL.n, nasStateHIL.e, nasStateHIL.d}, heading); + + GuidanceDataHIL guidanceData(psiRef, deltaA, heading[0], heading[1]); + + // Returning the feedback for the simulator + return ActuatorData( + nasStateHIL, actuatorsStateHIL, wesDataHIL, guidanceData, + (fmm->testState(&Payload::FlightModeManager::FlyingAscending) ? 3 : 0)); +}; +} // namespace Payload \ No newline at end of file diff --git a/src/boards/Payload/HIL/HIL.h b/src/boards/Payload/HIL/HIL.h new file mode 100644 index 000000000..72bd8b5a2 --- /dev/null +++ b/src/boards/Payload/HIL/HIL.h @@ -0,0 +1,85 @@ +/* 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 <Payload/Actuators/Actuators.h> +#include <Payload/Buses.h> +#include <Payload/Configs/HILSimulationConfig.h> +#include <Payload/HIL/HILData.h> +#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Payload/StateMachines/NASController/NASController.h> +#include <Payload/StateMachines/WingController/WingController.h> +#include <Payload/WindEstimationScheme/WindEstimation.h> +#include <common/Events.h> +#include <hil/HIL.h> +#include <utils/DependencyManager/DependencyManager.h> + +namespace Payload +{ + +class PayloadHILTransceiver + : public Boardcore::HILTransceiver<PayloadFlightPhases, SimulatorData, + ActuatorData> +{ + using Boardcore::HILTransceiver<PayloadFlightPhases, SimulatorData, + ActuatorData>::HILTransceiver; +}; + +class PayloadHILPhasesManager + : public Boardcore::HILPhasesManager<PayloadFlightPhases, SimulatorData, + ActuatorData> +{ +public: + explicit PayloadHILPhasesManager( + std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition); + + void processFlagsImpl( + const SimulatorData& simulatorData, + std::vector<PayloadFlightPhases>& changed_flags) override; + + void printOutcomes(); + +private: + void handleEventImpl( + const Boardcore::Event& e, + std::vector<PayloadFlightPhases>& changed_flags) override; +}; + +class PayloadHIL + : public Boardcore::HIL<PayloadFlightPhases, SimulatorData, ActuatorData>, + public Boardcore::InjectableWithDeps< + Payload::Buses, Payload::Actuators, Payload::FlightModeManager, + Payload::WindEstimation, Payload::WingController, + Payload::NASController> +{ + +public: + PayloadHIL(); + + bool start() override; + +private: + ActuatorData updateActuatorData(); +}; + +} // namespace Payload \ No newline at end of file diff --git a/src/boards/Payload/HIL/HILData.h b/src/boards/Payload/HIL/HILData.h new file mode 100644 index 000000000..71f0f04d9 --- /dev/null +++ b/src/boards/Payload/HIL/HILData.h @@ -0,0 +1,259 @@ +/* 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 <Payload/Configs/HILSimulationConfig.h> +#include <sensors/HILSimulatorData.h> + +// NAS +#include <Payload/StateMachines/NASController/NASControllerData.h> +#include <algorithms/NAS/NASState.h> + +// WingController +#include <Payload/StateMachines/WingController/WingControllerData.h> + +namespace Payload +{ + +// Sensors Data +using PayloadAccelerometerSimulatorData = + Boardcore::AccelerometerSimulatorData<Config::HIL::N_DATA_ACCEL>; +using PayloadGyroscopeSimulatorData = + Boardcore::GyroscopeSimulatorData<Config::HIL::N_DATA_GYRO>; +using PayloadMagnetometerSimulatorData = + Boardcore::MagnetometerSimulatorData<Config::HIL::N_DATA_MAGNETO>; +using PayloadGPSSimulatorData = + Boardcore::GPSSimulatorData<Config::HIL::N_DATA_GPS>; +using PayloadDigitalBarometerSimulatorData = + Boardcore::BarometerSimulatorData<Config::HIL::N_DATA_BARO_STATIC>; +using PayloadAnalogBarometerSimulatorData = + Boardcore::BarometerSimulatorData<Config::HIL::N_DATA_BARO_PITOT>; +using PayloadTemperatureSimulatorData = + Boardcore::TemperatureSimulatorData<Config::HIL::N_DATA_TEMP>; + +enum class HILSignal : int +{ + SIMULATION_STARTED = 1, + SIMULATION_STOPPED = 2, + SIMULATION_FORCE_LAUNCH = 3 +}; + +enum class PayloadFlightPhases +{ + SIMULATION_STARTED, + INITIALIZED, + CALIBRATION, + CALIBRATION_OK, + ARMED, + LIFTOFF_PIN_DETACHED, + LIFTOFF, + MOTOR_SHUTDOWN, + AEROBRAKES, + APOGEE, + PARA1, + PARA2, + SIMULATION_STOPPED +}; + +/** + * @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(0), + updating(0) + { + } + + explicit NASStateHIL(const Boardcore::NASState& nasState) + : n(nasState.n), e(nasState.e), d(nasState.d), vn(nasState.vn), + ve(nasState.ve), vd(nasState.vd), qx(nasState.qx), qy(nasState.qy), + qz(nasState.qz), qw(nasState.qw), updating(1.f) // To remove updating + { + } + + 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); + } +}; + +struct ActuatorsStateHIL +{ + float parafoilLeftPercentage = 0; + float parafoilRightPercentage = 0; + float cutterState = 0; + + ActuatorsStateHIL() + : parafoilLeftPercentage(0.0f), parafoilRightPercentage(0.0f), + cutterState(0.0f) + { + } + + ActuatorsStateHIL(float parafoilLeftPercentage, + float parafoilRightPercentage, float cutterState) + : parafoilLeftPercentage(parafoilLeftPercentage), + parafoilRightPercentage(parafoilRightPercentage), + cutterState(cutterState) + { + } + + void print() + { + printf( + "parafoilLeft: %f perc\n" + "parafoilRight: %f perc\n" + "cutterState: %f perc\n", + parafoilLeftPercentage * 100, parafoilRightPercentage * 100, + cutterState * 100); + } +}; + +struct WESDataHIL +{ + float windX; + float windY; + + explicit WESDataHIL(const Eigen::Vector2f& wind) + : windX(wind[0]), windY(wind[1]) + { + } + + WESDataHIL() : windX(0.0f), windY(0.0f) {} + + void print() { printf("wind: [%f,%f]\n", windX, windY); } +}; + +struct GuidanceDataHIL +{ + float psiRef; + float deltaA; + float currentTargetN; + float currentTargetE; + + GuidanceDataHIL(float psiRef, float deltaA, float currentTargetN, + float currentTargetE) + : psiRef(psiRef), deltaA(deltaA), currentTargetN(currentTargetN), + currentTargetE(currentTargetE) + { + } + + GuidanceDataHIL() + : psiRef(0.0f), deltaA(0.0f), currentTargetN(0), currentTargetE(0) + { + } + + void print() + { + printf( + "psiRef: %f\n" + "deltaA: %f\n" + "currentTargetN: %f\n" + "currentTargetE: %f\n", + psiRef, deltaA, currentTargetN, currentTargetE); + } +}; + +/** + * @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 +{ + PayloadAccelerometerSimulatorData accelerometer; + PayloadGyroscopeSimulatorData gyro; + PayloadMagnetometerSimulatorData magnetometer; + PayloadGPSSimulatorData gps; + PayloadDigitalBarometerSimulatorData barometer1, barometer2, barometer3; + PayloadAnalogBarometerSimulatorData staticPitot, dynamicPitot; + PayloadTemperatureSimulatorData temperature; + float signal; +}; + +/** + * @brief Data strudcture expected by the simulator + */ +struct ActuatorData +{ + NASStateHIL nasState; + ActuatorsStateHIL actuatorsState; + WESDataHIL wesData; + GuidanceDataHIL guidanceData; + float signal; + + ActuatorData() + : nasState(), actuatorsState(), wesData(), guidanceData(), signal(0) + { + } + + ActuatorData(const NASStateHIL& nasState, + const ActuatorsStateHIL& actuatorsState, + const WESDataHIL& wesData, const GuidanceDataHIL& guidanceData, + float signal) + : nasState(nasState), actuatorsState(actuatorsState), wesData(wesData), + guidanceData(guidanceData), signal(signal) + { + } + + void print() + { + nasState.print(); + actuatorsState.print(); + wesData.print(); + guidanceData.print(); + } +}; +} // namespace Payload \ No newline at end of file diff --git a/src/boards/Payload/PersistentVars/PersistentVars.cpp b/src/boards/Payload/PersistentVars/PersistentVars.cpp new file mode 100644 index 000000000..4d59bae1a --- /dev/null +++ b/src/boards/Payload/PersistentVars/PersistentVars.cpp @@ -0,0 +1,48 @@ +/* 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 "PersistentVars.h" + +#include <arch/common/drivers/stm32_bsram.h> +#include <miosix.h> + +using namespace miosix; + +static bool PRESERVE hilMode = false; + +namespace Payload +{ + +namespace PersistentVars +{ + +void setHilMode(bool _hilMode) +{ + BSRAM::EnableWriteLock l; + hilMode = _hilMode; +} + +bool getHilMode() { return hilMode; } + +} // namespace PersistentVars + +} // namespace Payload \ No newline at end of file diff --git a/src/boards/Payload/PersistentVars/PersistentVars.h b/src/boards/Payload/PersistentVars/PersistentVars.h new file mode 100644 index 000000000..9f2ed5db5 --- /dev/null +++ b/src/boards/Payload/PersistentVars/PersistentVars.h @@ -0,0 +1,37 @@ +/* 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 + +namespace Payload +{ + +namespace PersistentVars +{ + +void setHilMode(bool _hilMode); + +bool getHilMode(); + +} // namespace PersistentVars + +} // namespace Payload \ No newline at end of file diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp index cefef485c..11bae58af 100644 --- a/src/boards/Payload/Radio/MessageHandler.cpp +++ b/src/boards/Payload/Radio/MessageHandler.cpp @@ -613,7 +613,7 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId) tm.motor_can_status = canStatus.isMotorConnected(); tm.rig_can_status = canStatus.isRigConnected(); - tm.hil_state = 0; // TODO: hil + tm.hil_state = PersistentVars::getHilMode(); mavlink_msg_payload_stats_tm_encode(config::Mavlink::SYSTEM_ID, config::Mavlink::COMPONENT_ID, diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp index 8c1566d09..f82d18870 100644 --- a/src/boards/Payload/Radio/Radio.cpp +++ b/src/boards/Payload/Radio/Radio.cpp @@ -98,7 +98,9 @@ bool Radio::start() return false; } - if (config::MAVLINK_OVER_HIL_SERIAL_ENABLED) + // Enable Mavlink over HIL USART only when HIL is not active + if (config::MAVLINK_OVER_HIL_SERIAL_ENABLED && + !PersistentVars::getHilMode()) { initMavlinkOverSerial(); } @@ -132,8 +134,6 @@ bool Radio::isStarted() { return started; } void Radio::initMavlinkOverSerial() { - // Send Mavlink messages over the HIL USART when HIL is not active - // TODO: hil - don't use serial if hil is active serialTransceiver = std::make_unique<SerialTransceiver>(getModule<Buses>()->HILUart()); diff --git a/src/boards/Payload/Radio/Radio.h b/src/boards/Payload/Radio/Radio.h index 637c46629..6353e4d72 100644 --- a/src/boards/Payload/Radio/Radio.h +++ b/src/boards/Payload/Radio/Radio.h @@ -23,6 +23,7 @@ #pragma once #include <Payload/Configs/RadioConfig.h> +#include <Payload/PersistentVars/PersistentVars.h> #include <common/Mavlink.h> #include <radio/MavlinkDriver/MavlinkDriver.h> #include <radio/SX1278/SX1278Fsk.h> diff --git a/src/boards/Payload/Sensors/HILSensors.h b/src/boards/Payload/Sensors/HILSensors.h new file mode 100644 index 000000000..0ae5ab621 --- /dev/null +++ b/src/boards/Payload/Sensors/HILSensors.h @@ -0,0 +1,253 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: 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 <Payload/HIL/HIL.h> +#include <common/CanConfig.h> +#include <common/ReferenceConfig.h> +#include <drivers/timer/TimestampTimer.h> +#include <sensors/HILSensor.h> +#include <sensors/Sensor.h> + +#include "Sensors.h" + +namespace Payload +{ + +class HILSensors + : public Boardcore::InjectableWithDeps<Boardcore::InjectableBase<Sensors>, + PayloadHIL> +{ +public: + explicit HILSensors(bool enableHw) : Super{}, enableHw{enableHw} {} + +private: + bool postSensorCreationHook() override + { + using namespace Boardcore; + + hillificator<>(lps22df, enableHw, + [this]() { return updateLPS22DFData(); }); + hillificator<>(lps28dfw, enableHw, + [this]() { return updateLPS28DFWData(); }); + hillificator<>(h3lis331dl, enableHw, + [this]() { return updateH3LIS331DLData(); }); + hillificator<>(lis2mdl, enableHw, + [this]() { return updateLIS2MDLData(); }); + hillificator<>(ubxgps, enableHw, + [this]() { return updateUBXGPSData(); }); + hillificator<>(lsm6dsrx, enableHw, + [this]() { return updateLSM6DSRXData(); }); + hillificator<>(staticPressure, enableHw, + [this]() { return updateStaticPressureData(); }); + hillificator<>(dynamicPressure, enableHw, + [this]() { return updateDynamicPressureData(); }); + hillificator<>(rotatedImu, enableHw, + [this]() { return updateIMUData(); }); + + return true; + } + + int getSampleCounter(int nData) + { + auto ts = miosix::getTime(); + auto tsSensorData = + getModule<PayloadHIL>()->getTimestampSimulatorData(); + auto simulationPeriod = getModule<PayloadHIL>()->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; + } + + Boardcore::LPS28DFWData updateLPS28DFWData() + { + Boardcore::LPS28DFWData data; + + auto* sensorData = getModule<PayloadHIL>()->getSensorData(); + + int iBaro = getSampleCounter(sensorData->barometer1.NDATA); + int iTemp = getSampleCounter(sensorData->temperature.NDATA); + + data.pressureTimestamp = data.temperatureTimestamp = + Boardcore::TimestampTimer::getTimestamp(); + data.pressure = sensorData->barometer1.measures[iBaro]; + data.temperature = sensorData->temperature.measures[iTemp]; + + return data; + }; + + Boardcore::LPS22DFData updateLPS22DFData() + { + Boardcore::LPS22DFData data; + + auto* sensorData = getModule<PayloadHIL>()->getSensorData(); + + int iBaro = getSampleCounter(sensorData->barometer1.NDATA); + int iTemp = getSampleCounter(sensorData->temperature.NDATA); + + data.pressureTimestamp = data.temperatureTimestamp = + Boardcore::TimestampTimer::getTimestamp(); + data.pressure = sensorData->barometer1.measures[iBaro]; + data.temperature = sensorData->temperature.measures[iTemp]; + + return data; + }; + + Boardcore::H3LIS331DLData updateH3LIS331DLData() + { + Boardcore::H3LIS331DLData data; + + auto* sensorData = getModule<PayloadHIL>()->getSensorData(); + + int iAcc = getSampleCounter(sensorData->accelerometer.NDATA); + + data.accelerationTimestamp = Boardcore::TimestampTimer::getTimestamp(); + data.accelerationX = sensorData->accelerometer.measures[iAcc][0]; + data.accelerationY = sensorData->accelerometer.measures[iAcc][1]; + data.accelerationZ = sensorData->accelerometer.measures[iAcc][2]; + + return data; + }; + + Boardcore::LIS2MDLData updateLIS2MDLData() + { + Boardcore::LIS2MDLData data; + + auto* sensorData = getModule<PayloadHIL>()->getSensorData(); + + int iMag = getSampleCounter(sensorData->magnetometer.NDATA); + + data.magneticFieldTimestamp = Boardcore::TimestampTimer::getTimestamp(); + data.magneticFieldX = sensorData->magnetometer.measures[iMag][0]; + data.magneticFieldY = sensorData->magnetometer.measures[iMag][1]; + data.magneticFieldZ = sensorData->magnetometer.measures[iMag][2]; + + return data; + }; + + Boardcore::UBXGPSData updateUBXGPSData() + { + Boardcore::UBXGPSData data; + + auto* sensorData = getModule<PayloadHIL>()->getSensorData(); + + int iGps = getSampleCounter(sensorData->gps.NDATA); + + data.gpsTimestamp = Boardcore::TimestampTimer::getTimestamp(); + + 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; + }; + + Boardcore::LSM6DSRXData updateLSM6DSRXData() + { + Boardcore::LSM6DSRXData data; + + auto* sensorData = getModule<PayloadHIL>()->getSensorData(); + + int iAcc = getSampleCounter(sensorData->accelerometer.NDATA); + int iGyro = getSampleCounter(sensorData->gyro.NDATA); + + data.accelerationTimestamp = data.angularSpeedTimestamp = + Boardcore::TimestampTimer::getTimestamp(); + + 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; + }; + + Boardcore::MPXH6115AData updateStaticPressureData() + { + Boardcore::MPXH6115AData data; + + auto* sensorData = getModule<PayloadHIL>()->getSensorData(); + + int iBaro = getSampleCounter(sensorData->staticPitot.NDATA); + + data.pressureTimestamp = Boardcore::TimestampTimer::getTimestamp(); + data.pressure = sensorData->staticPitot.measures[iBaro]; + + return data; + }; + + Boardcore::MPXH6115AData updateDynamicPressureData() + { + Boardcore::MPXH6115AData data; + + auto* sensorData = getModule<PayloadHIL>()->getSensorData(); + + int iBaro = getSampleCounter(sensorData->dynamicPitot.NDATA); + + data.pressureTimestamp = Boardcore::TimestampTimer::getTimestamp(); + data.pressure = sensorData->dynamicPitot.measures[iBaro]; + + return data; + }; + + Boardcore::IMUData updateIMUData() + { + return Boardcore::IMUData{getLSM6DSRXLastSample(), + getLSM6DSRXLastSample(), + getCalibratedLIS2MDLLastSample()}; + }; + + bool enableHw; +}; +} // namespace Payload \ No newline at end of file diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp index 88af73d21..657dbf8c6 100644 --- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp +++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp @@ -42,6 +42,22 @@ namespace config = Payload::Config::FlightModeManager; namespace Payload { +void enterHilMode() +{ + PersistentVars::setHilMode(true); + miosix::reboot(); +} + +void exitHilMode() +{ + // Reboot only if in HIL mode + if (PersistentVars::getHilMode()) + { + PersistentVars::setHilMode(false); + miosix::reboot(); + } +} + FlightModeManager::FlightModeManager() : HSM(&FlightModeManager::OnGround, miosix::STACK_DEFAULT_FOR_PTHREAD, BoardScheduler::flightModeManagerPriority()) @@ -101,6 +117,20 @@ State FlightModeManager::OnGround(const Event& event) return HANDLED; } + case TMTC_EXIT_HIL_MODE: + { + getModule<CanHandler>()->sendEvent( + CanConfig::EventId::EXIT_HIL_MODE); + miosix::Thread::sleep(1000); + exitHilMode(); + return HANDLED; + } + case CAN_EXIT_HIL_MODE: + { + exitHilMode(); + return HANDLED; + } + case TMTC_RESET_BOARD: { Logger::getInstance().stop(); @@ -428,6 +458,20 @@ State FlightModeManager::OnGroundTestMode(const Event& event) return HANDLED; } + case TMTC_ENTER_HIL_MODE: + { + getModule<CanHandler>()->sendEvent( + CanConfig::EventId::ENTER_HIL_MODE); + miosix::Thread::sleep(1000); + enterHilMode(); + return HANDLED; + } + case CAN_ENTER_HIL_MODE: + { + enterHilMode(); + return HANDLED; + } + case TMTC_EXIT_TEST_MODE: { getModule<CanHandler>()->sendEvent( diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h index 3367c7e85..ecbf30878 100644 --- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h +++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.h @@ -22,6 +22,7 @@ #pragma once +#include <Payload/PersistentVars/PersistentVars.h> #include <diagnostic/PrintLogger.h> #include <events/HSM.h> #include <utils/DependencyManager/DependencyManager.h> diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp index 86fd32df8..90c4c6ff6 100644 --- a/src/entrypoints/Payload/payload-entry.cpp +++ b/src/entrypoints/Payload/payload-entry.cpp @@ -28,8 +28,11 @@ #include <Payload/Buses.h> #include <Payload/CanHandler/CanHandler.h> #include <Payload/FlightStatsRecorder/FlightStatsRecorder.h> +#include <Payload/HIL/HIL.h> +#include <Payload/PersistentVars/PersistentVars.h> #include <Payload/PinHandler/PinHandler.h> #include <Payload/Radio/Radio.h> +#include <Payload/Sensors/HILSensors.h> #include <Payload/Sensors/Sensors.h> #include <Payload/StateMachines/FlightModeManager/FlightModeManager.h> #include <Payload/StateMachines/NASController/NASController.h> @@ -130,7 +133,9 @@ int main() initResult &= depman.insert(nasController); // Sensors - auto sensors = new Sensors(); + auto sensors = + (PersistentVars::getHilMode() ? new HILSensors(Config::HIL::ENABLE_HW) + : new Sensors()); initResult &= depman.insert(sensors); auto pinHandler = new PinHandler(); initResult &= depman.insert(pinHandler); @@ -157,6 +162,17 @@ int main() auto statsRecorder = new FlightStatsRecorder(); initResult &= depman.insert(statsRecorder); + // HIL + PayloadHIL* hil = nullptr; + if (PersistentVars::getHilMode()) + { + std::cout << "PAYLOAD SimulatorData: " << sizeof(SimulatorData) + << ", ActuatorData: " << sizeof(ActuatorData) << std::endl; + + hil = new PayloadHIL(); + initResult &= depman.insert(hil); + } + std::cout << "Injecting module dependencies" << std::endl; initResult &= depman.inject(); @@ -176,7 +192,6 @@ int main() START_SINGLETON(EventBroker); // Start module instances - START_MODULE(sensors) { miosix::led1On(); } START_MODULE(pinHandler); START_MODULE(radio) { miosix::led2On(); } START_MODULE(canHandler) { miosix::led3On(); } @@ -199,6 +214,18 @@ int main() Logger::getInstance().log(ev); }); + if (hil) + { + START_MODULE(hil); + + std::cout << "Waiting start simulation" << std::endl; + hil->waitStartSimulation(); + } + + // Wait for simulation start before starting sensors to avoid initializing + // them with invalid data + START_MODULE(sensors) { miosix::led1On(); } + if (initResult) { EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM); -- GitLab