diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4870d33c334a2594066e3201cd12f27f00629e90..8724140c8e6cc6bb6b272006ebf3e14cae583836 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -33,6 +33,11 @@ project(OnBoardSoftware)
# Flight entrypoints #
#-----------------------------------------------------------------------------#
+add_executable(parafoil-entry-milano-hil src/entrypoints/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER} ${HIL})
+target_include_directories(parafoil-entry-milano-hil PRIVATE ${OBSW_INCLUDE_DIRS})
+target_compile_definitions(parafoil-entry-milano-hil PRIVATE MILANO HILParafoil)
+sbs_target(parafoil-entry-milano-hil stm32f429zi_skyward_death_stack_x)
+
add_executable(parafoil-entry-milano src/entrypoints/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER})
target_include_directories(parafoil-entry-milano PRIVATE ${OBSW_INCLUDE_DIRS})
target_compile_definitions(parafoil-entry-milano PRIVATE MILANO)
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 68d231d175cfc5bd6eb7a639dacf2e5a7bea210b..3a26c56283f60dde9dbe7f4e134748281f234be8 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -21,6 +21,7 @@
set(OBSW_INCLUDE_DIRS
src/boards
+ src/hardware_in_the_loop
)
set(PARAFOIL_COMPUTER
@@ -37,4 +38,11 @@ set(PARAFOIL_COMPUTER
src/boards/Parafoil/Wing/FileWingAlgorithm.cpp
src/boards/Parafoil/Wing/WingAlgorithm.cpp
src/boards/Parafoil/WindEstimationScheme/WindEstimation.cpp
+)
+
+set(HIL
+ src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
+ src/boards/Parafoil/Actuators/HILActuators.cpp
+ src/boards/Parafoil/Sensors/HILSensors.cpp
+ src/hardware_in_the_loop/HIL/HILTransceiver.cpp
)
\ No newline at end of file
diff --git a/src/boards/Parafoil/Actuators/Actuators.h b/src/boards/Parafoil/Actuators/Actuators.h
index cddb90369fb27cdd30fe55e2efb3c75ab4491b84..3b6c81af61b0c1e02fc7520da2170929b6116187 100644
--- a/src/boards/Parafoil/Actuators/Actuators.h
+++ b/src/boards/Parafoil/Actuators/Actuators.h
@@ -43,7 +43,7 @@ struct Actuators : public ParafoilModule
* @param percentage Angle to set [0-1].
* @return True if the the angle was set.
*/
- bool setServo(ServosList servoId, float percentage);
+ virtual bool setServo(ServosList servoId, float percentage);
/**
* @brief Moves the specified servo to the given position.
@@ -52,7 +52,7 @@ struct Actuators : public ParafoilModule
* @param angle Angle to set [degree] 0-120.
* @return True if the the angle was set.
*/
- bool setServoAngle(ServosList servoId, float angle);
+ virtual bool setServoAngle(ServosList servoId, float angle);
/**
* @brief Wiggles the servo for few seconds.
diff --git a/src/boards/Parafoil/Actuators/HILActuators.cpp b/src/boards/Parafoil/Actuators/HILActuators.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6866f1a0b2b8762582e10023dfa282a66f506bb4
--- /dev/null
+++ b/src/boards/Parafoil/Actuators/HILActuators.cpp
@@ -0,0 +1,87 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "HILActuators.h"
+
+#include <Parafoil/Configs/ActuatorsConfigs.h>
+#include <common/LedConfig.h>
+#include <interfaces-impl/bsp_impl.h>
+#include <logger/Logger.h>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Common;
+using namespace Parafoil::ActuatorsConfigs;
+
+namespace Parafoil
+{
+
+bool HILActuators::setServo(ServosList servoId, float percentage)
+{
+ if (percentage > WingConfig::MAX_SERVO_APERTURE)
+ percentage = WingConfig::MAX_SERVO_APERTURE;
+ switch (servoId)
+ {
+ case PARAFOIL_LEFT_SERVO:
+ leftServo.setPosition(percentage);
+ Logger::getInstance().log(leftServo.getState());
+ break;
+ case PARAFOIL_RIGHT_SERVO:
+ rightServo.setPosition(percentage);
+ Logger::getInstance().log(rightServo.getState());
+ break;
+ default:
+ return false;
+ }
+
+ return true;
+}
+
+bool HILActuators::setServoAngle(ServosList servoId, float angle)
+{
+ if (angle > WingConfig::MAX_SERVO_APERTURE * LEFT_SERVO_ROTATION)
+ angle = WingConfig::MAX_SERVO_APERTURE * LEFT_SERVO_ROTATION;
+ switch (servoId)
+ {
+ case PARAFOIL_LEFT_SERVO:
+ leftServo.setPosition(angle / LEFT_SERVO_ROTATION);
+ Logger::getInstance().log(leftServo.getState());
+ break;
+ case PARAFOIL_RIGHT_SERVO:
+ rightServo.setPosition(angle / RIGHT_SERVO_ROTATION);
+ Logger::getInstance().log(rightServo.getState());
+ break;
+ default:
+ return false;
+ }
+
+ return true;
+}
+
+HILActuators::HILActuators()
+ : leftServo(SERVO_1_TIMER, SERVO_1_PWM_CH, LEFT_SERVO_MIN_PULSE,
+ LEFT_SERVO_MAX_PULSE),
+ rightServo(SERVO_2_TIMER, SERVO_2_PWM_CH, RIGHT_SERVO_MIN_PULSE,
+ RIGHT_SERVO_MAX_PULSE)
+{
+}
+} // namespace Parafoil
diff --git a/src/boards/Parafoil/Actuators/HILActuators.h b/src/boards/Parafoil/Actuators/HILActuators.h
new file mode 100644
index 0000000000000000000000000000000000000000..97209d27b99e43f9bc86d4e0a3469c3d10825309
--- /dev/null
+++ b/src/boards/Parafoil/Actuators/HILActuators.h
@@ -0,0 +1,63 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Parafoil/ParafoilModule/ParafoilModule.h>
+#include <actuators/Servo/Servo.h>
+#include <common/Mavlink.h>
+#include <interfaces/gpio.h>
+
+#include "Actuators.h"
+#include "HIL_actuators/HILServo.h"
+
+namespace Parafoil
+{
+
+struct HILActuators : public Actuators
+{
+ HILActuators();
+
+ /**
+ * @brief Moves the specified servo to the given position.
+ *
+ * @param servoId Servo to move.
+ * @param percentage Angle to set [0-1].
+ * @return True if the the angle was set.
+ */
+ bool setServo(ServosList servoId, float percentage) override;
+
+ /**
+ * @brief Moves the specified servo to the given position.
+ *
+ * @param servoId Servo to move.
+ * @param angle Angle to set [degree] 0-120.
+ * @return True if the the angle was set.
+ */
+ bool setServoAngle(ServosList servoId, float angle) override;
+
+private:
+ HILServo leftServo;
+ HILServo rightServo;
+};
+
+} // namespace Parafoil
diff --git a/src/hardware_in_the_loop/HILSimulationConfig.h b/src/boards/Parafoil/Configs/HILSimulationConfig.h
similarity index 76%
rename from src/hardware_in_the_loop/HILSimulationConfig.h
rename to src/boards/Parafoil/Configs/HILSimulationConfig.h
index 43f0b32d9da12b579ae030ec01065bd0db17d2fe..b4d20ad9e67fb31427ab4c2ab3d72917549915d5 100644
--- a/src/hardware_in_the_loop/HILSimulationConfig.h
+++ b/src/boards/Parafoil/Configs/HILSimulationConfig.h
@@ -300,104 +300,12 @@ struct ADAdataHIL
typedef struct
{
// Airbrakes opening (percentage)
- float airbrakes_opening;
- /* here 4 bytes of padding in order to align next field (uint64_t) */
-
- // NAS
- Boardcore::NASState nasState;
-
- // ADA
- ADAdataHIL adaState;
+ float left_servo_opening;
+ float right_servo_opening;
void print() const
{
- TRACE(
- "size:%u, %u, %u\n"
- "abk:%f\n"
- "tsnas:%f\n"
- "ned:%f,%f,%f\n"
- "vned:%f,%f,%f\n"
- "q:%f,%f,%f,%f\n"
- "bias:%f,%f,%f\n"
- "tsada:%f\n"
- "ada:%f,%f\n\n",
- sizeof(airbrakes_opening), sizeof(Boardcore::NASState),
- sizeof(ADAdataHIL), airbrakes_opening, nasState.timestamp,
- nasState.n, nasState.e, nasState.d, nasState.vn, nasState.ve,
- nasState.vd, nasState.qx, nasState.qy, nasState.qz, nasState.qw,
- nasState.bx, nasState.by, nasState.bz, adaState.ada_timestamp,
- adaState.aglAltitude, adaState.verticalSpeed);
- }
-} ActuatorData;
-
-/**
- * @brief Data structure in order to store elaborated data by the algorithms
- */
-typedef struct
-{
- // Airbrakes opening (percentage)
- float airbrakes_opening;
-
- // NAS
- std::list<Boardcore::NASState> nasState;
-
- // ADA
- std::list<ADAdataHIL> adaState;
-
- void reset()
- {
- airbrakes_opening = 0;
- nasState.clear();
- adaState.clear();
- }
-
- void setAirBrakesOpening(float airbrakes_opening)
- {
- this->airbrakes_opening = airbrakes_opening;
- };
-
- void addNASState(Boardcore::NASState nasState)
- {
- this->nasState.push_back(nasState);
- };
-
- void addADAState(Boardcore::ADAState adaState)
- {
- ADAdataHIL data{adaState.timestamp, adaState.aglAltitude,
- adaState.verticalSpeed};
-
- this->adaState.push_back(data);
- };
-
- ActuatorData getAvgActuatorData()
- {
- // NAS
- int n = nasState.size();
- float nasTimestampAvg = 0;
- auto nasXAvg = this->nasState.front().getX().setZero();
-
- for (auto it = this->nasState.begin(); it != this->nasState.end(); ++it)
- {
- nasTimestampAvg += (it->timestamp / n);
- nasXAvg += (it->getX() / n);
- }
-
- // normalize quaternions
- nasXAvg.block<4, 1>(Boardcore::NAS::IDX_QUAT, 0).normalize();
- // TRACE("normA: %f\n", nasXAvg.block<4, 1>(Boardcore::NAS::IDX_QUAT,
- // 0).norm());
-
- Boardcore::NASState nasStateAvg(nasTimestampAvg, nasXAvg);
-
- // ADA
- auto adaStateAvg = this->adaState.front() * 0;
- n = adaState.size();
-
- for (auto x : this->adaState)
- {
- adaStateAvg += (x / n);
- }
-
- return ActuatorData{airbrakes_opening, nasStateAvg, adaStateAvg};
+ TRACE("left_opening: %.3f, right_opening: %.3f\n", left_servo_opening,
+ right_servo_opening);
}
-} ElaboratedData;
+} ActuatorData;
\ No newline at end of file
diff --git a/src/boards/Parafoil/Configs/RadioConfig.h b/src/boards/Parafoil/Configs/RadioConfig.h
index df20c18e01729da789fe2143ebda1455c892178f..eac933fd81ca8a5702e3d6a4f7c132e7cf12e2f8 100644
--- a/src/boards/Parafoil/Configs/RadioConfig.h
+++ b/src/boards/Parafoil/Configs/RadioConfig.h
@@ -46,7 +46,7 @@ constexpr uint8_t MAV_COMPONENT_ID = 96;
// XBee parameters
constexpr bool XBEE_80KBPS_DATA_RATE = true;
-constexpr int XBEE_TIMEOUT = 5000; // [ms]
+constexpr int XBEE_TIMEOUT = 5000; // [ms]
// Periodic telemetries frequency
constexpr uint32_t FLIGHT_TM_PERIOD = 250; // [ms]
diff --git a/src/boards/Parafoil/Radio/Radio.cpp b/src/boards/Parafoil/Radio/Radio.cpp
index ff9f358c7c2dcff037bbcba36ebddb3c3c2f3454..451fec7094c1c0dd4eba28473b1484874550e71a 100644
--- a/src/boards/Parafoil/Radio/Radio.cpp
+++ b/src/boards/Parafoil/Radio/Radio.cpp
@@ -34,6 +34,7 @@
#include <Parafoil/TMRepository/TMRepository.h>
#include <common/Events.h>
#include <drivers/interrupt/external_interrupts.h>
+#include <interfaces-impl/hwmapping.h>
#include <radio/Xbee/APIFramesLog.h>
#include <radio/Xbee/ATCommands.h>
diff --git a/src/boards/Parafoil/Sensors/HILSensors.cpp b/src/boards/Parafoil/Sensors/HILSensors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..435aa94e532a521a01f5aeeba07a1fa95d6b6915
--- /dev/null
+++ b/src/boards/Parafoil/Sensors/HILSensors.cpp
@@ -0,0 +1,250 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio, Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "HILSensors.h"
+
+#include <Parafoil/Actuators/Actuators.h>
+#include <Parafoil/Buses.h>
+#include <Parafoil/Configs/SensorsConfig.h>
+#include <common/Events.h>
+#include <common/ReferenceConfig.h>
+#include <drivers/interrupt/external_interrupts.h>
+#include <drivers/usart/USART.h>
+#include <events/EventBroker.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "HILConfig.h"
+
+using namespace std;
+using namespace Boardcore;
+using namespace Common;
+using namespace ReferenceConfig;
+using namespace Parafoil::SensorsConfig;
+
+namespace Parafoil
+{
+
+HILSensors::HILSensors()
+ : state{
+ new HILAccelerometer(N_DATA_ACCEL), new HILBarometer(N_DATA_BARO),
+ new HILPitot(N_DATA_PITOT), new HILGps(N_DATA_GPS),
+ new HILGyroscope(N_DATA_GYRO), new HILMagnetometer(N_DATA_MAGN),
+ new HILTemperature(N_DATA_TEMP), new HILImu(N_DATA_IMU),
+ new HILKalman(N_DATA_KALM)}
+{
+}
+
+HILSensors::~HILSensors()
+{
+ delete bmx160;
+ delete lis3mdl;
+ delete ms5803;
+ delete ubxGps;
+ delete ads1118;
+ delete staticPressure;
+ delete dplPressure;
+ delete pitotPressure;
+
+ delete state.accelerometer;
+ delete state.barometer;
+ delete state.pitot;
+ delete state.gps;
+ delete state.gyro;
+ delete state.magnetometer;
+ delete state.imu;
+ delete state.temperature;
+ delete state.kalman;
+
+ sensorManager->stop();
+ delete sensorManager;
+}
+
+bool HILSensors::startModule()
+{
+ sensorsMap = {{state.accelerometer, accelConfig},
+ {state.barometer, baroConfig},
+ {state.pitot, pitotConfig},
+ {state.magnetometer, magnConfig},
+ {state.imu, imuConfig},
+ {state.gps, gpsConfig},
+ {state.gyro, gyroConfig},
+ {state.temperature, tempConfig},
+ {state.kalman, kalmConfig}};
+
+ // Create the sensor manager
+ sensorManager = new SensorManager(sensorsMap);
+
+ return sensorManager->start();
+}
+
+bool HILSensors::isStarted()
+{
+ return sensorManager->areAllSensorsInitialized();
+}
+
+BMX160Data HILSensors::getBMX160LastSample()
+{
+ miosix::PauseKernelLock lock;
+ return bmx160 != nullptr ? bmx160->getLastSample() : BMX160Data{};
+}
+
+BMX160WithCorrectionData HILSensors::getBMX160WithCorrectionLastSample()
+{
+ miosix::PauseKernelLock lock;
+
+ auto imuData = state.imu->getLastSample();
+ BMX160WithCorrectionData data;
+
+ data.accelerationTimestamp = imuData.accelerationTimestamp;
+ data.accelerationX = imuData.accelerationX;
+ data.accelerationY = imuData.accelerationY;
+ data.accelerationZ = imuData.accelerationZ;
+ data.angularSpeedTimestamp = imuData.angularSpeedTimestamp;
+ data.angularSpeedX = imuData.angularSpeedX;
+ data.angularSpeedY = imuData.angularSpeedY;
+ data.angularSpeedZ = imuData.angularSpeedZ;
+ data.magneticFieldTimestamp = imuData.magneticFieldTimestamp;
+ data.magneticFieldX = imuData.magneticFieldX;
+ data.magneticFieldY = imuData.magneticFieldY;
+ data.magneticFieldZ = imuData.magneticFieldZ;
+
+ return data;
+}
+
+LIS3MDLData HILSensors::getMagnetometerLIS3MDLLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return LIS3MDLData(state.magnetometer->getLastSample(),
+ state.temperature->getLastSample());
+}
+
+MS5803Data HILSensors::getMS5803LastSample()
+{
+ miosix::PauseKernelLock lock;
+
+ auto baroData = state.barometer->getLastSample();
+ auto tempData = state.temperature->getLastSample();
+
+ return MS5803Data(baroData.pressureTimestamp, baroData.pressure,
+ tempData.temperatureTimestamp, tempData.temperature);
+}
+
+UBXGPSData HILSensors::getUbxGpsLastSample()
+{
+ miosix::PauseKernelLock lock;
+ auto data = state.gps->getLastSample();
+ UBXGPSData ubxData;
+
+ ubxData.gpsTimestamp = data.gpsTimestamp;
+ ubxData.latitude = data.latitude;
+ ubxData.longitude = data.longitude;
+ ubxData.height = data.height;
+ ubxData.velocityNorth = data.velocityNorth;
+ ubxData.velocityEast = data.velocityEast;
+ ubxData.velocityDown = data.velocityDown;
+ ubxData.speed = data.speed;
+ ubxData.track = data.track;
+ ubxData.positionDOP = data.positionDOP;
+ ubxData.satellites = data.satellites;
+ ubxData.fix = data.fix;
+
+ return ubxData;
+}
+
+ADS1118Data HILSensors::getADS1118LastSample()
+{
+ miosix::PauseKernelLock lock;
+ return ads1118 != nullptr ? ads1118->getLastSample() : ADS1118Data{};
+}
+
+MPXHZ6130AData HILSensors::getStaticPressureLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return staticPressure != nullptr ? staticPressure->getLastSample()
+ : MPXHZ6130AData{};
+}
+
+SSCDANN030PAAData HILSensors::getDplPressureLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return dplPressure != nullptr ? dplPressure->getLastSample()
+ : SSCDANN030PAAData{};
+}
+
+SSCDRRN015PDAData HILSensors::getPitotPressureLastSample()
+{
+ miosix::PauseKernelLock lock;
+ auto pitotData = state.pitot->getLastSample();
+ SSCDRRN015PDAData data;
+ data.pressureTimestamp = pitotData.timestamp;
+ data.pressure = pitotData.deltaP;
+ return data;
+}
+
+PitotData HILSensors::getPitotLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return state.pitot->getLastSample();
+}
+
+InternalADCData HILSensors::getInternalADCLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return internalADC != nullptr ? internalADC->getLastSample()
+ : InternalADCData{};
+}
+
+BatteryVoltageSensorData HILSensors::getBatteryVoltageLastSample()
+{
+ miosix::PauseKernelLock lock;
+ return batteryVoltage != nullptr ? batteryVoltage->getLastSample()
+ : BatteryVoltageSensorData{};
+}
+
+void HILSensors::calibrate()
+{
+ calibrating = true;
+
+ ms5803Stats.reset();
+ staticPressureStats.reset();
+ dplPressureStats.reset();
+ pitotPressureStats.reset();
+
+ bmx160WithCorrection->startCalibration();
+
+ Thread::sleep(CALIBRATION_DURATION);
+
+ bmx160WithCorrection->stopCalibration();
+
+ // Calibrate the analog pressure sensor to the digital one
+ float ms5803Mean = ms5803Stats.getStats().mean;
+ float staticPressureMean = staticPressureStats.getStats().mean;
+ float dplPressureMean = dplPressureStats.getStats().mean;
+ staticPressure->setOffset(staticPressureMean - ms5803Mean);
+ dplPressure->setOffset(dplPressureMean - ms5803Mean);
+ pitotPressure->setOffset(pitotPressureStats.getStats().mean);
+
+ calibrating = false;
+}
+
+} // namespace Parafoil
diff --git a/src/boards/Parafoil/Sensors/HILSensors.h b/src/boards/Parafoil/Sensors/HILSensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e95dfff6550d5f6a9cdb2e858f30028d4ec4d32
--- /dev/null
+++ b/src/boards/Parafoil/Sensors/HILSensors.h
@@ -0,0 +1,86 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include "HIL.h"
+#include "HIL_algorithms/HILMockKalman.h"
+#include "HIL_sensors/HILSensors.h"
+#include "Sensors.h"
+
+namespace Parafoil
+{
+
+class HILSensors : public Sensors
+{
+
+public:
+ HILSensors();
+
+ ~HILSensors();
+
+ bool startModule() override;
+
+ bool isStarted() override;
+
+ Boardcore::BMX160Data getBMX160LastSample() override;
+ Boardcore::BMX160WithCorrectionData getBMX160WithCorrectionLastSample()
+ override;
+ Boardcore::LIS3MDLData getMagnetometerLIS3MDLLastSample() override;
+ Boardcore::MS5803Data getMS5803LastSample() override;
+ Boardcore::UBXGPSData getUbxGpsLastSample() override;
+
+ Boardcore::ADS1118Data getADS1118LastSample() override;
+ Boardcore::MPXHZ6130AData getStaticPressureLastSample() override;
+ Boardcore::SSCDANN030PAAData getDplPressureLastSample() override;
+ Boardcore::SSCDRRN015PDAData getPitotPressureLastSample() override;
+ Boardcore::PitotData getPitotLastSample() override;
+ Boardcore::InternalADCData getInternalADCLastSample() override;
+ Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample() override;
+
+ /**
+ * @brief Blocking function that calibrates the sensors.
+ *
+ * The calibration works by capturing the mean of the sensor readings and
+ * then removing the offsets from the desired values.
+ */
+ void calibrate() override;
+
+private:
+ /**
+ * structure that contains all the sensors used in the simulation
+ */
+ struct StateComplete
+ {
+ HILAccelerometer* accelerometer;
+ HILBarometer* barometer;
+ HILPitot* pitot;
+ HILGps* gps;
+ HILGyroscope* gyro;
+ HILMagnetometer* magnetometer;
+ HILTemperature* temperature;
+ HILImu* imu;
+ HILKalman* kalman;
+ } state;
+};
+
+} // namespace Parafoil
diff --git a/src/boards/Parafoil/Sensors/Sensors.cpp b/src/boards/Parafoil/Sensors/Sensors.cpp
index 26da58904e905e98b1c73fbcb779ffd77de6013e..b747355438ee7c66a2afb723714cba68d161d968 100644
--- a/src/boards/Parafoil/Sensors/Sensors.cpp
+++ b/src/boards/Parafoil/Sensors/Sensors.cpp
@@ -71,29 +71,6 @@ bool Sensors::startModule()
bmx160Init();
bmx160WithCorrectionInit();
-#ifdef HILSimulation
- // Definition of the fake sensors for the simulation
- state.accelerometer = new HILAccelerometer(N_DATA_ACCEL);
- state.barometer = new HILBarometer(N_DATA_BARO);
- state.pitot = new HILPitot(N_DATA_PITOT);
- state.gps = new HILGps(N_DATA_GPS);
- state.gyro = new HILGyroscope(N_DATA_GYRO);
- state.magnetometer = new HILMagnetometer(N_DATA_MAGN);
- state.imu = new HILImu(N_DATA_IMU);
- state.temperature = new HILTemperature(N_DATA_TEMP);
- state.kalman = new HILKalman(N_DATA_KALM);
-
- sensorsMap = {{state.accelerometer, accelConfig},
- {state.barometer, baroConfig},
- {state.pitot, pitotConfig},
- {state.magnetometer, magnConfig},
- {state.imu, imuConfig},
- {state.gps, gpsConfig},
- {state.gyro, gyroConfig},
- {state.temperature, tempConfig},
- {state.kalman, kalmConfig}};
-#endif
-
// Create the sensor manager
sensorManager = new SensorManager(sensorsMap);
miosix::GpioPin interruptPin = miosix::sensors::bmx160::intr::getPin();
@@ -114,81 +91,27 @@ BMX160Data Sensors::getBMX160LastSample()
BMX160WithCorrectionData Sensors::getBMX160WithCorrectionLastSample()
{
miosix::PauseKernelLock lock;
-#ifndef HILSimulation
return bmx160WithCorrection != nullptr
? bmx160WithCorrection->getLastSample()
: BMX160WithCorrectionData{};
-#else
- auto imuData = state.imu->getLastSample();
- BMX160WithCorrectionData data;
-
- data.accelerationTimestamp = imuData.accelerationTimestamp;
- data.accelerationX = imuData.accelerationX;
- data.accelerationY = imuData.accelerationY;
- data.accelerationZ = imuData.accelerationZ;
- data.angularSpeedTimestamp = imuData.angularSpeedTimestamp;
- data.angularSpeedX = imuData.angularSpeedX;
- data.angularSpeedY = imuData.angularSpeedY;
- data.angularSpeedZ = imuData.angularSpeedZ;
- data.magneticFieldTimestamp = imuData.magneticFieldTimestamp;
- data.magneticFieldX = imuData.magneticFieldX;
- data.magneticFieldY = imuData.magneticFieldY;
- data.magneticFieldZ = imuData.magneticFieldZ;
-
- return data;
-#endif
}
LIS3MDLData Sensors::getMagnetometerLIS3MDLLastSample()
{
miosix::PauseKernelLock lock;
-#ifndef HILSimulation
return lis3mdl != nullptr ? lis3mdl->getLastSample() : LIS3MDLData{};
-#else
- return LIS3MDLData(state.magnetometer->getLastSample(),
- state.temperature->getLastSample());
-#endif
}
MS5803Data Sensors::getMS5803LastSample()
{
miosix::PauseKernelLock lock;
-
-#ifndef HILSimulation
return ms5803 != nullptr ? ms5803->getLastSample() : MS5803Data{};
-#else
- auto baroData = state.barometer->getLastSample();
- auto tempData = state.temperature->getLastSample();
-
- return MS5803Data(baroData.pressureTimestamp, baroData.pressure,
- tempData.temperatureTimestamp, tempData.temperature);
-#endif
}
UBXGPSData Sensors::getUbxGpsLastSample()
{
miosix::PauseKernelLock lock;
-#ifndef HILSimulation
return ubxGps != nullptr ? ubxGps->getLastSample() : UBXGPSData{};
-#else
- auto data = state.gps->getLastSample();
- UBXGPSData ubxData;
-
- ubxData.gpsTimestamp = data.gpsTimestamp;
- ubxData.latitude = data.latitude;
- ubxData.longitude = data.longitude;
- ubxData.height = data.height;
- ubxData.velocityNorth = data.velocityNorth;
- ubxData.velocityEast = data.velocityEast;
- ubxData.velocityDown = data.velocityDown;
- ubxData.speed = data.speed;
- ubxData.track = data.track;
- ubxData.positionDOP = data.positionDOP;
- ubxData.satellites = data.satellites;
- ubxData.fix = data.fix;
-
- return ubxData;
-#endif
}
ADS1118Data Sensors::getADS1118LastSample()
@@ -214,26 +137,14 @@ SSCDANN030PAAData Sensors::getDplPressureLastSample()
SSCDRRN015PDAData Sensors::getPitotPressureLastSample()
{
miosix::PauseKernelLock lock;
-#ifndef HILSimulation
return pitotPressure != nullptr ? pitotPressure->getLastSample()
: SSCDRRN015PDAData{};
-#else
- auto pitotData = state.pitot->getLastSample();
- SSCDRRN015PDAData data;
- data.pressureTimestamp = pitotData.timestamp;
- data.pressure = pitotData.deltaP;
- return data;
-#endif
}
PitotData Sensors::getPitotLastSample()
{
miosix::PauseKernelLock lock;
-#ifndef HILSimulation
return pitot != nullptr ? pitot->getLastSample() : PitotData{};
-#else
- return state.pitot->getLastSample();
-#endif
}
InternalADCData Sensors::getInternalADCLastSample()
@@ -322,18 +233,6 @@ Sensors::~Sensors()
delete dplPressure;
delete pitotPressure;
-#ifdef HILSimulation
- delete state.accelerometer;
- delete state.barometer;
- delete state.pitot;
- delete state.gps;
- delete state.gyro;
- delete state.magnetometer;
- delete state.imu;
- delete state.temperature;
- delete state.kalman;
-#endif
-
sensorManager->stop();
delete sensorManager;
}
diff --git a/src/boards/Parafoil/Sensors/Sensors.h b/src/boards/Parafoil/Sensors/Sensors.h
index 9f02e58a39f48d5c6e69926d98522b21bcb7ebf4..a25a743abebbb3371e7d6921ebf17564877c2f22 100644
--- a/src/boards/Parafoil/Sensors/Sensors.h
+++ b/src/boards/Parafoil/Sensors/Sensors.h
@@ -21,7 +21,7 @@
*/
#pragma once
-
+#include <Parafoil/ParafoilModule/ParafoilModule.h>
#include <drivers/adc/InternalADC.h>
#include <drivers/adc/InternalTemp.h>
#include <scheduler/TaskScheduler.h>
@@ -37,11 +37,6 @@
#include <sensors/analog/pressure/honeywell/SSCDANN030PAA.h>
#include <sensors/analog/pressure/honeywell/SSCDRRN015PDA.h>
#include <sensors/analog/pressure/nxp/MPXHZ6130A.h>
-#ifdef HILSimulation
-#include <HIL_algorithms/HILMockKalman.h>
-#include <HIL_sensors/HILSensors.h>
-#endif // HILSimulation
-#include <Parafoil/ParafoilModule/ParafoilModule.h>
namespace Parafoil
{
@@ -54,44 +49,26 @@ public:
~Sensors();
- bool startModule() override;
-
- bool isStarted();
+ virtual bool startModule();
-#ifdef HILSimulation
-public:
- /**
- * structure that contains all the sensors used in the simulation
- */
- struct StateComplete
- {
- HILAccelerometer* accelerometer;
- HILBarometer* barometer;
- HILPitot* pitot;
- HILGps* gps;
- HILGyroscope* gyro;
- HILMagnetometer* magnetometer;
- HILTemperature* temperature;
- HILImu* imu;
- HILKalman* kalman;
- } state;
-#endif // HILSimulation
+ virtual bool isStarted();
Boardcore::BMX160* bmx160;
- Boardcore::BMX160Data getBMX160LastSample();
- Boardcore::BMX160WithCorrectionData getBMX160WithCorrectionLastSample();
- Boardcore::LIS3MDLData getMagnetometerLIS3MDLLastSample();
- Boardcore::MS5803Data getMS5803LastSample();
- Boardcore::UBXGPSData getUbxGpsLastSample();
-
- Boardcore::ADS1118Data getADS1118LastSample();
- Boardcore::MPXHZ6130AData getStaticPressureLastSample();
- Boardcore::SSCDANN030PAAData getDplPressureLastSample();
- Boardcore::SSCDRRN015PDAData getPitotPressureLastSample();
- Boardcore::PitotData getPitotLastSample();
- Boardcore::InternalADCData getInternalADCLastSample();
- Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
+ virtual Boardcore::BMX160Data getBMX160LastSample();
+ virtual Boardcore::BMX160WithCorrectionData
+ getBMX160WithCorrectionLastSample();
+ virtual Boardcore::LIS3MDLData getMagnetometerLIS3MDLLastSample();
+ virtual Boardcore::MS5803Data getMS5803LastSample();
+ virtual Boardcore::UBXGPSData getUbxGpsLastSample();
+
+ virtual Boardcore::ADS1118Data getADS1118LastSample();
+ virtual Boardcore::MPXHZ6130AData getStaticPressureLastSample();
+ virtual Boardcore::SSCDANN030PAAData getDplPressureLastSample();
+ virtual Boardcore::SSCDRRN015PDAData getPitotPressureLastSample();
+ virtual Boardcore::PitotData getPitotLastSample();
+ virtual Boardcore::InternalADCData getInternalADCLastSample();
+ virtual Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
/**
* @brief Blocking function that calibrates the sensors.
@@ -99,14 +76,14 @@ public:
* The calibration works by capturing the mean of the sensor readings and
* then removing the offsets from the desired values.
*/
- void calibrate();
+ virtual void calibrate();
void pitotSetReferenceAltitude(float altitude);
void pitotSetReferenceTemperature(float temperature);
std::map<string, bool> getSensorsState();
-private:
+protected:
void bmx160Init();
void bmx160Callback();
@@ -134,7 +111,7 @@ private:
Boardcore::BMX160WithCorrection* bmx160WithCorrection;
Boardcore::LIS3MDL* lis3mdl;
- Boardcore::MS5803* ms5803; // barometro digitale
+ Boardcore::MS5803* ms5803; // digital barometer
Boardcore::UBXGPSSerial* ubxGps;
Boardcore::ADS1118* ads1118; // adc
diff --git a/src/entrypoints/Parafoil/parafoil-entry.cpp b/src/entrypoints/Parafoil/parafoil-entry.cpp
index 0ad17d9b218f912ecf2e52aa0c24ca478e3df194..fdaf3ec47bf21f638c0ffafd70bb991b56e7a634 100644
--- a/src/entrypoints/Parafoil/parafoil-entry.cpp
+++ b/src/entrypoints/Parafoil/parafoil-entry.cpp
@@ -19,6 +19,12 @@
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
+#ifdef HILParafoil
+#include <HIL.h>
+#include <HILConfig.h>
+#include <Parafoil/Actuators/HILActuators.h>
+#include <Parafoil/Sensors/HILSensors.h>
+#endif
#include <Parafoil/Actuators/Actuators.h>
#include <Parafoil/AltitudeTrigger/AltitudeTrigger.h>
@@ -71,19 +77,45 @@ int main()
}
if (!Logger::getInstance().start())
{
- initResult = false;
+ initResult = false; // This fails in HIL
LOG_ERR(logger, "Error initializing the Logger");
}
}
// Initialize the modules
{
+#ifndef HILParafoil
if (!modules.insert<Actuators>(new Actuators()))
{
initResult = false;
LOG_ERR(logger, "Error inserting the Actuators module");
}
+ if (!modules.insert<Sensors>(new Sensors()))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the Sensors module");
+ }
+#else
+ if (!modules.insert<HIL>(new HIL()))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the HIL module");
+ }
+
+ if (!modules.insert<Actuators>(new HILActuators()))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the HILActuators module");
+ }
+
+ if (!modules.insert<Sensors>(new HILSensors()))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the Sensors module");
+ }
+#endif
+
if (!modules.insert<AltitudeTrigger>(new AltitudeTrigger()))
{
initResult = false;
@@ -120,12 +152,6 @@ int main()
LOG_ERR(logger, "Error inserting the Radio module");
}
- if (!modules.insert<Sensors>(new Sensors()))
- {
- initResult = false;
- LOG_ERR(logger, "Error inserting the Sensors module");
- }
-
if (!modules.insert<TMRepository>(new TMRepository()))
{
initResult = false;
@@ -147,6 +173,14 @@ int main()
// Start the modules
{
+#ifdef HILParafoil
+ if (!modules.get<HIL>()->startModule())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error inserting the HIL module");
+ }
+#endif
+
if (!modules.get<Actuators>()->startModule())
{
initResult = false;
@@ -213,6 +247,13 @@ int main()
LOG_ERR(logger, "Error starting the WingController module");
}
}
+
+#ifdef HILParafoil
+ // Task to send periodically the elaborated actuator data
+ BoardScheduler::getInstance().getScheduler().addTask(
+ [&]() { modules.get<HIL>()->send(); }, SIMULATION_PERIOD);
+#endif
+
WingConfig::WingConfigStruct f;
modules.get<WingController>()->addAlgorithm(WingConfig::SELECTED_ALGORITHM);
Logger::getInstance().log(f); // logs the config file
diff --git a/src/hardware_in_the_loop/HIL.h b/src/hardware_in_the_loop/HIL.h
index 5b2d619ba68af3b956949097a565dac773e0fabd..e1963feb0e34276bb9462ccca3e49da9af70fe0f 100644
--- a/src/hardware_in_the_loop/HIL.h
+++ b/src/hardware_in_the_loop/HIL.h
@@ -24,33 +24,32 @@
#include <HIL/HILFlightPhasesManager.h>
#include <HIL/HILTransceiver.h>
+#include <Parafoil/ParafoilModule/ParafoilModule.h>
/**
* @brief Single interface to the hardware-in-the-loop framework.
*/
-class HIL : public Boardcore::Singleton<HIL>
+class HIL : public Parafoil::ParafoilModule
{
- friend class Boardcore::Singleton<HIL>;
-
public:
- HILTransceiver *simulator;
- HILFlightPhasesManager *flightPhasesManager;
+ HILTransceiver *simulator = nullptr;
+ HILFlightPhasesManager *flightPhasesManager = nullptr;
+
+ HIL() {}
/**
* @brief Start the needed hardware-in-the-loop components.
*/
- bool start() { return simulator->start() && flightPhasesManager->start(); }
+ bool startModule()
+ {
+ flightPhasesManager = new HILFlightPhasesManager();
+ simulator = new HILTransceiver();
+ return simulator->start() && flightPhasesManager->start();
+ }
void stop() { simulator->stop(); }
- void send(float airbrakes_opening)
- {
- // TRACE("[HIL] Sending\n");
-
- elaboratedData.setAirBrakesOpening(airbrakes_opening);
- simulator->setActuatorData(elaboratedData.getAvgActuatorData());
- elaboratedData.reset();
- }
+ void send() { simulator->setActuatorData(actuatorsData); }
/**
* @brief method that signals to the simulator that the liftoff pin has
@@ -60,19 +59,10 @@ public:
{
flightPhasesManager->setFlagFlightPhase(
FlightPhases::LIFTOFF_PIN_DETACHED, true);
-
- // start code for the flight
- this->send(-1);
}
- ElaboratedData *getElaboratedData() { return &elaboratedData; }
+ ActuatorData &getActuatorData() { return actuatorsData; }
private:
- HIL()
- {
- flightPhasesManager = new HILFlightPhasesManager();
- simulator = new HILTransceiver();
- }
-
- ElaboratedData elaboratedData;
+ ActuatorData actuatorsData{0};
};
diff --git a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
index 8bcadd31aa1090b9f343d9bea4085761d19089bc..6db901074e0b202b6fa4df1b34f553cd91740369 100644
--- a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
+++ b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.cpp
@@ -26,9 +26,6 @@
#include "events/EventBroker.h"
-/* CPUMeter */
-// #include "diagnostic/CpuMeter/CpuMeter.h"
-
using namespace Common;
HILFlightPhasesManager::HILFlightPhasesManager()
@@ -212,7 +209,7 @@ void HILFlightPhasesManager::handleEvent(const Boardcore::Event& e)
changed_flags.push_back(FlightPhases::CALIBRATION);
break;
case NAS_READY:
- case FMM_ALGOS_CAL_DONE:
+ // case FMM_ALGOS_CAL_DONE:
setFlagFlightPhase(FlightPhases::CALIBRATION_OK, true);
TRACE("[HIL] CALIBRATION OK!\n");
changed_flags.push_back(FlightPhases::CALIBRATION_OK);
diff --git a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h
index 44c53b8e86d4415ac509f5a33259534f346d9524..440cd916271f689cc84230b0e449a0c954866a19 100644
--- a/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h
+++ b/src/hardware_in_the_loop/HIL/HILFlightPhasesManager.h
@@ -23,9 +23,8 @@
#pragma once
#include <HIL/HILTransceiver.h>
-#include <Main/BoardScheduler.h>
#include <algorithms/AirBrakes/TrajectoryPoint.h>
-#include <common/events/Events.h>
+#include <common/Events.h>
#include <drivers/timer/TimestampTimer.h>
#include <events/Event.h>
#include <events/EventHandler.h>
diff --git a/src/hardware_in_the_loop/HIL/HILTransceiver.cpp b/src/hardware_in_the_loop/HIL/HILTransceiver.cpp
index f5e23848123a92fadb8c29974db5fc6abe64eaa2..41a173faa5455adba25f159d1abf550bfc1bf990 100644
--- a/src/hardware_in_the_loop/HIL/HILTransceiver.cpp
+++ b/src/hardware_in_the_loop/HIL/HILTransceiver.cpp
@@ -25,16 +25,17 @@
#ifdef PAYLOAD_ENTRY
#include <Payload/Buses.h>
using namespace Payload;
-#else
-#include <Main/Buses.h>
-using namespace Main;
+#elif defined(HILParafoil)
+#include <Parafoil/Buses.h>
+using namespace Parafoil;
#endif
/**
* @brief Construct a serial connection attached to a control algorithm
*/
HILTransceiver::HILTransceiver()
- : hilSerial(Buses::getInstance().uart4), actuatorData(ActuatorData{})
+ : hilSerial(Boardcore::ModuleManager::getInstance().get<Buses>()->uart4),
+ actuatorData(ActuatorData{})
{
}
@@ -102,7 +103,7 @@ void HILTransceiver::run()
TRACE("[HILT] Transceiver started\n");
bool lostUpdate = false;
HILFlightPhasesManager *flightPhasesManager =
- HIL::getInstance().flightPhasesManager;
+ Boardcore::ModuleManager::getInstance().get<HIL>()->flightPhasesManager;
while (true)
{
diff --git a/src/hardware_in_the_loop/HILConfig.h b/src/hardware_in_the_loop/HILConfig.h
index 18834996c2fc9413487ad8d57a3e477e7e9b7e28..b0f6ea593b57889114d08562fe47a7e44ce0f4a5 100644
--- a/src/hardware_in_the_loop/HILConfig.h
+++ b/src/hardware_in_the_loop/HILConfig.h
@@ -40,8 +40,8 @@
*/
/* Hardware in the loop entrypoint */
-#if defined(HILSimulation)
-#include <HILSimulationConfig.h>
+#if defined(HILParafoil)
+#include <Parafoil/Configs/HILSimulationConfig.h>
/*
#elif defined(HIL_<tuoFlag>)
#include "<test-directory>/HILSimulationConfig.h"
diff --git a/src/hardware_in_the_loop/HIL_actuators/HILServo.h b/src/hardware_in_the_loop/HIL_actuators/HILServo.h
index 947bd8ae87e0f30d7b69306ebfc6087dfafd87bd..a4370dfaf65b849dba7c0b3d1f5a7b61740b5791 100644
--- a/src/hardware_in_the_loop/HIL_actuators/HILServo.h
+++ b/src/hardware_in_the_loop/HIL_actuators/HILServo.h
@@ -36,6 +36,4 @@ public:
: Servo(timer, pwmChannel, minPulse, maxPulse, frequency)
{
}
-
- void sendToSimulator() { HIL::getInstance().send(getPosition()); }
};
diff --git a/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h b/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h
index 3057ea203b14593d45dd29702cb070ac73a13744..2a9e5b032cd797008d7b8812e61d9a77679424ba 100644
--- a/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h
+++ b/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h
@@ -22,13 +22,13 @@
#pragma once
+#include "HIL_sensors/HILAccelerometer.h"
+#include "HIL_sensors/HILBarometer.h"
+#include "HIL_sensors/HILGps.h"
+#include "HIL_sensors/HILGyroscope.h"
+#include "HIL_sensors/HILMagnetometer.h"
+#include "HIL_sensors/HILSensor.h"
#include "NavigationSystem/NASController.h"
-#include "hardware_in_the_loop/HIL_sensors/HILAccelerometer.h"
-#include "hardware_in_the_loop/HIL_sensors/HILBarometer.h"
-#include "hardware_in_the_loop/HIL_sensors/HILGps.h"
-#include "hardware_in_the_loop/HIL_sensors/HILGyroscope.h"
-#include "hardware_in_the_loop/HIL_sensors/HILMagnetometer.h"
-#include "hardware_in_the_loop/HIL_sensors/HILSensor.h"
struct HILNasData : public TimestampData
{
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h b/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h
index 1a2dc3f5ddcec50d273b1ee907d0eab1042c136f..f28499c34537181d17aad08f7581a35fadc44184 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILGyroscope.h
@@ -45,10 +45,10 @@ protected:
* struct */
Boardcore::Vec3 matlabData = sensorData->gyro.measures[sampleCounter];
- tempData.angularVelocityX = matlabData.getX();
- tempData.angularVelocityY = matlabData.getY();
- tempData.angularVelocityZ = matlabData.getZ();
- tempData.angularVelocityTimestamp = updateTimestamp();
+ tempData.angularSpeedX = matlabData.getX();
+ tempData.angularSpeedY = matlabData.getY();
+ tempData.angularSpeedZ = matlabData.getZ();
+ tempData.angularSpeedTimestamp = updateTimestamp();
return tempData;
}
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILImu.h b/src/hardware_in_the_loop/HIL_sensors/HILImu.h
index 6c2ee30ac6744c197d02d5c8aac368c29de2e3ea..d870872e119e8dae757b896b690c430f4b5050a7 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILImu.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILImu.h
@@ -52,10 +52,10 @@ protected:
tempData.accelerationY = matlabData.getY();
tempData.accelerationZ = matlabData.getZ();
- matlabData = sensorData->gyro.measures[sampleCounter];
- tempData.angularVelocityX = matlabData.getX();
- tempData.angularVelocityY = matlabData.getY();
- tempData.angularVelocityZ = matlabData.getZ();
+ matlabData = sensorData->gyro.measures[sampleCounter];
+ tempData.angularSpeedX = matlabData.getX();
+ tempData.angularSpeedY = matlabData.getY();
+ tempData.angularSpeedZ = matlabData.getZ();
matlabData = sensorData->magnetometer.measures[sampleCounter];
tempData.magneticFieldX = matlabData.getX();
@@ -65,9 +65,9 @@ protected:
// only update the timestamp once and use it for all the 3 sensors
// (this sensor assumes the same frequency for accel, gyro and mag)
- tempData.accelerationTimestamp = updateTimestamp();
- tempData.angularVelocityTimestamp = tempData.accelerationTimestamp;
- tempData.magneticFieldTimestamp = tempData.accelerationTimestamp;
+ tempData.accelerationTimestamp = updateTimestamp();
+ tempData.angularSpeedTimestamp = tempData.accelerationTimestamp;
+ tempData.magneticFieldTimestamp = tempData.accelerationTimestamp;
return tempData;
}
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensor.h b/src/hardware_in_the_loop/HIL_sensors/HILSensor.h
index cd77b1198f2584a0f2001ca89e5a083ddc37e360..328dcadb1efac2f78d2d9d1cbce1e756f8a551a6 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILSensor.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILSensor.h
@@ -25,6 +25,7 @@
#include <HIL/Vec3.h>
#include <typeinfo>
+#include <utils/ModuleManager/ModuleManager.hpp>
#include "HIL.h"
#include "HILConfig.h"
@@ -50,19 +51,9 @@ public:
/**
* @brief constructor of the fake sensor used for the simulation.
*
- * @param matlab reference of the MatlabTransceiver object that deals with
- * the simulator
* @param n_data_sensor number of samples in every period of simulation
*/
- HILSensor(int n_data_sensor)
- {
- this->sensorData = HIL::getInstance().simulator->getSensorData();
- this->n_data_sensor = n_data_sensor;
-
- /* Registers the sensor on the MatlabTransceiver to be notified when a
- * new packet of simulated data arrives */
- HIL::getInstance().simulator->addResetSampleCounter(this);
- }
+ HILSensor(int n_data_sensor) { this->n_data_sensor = n_data_sensor; }
/**
* @brief sets the sample counter to 0.
@@ -82,6 +73,16 @@ public:
*/
bool init() override
{
+ this->sensorData = Boardcore::ModuleManager::getInstance()
+ .get<HIL>()
+ ->simulator->getSensorData();
+
+ /* Registers the sensor on the MatlabTransceiver to be notified when a
+ * new packet of simulated data arrives */
+ Boardcore::ModuleManager::getInstance()
+ .get<HIL>()
+ ->simulator->addResetSampleCounter(this);
+
if (initialized)
{
this->lastError = Boardcore::SensorErrors::ALREADY_INIT;
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensors.h b/src/hardware_in_the_loop/HIL_sensors/HILSensors.h
index 79fdf3dcb4477b698f97d0109a6444708a06ad0d..3a150a5b38832d66f534bf12e24122a30ccef25e 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILSensors.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILSensors.h
@@ -22,13 +22,14 @@
#pragma once
-#include "HILAccelerometer.h"
-#include "HILBarometer.h"
-#include "HILGps.h"
-#include "HILGyroscope.h"
-#include "HILImu.h"
-#include "HILMagnetometer.h"
-#include "HILPitot.h"
-#include "HILSensor.h"
-#include "HILTemperature.h"
-#include "HILTimestampManagement.h"
\ No newline at end of file
+#include "HIL_algorithms/HILMockKalman.h"
+#include "HIL_sensors/HILAccelerometer.h"
+#include "HIL_sensors/HILBarometer.h"
+#include "HIL_sensors/HILGps.h"
+#include "HIL_sensors/HILGyroscope.h"
+#include "HIL_sensors/HILImu.h"
+#include "HIL_sensors/HILMagnetometer.h"
+#include "HIL_sensors/HILPitot.h"
+#include "HIL_sensors/HILSensor.h"
+#include "HIL_sensors/HILTemperature.h"
+#include "HIL_sensors/HILTimestampManagement.h"
diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h b/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h
index 62f276016a30710f034397199d6a2ace20c5ddf0..57061105f6d279491c4a3635d954b0ecb1209e43 100644
--- a/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h
+++ b/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h
@@ -21,6 +21,7 @@
*/
#include <sensors/SensorData.h>
+#include <sensors/analog/Pitot/PitotData.h>
struct HILAccelData : public Boardcore::AccelerometerData
{
@@ -40,13 +41,13 @@ struct HILGyroscopeData : public Boardcore::GyroscopeData
{
static std::string header()
{
- return "timestamp,angularVelocityX,angularVelocityY,angularVelocityZ\n";
+ return "timestamp,angularSpeedX,angularSpeedY,angularSpeedZ\n";
}
void print(std::ostream& os) const
{
- os << angularVelocityTimestamp << "," << angularVelocityX << ","
- << angularVelocityY << "," << angularVelocityZ << "\n";
+ os << angularSpeedTimestamp << "," << angularSpeedX << ","
+ << angularSpeedY << "," << angularSpeedZ << "\n";
}
};
@@ -71,8 +72,8 @@ struct HILImuData : public HILAccelData,
static std::string header()
{
return "accelerationTimestamp,accelerationX,accelerationY,"
- "accelerationZ,angularVelocityTimestamp,angularVelocityX,"
- "angularVelocityY,angularVelocityZ,magneticFieldTimestamp,"
+ "accelerationZ,angularSpeedTimestamp,angularSpeedX,"
+ "angularSpeedY,angularSpeedZ,magneticFieldTimestamp,"
"magneticFieldX,magneticFieldY,magneticFieldZ\n";
}
@@ -80,8 +81,8 @@ struct HILImuData : public HILAccelData,
{
os << accelerationTimestamp << "," << accelerationX << ","
<< accelerationY << "," << accelerationZ << ","
- << angularVelocityTimestamp << "," << angularVelocityX << ","
- << angularVelocityY << "," << angularVelocityZ << ","
+ << angularSpeedTimestamp << "," << angularSpeedX << ","
+ << angularSpeedY << "," << angularSpeedZ << ","
<< magneticFieldTimestamp << "," << magneticFieldX << ","
<< magneticFieldY << "," << magneticFieldZ << "\n";
}
@@ -91,8 +92,8 @@ struct HILGpsData : public Boardcore::GPSData
{
static std::string header()
{
- return "timestamp,latitude,longitude,height,velocityNorth,velocityEast,"
- "velocityDown,speed,track,positionDOP,satellites,fix\n";
+ return "timestamp,latitude,longitude,height,speedNorth,speedEast,"
+ "SpeedDown,speed,track,positionDOP,satellites,fix\n";
}
void print(std::ostream& os) const