diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 294bc9c2dc3a492a9241fe88a31a81b719d02f01..90f09ba85f011a3a6d38a38c56d334eb04901371 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -75,6 +75,7 @@ set(PAYLOAD_COMPUTER
src/boards/Payload/CanHandler/CanHandler.cpp
src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp
src/boards/Payload/Sensors/Sensors.cpp
+ src/boards/Payload/Sensors/HILSensors.cpp
src/boards/Payload/BoardScheduler.cpp
src/boards/Payload/PinHandler/PinHandler.cpp
src/boards/Payload/Radio/Radio.cpp
diff --git a/src/boards/Payload/Buses.h b/src/boards/Payload/Buses.h
index d4f49813721bcd92a32e125a8547633c4bd8cf01..4dcbddab00e4251454ef7d91449f801e89c9aab4 100644
--- a/src/boards/Payload/Buses.h
+++ b/src/boards/Payload/Buses.h
@@ -48,7 +48,7 @@ public:
: spi1(SPI1), spi3(SPI3), spi4(SPI4), spi6(SPI6),
i2c1(I2C1, miosix::interfaces::i2c1::scl::getPin(),
miosix::interfaces::i2c1::sda::getPin()),
- usart1(USART1, 115200), usart2(USART2, 115200), uart4(UART4, 115200)
+ usart1(USART1, 115200), usart2(USART2, 115200, 1024), uart4(UART4, 115200)
{
}
};
diff --git a/src/boards/Payload/Configs/HILSimulationConfig.h b/src/boards/Payload/Configs/HILSimulationConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..a395aab1e1338bf58f1bb1372d4b5782ae905548
--- /dev/null
+++ b/src/boards/Payload/Configs/HILSimulationConfig.h
@@ -0,0 +1,624 @@
+/* Copyright (c) 2023-2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Payload/Buses.h>
+#include <Payload/StateMachines/FlightModeManager/FlightModeManager.h>
+#include <common/Events.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <drivers/usart/USART.h>
+#include <events/EventBroker.h>
+#include <math.h>
+#include <sensors/HILSensors/IncludeHILSensors.h>
+#include <sensors/SensorInfo.h>
+#include <utils/Debug.h>
+#include <utils/Stats/Stats.h>
+
+#include <list>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+// NAS
+#include <Payload/StateMachines/NASController/NASControllerData.h>
+#include <algorithms/NAS/NASState.h>
+
+// WingController
+#include <Payload/StateMachines/WingController/WingControllerData.h>
+
+namespace HILConfig
+{
+
+/** Period of simulation in milliseconds */
+constexpr int SIMULATION_PERIOD = 100;
+
+/** sample frequency of sensor (samples/second) */
+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;
+
+/** Number of samples per sensor at each simulator iteration */
+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
+{
+ 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)
+ {
+ }
+
+ 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)
+ {
+ }
+
+ void print()
+ {
+ 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);
+ }
+};
+
+/**
+ * @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)
+ {
+ }
+
+ NASStateHIL(Boardcore::NASState adaState,
+ Payload::NASControllerStatus adaStatus)
+ : n(adaState.n), e(adaState.e), d(adaState.d), vn(adaState.vn),
+ ve(adaState.ve), vd(adaState.vd), qx(adaState.qx), qy(adaState.qy),
+ qz(adaState.qz), qw(adaState.qw),
+ updating(adaStatus.state == Payload::NASControllerState::ACTIVE)
+ {
+ }
+
+ 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 airbrakesPercentage = 0; //
+ float expulsionPercentage = 0;
+ float parafoilLeftPercentage = 0;
+ float parafoilRightPercentage = 0;
+ float mainValvePercentage = 0; //
+ float ventingValvePercentage = 0; //
+ float releaseValvePercentage = 0;
+ float fillingValvePercentage = 0; //
+ float disconnectValvePercentage = 0; //
+
+ ActuatorsStateHIL()
+ : airbrakesPercentage(0.0f), expulsionPercentage(0.0f),
+ parafoilLeftPercentage(0.0f), parafoilRightPercentage(0.0f),
+ mainValvePercentage(0.0f), ventingValvePercentage(0.0f),
+ releaseValvePercentage(0.0f), fillingValvePercentage(0.0f),
+ disconnectValvePercentage(0.0f)
+ {
+ }
+
+ ActuatorsStateHIL(float airbrakesPercentage, float expulsionPercentage,
+ float parafoilLeftPercentage,
+ float parafoilRightPercentage, float mainValvePercentage,
+ float ventingValvePercentage,
+ float releaseValvePercentage,
+ float fillingValvePercentage,
+ float disconnectValvePercentage)
+ : airbrakesPercentage(airbrakesPercentage),
+ expulsionPercentage(expulsionPercentage),
+ parafoilLeftPercentage(parafoilLeftPercentage),
+ parafoilRightPercentage(parafoilRightPercentage),
+ mainValvePercentage(mainValvePercentage),
+ ventingValvePercentage(ventingValvePercentage),
+ releaseValvePercentage(releaseValvePercentage),
+ fillingValvePercentage(fillingValvePercentage),
+ disconnectValvePercentage(disconnectValvePercentage)
+ {
+ }
+
+ void print()
+ {
+ printf(
+ "airbrakes: %f perc\n"
+ "expulsion: %f perc\n"
+ "parafoilLeft: %f perc\n"
+ "parafoilRight: %f perc\n"
+ "mainValve: %f perc\n"
+ "ventingValve: %f perc\n"
+ "releaseValve: %f perc\n"
+ "fillingValve: %f perc\n"
+ "disconnectValve: %f perc\n",
+ airbrakesPercentage * 100, expulsionPercentage * 100,
+ parafoilLeftPercentage * 100, parafoilRightPercentage * 100,
+ mainValvePercentage * 100, ventingValvePercentage * 100,
+ releaseValvePercentage * 100, fillingValvePercentage * 100,
+ disconnectValvePercentage * 100);
+ }
+};
+
+struct WESDataHIL
+{
+ float windX;
+ float windY;
+
+ WESDataHIL(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;
+
+ GuidanceDataHIL(float psiRef, float deltaA) : psiRef(psiRef), deltaA(deltaA)
+ {
+ }
+
+ GuidanceDataHIL() : psiRef(0.0f), deltaA(0.0f) {}
+
+ void print()
+ {
+ printf(
+ "psiRef: %f\n"
+ "deltaA: %f\n",
+ psiRef, deltaA);
+ }
+};
+
+/**
+ * @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
+{
+ 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;
+
+ void print()
+ {
+ printf("%f,%f,%f\n", accelerometer.measures[0][0],
+ accelerometer.measures[0][1], accelerometer.measures[0][2]);
+ printf("%f,%f,%f\n", gyro.measures[0][0],
+ gyro.measures[0][1], gyro.measures[0][2]);
+ printf("%f,%f,%f\n", magnetometer.measures[0][0],
+ magnetometer.measures[0][1], magnetometer.measures[0][2]);
+ printf("%f\n", temperature.measures[0]);
+ flags.print();
+ }
+};
+
+/**
+ * @brief Data strudcture expected by the simulator
+ */
+struct ActuatorData
+{
+ NASStateHIL nasState;
+ ActuatorsStateHIL actuatorsState;
+ WESDataHIL wesData;
+ GuidanceDataHIL guidanceData;
+ FlagsHIL flags;
+
+ ActuatorData()
+ : nasState(), actuatorsState(), wesData(), guidanceData(), flags()
+ {
+ }
+
+ ActuatorData(NASStateHIL nasState, ActuatorsStateHIL actuatorsState,
+ WESDataHIL wesData, GuidanceDataHIL guidanceData,
+ FlagsHIL flagsIn, Payload::FlightModeManager* fmm)
+ : nasState(nasState), actuatorsState(actuatorsState), wesData(wesData),
+ guidanceData(guidanceData)
+ {
+ flags.flag_flight =
+ (fmm->testState(&Payload::FlightModeManager::state_ascending) ||
+ fmm->testState(
+ &Payload::FlightModeManager::state_drogue_descent) ||
+ fmm->testState(
+ &Payload::FlightModeManager::state_wing_descent)
+ ? 1
+ : 0);
+ flags.flag_ascent =
+ (fmm->testState(&Payload::FlightModeManager::state_ascending) ? 1
+ : 0);
+ flags.flag_burning = flagsIn.flag_burning;
+ flags.flag_airbrakes = flagsIn.flag_airbrakes;
+ flags.flag_para1 =
+ (fmm->testState(&Payload::FlightModeManager::state_drogue_descent)
+ ? 1
+ : 0);
+ flags.flag_para2 =
+ (fmm->testState(&Payload::FlightModeManager::state_wing_descent)
+ ? 1
+ : 0);
+ }
+
+ void print()
+ {
+ nasState.print();
+ actuatorsState.print();
+ wesData.print();
+ guidanceData.print();
+ flags.print();
+ }
+};
+
+enum PayloadFlightPhases
+{
+ 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 PayloadHILAccelerometer = HILAccelerometer<N_DATA_ACCEL>;
+using PayloadHILGyroscope = HILGyroscope<N_DATA_GYRO>;
+using PayloadHILMagnetometer = HILMagnetometer<N_DATA_MAGNETO>;
+using PayloadHILGps = HILGps<N_DATA_GPS>;
+using PayloadHILBarometer = HILBarometer<N_DATA_BARO>;
+using PayloadHILBarometer = HILBarometer<N_DATA_BARO_CHAMBER>;
+using PayloadHILPitot = HILPitot<N_DATA_PITOT>;
+using PayloadHILTemperature = HILTemperature<N_DATA_TEMP>;
+
+using PayloadHILTransceiver =
+ HILTransceiver<PayloadFlightPhases, SimulatorData, ActuatorData>;
+using PayloadHIL = HIL<PayloadFlightPhases, SimulatorData, ActuatorData>;
+
+class PayloadHILPhasesManager
+ : public HILPhasesManager<PayloadFlightPhases, SimulatorData, ActuatorData>
+{
+public:
+ explicit PayloadHILPhasesManager(
+ std::function<Boardcore::TimedTrajectoryPoint()> getCurrentPosition)
+ : HILPhasesManager<PayloadFlightPhases, SimulatorData, ActuatorData>(
+ getCurrentPosition)
+ {
+ flagsFlightPhases = {{PayloadFlightPhases::SIM_FLYING, false},
+ {PayloadFlightPhases::SIM_ASCENT, false},
+ {PayloadFlightPhases::SIM_BURNING, false},
+ {PayloadFlightPhases::SIM_AEROBRAKES, false},
+ {PayloadFlightPhases::SIM_PARA1, false},
+ {PayloadFlightPhases::SIM_PARA2, false},
+ {PayloadFlightPhases::SIMULATION_STARTED, false},
+ {PayloadFlightPhases::CALIBRATION, false},
+ {PayloadFlightPhases::CALIBRATION_OK, false},
+ {PayloadFlightPhases::ARMED, false},
+ {PayloadFlightPhases::LIFTOFF_PIN_DETACHED, false},
+ {PayloadFlightPhases::LIFTOFF, false},
+ {PayloadFlightPhases::AEROBRAKES, false},
+ {PayloadFlightPhases::APOGEE, false},
+ {PayloadFlightPhases::PARA1, false},
+ {PayloadFlightPhases::PARA2, false},
+ {PayloadFlightPhases::SIMULATION_STOPPED, false}};
+
+ prev_flagsFlightPhases = flagsFlightPhases;
+
+ auto& eventBroker = Boardcore::EventBroker::getInstance();
+ eventBroker.subscribe(this, Common::TOPIC_ABK);
+ eventBroker.subscribe(this, Common::TOPIC_ADA);
+ eventBroker.subscribe(this, Common::TOPIC_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 processFlags(const SimulatorData& simulatorData) override
+ {
+ updateSimulatorFlags(simulatorData);
+
+ std::vector<PayloadFlightPhases> changed_flags;
+
+ // set true when the first packet from the simulator arrives
+ if (isSetTrue(PayloadFlightPhases::SIMULATION_STARTED))
+ {
+ t_start = Boardcore::TimestampTimer::getTimestamp();
+
+ TRACE("[HIL] ------- SIMULATION STARTED ! ------- \n");
+ changed_flags.push_back(PayloadFlightPhases::SIMULATION_STARTED);
+ }
+
+ if (flagsFlightPhases[PayloadFlightPhases::SIM_FLYING])
+ {
+ if (isSetTrue(PayloadFlightPhases::SIM_FLYING))
+ {
+ registerOutcomes(PayloadFlightPhases::SIM_FLYING);
+ TRACE("[HIL] ------- SIMULATOR LIFTOFF ! ------- \n");
+ changed_flags.push_back(PayloadFlightPhases::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 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[PayloadFlightPhases::SIM_BURNING].print(t_liftoff);
+
+ printf("Airbrakes exit shadowmode: \n");
+ outcomes[PayloadFlightPhases::AEROBRAKES].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);
+
+ // auto cpuMeter = Boardcore::CpuMeter::getCpuStats();
+ // printf("max cpu usage: %+.3f\n", cpuMeter.maxValue);
+ // printf("avg cpu usage: %+.3f\n", cpuMeter.mean);
+ // printf("min free heap: %+.3lu\n", cpuMeter.minFreeHeap);
+ // printf("max free stack:%+.3lu\n", cpuMeter.minFreeStack);
+ }
+
+private:
+ void handleEvent(const Boardcore::Event& e) override
+ {
+ 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::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_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());
+ }
+
+ /* 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 Updates the flags of the object with the flags sent from matlab
+ * and checks for the apogee
+ */
+ void updateSimulatorFlags(const SimulatorData& simulatorData)
+ {
+ flagsFlightPhases[PayloadFlightPhases::SIM_ASCENT] =
+ simulatorData.flags.flag_ascent;
+ flagsFlightPhases[PayloadFlightPhases::SIM_FLYING] =
+ simulatorData.flags.flag_flight;
+ flagsFlightPhases[PayloadFlightPhases::SIM_BURNING] =
+ simulatorData.flags.flag_burning;
+ flagsFlightPhases[PayloadFlightPhases::SIM_AEROBRAKES] =
+ simulatorData.flags.flag_airbrakes;
+ flagsFlightPhases[PayloadFlightPhases::SIM_PARA1] =
+ simulatorData.flags.flag_para1;
+ flagsFlightPhases[PayloadFlightPhases::SIM_PARA2] =
+ simulatorData.flags.flag_para2;
+
+ flagsFlightPhases[PayloadFlightPhases::SIMULATION_STOPPED] =
+ isSetFalse(PayloadFlightPhases::SIM_FLYING) ||
+ prev_flagsFlightPhases[PayloadFlightPhases::SIMULATION_STOPPED];
+ }
+};
+
+} // namespace HILConfig
\ No newline at end of file
diff --git a/src/boards/Payload/Sensors/HILSensors.cpp b/src/boards/Payload/Sensors/HILSensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e57944ba95e34810a796145295c48119c9061a27
--- /dev/null
+++ b/src/boards/Payload/Sensors/HILSensors.cpp
@@ -0,0 +1,503 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, 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 "HILSensors.h"
+
+#include <Payload/Buses.h>
+#include <Payload/Configs/HILSimulationConfig.h>
+#include <Payload/Configs/SensorsConfig.h>
+#include <common/ReferenceConfig.h>
+#include <interfaces-impl/hwmapping.h>
+
+using namespace Boardcore;
+using namespace std;
+using namespace Payload::SensorsConfig;
+using namespace HILConfig;
+
+namespace Payload
+{
+LPS22DFData HILSensors::getLPS22DFLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return (lps22df != nullptr
+ ? LPS22DFData{lps22df->getLastSample().pressureTimestamp,
+ lps22df->getLastSample().pressure,
+ temperature->getLastSample().temperature}
+ : LPS22DFData{});
+}
+LPS28DFWData HILSensors::getLPS28DFW_1LastSample()
+{
+ miosix::PauseKernelLock lock;
+ return lps28dfw_1 != nullptr
+ ? LPS28DFWData{lps28dfw_1->getLastSample().pressureTimestamp,
+ lps28dfw_1->getLastSample().pressure,
+ temperature->getLastSample().temperatureTimestamp,
+ temperature->getLastSample().temperature}
+ : LPS28DFWData{};
+}
+LPS28DFWData HILSensors::getLPS28DFW_2LastSample()
+{
+ miosix::PauseKernelLock lock;
+ return lps28dfw_2 != nullptr
+ ? LPS28DFWData{lps28dfw_2->getLastSample().pressureTimestamp,
+ lps28dfw_2->getLastSample().pressure,
+ temperature->getLastSample().temperatureTimestamp,
+ temperature->getLastSample().temperature}
+ : LPS28DFWData{};
+}
+H3LIS331DLData HILSensors::getH3LIS331DLLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return h3lis331dl != nullptr
+ ? H3LIS331DLData{static_cast<Boardcore::AccelerometerData>(
+ h3lis331dl->getLastSample())}
+ : H3LIS331DLData{};
+}
+LIS2MDLData HILSensors::getLIS2MDLLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return lis2mdl != nullptr
+ ? LIS2MDLData{static_cast<Boardcore::MagnetometerData>(
+ lis2mdl->getLastSample()),
+ static_cast<Boardcore::TemperatureData>(
+ temperature->getLastSample())}
+ : LIS2MDLData{};
+}
+UBXGPSData HILSensors::getGPSLastSample()
+{
+ if (ubxgps == nullptr)
+ {
+ return UBXGPSData{};
+ }
+ UBXGPSData lastSample;
+
+ miosix::PauseKernelLock lock;
+ GPSData gps = static_cast<Boardcore::GPSData>(ubxgps->getLastSample());
+ lastSample.ubxTime = UBXDateTime{};
+ lastSample.gpsTimestamp = gps.gpsTimestamp;
+ lastSample.latitude = gps.latitude;
+ lastSample.longitude = gps.longitude;
+ lastSample.height = gps.height;
+ lastSample.velocityNorth = gps.velocityNorth;
+ lastSample.velocityEast = gps.velocityEast;
+ lastSample.velocityDown = gps.velocityDown;
+ lastSample.speed = gps.speed;
+ lastSample.track = gps.track;
+ lastSample.positionDOP = gps.positionDOP;
+ lastSample.satellites = gps.satellites;
+ lastSample.fix = gps.fix;
+ return lastSample;
+}
+LSM6DSRXData HILSensors::getLSM6DSRXLastSample()
+{
+ if (lsm6dsrx_accel == nullptr || lsm6dsrx_gyro == nullptr)
+ {
+ return LSM6DSRXData{};
+ }
+
+ LSM6DSRXData lastSample;
+
+ HILAccelerometerData accelData;
+ HILGyroscopeData gyroData;
+ {
+ miosix::PauseKernelLock lock;
+ accelData = lsm6dsrx_accel->getLastSample();
+ gyroData = lsm6dsrx_gyro->getLastSample();
+ }
+
+ lastSample.accelerationTimestamp = accelData.accelerationTimestamp;
+ lastSample.accelerationX = accelData.accelerationX;
+ lastSample.accelerationY = accelData.accelerationY;
+ lastSample.accelerationZ = accelData.accelerationZ;
+
+ lastSample.angularSpeedTimestamp = gyroData.angularSpeedTimestamp;
+ lastSample.angularSpeedX = gyroData.angularSpeedX;
+ lastSample.angularSpeedY = gyroData.angularSpeedY;
+ lastSample.angularSpeedZ = gyroData.angularSpeedZ;
+
+ return lastSample;
+}
+ADS131M08Data HILSensors::getADS131M08LastSample()
+{
+ miosix::PauseKernelLock lock;
+ return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{};
+ return ADS131M08Data{};
+}
+
+Boardcore::HSCMRNN015PAData HILSensors::getStaticPressureLastSample()
+{
+ Boardcore::HSCMRNN015PAData data;
+ miosix::PauseKernelLock lock;
+
+ auto baro = getLPS22DFLastSample();
+ data.pressureTimestamp = baro.pressureTimestamp;
+ data.pressure = baro.pressure;
+
+ return data;
+}
+
+Boardcore::SSCMRNN030PAData HILSensors::getDynamicPressureLastSample()
+{
+ Boardcore::SSCMRNN030PAData data;
+ miosix::PauseKernelLock lock;
+
+ // data.pressureTimestamp =
+ // dynamicPressure->getLastSample().pressureTimestamp; data.pressure =
+ // dynamicPressure->getLastSample().pressure;
+
+ return data;
+}
+
+Boardcore::PitotData HILSensors::getPitotLastSample()
+{
+ Boardcore::PitotData data;
+ miosix::PauseKernelLock lock;
+
+ auto sample = pitot->getLastSample();
+ data.timestamp = sample.timestamp;
+ data.airspeed = sample.airspeed;
+ data.deltaP = sample.deltaP;
+
+ return data;
+}
+
+HILSensors::HILSensors(TaskScheduler* sched) : Payload::Sensors(sched) {}
+
+bool HILSensors::start()
+{
+ // Init all the sensors
+ temperatureInit();
+ LOG_INFO(logger, "temperatureInit\n");
+ lps22dfInit();
+ LOG_INFO(logger, "lps22dfInit\n");
+ lps28dfw_1Init();
+ LOG_INFO(logger, "lps28dfw_1Init\n");
+ lps28dfw_2Init();
+ LOG_INFO(logger, "lps28dfw_2Init\n");
+ h3lis331dlInit();
+ LOG_INFO(logger, "h3lis331dlInit\n");
+ lis2mdlInit();
+ LOG_INFO(logger, "lis2mdlInit\n");
+ ubxgpsInit();
+ LOG_INFO(logger, "ubxgpsInit\n");
+ lsm6dsrxInit();
+ LOG_INFO(logger, "lsm6dsrxInit\n");
+ // ads131m08Init();
+ // LOG_INFO(logger, "ads131m08Init\n");
+ staticPressureInit();
+ LOG_INFO(logger, "staticPressureInit\n");
+ dynamicPressureInit();
+ LOG_INFO(logger, "dynamicPressureInit\n");
+ pitotInit();
+ LOG_INFO(logger, "pitotInit\n");
+ imuInit();
+ LOG_INFO(logger, "imuInit\n");
+
+ // Add the magnetometer calibration to the scheduler
+ size_t result = scheduler->addTask(
+ [&]()
+ {
+ // Gather the last sample data
+ MagnetometerData lastSample = getLIS2MDLLastSample();
+
+ // Feed the data to the calibrator inside a protected area.
+ // Contention is not high and the use of a mutex is suitable to
+ // avoid pausing the kernel for this calibration operation
+ {
+ miosix::Lock<FastMutex> l(calibrationMutex);
+ magCalibrator.feed(lastSample);
+ }
+ },
+ MAG_CALIBRATION_PERIOD);
+
+ // Create sensor manager with populated map and configured scheduler
+ manager = new SensorManager(sensorMap, scheduler);
+ return manager->start() && result != 0;
+}
+
+void HILSensors::temperatureInit()
+{
+ temperature =
+ new PayloadHILTemperature(&Boardcore::ModuleManager::getInstance()
+ .get<PayloadHIL>()
+ ->hilTransceiver->getSensorData()
+ ->temperature);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("TEMP_PayloadHIL", 1000 / TEMP_FREQ);
+
+ sensorMap.emplace(make_pair(temperature, info));
+}
+
+void HILSensors::lps22dfInit()
+{
+ // Create sensor instance with configured parameters
+ lps22df = new PayloadHILBarometer(&Boardcore::ModuleManager::getInstance()
+ .get<PayloadHIL>()
+ ->hilTransceiver->getSensorData()
+ ->barometer1);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("LPS22DF_PayloadHIL", LPS22DF_PERIOD,
+ bind(&HILSensors::lps22dfCallback, this));
+ sensorMap.emplace(make_pair(lps22df, info));
+}
+
+void HILSensors::lps28dfw_1Init()
+{
+ // Create sensor instance with configured parameters
+ lps28dfw_1 =
+ new PayloadHILBarometer(&Boardcore::ModuleManager::getInstance()
+ .get<PayloadHIL>()
+ ->hilTransceiver->getSensorData()
+ ->barometer2);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("LPS28DFW_1_PayloadHIL", LPS28DFW_PERIOD,
+ bind(&HILSensors::lps28dfw_1Callback, this));
+ sensorMap.emplace(make_pair(lps28dfw_1, info));
+}
+
+void HILSensors::lps28dfw_2Init()
+{
+ // Create sensor instance with configured parameters
+ lps28dfw_2 =
+ new PayloadHILBarometer(&Boardcore::ModuleManager::getInstance()
+ .get<PayloadHIL>()
+ ->hilTransceiver->getSensorData()
+ ->barometer3);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("LPS28DFW_2_PayloadHIL", LPS28DFW_PERIOD,
+ bind(&HILSensors::lps28dfw_2Callback, this));
+ sensorMap.emplace(make_pair(lps28dfw_2, info));
+}
+
+void HILSensors::pitotInit()
+{
+ // create lambda function to read the pressure
+ pitot = new PayloadHILPitot(&Boardcore::ModuleManager::getInstance()
+ .get<PayloadHIL>()
+ ->hilTransceiver->getSensorData()
+ ->pitot);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("Pitot", 1000 / PITOT_FREQ,
+ bind(&HILSensors::pitotCallback, this));
+ sensorMap.emplace(make_pair(pitot, info));
+}
+
+void HILSensors::h3lis331dlInit()
+{
+ // Create sensor instance with configured parameters
+ h3lis331dl =
+ new PayloadHILAccelerometer(&Boardcore::ModuleManager::getInstance()
+ .get<PayloadHIL>()
+ ->hilTransceiver->getSensorData()
+ ->accelerometer);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("H3LIS331DL_PayloadHIL", H3LIS331DL_PERIOD,
+ bind(&HILSensors::h3lis331dlCallback, this));
+ sensorMap.emplace(make_pair(h3lis331dl, info));
+}
+
+void HILSensors::lis2mdlInit()
+{
+ // Create sensor instance with configured parameters
+ lis2mdl =
+ new PayloadHILMagnetometer(&Boardcore::ModuleManager::getInstance()
+ .get<PayloadHIL>()
+ ->hilTransceiver->getSensorData()
+ ->magnetometer);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("LIS2MDL_PayloadHIL", LIS2MDL_PERIOD,
+ bind(&HILSensors::lis2mdlCallback, this));
+ sensorMap.emplace(make_pair(lis2mdl, info));
+}
+
+void HILSensors::ubxgpsInit()
+{
+ // Create sensor instance with configured parameters
+ ubxgps = new PayloadHILGps(&Boardcore::ModuleManager::getInstance()
+ .get<PayloadHIL>()
+ ->hilTransceiver->getSensorData()
+ ->gps);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("UBXGPS_PayloadHIL", UBXGPS_PERIOD,
+ bind(&HILSensors::ubxgpsCallback, this));
+ sensorMap.emplace(make_pair(ubxgps, info));
+}
+
+void HILSensors::lsm6dsrxInit()
+{
+ // Create sensor instance with configured parameters
+ lsm6dsrx_accel =
+ new PayloadHILAccelerometer(&Boardcore::ModuleManager::getInstance()
+ .get<PayloadHIL>()
+ ->hilTransceiver->getSensorData()
+ ->accelerometer);
+ lsm6dsrx_gyro =
+ new PayloadHILGyroscope(&Boardcore::ModuleManager::getInstance()
+ .get<PayloadHIL>()
+ ->hilTransceiver->getSensorData()
+ ->gyro);
+
+ // Emplace the sensor inside the map
+ SensorInfo info("LSM6DSRX", LSM6DSRX_PERIOD,
+ bind(&HILSensors::lsm6dsrxCallback, this));
+ sensorMap.emplace(make_pair(lsm6dsrx_accel, info));
+ sensorMap.emplace(make_pair(lsm6dsrx_gyro, info));
+}
+
+void HILSensors::ads131m08Init()
+{
+ // ModuleManager& modules = ModuleManager::getInstance();
+
+ // Configure the SPI
+ // SPIBusConfig config;
+ // config.clockDivider = SPI::ClockDivider::DIV_32;
+
+ // // Configure the device
+ // ADS131M08::Config sensorConfig;
+ // sensorConfig.oversamplingRatio = ADS131M08_OVERSAMPLING_RATIO;
+ // sensorConfig.globalChopModeEnabled = ADS131M08_GLOBAL_CHOP_MODE;
+
+ // // Create the sensor instance with configured parameters
+ // ads131m08 = new ADS131M08(modules.get<Buses>()->spi4,
+ // miosix::sensors::ADS131::cs::getPin(), config,
+ // sensorConfig);
+
+ // // Emplace the sensor inside the map
+ // SensorInfo info("ADS131M08", ADS131M08_PERIOD,
+ // bind(&HILSensors::ads131m08Callback, this));
+ // sensorMap.emplace(make_pair(ads131m08, info));
+}
+
+void HILSensors::staticPressureInit()
+{
+ // // Create sensor instance with configured parameters
+ // staticPressure =
+ // new PayloadHILBarometer(&Boardcore::ModuleManager::getInstance()
+ // .get<PayloadHIL>()
+ // ->hilTransceiver->getSensorData()
+ // ->staticPitot);
+
+ // // Emplace the sensor inside the map
+ // SensorInfo info("StaticPitot", 1000 / PITOT_FREQ,
+ // bind(&HILSensors::staticPressureCallback, this));
+ // sensorMap.emplace(make_pair(staticPressure, info));
+}
+
+void HILSensors::dynamicPressureInit()
+{
+ // // Create sensor instance with configured parameters
+ // dynamicPressure = new PayloadHILBarometer(
+ // N_DATA_PITOT, &Boardcore::ModuleManager::getInstance()
+ // .get<PayloadHIL>()
+ // ->hilTransceiver->getSensorData()
+ // ->dynamicPitot);
+
+ // // Emplace the sensor inside the map
+ // SensorInfo info("DynamicPitot", 1000 / PITOT_FREQ,
+ // bind(&HILSensors::dynamicPressureCallback, this));
+ // sensorMap.emplace(make_pair(dynamicPressure, info));
+}
+
+void HILSensors::imuInit()
+{
+ // Register the IMU as the fake sensor, passing as parameters the
+ // methods to retrieve real data. The sensor is not synchronized, but
+ // the sampling thread is always the same.
+
+ imu = new RotatedIMU(
+ bind(&HILSensors::getLSM6DSRXLastSample, this),
+ bind(&HILSensors::getCalibratedMagnetometerLastSample, this),
+ bind(&HILSensors::getLSM6DSRXLastSample, this));
+
+ // Invert the Y axis on the magnetometer
+ // Eigen::Matrix3f m{{1, 0, 0}, {0, -1, 0}, {0, 0, 1}};
+ // imu->addMagTransformation(m);
+
+ // Emplace the sensor inside the map (TODO CHANGE PERIOD INTO NON MAGIC)
+ SensorInfo info("RotatedIMU", IMU_PERIOD,
+ bind(&HILSensors::imuCallback, this));
+ sensorMap.emplace(make_pair(imu, info));
+}
+
+void HILSensors::dynamicPressureCallback() { return; }
+void HILSensors::pitotCallback() { return; }
+void HILSensors::lps22dfCallback()
+{
+ miosix::PauseKernelLock lock;
+ Logger::getInstance().log(getLPS22DFLastSample());
+}
+void HILSensors::lps28dfw_1Callback()
+{
+ miosix::PauseKernelLock lock;
+ Logger::getInstance().log(
+ static_cast<LPS28DFW_1Data>(getLPS28DFW_1LastSample()));
+}
+void HILSensors::lps28dfw_2Callback()
+{
+ miosix::PauseKernelLock lock;
+ Logger::getInstance().log(
+ static_cast<LPS28DFW_2Data>(getLPS28DFW_2LastSample()));
+}
+void HILSensors::h3lis331dlCallback()
+{
+ miosix::PauseKernelLock lock;
+ Logger::getInstance().log(getH3LIS331DLLastSample());
+}
+void HILSensors::lis2mdlCallback()
+{
+ miosix::PauseKernelLock lock;
+ Logger::getInstance().log(getLIS2MDLLastSample());
+}
+void HILSensors::ubxgpsCallback()
+{
+ miosix::PauseKernelLock lock;
+ Logger::getInstance().log(getGPSLastSample());
+}
+void HILSensors::lsm6dsrxCallback()
+{
+ miosix::PauseKernelLock lock;
+ Logger::getInstance().log(getLSM6DSRXLastSample());
+}
+void HILSensors::ads131m08Callback()
+{
+ miosix::PauseKernelLock lock;
+ Logger::getInstance().log(getADS131M08LastSample());
+}
+void HILSensors::staticPressureCallback()
+{
+ miosix::PauseKernelLock lock;
+ Logger::getInstance().log(getStaticPressureLastSample());
+}
+void HILSensors::imuCallback()
+{
+ miosix::PauseKernelLock lock;
+ Logger::getInstance().log(getIMULastSample());
+}
+} // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/Sensors/HILSensors.h b/src/boards/Payload/Sensors/HILSensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..85830b74d5b83bad213ff26cbc87b3c4c89ee23d
--- /dev/null
+++ b/src/boards/Payload/Sensors/HILSensors.h
@@ -0,0 +1,114 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Authors: Matteo Pignataro, 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/HILSensors/IncludeHILSensors.h>
+#include <sensors/SensorManager.h>
+#include <sensors/analog/Pitot/Pitot.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "Sensors.h"
+
+namespace Payload
+{
+
+class HILSensors : public Sensors
+{
+public:
+ explicit HILSensors(Boardcore::TaskScheduler* sched);
+
+ [[nodiscard]] bool start() override;
+
+ // Sensor getters
+ Boardcore::LPS22DFData getLPS22DFLastSample() override;
+ Boardcore::LPS28DFWData getLPS28DFW_1LastSample() override;
+ Boardcore::LPS28DFWData getLPS28DFW_2LastSample() override;
+ Boardcore::H3LIS331DLData getH3LIS331DLLastSample() override;
+ Boardcore::LIS2MDLData getLIS2MDLLastSample() override;
+ Boardcore::UBXGPSData getGPSLastSample() override;
+ Boardcore::LSM6DSRXData getLSM6DSRXLastSample() override;
+ Boardcore::ADS131M08Data getADS131M08LastSample() override;
+
+ Boardcore::HSCMRNN015PAData getStaticPressureLastSample() override;
+ Boardcore::SSCMRNN030PAData getDynamicPressureLastSample() override;
+ Boardcore::PitotData getPitotLastSample() override;
+
+private:
+ // Init and callbacks methods
+ void temperatureInit();
+
+ void lps22dfInit() override;
+ void lps22dfCallback() override;
+
+ void lps28dfw_1Init();
+ void lps28dfw_1Callback();
+
+ void lps28dfw_2Init();
+ void lps28dfw_2Callback();
+
+ void h3lis331dlInit() override;
+ void h3lis331dlCallback() override;
+
+ void lis2mdlInit() override;
+ void lis2mdlCallback() override;
+
+ void ubxgpsInit() override;
+ void ubxgpsCallback() override;
+
+ void lsm6dsrxInit() override;
+ void lsm6dsrxCallback() override;
+
+ void ads131m08Init() override;
+ void ads131m08Callback() override;
+
+ void staticPressureInit() override;
+ void staticPressureCallback() override;
+
+ void dynamicPressureInit() override;
+ void dynamicPressureCallback() override;
+
+ void pitotInit() override;
+ void pitotCallback() override;
+
+ void imuInit() override;
+ void imuCallback() override;
+
+ // HILSensors instances
+ HILConfig::PayloadHILTemperature* temperature = nullptr;
+ HILConfig::PayloadHILBarometer* lps22df = nullptr;
+ HILConfig::PayloadHILBarometer* lps28dfw_1 = nullptr;
+ HILConfig::PayloadHILBarometer* lps28dfw_2 = nullptr;
+ HILConfig::PayloadHILAccelerometer* h3lis331dl = nullptr;
+ HILConfig::PayloadHILMagnetometer* lis2mdl = nullptr;
+ HILConfig::PayloadHILGps* ubxgps = nullptr;
+ HILConfig::PayloadHILAccelerometer* lsm6dsrx_accel = nullptr;
+ HILConfig::PayloadHILGyroscope* lsm6dsrx_gyro = nullptr;
+ HILConfig::PayloadHILPitot* pitot = nullptr;
+ HILConfig::PayloadHILBarometer* staticPressure = nullptr;
+ HILConfig::PayloadHILBarometer* dynamicPressure = nullptr;
+ // Boardcore::ADS131M08* ads131m08 = nullptr;
+
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("HILSensors");
+};
+} // namespace Payload
\ No newline at end of file
diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h
index faeee0b846d2c6b90b85ad3772128a35ce119fe8..98845bf44824f6584273dd6fdd7b71f19cbb4df3 100644
--- a/src/boards/Payload/Sensors/Sensors.h
+++ b/src/boards/Payload/Sensors/Sensors.h
@@ -51,23 +51,23 @@ class Sensors : public Boardcore::Module
public:
explicit Sensors(Boardcore::TaskScheduler* sched);
- [[nodiscard]] bool start();
+ [[nodiscard]] virtual bool start();
/**
* @brief Stops the sensor manager
* @warning Stops the passed scheduler
*/
- void stop();
+ virtual void stop();
/**
* @brief Returns if all the sensors are started successfully
*/
- bool isStarted();
+ virtual bool isStarted();
/**
* @brief Calibrates the sensors with an offset
*/
- void calibrate();
+ virtual void calibrate();
/**
* @brief Takes the result of the live magnetometer calibration and applies
@@ -75,70 +75,70 @@ public:
*
* @return true if the write was successful
*/
- bool writeMagCalibration();
+ virtual bool writeMagCalibration();
// Sensor getters
- Boardcore::LPS22DFData getLPS22DFLastSample();
- Boardcore::LPS28DFWData getLPS28DFW_1LastSample();
- Boardcore::LPS28DFWData getLPS28DFW_2LastSample();
- Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
- Boardcore::LIS2MDLData getLIS2MDLLastSample();
- Boardcore::UBXGPSData getGPSLastSample();
- Boardcore::LSM6DSRXData getLSM6DSRXLastSample();
- Boardcore::ADS131M08Data getADS131M08LastSample();
+ virtual Boardcore::LPS22DFData getLPS22DFLastSample();
+ virtual Boardcore::LPS28DFWData getLPS28DFW_1LastSample();
+ virtual Boardcore::LPS28DFWData getLPS28DFW_2LastSample();
+ virtual Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
+ virtual Boardcore::LIS2MDLData getLIS2MDLLastSample();
+ virtual Boardcore::UBXGPSData getGPSLastSample();
+ virtual Boardcore::LSM6DSRXData getLSM6DSRXLastSample();
+ virtual Boardcore::ADS131M08Data getADS131M08LastSample();
// Processed getters
- Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
- Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample();
- Boardcore::CurrentData getCurrentLastSample();
- RotatedIMUData getIMULastSample();
- Boardcore::MagnetometerData getCalibratedMagnetometerLastSample();
- Boardcore::HSCMRNN015PAData getStaticPressureLastSample();
- Boardcore::SSCMRNN030PAData getDynamicPressureLastSample();
- Boardcore::PitotData getPitotLastSample();
+ virtual Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
+ virtual Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample();
+ virtual Boardcore::CurrentData getCurrentLastSample();
+ virtual RotatedIMUData getIMULastSample();
+ virtual Boardcore::MagnetometerData getCalibratedMagnetometerLastSample();
+ virtual Boardcore::HSCMRNN015PAData getStaticPressureLastSample();
+ virtual Boardcore::SSCMRNN030PAData getDynamicPressureLastSample();
+ virtual Boardcore::PitotData getPitotLastSample();
void pitotSetReferenceAltitude(float altitude);
void pitotSetReferenceTemperature(float temperature);
std::array<Boardcore::SensorInfo, 8> getSensorInfo();
-private:
+protected:
// Init and callbacks methods
- void lps22dfInit();
- void lps22dfCallback();
+ virtual void lps22dfInit();
+ virtual void lps22dfCallback();
- void lps28dfw_1Init();
- void lps28dfw_1Callback();
+ virtual void lps28dfw_1Init();
+ virtual void lps28dfw_1Callback();
- void lps28dfw_2Init();
- void lps28dfw_2Callback();
+ virtual void lps28dfw_2Init();
+ virtual void lps28dfw_2Callback();
- void h3lis331dlInit();
- void h3lis331dlCallback();
+ virtual void h3lis331dlInit();
+ virtual void h3lis331dlCallback();
- void lis2mdlInit();
- void lis2mdlCallback();
+ virtual void lis2mdlInit();
+ virtual void lis2mdlCallback();
- void ubxgpsInit();
- void ubxgpsCallback();
+ virtual void ubxgpsInit();
+ virtual void ubxgpsCallback();
- void lsm6dsrxInit();
- void lsm6dsrxCallback();
+ virtual void lsm6dsrxInit();
+ virtual void lsm6dsrxCallback();
- void ads131m08Init();
- void ads131m08Callback();
+ virtual void ads131m08Init();
+ virtual void ads131m08Callback();
- void staticPressureInit();
- void staticPressureCallback();
+ virtual void staticPressureInit();
+ virtual void staticPressureCallback();
- void dynamicPressureInit();
- void dynamicPressureCallback();
+ virtual void dynamicPressureInit();
+ virtual void dynamicPressureCallback();
- void pitotInit();
- void pitotCallback();
+ virtual void pitotInit();
+ virtual void pitotCallback();
- void imuInit();
- void imuCallback();
+ virtual void imuInit();
+ virtual void imuCallback();
// Sensors instances
Boardcore::LPS22DF* lps22df = nullptr;
diff --git a/src/boards/Payload/StateMachines/WingController/WingController.h b/src/boards/Payload/StateMachines/WingController/WingController.h
index 1fb84ee6638df0990638911734ca37c7d6f9b8c1..e608a1a77943b33477096e3c79f175a7437190ad 100644
--- a/src/boards/Payload/StateMachines/WingController/WingController.h
+++ b/src/boards/Payload/StateMachines/WingController/WingController.h
@@ -95,6 +95,12 @@ public:
bool start() override;
+ /**
+ * @brief Instance of the Early Maneuver Guidance Algorithm used by
+ * AutomaticWingAlgorithm
+ */
+ EarlyManeuversGuidanceAlgorithm emGuidance;
+
private:
/**
* @brief Method to add the algorithm in the list
@@ -148,12 +154,6 @@ private:
*/
std::atomic<size_t> selectedAlgorithm;
- /**
- * @brief Instance of the Early Maneuver Guidance Algorithm used by
- * AutomaticWingAlgorithm
- */
- EarlyManeuversGuidanceAlgorithm emGuidance;
-
/**
* @brief starts the selected algorithm
*/
diff --git a/src/entrypoints/Payload/payload-entry.cpp b/src/entrypoints/Payload/payload-entry.cpp
index b174e140bdca613a43e8da65d09215c41521cc5e..0a277b411e5f328611a2adcfc4211c0da7f756bb 100644
--- a/src/entrypoints/Payload/payload-entry.cpp
+++ b/src/entrypoints/Payload/payload-entry.cpp
@@ -26,9 +26,11 @@
#include <Payload/BoardScheduler.h>
#include <Payload/Buses.h>
#include <Payload/CanHandler/CanHandler.h>
+#include <Payload/Configs/HILSimulationConfig.h>
#include <Payload/FlightStatsRecorder/FlightStatsRecorder.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>
@@ -51,6 +53,9 @@
using namespace Boardcore;
using namespace Payload;
using namespace Common;
+using namespace HILConfig;
+
+constexpr bool hilSimulationActive = true;
int main()
{
@@ -70,7 +75,9 @@ int main()
// Sensors priority (MAX - 1)
Sensors* sensors =
- new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1));
+ (hilSimulationActive
+ ? new HILSensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1))
+ : new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1)));
// Other critical components (Max - 2)
Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2));
@@ -100,6 +107,95 @@ int main()
Buses* buses = new Buses();
PinHandler* pinHandler = new PinHandler();
+ // HIL
+ if (hilSimulationActive)
+ {
+ PayloadHILTransceiver* hilTransceiver =
+ new PayloadHILTransceiver(buses->usart2);
+ PayloadHILPhasesManager* hilPhasesManager = new PayloadHILPhasesManager(
+ [&]()
+ { return Boardcore::TimedTrajectoryPoint(nas->getNasState()); });
+
+ auto updateActuatorData = [&]()
+ {
+ Boardcore::ModuleManager& modules =
+ Boardcore::ModuleManager::getInstance();
+
+ NASStateHIL nasStateHIL(modules.get<NASController>()->getNasState(),
+ modules.get<NASController>()->getStatus());
+
+ ActuatorsStateHIL actuatorsStateHIL(
+ actuators->getServoPosition(ServosList::AIR_BRAKES_SERVO),
+ actuators->getServoPosition(ServosList::EXPULSION_SERVO),
+ actuators->getServoPosition(ServosList::PARAFOIL_LEFT_SERVO),
+ actuators->getServoPosition(ServosList::PARAFOIL_RIGHT_SERVO),
+ actuators->getServoPosition(ServosList::MAIN_VALVE),
+ actuators->getServoPosition(ServosList::VENTING_VALVE),
+ actuators->getServoPosition(ServosList::RELEASE_VALVE),
+ actuators->getServoPosition(ServosList::FILLING_VALVE),
+ actuators->getServoPosition(ServosList::DISCONNECT_SERVO));
+
+ WESDataHIL wesDataHIL(windEstimation->getWindEstimationScheme());
+
+ auto deltaA =
+ actuators->getServoPosition(ServosList::PARAFOIL_LEFT_SERVO) -
+ actuators->getServoPosition(ServosList::PARAFOIL_RIGHT_SERVO);
+
+ Eigen::Vector2f heading;
+ auto psiRef = wingController->emGuidance.calculateTargetAngle(
+ {nasStateHIL.n, nasStateHIL.e, nasStateHIL.d}, heading);
+
+ GuidanceDataHIL guidanceData(psiRef, deltaA);
+
+ FlagsHIL flags(hilTransceiver->getSensorData()->flags);
+
+ // Returning the feedback for the simulator
+ return ActuatorData(nasStateHIL, actuatorsStateHIL, wesDataHIL,
+ guidanceData, flags, fmm);
+ };
+
+ PayloadHIL* hil = new PayloadHIL(hilTransceiver, hilPhasesManager,
+ updateActuatorData, SIMULATION_PERIOD);
+
+ if (!modules.insert<PayloadHIL>(hil))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the HIL module");
+ }
+ else
+ {
+ LOG_INFO(logger, "Inserted the HIL module");
+ }
+
+ hilPhasesManager->registerToFlightPhase(
+ PayloadFlightPhases::SIM_FLYING,
+ [&]()
+ {
+ printf("Flying\n");
+ EventBroker::getInstance().post(
+ Events::CAN_LIFTOFF, Topics::TOPIC_CAN);
+ });
+
+ hilPhasesManager->registerToFlightPhase(
+ PayloadFlightPhases::ARMED,
+ [&]()
+ {
+ printf("ARMED\n");
+ EventBroker::getInstance().post(
+ Events::CAN_LIFTOFF, Topics::TOPIC_CAN);
+ miosix::ledOn();
+ });
+
+ hilPhasesManager->registerToFlightPhase(
+ PayloadFlightPhases::CALIBRATION_OK,
+ [&]()
+ {
+ TRACE("ARM COMMAND SENT\n");
+ EventBroker::getInstance().post(Events::TMTC_ARM,
+ Topics::TOPIC_TMTC);
+ });
+ }
+
// Insert modules
if (!modules.insert<BoardScheduler>(scheduler))
{
@@ -191,6 +287,20 @@ int main()
}
// Start modules
+ if (hilSimulationActive)
+ {
+ if (!modules.get<PayloadHIL>()->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error starting the HIL module");
+ }
+
+ // Waiting for start of simulation
+ printf("SimulatorData: %d\n", sizeof(SimulatorData)/sizeof(float));
+ printf("ActuatorData: %d\n", sizeof(ActuatorData)/sizeof(float));
+ ModuleManager::getInstance().get<PayloadHIL>()->waitStartSimulation();
+ }
+
if (!Logger::getInstance().testSDCard())
{
initResult = false;